diff --git a/awapi/awapi_awiv_adapter/CMakeLists.txt b/awapi/awapi_awiv_adapter/CMakeLists.txt deleted file mode 100644 index 2f6171b969440..0000000000000 --- a/awapi/awapi_awiv_adapter/CMakeLists.txt +++ /dev/null @@ -1,35 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(awapi_awiv_adapter) - -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) - set(CMAKE_CXX_STANDARD_REQUIRED ON) - set(CMAKE_CXX_EXTENSIONS OFF) -endif() -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic -Werror) -endif() - -find_package(ament_cmake_auto REQUIRED) -ament_auto_find_build_dependencies() - -ament_auto_add_executable(awapi_awiv_adapter - src/awapi_awiv_adapter_node.cpp - src/awapi_awiv_adapter_core.cpp - src/awapi_vehicle_state_publisher.cpp - src/awapi_autoware_state_publisher.cpp - src/awapi_stop_reason_aggregator.cpp - src/awapi_v2x_aggregator.cpp - src/awapi_lane_change_state_publisher.cpp - src/awapi_obstacle_avoidance_state_publisher.cpp - src/awapi_max_velocity_publisher.cpp - src/awapi_autoware_util.cpp - src/awapi_pacmod_util.cpp -) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() - -ament_auto_package(INSTALL_TO_SHARE launch) diff --git a/awapi/awapi_awiv_adapter/Readme.md b/awapi/awapi_awiv_adapter/Readme.md deleted file mode 100644 index b762812173e22..0000000000000 --- a/awapi/awapi_awiv_adapter/Readme.md +++ /dev/null @@ -1,299 +0,0 @@ -# awapi_awiv_adapter - -✓: confirmed, (blank): not confirmed - -## get topic - -### /awapi/vehicle/get/status - -- get vehicle status -- MessageType: awapi_awiv_adapter/AwapiVehicleStatus - -| ✓ | type | name | unit | note | -| --- | :----------------------- | :----------------------- | :----------------------------------------- | :--------------------------------------- | -| ✓ | std_msgs/Header | header | | | -| ✓ | geometry_msgs/Pose | pose | position:[m] | | -| ✓ | awapi_awiv_adapter/Euler | eulerangle | [rad] | roll/pitch/yaw | -| | geographic_msgs/GeoPoint | geo_point | | lat/lon/alt | -| ✓ | float64 | velocity | [m/s] | | -| ✓ | float64 | acceleration | [m/ss] | calculate from velocity in awapi_adapter | -| ✓ | float64 | steering | [rad] | | -| ✓ | float64 | steering_velocity | [rad/s] | calculate from steering in awapi_adapter | -| ✓ | float64 | angular_velocity | [rad/s] | | -| | int32 | gear | according to tier4_vehicle_msgs/Shift | | -| | float32 | energy_level | | available only for golf-cart | -| ✓ | int32 | turn_signal | according to tier4_vehicle_msgs/TurnSignal | | -| ✓ | float64 | target_velocity | [m/s] | | -| ✓ | float64 | target_acceleration | [m/ss] | | -| ✓ | float64 | target_steering | [rad] | | -| ✓ | float64 | target_steering_velocity | [rad/s] | | - -### /awapi/autoware/get/status - -- get autoware status -- MessageType: awapi_awiv_adapter/AwapiVehicleStatus - -| ✓ | type | name | unit | note | -| --- | :---------------------------------- | :------------------- | :------------------------------------------ | :-------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| ✓ | std_msgs/Header | header | | | -| ✓ | string | autoware_state | | | -| ✓ | int32 | control_mode | according to tier4_vehicle_msgs/ControlMode | manual/auto (changed by /awapi/autoware/put/engage) | -| | int32 | gate_mode | tier4_vehicle_msgs/GateMode | auto/remote (it is valid only when control_mode=auto)) | -| ✓ | bool | emergency_stopped | True in emergency mode | | -| ✓ | float32 | current_max_velocity | [m/s] | | -| | tier4_system_msgs/HazardStatus | hazard_status | | system hazard status | -| ✓ | tier4_planning_msgs/StopReasonArray | stop_reason | | "stop_pose" represents the position of "base_link" (not the head of the car) | -| ✓ | diagnostic_msgs/DiagnosticStatus[] | diagnostics | | output only diag. of leaf node (diag. of parent node are cut) | -| ✓ | diagnostic_msgs/DiagnosticStatus[] | error_diagnostics | | diagnostics that are the cause of system emergency | -| ✓ | bool | arrived_goal | | True if the autoware_state is changed from Driving to ArrivedGoal or WaitingForRoute. False if the autoware_state is changed to WaitingForEngage or Driving. Default False. | - -- specification of stop_reason - - stop_reason is output only when the following conditions are met. - - stop_point in stop_reason is close to /planning/scenario_planning/trajectory (within 10m). - - The distance between current position and stop_point is less than stop_reason_thresh_dist. - -### /awapi/autoware/get/route - -- get route -- MessageType: tier4_planning_msgs/Route - -| ✓ | type | name | unit | note | -| --- | :------------------------ | :--- | :--- | :--- | -| ✓ | tier4_planning_msgs/Route | | | | - -### /awapi/autoware/get/stop_speed_exceeded - -- get flag of exceeding stop speed or not - - True: exceed the stop speed ( = "cannot stop before the stop line") - - False: not exceed the stop speed ( = "no stop line in the trajectory" or "possible to stop before the stop line" ) -- MessageType: tier4_planning_msgs/StopSpeedExceedStatus - -| ✓ | type | name | unit | note | -| --- | :---------------------------------------- | :--- | :--- | :--- | -| | tier4_planning_msgs/StopSpeedExceedStatus | | - | | - -### /awapi/prediction/get/objects - -- get predicted object -- MessageType: tier4_api_msgs/DynamicObjectArray - -| ✓ | type | name | unit | note | -| --- | :-------------------------------- | :--- | :--- | :--- | -| ✓ | tier4_api_msgs/DynamicObjectArray | | | | - -### /awapi/lane_change/get/status - -- get lane change information -- MessageType: awapi_awiv_adapter/LaneChangeStatus - -| ✓ | type | name | unit | note | -| --- | :----------------------- | :-------------------------- | :------------------------------------ | :--------------------------------------------------------------------------------- | -| | std_msgs/Header | header | | | -| | bool | force_lane_change_available | True when lane change is available | available: Physically lane changeable state (do not consider other vehicle) | -| | bool | lane_change_ready | True when lane change is ready | ready: State that ego-vehicle can change lane without collision with other vehicle | -| | tier4_planning_msgs/Path | candidate_path | according to tier4_planning_msgs/Path | | - -### /awapi/object_avoidance/get/status - -- get obstacle avoidance information -- MessageType: awapi_awiv_adapter/ObstacleAvoidanceStatus - -| ✓ | type | name | unit | note | -| --- | :----------------------------- | :----------------------- | :------------------------------------------ | :---------------------------------------------------- | -| | std_msgs/Header | header | | | -| | bool | obstacle_avoidance_ready | True when obstacle avoidance is ready | | -| | tier4_planning_msgs/Trajectory | candidate_path | according to tier4_planning_msgs/Trajectory | Msg type is different from lane change candidate path | - -### /awapi/traffic_light/get/traffic_signals - -- get recognition result of traffic light -- MessageType: autoware_auto_perception_msgs/msg/TrafficSignalArray - -| ✓ | type | name | unit | note | -| --- | :--------------------------------------------------- | :--- | :--- | :--- | -| | autoware_auto_perception_msgs/msg/TrafficSignalArray | | | | - -### /awapi/traffic_light/get/nearest_traffic_signal - -- get recognition result of nearest traffic light -- MessageType: autoware_auto_perception_msgs/LookingTrafficSignal - -| | type | name | unit | note | -| --- | :--------------------------------------------------- | :--------- | :--- | :------------------------------------------------------------ | -| | std_msgs/Header | header | | | -| | autoware_auto_perception_msgs/TrafficSignalWithJudge | perception | | traffic light information from autoware perception module | -| | autoware_auto_perception_msgs/TrafficSignalWithJudge | external | | traffic light information from external tool/module | -| | autoware_auto_perception_msgs/TrafficSignalWithJudge | final | | traffic light information used by the planning module finally | - -- The contents of TrafficSignalWithJudge.msg is following. - -| | type | name | unit | note | -| --- | :------------------------------------------ | :----- | :------------------- | :------------------------------------------------------------- | -| | autoware_auto_perception_msgs/TrafficSignal | signal | | traffic light color/arrow | -| | uint8 | judge | 0:NONE, 1:STOP, 2:GO | go/stop judgment based on the color/arrow of the traffic light | - -### /awapi/vehicle/get/door - -- get door status -- MessageType: tier4_api_msgs/DoorStatus.msg - -| ✓ | type | name | unit | note | -| --- | :------------------------ | :----- | :--------------------------------------------------------------------------------------- | :------------------------------------------ | -| | tier4_api_msgs/DoorStatus | status | 0:UNKNOWN, 1:DOOR_OPENED, 2:DOOR_CLOSED 3:DOOR_OPENING, 4:DOOR_CLOSING, 5:NOT_APPLICABLE | available only for the vehicle using pacmod | - -- Now, available status is following: (0:UNKNOWN, 1:DOOR_OPENED, 2:DOOR_CLOSED, 5:NOT_APPLICABLE ). -- 5 (NOT_APPLICABLE) is published if the pacmod is not used -- Due to the specifications of pacmod, the last door open / close command is published as the status. -- The status is 0 (UNKNOWN) until the door open / close command is published once. - -## put topic - -### /awapi/vehicle/put/velocity - -- set upper velocity -- MessageType: tier4_api_msgs/VelocityLimit - -| ✓ | type | name | unit | note | -| --- | :--------------------------- | :--- | :--- | :----------- | -| ✓ | tier4_api_msgs/VelocityLimit | | | max velocity | - -### /awapi/vehicle/put/stop - -- set temporary stop signal -- MessageType: tier4_api_msgs/StopCommand -- Specification - - - send True: send upper velocity to 0 - - send False: resend last received upper velocity - - (if upper velocity have never received, send _default_max_velocity_ value.) - - _default_max_velocity_ refers to the param: _/planning/scenario_planning/motion_velocity_optimizer/max_velocity_ - - | ✓ | type | name | unit | note | - | --- | :------------------------- | :--- | :--- | :--- | - | ✓ | tier4_api_msgs/StopCommand | | | | - -### /awapi/autoware/put/gate_mode - -- send gate mode (auto/remote) -- MessageType: tier4_control_msgs/GateMode - -| ✓ | type | name | unit | note | -| --- | :-------------------------- | :--- | :--- | :--- | -| | tier4_control_msgs/GateMode | | | | - -### /awapi/autoware/put/engage - -- send engage signal (both of autoware/engage and vehicle/engage) -- MessageType: tier4_vehicle_msgs/Engage - -| ✓ | type | name | unit | note | -| --- | :------------------------ | :--- | :--- | :--- | -| ✓ | tier4_vehicle_msgs/Engage | | | | - -### /awapi/autoware/put/goal - -- send goal pose -- MessageType: geometry_msgs/PoseStamped - -| ✓ | type | name | unit | note | -| --- | :------------------------ | :--- | :--- | :--- | -| | geometry_msgs/PoseStamped | | | | - -### /awapi/autoware/put/route - -- send route -- MessageType: tier4_planning_msgs/Route - -| ✓ | type | name | unit | note | -| --- | :------------------------ | :--- | :--- | :--- | -| ✓ | tier4_planning_msgs/Route | | | | - -### /awapi/lane_change/put/approval - -- send lane change approval flag -- send True: start lane change when **lane_change_ready** is true -- MessageType: tier4_planning_msgs/LaneChangeCommand - -| ✓ | type | name | unit | note | -| --- | :---------------------------------------- | :--- | :--- | :--- | -| | tier4_planning_msgs/msg/LaneChangeCommand | | | | - -### /awapi/lane_change/put/force - -- send force lane change flag -- send True: start lane change when **force_lane_change_available** is true -- MessageType: tier4_planning_msgs/LaneChangeCommand - -| ✓ | type | name | unit | note | -| --- | :------------------------------------ | :--- | :--- | :--- | -| | tier4_planning_msgs/LaneChangeCommand | | | | - -### /awapi/object_avoidance/put/approval - -- send object avoidance approval flag -- MessageType: tier4_planning_msgs/EnableAvoidance - -| ✓ | type | name | unit | note | -| --- | :---------------------------------- | :--- | :--- | :--- | -| | tier4_planning_msgs/EnableAvoidance | | | | - -### /awapi/object_avoidance/put/force - -- send force object avoidance flag -- **not implemented (Autoware does not have corresponded topic)** - -| ✓ | type | name | unit | note | -| --- | :--- | :--- | :--- | :--- | - -### /awapi/traffic_light/put/traffic_signals - -- Overwrite the recognition result of traffic light -- MessageType: autoware_auto_perception_msgs/TrafficSignalArray - -| ✓ | type | name | unit | note | -| --- | :----------------------------------------------- | :--- | :--- | :--- | -| | autoware_auto_perception_msgs/TrafficSignalArray | | | | - -### /awapi/vehicle/put/door - -- send door command -- MessageType: tier4_api_msgs/DoorCommand - - send True: open door - - send False: close door - -| ✓ | type | name | unit | note | -| --- | :------------------------- | :--- | :--- | :------------------------------------------ | -| | tier4_api_msgs/DoorCommand | | | available only for the vehicle using pacmod | - -### /awapi/autoware/put/crosswalk_states - -- send crosswalk status - - forcibly rewrite the internal state of crosswalk module -- MessageType: tier4_api_msgs/CrossWalkStatus - -| ✓ | type | name | unit | note | -| --- | :-------------- | :----- | :----------------------- | :--- | -| | std_msgs/Header | header | | | -| | int32 | status | 0:STOP, 1:GO, 2:SLOWDOWN | | - -### /awapi/autoware/put/intersection_states - -- send intersection status - - forcibly rewrite the internal state of intersection module -- MessageType: tier4_api_msgs/CrosswalkStatus - -| ✓ | type | name | unit | note | -| --- | :-------------- | :----- | :----------- | :--- | -| | std_msgs/Header | header | | | -| | int32 | status | 0:STOP, 1:GO | | - -### /awapi/autoware/put/expand_stop_range - -- send expand range of the polygon used by obstacle stop [m] -- MessageType: tier4_planning_msgs/ExpandStopRange - -| ✓ | type | name | unit | note | -| --- | :---------------------------------- | :--- | :--- | :--- | -| | tier4_planning_msgs/ExpandStopRange | | | | - ---- diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.hpp deleted file mode 100644 index 7352e43433276..0000000000000 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.hpp +++ /dev/null @@ -1,80 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AWAPI_AWIV_ADAPTER__AWAPI_AUTOWARE_STATE_PUBLISHER_HPP_ -#define AWAPI_AWIV_ADAPTER__AWAPI_AUTOWARE_STATE_PUBLISHER_HPP_ - -#include "awapi_awiv_adapter/awapi_autoware_util.hpp" - -#include - -#include - -#include -#include -#include - -namespace autoware_api -{ -class AutowareIvAutowareStatePublisher -{ -public: - explicit AutowareIvAutowareStatePublisher(rclcpp::Node & node); - void statePublisher(const AutowareInfo & aw_info); - -private: - rclcpp::Logger logger_; - rclcpp::Clock::SharedPtr clock_; - // publisher - rclcpp::Publisher::SharedPtr pub_state_; - - // parameter - - /* parameter for judging goal now */ - bool arrived_goal_; - autoware_auto_system_msgs::msg::AutowareState::_state_type prev_state_; - - void getAutowareStateInfo( - const autoware_auto_system_msgs::msg::AutowareState::ConstSharedPtr & autoware_state_ptr, - tier4_api_msgs::msg::AwapiAutowareStatus * status); - void getControlModeInfo( - const autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr & control_mode_ptr, - tier4_api_msgs::msg::AwapiAutowareStatus * status); - void getGateModeInfo( - const tier4_control_msgs::msg::GateMode::ConstSharedPtr & gate_mode_ptr, - tier4_api_msgs::msg::AwapiAutowareStatus * status); - void getEmergencyStateInfo( - const autoware_auto_system_msgs::msg::EmergencyState::ConstSharedPtr & emergency_state_ptr, - tier4_api_msgs::msg::AwapiAutowareStatus * status); - void getCurrentMaxVelInfo( - const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr & current_max_velocity_ptr, - tier4_api_msgs::msg::AwapiAutowareStatus * status); - void getHazardStatusInfo( - const AutowareInfo & aw_info, tier4_api_msgs::msg::AwapiAutowareStatus * status); - void getStopReasonInfo( - const tier4_planning_msgs::msg::StopReasonArray::ConstSharedPtr & stop_reason_ptr, - tier4_api_msgs::msg::AwapiAutowareStatus * status); - void getDiagInfo(const AutowareInfo & aw_info, tier4_api_msgs::msg::AwapiAutowareStatus * status); - void getErrorDiagInfo( - const AutowareInfo & aw_info, tier4_api_msgs::msg::AwapiAutowareStatus * status); - void getGlobalRptInfo( - const pacmod3_msgs::msg::GlobalRpt::ConstSharedPtr & global_rpt_ptr, - tier4_api_msgs::msg::AwapiAutowareStatus * status); - - bool isGoal(const autoware_auto_system_msgs::msg::AutowareState::ConstSharedPtr & autoware_state); -}; - -} // namespace autoware_api - -#endif // AWAPI_AWIV_ADAPTER__AWAPI_AUTOWARE_STATE_PUBLISHER_HPP_ diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.hpp deleted file mode 100644 index bacca29abfa15..0000000000000 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.hpp +++ /dev/null @@ -1,148 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AWAPI_AWIV_ADAPTER__AWAPI_AUTOWARE_UTIL_HPP_ -#define AWAPI_AWIV_ADAPTER__AWAPI_AUTOWARE_UTIL_HPP_ - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include -#include - -namespace autoware_api -{ -struct AutowareInfo -{ - std::shared_ptr current_pose_ptr; - autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr steer_ptr; - autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr vehicle_cmd_ptr; - autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr turn_indicators_ptr; - autoware_auto_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr hazard_lights_ptr; - nav_msgs::msg::Odometry::ConstSharedPtr odometry_ptr; - autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr gear_ptr; - tier4_vehicle_msgs::msg::BatteryStatus::ConstSharedPtr battery_ptr; - sensor_msgs::msg::NavSatFix::ConstSharedPtr nav_sat_ptr; - autoware_auto_system_msgs::msg::AutowareState::ConstSharedPtr autoware_state_ptr; - autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr control_mode_ptr; - tier4_control_msgs::msg::GateMode::ConstSharedPtr gate_mode_ptr; - autoware_auto_system_msgs::msg::EmergencyState::ConstSharedPtr emergency_state_ptr; - autoware_auto_system_msgs::msg::HazardStatusStamped::ConstSharedPtr hazard_status_ptr; - tier4_planning_msgs::msg::StopReasonArray::ConstSharedPtr stop_reason_ptr; - tier4_v2x_msgs::msg::InfrastructureCommandArray::ConstSharedPtr v2x_command_ptr; - tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr v2x_state_ptr; - diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr diagnostic_ptr; - pacmod3_msgs::msg::GlobalRpt::ConstSharedPtr global_rpt_ptr; - tier4_planning_msgs::msg::LaneChangeStatus::ConstSharedPtr lane_change_available_ptr; - tier4_planning_msgs::msg::LaneChangeStatus::ConstSharedPtr lane_change_ready_ptr; - autoware_auto_planning_msgs::msg::Path::ConstSharedPtr lane_change_candidate_ptr; - tier4_planning_msgs::msg::IsAvoidancePossible::ConstSharedPtr obstacle_avoid_ready_ptr; - autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr obstacle_avoid_candidate_ptr; - tier4_api_msgs::msg::VelocityLimit::ConstSharedPtr max_velocity_ptr; - tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr current_max_velocity_ptr; - tier4_api_msgs::msg::StopCommand::ConstSharedPtr temporary_stop_ptr; - autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr autoware_planning_traj_ptr; - pacmod3_msgs::msg::SystemRptInt::ConstSharedPtr door_state_ptr; -}; - -template -T waitForParam( - rclcpp::Node * node, const std::string & remote_node_name, const std::string & param_name) -{ - std::chrono::seconds sec(1); - - auto param_client = std::make_shared(node, remote_node_name); - - while (!param_client->wait_for_service(sec)) { - if (!rclcpp::ok()) { - RCLCPP_ERROR(node->get_logger(), "Interrupted while waiting for the service."); - return {}; - } - RCLCPP_INFO_THROTTLE( - node->get_logger(), *node->get_clock(), 1000 /* ms */, "waiting for node: %s, param: %s\n", - remote_node_name.c_str(), param_name.c_str()); - } - - if (param_client->has_parameter(param_name)) { - return param_client->get_parameter(param_name); - } - - return {}; -} - -double lowpass_filter(const double current_value, const double prev_value, const double gain); - -namespace planning_util -{ -bool calcClosestIndex( - const autoware_auto_planning_msgs::msg::Trajectory & traj, const geometry_msgs::msg::Pose & pose, - size_t & output_closest_idx, const double dist_thr = 10.0, const double angle_thr = M_PI / 2.0); - -inline geometry_msgs::msg::Pose getPose( - const autoware_auto_planning_msgs::msg::Trajectory & traj, const int idx) -{ - return traj.points.at(idx).pose; -} - -inline double calcDist2d(const geometry_msgs::msg::Point & a, const geometry_msgs::msg::Point & b) -{ - return std::hypot((a.x - b.x), (a.y - b.y)); -} - -double normalizeEulerAngle(double euler); - -double calcArcLengthFromWayPoint( - const autoware_auto_planning_msgs::msg::Trajectory & input_path, const size_t src_idx, - const size_t dst_idx); - -double calcDistanceAlongTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory & trajectory, - const geometry_msgs::msg::Pose & current_pose, const geometry_msgs::msg::Pose & target_pose); - -} // namespace planning_util - -} // namespace autoware_api - -#endif // AWAPI_AWIV_ADAPTER__AWAPI_AUTOWARE_UTIL_HPP_ diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.hpp deleted file mode 100644 index 61d92f635e33e..0000000000000 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.hpp +++ /dev/null @@ -1,188 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AWAPI_AWIV_ADAPTER__AWAPI_AWIV_ADAPTER_CORE_HPP_ -#define AWAPI_AWIV_ADAPTER__AWAPI_AWIV_ADAPTER_CORE_HPP_ - -#include "awapi_awiv_adapter/awapi_autoware_state_publisher.hpp" -#include "awapi_awiv_adapter/awapi_autoware_util.hpp" -#include "awapi_awiv_adapter/awapi_lane_change_state_publisher.hpp" -#include "awapi_awiv_adapter/awapi_max_velocity_publisher.hpp" -#include "awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.hpp" -#include "awapi_awiv_adapter/awapi_pacmod_util.hpp" -#include "awapi_awiv_adapter/awapi_stop_reason_aggregator.hpp" -#include "awapi_awiv_adapter/awapi_v2x_aggregator.hpp" -#include "awapi_awiv_adapter/awapi_vehicle_state_publisher.hpp" - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include - -namespace autoware_api -{ -class AutowareIvAdapter : public rclcpp::Node -{ -public: - AutowareIvAdapter(); - -private: - // subscriber - rclcpp::Subscription::SharedPtr sub_steer_; - rclcpp::Subscription::SharedPtr - sub_vehicle_cmd_; - rclcpp::Subscription::SharedPtr - sub_turn_indicators_; - rclcpp::Subscription::SharedPtr - sub_hazard_lights_; - rclcpp::Subscription::SharedPtr sub_odometry_; - rclcpp::Subscription::SharedPtr sub_gear_; - rclcpp::Subscription::SharedPtr sub_battery_; - rclcpp::Subscription::SharedPtr sub_nav_sat_; - rclcpp::Subscription::SharedPtr - sub_autoware_state_; - rclcpp::Subscription::SharedPtr - sub_control_mode_; - rclcpp::Subscription::SharedPtr sub_gate_mode_; - rclcpp::Subscription::SharedPtr sub_emergency_; - rclcpp::Subscription::SharedPtr - sub_hazard_status_; - rclcpp::Subscription::SharedPtr sub_stop_reason_; - rclcpp::Subscription::SharedPtr sub_v2x_command_; - rclcpp::Subscription::SharedPtr - sub_v2x_state_; - rclcpp::Subscription::SharedPtr sub_diagnostics_; - rclcpp::Subscription::SharedPtr sub_global_rpt_; - rclcpp::Subscription::SharedPtr - sub_lane_change_available_; - rclcpp::Subscription::SharedPtr - sub_lane_change_ready_; - rclcpp::Subscription::SharedPtr - sub_lane_change_candidate_; - rclcpp::Subscription::SharedPtr - sub_obstacle_avoid_ready_; - rclcpp::Subscription::SharedPtr - sub_obstacle_avoid_candidate_; - rclcpp::Subscription::SharedPtr sub_max_velocity_; - rclcpp::Subscription::SharedPtr - sub_current_max_velocity_; - rclcpp::Subscription::SharedPtr sub_temporary_stop_; - rclcpp::Subscription::SharedPtr sub_autoware_traj_; - rclcpp::Subscription::SharedPtr sub_door_control_; - rclcpp::Subscription::SharedPtr sub_door_status_; - - // publisher - rclcpp::Publisher::SharedPtr pub_door_control_; - rclcpp::Publisher::SharedPtr pub_door_status_; - rclcpp::Publisher::SharedPtr pub_v2x_command_; - rclcpp::Publisher::SharedPtr pub_v2x_state_; - - // timer - rclcpp::TimerBase::SharedPtr timer_; - - // tf - tf2_ros::Buffer tf_buffer_; - tf2_ros::TransformListener tf_listener_; - - // callback function - void callbackSteer(const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr msg_ptr); - void callbackVehicleCmd( - const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg_ptr); - void callbackTurnIndicators( - const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr msg_ptr); - void callbackHazardLights( - const autoware_auto_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr msg_ptr); - void callbackTwist(const nav_msgs::msg::Odometry::ConstSharedPtr msg_ptr); - void callbackGear(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg_ptr); - void callbackBattery(const tier4_vehicle_msgs::msg::BatteryStatus::ConstSharedPtr msg_ptr); - void callbackNavSat(const sensor_msgs::msg::NavSatFix::ConstSharedPtr msg_ptr); - void callbackAutowareState( - const autoware_auto_system_msgs::msg::AutowareState::ConstSharedPtr msg_ptr); - void callbackControlMode( - const autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg_ptr); - void callbackGateMode(const tier4_control_msgs::msg::GateMode::ConstSharedPtr msg_ptr); - void callbackEmergencyState( - const autoware_auto_system_msgs::msg::EmergencyState::ConstSharedPtr msg_ptr); - void callbackHazardStatus( - const autoware_auto_system_msgs::msg::HazardStatusStamped::ConstSharedPtr msg_ptr); - void callbackStopReason(const tier4_planning_msgs::msg::StopReasonArray::ConstSharedPtr msg_ptr); - void callbackV2XCommand( - const tier4_v2x_msgs::msg::InfrastructureCommandArray::ConstSharedPtr msg_ptr); - void callbackV2XState( - const tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr msg_ptr); - void callbackDiagnostics(const diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr msg_ptr); - void callbackGlobalRpt(const pacmod3_msgs::msg::GlobalRpt::ConstSharedPtr msg_ptr); - void callbackLaneChangeAvailable( - const tier4_planning_msgs::msg::LaneChangeStatus::ConstSharedPtr msg_ptr); - void callbackLaneChangeReady( - const tier4_planning_msgs::msg::LaneChangeStatus::ConstSharedPtr msg_ptr); - void callbackLaneChangeCandidatePath( - const autoware_auto_planning_msgs::msg::Path::ConstSharedPtr msg_ptr); - void callbackLaneObstacleAvoidReady( - const tier4_planning_msgs::msg::IsAvoidancePossible::ConstSharedPtr msg_ptr); - void callbackLaneObstacleAvoidCandidatePath( - const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg_ptr); - void callbackMaxVelocity(const tier4_api_msgs::msg::VelocityLimit::ConstSharedPtr msg_ptr); - void callbackCurrentMaxVelocity( - const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg_ptr); - void callbackTemporaryStop(const tier4_api_msgs::msg::StopCommand::ConstSharedPtr msg_ptr); - void callbackAutowareTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg_ptr); - void callbackDoorControl(const tier4_api_msgs::msg::DoorControlCommand::ConstSharedPtr msg_ptr); - void callbackDoorStatus(const pacmod3_msgs::msg::SystemRptInt::ConstSharedPtr msg_ptr); - - // timer function - void timerCallback(); - - void emergencyParamCheck(const bool emergency_stop_param); - void getCurrentPose(); - - // parameter - AutowareInfo aw_info_; - std::unique_ptr vehicle_state_publisher_; - std::unique_ptr autoware_state_publisher_; - std::unique_ptr stop_reason_aggregator_; - std::unique_ptr v2x_aggregator_; - std::unique_ptr lane_change_state_publisher_; - std::unique_ptr obstacle_avoidance_state_publisher_; - std::unique_ptr max_velocity_publisher_; - double status_pub_hz_; - double stop_reason_timeout_; - double stop_reason_thresh_dist_; -}; - -} // namespace autoware_api - -#endif // AWAPI_AWIV_ADAPTER__AWAPI_AWIV_ADAPTER_CORE_HPP_ diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_lane_change_state_publisher.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_lane_change_state_publisher.hpp deleted file mode 100644 index c540aa7992ae8..0000000000000 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_lane_change_state_publisher.hpp +++ /dev/null @@ -1,52 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AWAPI_AWIV_ADAPTER__AWAPI_LANE_CHANGE_STATE_PUBLISHER_HPP_ -#define AWAPI_AWIV_ADAPTER__AWAPI_LANE_CHANGE_STATE_PUBLISHER_HPP_ - -#include "awapi_awiv_adapter/awapi_autoware_util.hpp" - -#include - -#include - -namespace autoware_api -{ -class AutowareIvLaneChangeStatePublisher -{ -public: - explicit AutowareIvLaneChangeStatePublisher(rclcpp::Node & node); - void statePublisher(const AutowareInfo & aw_info); - -private: - rclcpp::Logger logger_; - rclcpp::Clock::SharedPtr clock_; - - // publisher - rclcpp::Publisher::SharedPtr pub_state_; - - void getLaneChangeAvailableInfo( - const tier4_planning_msgs::msg::LaneChangeStatus::ConstSharedPtr & available_ptr, - tier4_api_msgs::msg::LaneChangeStatus * status); - void getLaneChangeReadyInfo( - const tier4_planning_msgs::msg::LaneChangeStatus::ConstSharedPtr & ready_ptr, - tier4_api_msgs::msg::LaneChangeStatus * status); - void getCandidatePathInfo( - const autoware_auto_planning_msgs::msg::Path::ConstSharedPtr & path_ptr, - tier4_api_msgs::msg::LaneChangeStatus * status); -}; - -} // namespace autoware_api - -#endif // AWAPI_AWIV_ADAPTER__AWAPI_LANE_CHANGE_STATE_PUBLISHER_HPP_ diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_max_velocity_publisher.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_max_velocity_publisher.hpp deleted file mode 100644 index 507afd688af9e..0000000000000 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_max_velocity_publisher.hpp +++ /dev/null @@ -1,46 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AWAPI_AWIV_ADAPTER__AWAPI_MAX_VELOCITY_PUBLISHER_HPP_ -#define AWAPI_AWIV_ADAPTER__AWAPI_MAX_VELOCITY_PUBLISHER_HPP_ - -#include "awapi_awiv_adapter/awapi_autoware_util.hpp" - -#include - -#include - -namespace autoware_api -{ -class AutowareIvMaxVelocityPublisher -{ -public: - AutowareIvMaxVelocityPublisher(rclcpp::Node & node, const double default_max_velocity); - void statePublisher(const AutowareInfo & aw_info); - -private: - // publisher - rclcpp::Publisher::SharedPtr pub_state_; - - bool calcMaxVelocity( - const tier4_api_msgs::msg::VelocityLimit::ConstSharedPtr & max_velocity_ptr, - const tier4_api_msgs::msg::StopCommand::ConstSharedPtr & temporary_stop_ptr, - float * max_velocity); - - double default_max_velocity_; -}; - -} // namespace autoware_api - -#endif // AWAPI_AWIV_ADAPTER__AWAPI_MAX_VELOCITY_PUBLISHER_HPP_ diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.hpp deleted file mode 100644 index 7ac755e8102c6..0000000000000 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.hpp +++ /dev/null @@ -1,49 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AWAPI_AWIV_ADAPTER__AWAPI_OBSTACLE_AVOIDANCE_STATE_PUBLISHER_HPP_ -#define AWAPI_AWIV_ADAPTER__AWAPI_OBSTACLE_AVOIDANCE_STATE_PUBLISHER_HPP_ - -#include "awapi_awiv_adapter/awapi_autoware_util.hpp" - -#include - -#include - -namespace autoware_api -{ -class AutowareIvObstacleAvoidanceStatePublisher -{ -public: - explicit AutowareIvObstacleAvoidanceStatePublisher(rclcpp::Node & node); - void statePublisher(const AutowareInfo & aw_info); - -private: - rclcpp::Logger logger_; - rclcpp::Clock::SharedPtr clock_; - - // publisher - rclcpp::Publisher::SharedPtr pub_state_; - - void getObstacleAvoidReadyInfo( - const tier4_planning_msgs::msg::IsAvoidancePossible::ConstSharedPtr & ready_ptr, - tier4_api_msgs::msg::ObstacleAvoidanceStatus * status); - void getCandidatePathInfo( - const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr & path_ptr, - tier4_api_msgs::msg::ObstacleAvoidanceStatus * status); -}; - -} // namespace autoware_api - -#endif // AWAPI_AWIV_ADAPTER__AWAPI_OBSTACLE_AVOIDANCE_STATE_PUBLISHER_HPP_ diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_pacmod_util.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_pacmod_util.hpp deleted file mode 100644 index 8426a385e3a65..0000000000000 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_pacmod_util.hpp +++ /dev/null @@ -1,40 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AWAPI_AWIV_ADAPTER__AWAPI_PACMOD_UTIL_HPP_ -#define AWAPI_AWIV_ADAPTER__AWAPI_PACMOD_UTIL_HPP_ - -#include - -#include -#include -#include -#include - -namespace autoware_api -{ -namespace pacmod_util -{ -tier4_api_msgs::msg::DoorStatus getDoorStatusMsg( - const pacmod3_msgs::msg::SystemRptInt::ConstSharedPtr & msg_ptr); -pacmod3_msgs::msg::SystemCmdInt createClearOverrideDoorCommand( - const rclcpp::Clock::SharedPtr & clock); -pacmod3_msgs::msg::SystemCmdInt createDoorCommand( - const rclcpp::Clock::SharedPtr & clock, - const tier4_api_msgs::msg::DoorControlCommand::ConstSharedPtr & msg_ptr); -} // namespace pacmod_util - -} // namespace autoware_api - -#endif // AWAPI_AWIV_ADAPTER__AWAPI_PACMOD_UTIL_HPP_ diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_stop_reason_aggregator.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_stop_reason_aggregator.hpp deleted file mode 100644 index b05ec18817537..0000000000000 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_stop_reason_aggregator.hpp +++ /dev/null @@ -1,64 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AWAPI_AWIV_ADAPTER__AWAPI_STOP_REASON_AGGREGATOR_HPP_ -#define AWAPI_AWIV_ADAPTER__AWAPI_STOP_REASON_AGGREGATOR_HPP_ - -#include "awapi_awiv_adapter/awapi_autoware_util.hpp" - -#include - -#include - -namespace autoware_api -{ -class AutowareIvStopReasonAggregator -{ -public: - AutowareIvStopReasonAggregator( - rclcpp::Node & node, const double timeout, const double thresh_dist_to_stop_pose); - tier4_planning_msgs::msg::StopReasonArray::ConstSharedPtr updateStopReasonArray( - const tier4_planning_msgs::msg::StopReasonArray::ConstSharedPtr & msg_ptr, - const AutowareInfo & aw_info); - -private: - void applyUpdate( - const tier4_planning_msgs::msg::StopReasonArray::ConstSharedPtr & msg_ptr, - const AutowareInfo & aw_info); - bool checkMatchingReason( - const tier4_planning_msgs::msg::StopReasonArray::ConstSharedPtr & msg_stop_reason_array, - const tier4_planning_msgs::msg::StopReasonArray & stop_reason_array); - void applyTimeOut(); - void appendStopReasonToArray( - const tier4_planning_msgs::msg::StopReason & stop_reason, - tier4_planning_msgs::msg::StopReasonArray * stop_reason_array, const AutowareInfo & aw_info); - tier4_planning_msgs::msg::StopReasonArray::ConstSharedPtr makeStopReasonArray( - const AutowareInfo & aw_info); - tier4_planning_msgs::msg::StopReason inputStopDistToStopReason( - const tier4_planning_msgs::msg::StopReason & stop_reason, const AutowareInfo & aw_info); - double calcStopDistToStopFactor( - const tier4_planning_msgs::msg::StopFactor & stop_factor, const AutowareInfo & aw_info); - tier4_planning_msgs::msg::StopReason getNearStopReason( - const tier4_planning_msgs::msg::StopReason & stop_reason, const AutowareInfo & aw_info); - - rclcpp::Logger logger_; - rclcpp::Clock::SharedPtr clock_; - double timeout_; - double thresh_dist_to_stop_pose_; - std::vector stop_reason_array_vec_; -}; - -} // namespace autoware_api - -#endif // AWAPI_AWIV_ADAPTER__AWAPI_STOP_REASON_AGGREGATOR_HPP_ diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_v2x_aggregator.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_v2x_aggregator.hpp deleted file mode 100644 index 11406da27deab..0000000000000 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_v2x_aggregator.hpp +++ /dev/null @@ -1,54 +0,0 @@ -// Copyright 2021 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AWAPI_AWIV_ADAPTER__AWAPI_V2X_AGGREGATOR_HPP_ -#define AWAPI_AWIV_ADAPTER__AWAPI_V2X_AGGREGATOR_HPP_ - -#include - -#include -#include - -#include -#include -#include - -namespace autoware_api -{ -using Command = tier4_v2x_msgs::msg::InfrastructureCommand; -using CommandArray = tier4_v2x_msgs::msg::InfrastructureCommandArray; -using State = tier4_v2x_msgs::msg::VirtualTrafficLightState; -using StateArray = tier4_v2x_msgs::msg::VirtualTrafficLightStateArray; - -class AutowareIvV2XAggregator -{ -public: - explicit AutowareIvV2XAggregator(rclcpp::Node & node); - - CommandArray::ConstSharedPtr updateV2XCommand(const CommandArray::ConstSharedPtr & msg); - - StateArray::ConstSharedPtr updateV2XState(const StateArray::ConstSharedPtr & msg); - -private: - rclcpp::Logger logger_; - rclcpp::Clock::SharedPtr clock_; - const double max_delay_sec_{5.0}; - const double max_clock_error_sec_{300.0}; - std::map command_map_; - std::map state_map_; -}; - -} // namespace autoware_api - -#endif // AWAPI_AWIV_ADAPTER__AWAPI_V2X_AGGREGATOR_HPP_ diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_vehicle_state_publisher.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_vehicle_state_publisher.hpp deleted file mode 100644 index 20d44297f21ee..0000000000000 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_vehicle_state_publisher.hpp +++ /dev/null @@ -1,83 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AWAPI_AWIV_ADAPTER__AWAPI_VEHICLE_STATE_PUBLISHER_HPP_ -#define AWAPI_AWIV_ADAPTER__AWAPI_VEHICLE_STATE_PUBLISHER_HPP_ - -#include "awapi_awiv_adapter/awapi_autoware_util.hpp" - -#include - -#include - -#include - -namespace autoware_api -{ -class AutowareIvVehicleStatePublisher -{ -public: - explicit AutowareIvVehicleStatePublisher(rclcpp::Node & node); - void statePublisher(const AutowareInfo & aw_info); - -private: - // publisher - rclcpp::Publisher::SharedPtr pub_state_; - - tier4_api_msgs::msg::AwapiVehicleStatus initVehicleStatus(); - void getPoseInfo( - const std::shared_ptr & pose_ptr, - tier4_api_msgs::msg::AwapiVehicleStatus * status); - void getSteerInfo( - const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr & steer_ptr, - tier4_api_msgs::msg::AwapiVehicleStatus * status); - void getVehicleCmdInfo( - const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr & - vehicle_cmd_ptr, - tier4_api_msgs::msg::AwapiVehicleStatus * status); - void getTurnSignalInfo( - const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr & - turn_indicators_ptr, - const autoware_auto_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr & hazard_lights_ptr, - tier4_api_msgs::msg::AwapiVehicleStatus * status); - void getTwistInfo( - const nav_msgs::msg::Odometry::ConstSharedPtr & odometry_ptr, - tier4_api_msgs::msg::AwapiVehicleStatus * status); - void getGearInfo( - const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr & gear_ptr, - tier4_api_msgs::msg::AwapiVehicleStatus * status); - void getBatteryInfo( - const tier4_vehicle_msgs::msg::BatteryStatus::ConstSharedPtr & battery_ptr, - tier4_api_msgs::msg::AwapiVehicleStatus * status); - void getGpsInfo( - const sensor_msgs::msg::NavSatFix::ConstSharedPtr & nav_sat_ptr, - tier4_api_msgs::msg::AwapiVehicleStatus * status); - - rclcpp::Logger logger_; - rclcpp::Clock::SharedPtr clock_; - - // parameters - nav_msgs::msg::Odometry::ConstSharedPtr previous_odometry_ptr_; - autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr previous_steer_ptr_; - double prev_accel_; - double prev_steer_vel_; - - // defined value - const double accel_lowpass_gain_ = 0.2; - const double steer_vel_lowpass_gain_ = 0.2; -}; - -} // namespace autoware_api - -#endif // AWAPI_AWIV_ADAPTER__AWAPI_VEHICLE_STATE_PUBLISHER_HPP_ diff --git a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/diagnostics_filter.hpp b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/diagnostics_filter.hpp deleted file mode 100644 index 87e84861b49ee..0000000000000 --- a/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/diagnostics_filter.hpp +++ /dev/null @@ -1,118 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AWAPI_AWIV_ADAPTER__DIAGNOSTICS_FILTER_HPP_ -#define AWAPI_AWIV_ADAPTER__DIAGNOSTICS_FILTER_HPP_ - -#include - -#include -#include -#include - -namespace diagnostics_filter -{ -inline std::string splitStringByLastSlash(const std::string & str) -{ - const auto last_slash = str.find_last_of("/"); - - if (last_slash == std::string::npos) { - return ""; - } - - return str.substr(0, last_slash); -} - -std::vector getAllParentNames(const std::string & diag_name) -{ - std::vector all_parent_names; - - auto parent_name = splitStringByLastSlash(diag_name); - while (parent_name != "") { - all_parent_names.push_back(parent_name); - parent_name = splitStringByLastSlash(parent_name); - } - - return all_parent_names; -} - -inline bool isChild( - const diagnostic_msgs::msg::DiagnosticStatus & child, - const diagnostic_msgs::msg::DiagnosticStatus & parent) -{ - auto name = splitStringByLastSlash(child.name); - while (name != "") { - if (name == parent.name) { - return true; - } - - name = splitStringByLastSlash(name); - } - - return false; -} - -inline bool isLeaf( - const std::unordered_set & diag_name_set, - const diagnostic_msgs::msg::DiagnosticStatus & diag) -{ - return diag_name_set.count(diag.name) == 0; -} - -inline std::unordered_set createDiagNameSet( - const std::vector & diagnostics) -{ - std::unordered_set diag_name_set; - - for (const auto & diag : diagnostics) { - for (const auto & parent_name : getAllParentNames(diag.name)) { - diag_name_set.insert(parent_name); - } - } - - return diag_name_set; -} - -inline std::vector extractLeafDiagnostics( - const std::vector & diagnostics) -{ - const auto diag_name_set = createDiagNameSet(diagnostics); - - std::vector leaf_diagnostics; - for (const auto & diag : diagnostics) { - if (isLeaf(diag_name_set, diag)) { - leaf_diagnostics.push_back(diag); - } - } - - return leaf_diagnostics; -} - -inline std::vector extractLeafChildrenDiagnostics( - const diagnostic_msgs::msg::DiagnosticStatus & parent, - const std::vector & diagnostics) -{ - std::vector leaf_children_diagnostics; - for (const auto & diag : extractLeafDiagnostics(diagnostics)) { - if (isChild(diag, parent)) { - leaf_children_diagnostics.push_back(diag); - } - } - - return leaf_children_diagnostics; -} - -} // namespace diagnostics_filter - -#endif // AWAPI_AWIV_ADAPTER__DIAGNOSTICS_FILTER_HPP_ diff --git a/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml b/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml deleted file mode 100644 index 5038612f87644..0000000000000 --- a/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml +++ /dev/null @@ -1,157 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/awapi/awapi_awiv_adapter/launch/awapi_relay_container.launch.py b/awapi/awapi_awiv_adapter/launch/awapi_relay_container.launch.py deleted file mode 100644 index e743f0a5f7d19..0000000000000 --- a/awapi/awapi_awiv_adapter/launch/awapi_relay_container.launch.py +++ /dev/null @@ -1,425 +0,0 @@ -# Copyright 2020 Tier IV, Inc. All rights reserved. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import launch -from launch.actions import SetLaunchConfiguration -from launch.conditions import IfCondition -from launch.conditions import UnlessCondition -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import ComposableNodeContainer -from launch_ros.descriptions import ComposableNode - - -def generate_launch_description(): - relay_components = [] - - relay_components.append( - ComposableNode( - package="topic_tools", - plugin="topic_tools::RelayNode", - name="route_relay", - namespace="awapi", - parameters=[ - { - "input_topic": LaunchConfiguration("input_route"), - "output_topic": LaunchConfiguration("get_route"), - "type": "autoware_auto_planning_msgs/msg/HADMapRoute", - } - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - ) - - relay_components.append( - ComposableNode( - package="topic_tools", - plugin="topic_tools::RelayNode", - name="predict_object_relay", - namespace="awapi", - parameters=[ - { - "input_topic": LaunchConfiguration("input_object"), - "output_topic": LaunchConfiguration("get_predicted_object"), - "type": "autoware_auto_perception_msgs/msg/PredictedObjects", - } - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - ) - - relay_components.append( - ComposableNode( - package="topic_tools", - plugin="topic_tools::RelayNode", - name="nearest_traffic_signal_relay", - namespace="awapi", - parameters=[ - { - "input_topic": LaunchConfiguration("input_nearest_traffic_signal"), - "output_topic": LaunchConfiguration("get_nearest_traffic_signal"), - "type": "autoware_auto_perception_msgs/msg/LookingTrafficSignal", - } - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - ) - - relay_components.append( - ComposableNode( - package="topic_tools", - plugin="topic_tools::RelayNode", - name="ready_module_relay", - namespace="awapi", - parameters=[ - { - "input_topic": LaunchConfiguration("input_path_change_ready"), - "output_topic": LaunchConfiguration("get_path_change_ready"), - "type": "tier4_planning_msgs/msg/PathChangeModule", - } - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - ) - - relay_components.append( - ComposableNode( - package="topic_tools", - plugin="topic_tools::RelayNode", - name="force_available_relay", - namespace="awapi", - parameters=[ - { - "input_topic": LaunchConfiguration("input_path_change_force_available"), - "output_topic": LaunchConfiguration("get_path_change_force_available"), - "type": "tier4_planning_msgs/msg/PathChangeModuleArray", - } - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - ) - - relay_components.append( - ComposableNode( - package="topic_tools", - plugin="topic_tools::RelayNode", - name="running_modules_relay", - namespace="awapi", - parameters=[ - { - "input_topic": LaunchConfiguration("input_path_change_running"), - "output_topic": LaunchConfiguration("get_path_change_running"), - "type": "tier4_planning_msgs/msg/PathChangeModuleArray", - } - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - ) - - relay_components.append( - ComposableNode( - package="topic_tools", - plugin="topic_tools::RelayNode", - name="autoware_engage_relay", - namespace="awapi", - parameters=[ - { - "input_topic": LaunchConfiguration("set_engage"), - "output_topic": LaunchConfiguration("output_autoware_engage"), - "type": "autoware_auto_vehicle_msgs/msg/Engage", - } - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - ) - - relay_components.append( - ComposableNode( - package="topic_tools", - plugin="topic_tools::RelayNode", - name="vehicle_engage_relay", - namespace="awapi", - parameters=[ - { - "input_topic": LaunchConfiguration("set_engage"), - "output_topic": LaunchConfiguration("output_vehicle_engage"), - "type": "autoware_auto_vehicle_msgs/msg/Engage", - } - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - ) - - relay_components.append( - ComposableNode( - package="topic_tools", - plugin="topic_tools::RelayNode", - name="put_route_relay", - namespace="awapi", - parameters=[ - { - "input_topic": LaunchConfiguration("set_route"), - "output_topic": LaunchConfiguration("output_route"), - "type": "autoware_auto_planning_msgs/msg/HADMapRoute", - "durability": "transient_local", - } - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - ) - - relay_components.append( - ComposableNode( - package="topic_tools", - plugin="topic_tools::RelayNode", - name="put_goal_relay", - namespace="awapi", - parameters=[ - { - "input_topic": LaunchConfiguration("set_goal"), - "output_topic": LaunchConfiguration("output_goal"), - "type": "geometry_msgs/msg/PoseStamped", - } - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - ) - - relay_components.append( - ComposableNode( - package="topic_tools", - plugin="topic_tools::RelayNode", - name="lane_change_approval_relay", - namespace="awapi", - parameters=[ - { - "input_topic": LaunchConfiguration("set_lane_change_approval"), - "output_topic": LaunchConfiguration("output_lane_change_approval"), - "type": "tier4_planning_msgs/msg/LaneChangeCommand", - } - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - ) - - relay_components.append( - ComposableNode( - package="topic_tools", - plugin="topic_tools::RelayNode", - name="force_lane_change_relay", - namespace="awapi", - parameters=[ - { - "input_topic": LaunchConfiguration("set_force_lane_change"), - "output_topic": LaunchConfiguration("output_force_lane_change"), - "type": "tier4_planning_msgs/msg/LaneChangeCommand", - } - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - ) - - relay_components.append( - ComposableNode( - package="topic_tools", - plugin="topic_tools::RelayNode", - name="external_approval_relay", - namespace="awapi", - parameters=[ - { - "input_topic": LaunchConfiguration("set_path_change_approval"), - "output_topic": LaunchConfiguration("output_path_change_approval"), - "type": "tier4_planning_msgs/msg/Approval", - } - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - ) - - relay_components.append( - ComposableNode( - package="topic_tools", - plugin="topic_tools::RelayNode", - name="force_approval_relay", - namespace="awapi", - parameters=[ - { - "input_topic": LaunchConfiguration("set_path_change_force"), - "output_topic": LaunchConfiguration("output_path_change_force"), - "type": "tier4_planning_msgs/msg/PathChangeModule", - } - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - ) - - relay_components.append( - ComposableNode( - package="topic_tools", - plugin="topic_tools::RelayNode", - name="obstacle_avoid_approval_relay", - namespace="awapi", - parameters=[ - { - "input_topic": LaunchConfiguration("set_obstacle_avoid_approval"), - "output_topic": LaunchConfiguration("output_obstacle_avoid_approval"), - "type": "tier4_planning_msgs/msg/EnableAvoidance", - } - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - ) - - relay_components.append( - ComposableNode( - package="topic_tools", - plugin="topic_tools::RelayNode", - name="traffic_signal_relay", - namespace="awapi", - parameters=[ - { - "input_topic": LaunchConfiguration("input_traffic_signals"), - "output_topic": LaunchConfiguration("get_traffic_signals"), - "type": "autoware_auto_perception_msgs/msg/TrafficSignalArray", - } - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - ) - - relay_components.append( - ComposableNode( - package="topic_tools", - plugin="topic_tools::RelayNode", - name="overwrite_traffic_signals_relay", - namespace="awapi", - parameters=[ - { - "input_topic": LaunchConfiguration("set_overwrite_traffic_signals"), - "output_topic": LaunchConfiguration("output_overwrite_traffic_signals"), - "type": "autoware_auto_perception_msgs/msg/TrafficSignalArray", - } - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - ) - - relay_components.append( - ComposableNode( - package="topic_tools", - plugin="topic_tools::RelayNode", - name="speed_exceeded_relay", - namespace="awapi", - parameters=[ - { - "input_topic": LaunchConfiguration("input_stop_speed_exceeded"), - "output_topic": LaunchConfiguration("get_stop_speed_exceeded"), - "type": "tier4_planning_msgs/msg/StopSpeedExceeded", - } - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - ) - - relay_components.append( - ComposableNode( - package="topic_tools", - plugin="topic_tools::RelayNode", - name="crosswalk_status_relay", - namespace="awapi", - parameters=[ - { - "input_topic": LaunchConfiguration("set_crosswalk_status"), - "output_topic": LaunchConfiguration("input_external_crosswalk_status"), - "type": "tier4_api_msgs/msg/CrosswalkStatus", - } - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - ) - - relay_components.append( - ComposableNode( - package="topic_tools", - plugin="topic_tools::RelayNode", - name="intersection_status_relay", - namespace="awapi", - parameters=[ - { - "input_topic": LaunchConfiguration("set_intersection_status"), - "output_topic": LaunchConfiguration("input_external_intersection_status"), - "type": "tier4_api_msgs/msg/IntersectionStatus", - } - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - ) - - relay_components.append( - ComposableNode( - package="topic_tools", - plugin="topic_tools::RelayNode", - name="expand_stop_range_relay", - namespace="awapi", - parameters=[ - { - "input_topic": LaunchConfiguration("set_expand_stop_range"), - "output_topic": LaunchConfiguration("input_expand_stop_range"), - "type": "tier4_planning_msgs/msg/ExpandStopRange", - } - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - ) - - relay_components.append( - ComposableNode( - package="topic_tools", - plugin="topic_tools::RelayNode", - name="pose_initialization_request_relay", - namespace="awapi", - parameters=[ - { - "input_topic": LaunchConfiguration("set_pose_initialization_request"), - "output_topic": LaunchConfiguration("input_pose_initialization_request"), - "type": "tier4_localization_msgs/msg/PoseInitializationRequest", - } - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - ) - - container = ComposableNodeContainer( - name="awapi_relay_container", - namespace="awapi", - package="rclcpp_components", - executable=LaunchConfiguration("container_executable"), - composable_node_descriptions=relay_components, - output="screen", - ) - - set_container_executable = SetLaunchConfiguration( - "container_executable", - "component_container", - condition=UnlessCondition(LaunchConfiguration("use_multithread")), - ) - - set_container_mt_executable = SetLaunchConfiguration( - "container_executable", - "component_container_mt", - condition=IfCondition(LaunchConfiguration("use_multithread")), - ) - - return launch.LaunchDescription( - [set_container_executable, set_container_mt_executable] + [container] - ) diff --git a/awapi/awapi_awiv_adapter/package.xml b/awapi/awapi_awiv_adapter/package.xml deleted file mode 100644 index 0edb0a9072933..0000000000000 --- a/awapi/awapi_awiv_adapter/package.xml +++ /dev/null @@ -1,43 +0,0 @@ - - - awapi_awiv_adapter - 0.1.0 - The awapi_awiv_adapter package - Tomoya Kimura - Tomoya Kimura - Apache License 2.0 - - ament_cmake_auto - - autoware_auto_control_msgs - autoware_auto_planning_msgs - autoware_auto_system_msgs - autoware_auto_vehicle_msgs - diagnostic_msgs - geometry_msgs - nav_msgs - pacmod3_msgs - rclcpp - sensor_msgs - std_msgs - tf2 - tf2_geometry_msgs - tier4_api_msgs - tier4_auto_msgs_converter - tier4_control_msgs - tier4_external_api_msgs - tier4_planning_msgs - tier4_v2x_msgs - tier4_vehicle_msgs - - autoware_auto_perception_msgs - tier4_perception_msgs - topic_tools - - ament_lint_auto - autoware_lint_common - - - ament_cmake - - diff --git a/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp deleted file mode 100644 index 12e7af3651ad9..0000000000000 --- a/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp +++ /dev/null @@ -1,302 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "awapi_awiv_adapter/awapi_autoware_state_publisher.hpp" - -#include "awapi_awiv_adapter/diagnostics_filter.hpp" -#include "tier4_auto_msgs_converter/tier4_auto_msgs_converter.hpp" - -#include -#include -#include - -namespace autoware_api -{ -AutowareIvAutowareStatePublisher::AutowareIvAutowareStatePublisher(rclcpp::Node & node) -: logger_(node.get_logger().get_child("awapi_awiv_autoware_state_publisher")), - clock_(node.get_clock()), - arrived_goal_(false) -{ - // publisher - pub_state_ = - node.create_publisher("output/autoware_status", 1); -} - -void AutowareIvAutowareStatePublisher::statePublisher(const AutowareInfo & aw_info) -{ - tier4_api_msgs::msg::AwapiAutowareStatus status; - - // input header - status.header.frame_id = "base_link"; - status.header.stamp = clock_->now(); - - // get all info - getAutowareStateInfo(aw_info.autoware_state_ptr, &status); - getControlModeInfo(aw_info.control_mode_ptr, &status); - getGateModeInfo(aw_info.gate_mode_ptr, &status); - getEmergencyStateInfo(aw_info.emergency_state_ptr, &status); - getCurrentMaxVelInfo(aw_info.current_max_velocity_ptr, &status); - getHazardStatusInfo(aw_info, &status); - getStopReasonInfo(aw_info.stop_reason_ptr, &status); - getDiagInfo(aw_info, &status); - getErrorDiagInfo(aw_info, &status); - getGlobalRptInfo(aw_info.global_rpt_ptr, &status); - - // publish info - pub_state_->publish(status); -} - -void AutowareIvAutowareStatePublisher::getAutowareStateInfo( - const autoware_auto_system_msgs::msg::AutowareState::ConstSharedPtr & autoware_state_ptr, - tier4_api_msgs::msg::AwapiAutowareStatus * status) -{ - if (!autoware_state_ptr) { - RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *clock_, 5000 /* ms */, "autoware_state is nullptr"); - return; - } - - // get autoware_state - using tier4_auto_msgs_converter::convert; - status->autoware_state = convert(*autoware_state_ptr).state; - status->arrived_goal = isGoal(autoware_state_ptr); -} - -void AutowareIvAutowareStatePublisher::getControlModeInfo( - const autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr & control_mode_ptr, - tier4_api_msgs::msg::AwapiAutowareStatus * status) -{ - if (!control_mode_ptr) { - RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *clock_, 5000 /* ms */, "control mode is nullptr"); - return; - } - - // get control mode - using tier4_auto_msgs_converter::convert; - status->control_mode = convert(*control_mode_ptr).data; -} - -void AutowareIvAutowareStatePublisher::getGateModeInfo( - const tier4_control_msgs::msg::GateMode::ConstSharedPtr & gate_mode_ptr, - tier4_api_msgs::msg::AwapiAutowareStatus * status) -{ - if (!gate_mode_ptr) { - RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *clock_, 5000 /* ms */, "gate mode is nullptr"); - return; - } - - // get control mode - status->gate_mode = gate_mode_ptr->data; -} - -void AutowareIvAutowareStatePublisher::getEmergencyStateInfo( - const autoware_auto_system_msgs::msg::EmergencyState::ConstSharedPtr & emergency_state_ptr, - tier4_api_msgs::msg::AwapiAutowareStatus * status) -{ - if (!emergency_state_ptr) { - RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *clock_, 5000 /* ms */, "emergency_state is nullptr"); - return; - } - - // get emergency - using autoware_auto_system_msgs::msg::EmergencyState; - status->emergency_stopped = (emergency_state_ptr->state == EmergencyState::MRM_OPERATING) || - (emergency_state_ptr->state == EmergencyState::MRM_SUCCEEDED) || - (emergency_state_ptr->state == EmergencyState::MRM_FAILED); -} - -void AutowareIvAutowareStatePublisher::getCurrentMaxVelInfo( - const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr & current_max_velocity_ptr, - tier4_api_msgs::msg::AwapiAutowareStatus * status) -{ - if (!current_max_velocity_ptr) { - RCLCPP_DEBUG_STREAM_THROTTLE( - logger_, *clock_, 5000 /* ms */, - "[AutowareIvAutowareStatePublisher] current_max_velocity is nullptr"); - return; - } - - // get current max velocity - status->current_max_velocity = current_max_velocity_ptr->max_velocity; -} - -void AutowareIvAutowareStatePublisher::getHazardStatusInfo( - const AutowareInfo & aw_info, tier4_api_msgs::msg::AwapiAutowareStatus * status) -{ - if (!aw_info.autoware_state_ptr) { - RCLCPP_DEBUG_STREAM_THROTTLE( - logger_, *clock_, 5000 /* ms */, - "[AutowareIvAutowareStatePublisher] autoware_state is nullptr"); - return; - } - - if (!aw_info.control_mode_ptr) { - RCLCPP_DEBUG_STREAM_THROTTLE( - logger_, *clock_, 5000 /* ms */, - "[AutowareIvAutowareStatePublisher] control_mode is nullptr"); - return; - } - - if (!aw_info.hazard_status_ptr) { - RCLCPP_DEBUG_STREAM_THROTTLE( - logger_, *clock_, 5000 /* ms */, - "[AutowareIvAutowareStatePublisher] hazard_status is nullptr"); - return; - } - - // get emergency - using tier4_auto_msgs_converter::convert; - status->hazard_status = convert(*aw_info.hazard_status_ptr); - - // filter leaf diagnostics - status->hazard_status.status.diagnostics_spf = - diagnostics_filter::extractLeafDiagnostics(status->hazard_status.status.diagnostics_spf); - status->hazard_status.status.diagnostics_lf = - diagnostics_filter::extractLeafDiagnostics(status->hazard_status.status.diagnostics_lf); - status->hazard_status.status.diagnostics_sf = - diagnostics_filter::extractLeafDiagnostics(status->hazard_status.status.diagnostics_sf); - status->hazard_status.status.diagnostics_nf = - diagnostics_filter::extractLeafDiagnostics(status->hazard_status.status.diagnostics_nf); -} - -void AutowareIvAutowareStatePublisher::getStopReasonInfo( - const tier4_planning_msgs::msg::StopReasonArray::ConstSharedPtr & stop_reason_ptr, - tier4_api_msgs::msg::AwapiAutowareStatus * status) -{ - if (!stop_reason_ptr) { - RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *clock_, 5000 /* ms */, "stop reason is nullptr"); - return; - } - - status->stop_reason = *stop_reason_ptr; -} - -void AutowareIvAutowareStatePublisher::getDiagInfo( - const AutowareInfo & aw_info, tier4_api_msgs::msg::AwapiAutowareStatus * status) -{ - if (!aw_info.diagnostic_ptr) { - RCLCPP_DEBUG_STREAM_THROTTLE( - logger_, *clock_, 5000 /* ms */, "[AutowareIvAutowareStatePublisher] diagnostics is nullptr"); - return; - } - - // get diag - status->diagnostics = diagnostics_filter::extractLeafDiagnostics(aw_info.diagnostic_ptr->status); -} - -// This function is tentative and should be replaced with getHazardStatusInfo. -// TODO(Kenji Miyake): Make getErrorDiagInfo users to use getHazardStatusInfo. -void AutowareIvAutowareStatePublisher::getErrorDiagInfo( - const AutowareInfo & aw_info, tier4_api_msgs::msg::AwapiAutowareStatus * status) -{ - using autoware_auto_system_msgs::msg::AutowareState; - using autoware_auto_vehicle_msgs::msg::ControlModeReport; - - if (!aw_info.autoware_state_ptr) { - RCLCPP_DEBUG_STREAM_THROTTLE( - logger_, *clock_, 5000 /* ms */, - "[AutowareIvAutowareStatePublisher] autoware_state is nullptr"); - return; - } - - if (!aw_info.control_mode_ptr) { - RCLCPP_DEBUG_STREAM_THROTTLE( - logger_, *clock_, 5000 /* ms */, - "[AutowareIvAutowareStatePublisher] control mode is nullptr"); - return; - } - - if (!aw_info.diagnostic_ptr) { - RCLCPP_DEBUG_STREAM_THROTTLE( - logger_, *clock_, 5000 /* ms */, "[AutowareIvAutowareStatePublisher] diagnostics is nullptr"); - return; - } - - if (!aw_info.hazard_status_ptr) { - RCLCPP_DEBUG_STREAM_THROTTLE( - logger_, *clock_, 5000 /* ms */, - "[AutowareIvAutowareStatePublisher] hazard_status is nullptr"); - return; - } - - // get diag - using diagnostic_msgs::msg::DiagnosticStatus; - const auto & hazard_status = aw_info.hazard_status_ptr->status; - std::vector error_diagnostics; - - for (const auto & hazard_diag : hazard_status.diag_single_point_fault) { - auto diag = hazard_diag; - diag.message = "[Single Point Fault]" + hazard_diag.message; - error_diagnostics.push_back(diag); - } - for (const auto & hazard_diag : hazard_status.diag_latent_fault) { - auto diag = hazard_diag; - diag.message = "[Latent Fault]" + hazard_diag.message; - error_diagnostics.push_back(diag); - } - for (const auto & hazard_diag : hazard_status.diag_safe_fault) { - auto diag = hazard_diag; - diag.message = "[Safe Fault]" + hazard_diag.message; - error_diagnostics.push_back(diag); - } - for (const auto & hazard_diag : hazard_status.diag_no_fault) { - auto diag = hazard_diag; - diag.message = "[No Fault]" + hazard_diag.message; - diag.level = DiagnosticStatus::OK; - error_diagnostics.push_back(diag); - } - - // filter leaf diag - status->error_diagnostics = diagnostics_filter::extractLeafDiagnostics(error_diagnostics); -} - -void AutowareIvAutowareStatePublisher::getGlobalRptInfo( - const pacmod3_msgs::msg::GlobalRpt::ConstSharedPtr & global_rpt_ptr, - tier4_api_msgs::msg::AwapiAutowareStatus * status) -{ - if (!global_rpt_ptr) { - RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *clock_, 5000 /* ms */, "global_rpt is nullptr"); - return; - } - - // get global_rpt - status->autonomous_overridden = global_rpt_ptr->override_active; -} - -bool AutowareIvAutowareStatePublisher::isGoal( - const autoware_auto_system_msgs::msg::AutowareState::ConstSharedPtr & autoware_state) -{ - // rename - const auto & aw_state = autoware_state->state; - - if (aw_state == autoware_auto_system_msgs::msg::AutowareState::ARRIVED_GOAL) { - arrived_goal_ = true; - } else if ( // NOLINT - prev_state_ == autoware_auto_system_msgs::msg::AutowareState::DRIVING && - aw_state == autoware_auto_system_msgs::msg::AutowareState::WAITING_FOR_ROUTE) { - arrived_goal_ = true; - } - - if ( - aw_state == autoware_auto_system_msgs::msg::AutowareState::WAITING_FOR_ENGAGE || - aw_state == autoware_auto_system_msgs::msg::AutowareState::DRIVING) { - // cancel goal state - arrived_goal_ = false; - } - - prev_state_ = aw_state; - - return arrived_goal_; -} - -} // namespace autoware_api diff --git a/awapi/awapi_awiv_adapter/src/awapi_autoware_util.cpp b/awapi/awapi_awiv_adapter/src/awapi_autoware_util.cpp deleted file mode 100644 index 7d6ccba672996..0000000000000 --- a/awapi/awapi_awiv_adapter/src/awapi_autoware_util.cpp +++ /dev/null @@ -1,108 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "awapi_awiv_adapter/awapi_autoware_util.hpp" - -#include - -namespace autoware_api -{ -double lowpass_filter(const double current_value, const double prev_value, const double gain) -{ - return gain * prev_value + (1.0 - gain) * current_value; -} - -namespace planning_util -{ -bool calcClosestIndex( - const autoware_auto_planning_msgs::msg::Trajectory & traj, const geometry_msgs::msg::Pose & pose, - size_t & output_closest_idx, const double dist_thr, const double angle_thr) -{ - double dist_min = std::numeric_limits::max(); - const double yaw_pose = tf2::getYaw(pose.orientation); - int closest_idx = -1; - - for (int i = 0; i < static_cast(traj.points.size()); ++i) { - const double dist = calcDist2d(getPose(traj, i).position, pose.position); - - /* check distance threshold */ - if (dist > dist_thr) { - continue; - } - - /* check angle threshold */ - double yaw_i = tf2::getYaw(getPose(traj, i).orientation); - double yaw_diff = normalizeEulerAngle(yaw_pose - yaw_i); - - if (std::fabs(yaw_diff) > angle_thr) { - continue; - } - - if (dist < dist_min) { - dist_min = dist; - closest_idx = i; - } - } - - output_closest_idx = static_cast(closest_idx); - - return closest_idx != -1; -} - -double normalizeEulerAngle(double euler) -{ - double res = euler; - while (res > M_PI) { - res -= (2.0 * M_PI); - } - while (res < -M_PI) { - res += 2.0 * M_PI; - } - - return res; -} - -double calcArcLengthFromWayPoint( - const autoware_auto_planning_msgs::msg::Trajectory & input_path, const size_t src_idx, - const size_t dst_idx) -{ - double length = 0; - for (size_t i = src_idx; i < dst_idx; ++i) { - const double dx_wp = - input_path.points.at(i + 1).pose.position.x - input_path.points.at(i).pose.position.x; - const double dy_wp = - input_path.points.at(i + 1).pose.position.y - input_path.points.at(i).pose.position.y; - length += std::hypot(dx_wp, dy_wp); - } - return length; -} - -double calcDistanceAlongTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory & trajectory, - const geometry_msgs::msg::Pose & current_pose, const geometry_msgs::msg::Pose & target_pose) -{ - size_t self_idx; - size_t stop_idx; - if ( - !calcClosestIndex(trajectory, current_pose, self_idx) || - !calcClosestIndex(trajectory, target_pose, stop_idx)) { - return std::numeric_limits::max(); - } - const double dist_to_stop_pose = calcArcLengthFromWayPoint(trajectory, self_idx, stop_idx); - return dist_to_stop_pose; -} - -} // namespace planning_util - -} // namespace autoware_api diff --git a/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp b/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp deleted file mode 100644 index f1ba0b1ced6b7..0000000000000 --- a/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp +++ /dev/null @@ -1,384 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "awapi_awiv_adapter/awapi_awiv_adapter_core.hpp" - -#include -#include -#include - -namespace autoware_api -{ -using std::placeholders::_1; - -AutowareIvAdapter::AutowareIvAdapter() -: Node("awapi_awiv_adapter_node"), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_) -{ - // get param - status_pub_hz_ = this->declare_parameter("status_pub_hz", 5.0); - stop_reason_timeout_ = this->declare_parameter("stop_reason_timeout", 0.5); - stop_reason_thresh_dist_ = this->declare_parameter("stop_reason_thresh_dist", 100.0); - const double default_max_velocity = waitForParam( - this, declare_parameter("node/max_velocity", ""), declare_parameter("param/max_velocity", "")); - const bool em_stop_param = waitForParam( - this, declare_parameter("node/emergency_stop", ""), - declare_parameter("param/emergency_stop", "")); - emergencyParamCheck(em_stop_param); - - // setup instance - vehicle_state_publisher_ = std::make_unique(*this); - autoware_state_publisher_ = std::make_unique(*this); - stop_reason_aggregator_ = std::make_unique( - *this, stop_reason_timeout_, stop_reason_thresh_dist_); - v2x_aggregator_ = std::make_unique(*this); - lane_change_state_publisher_ = std::make_unique(*this); - obstacle_avoidance_state_publisher_ = - std::make_unique(*this); - max_velocity_publisher_ = - std::make_unique(*this, default_max_velocity); - - // publisher - pub_door_control_ = - this->create_publisher("output/door_control", 1); - pub_door_status_ = - this->create_publisher("output/door_status", 1); - pub_v2x_command_ = this->create_publisher( - "output/v2x_command", 1); - pub_v2x_state_ = this->create_publisher( - "output/v2x_state", 1); - - // subscriber - - auto durable_qos = rclcpp::QoS{1}.transient_local(); - - sub_steer_ = this->create_subscription( - "input/steer", 1, std::bind(&AutowareIvAdapter::callbackSteer, this, _1)); - sub_vehicle_cmd_ = - this->create_subscription( - "input/vehicle_cmd", durable_qos, - std::bind(&AutowareIvAdapter::callbackVehicleCmd, this, _1)); - sub_turn_indicators_ = - this->create_subscription( - "input/turn_indicators", 1, std::bind(&AutowareIvAdapter::callbackTurnIndicators, this, _1)); - sub_hazard_lights_ = - this->create_subscription( - "input/hazard_lights", 1, std::bind(&AutowareIvAdapter::callbackHazardLights, this, _1)); - sub_odometry_ = this->create_subscription( - "input/odometry", 1, std::bind(&AutowareIvAdapter::callbackTwist, this, _1)); - sub_gear_ = this->create_subscription( - "input/gear", 1, std::bind(&AutowareIvAdapter::callbackGear, this, _1)); - sub_battery_ = this->create_subscription( - "input/battery", 1, std::bind(&AutowareIvAdapter::callbackBattery, this, _1)); - sub_nav_sat_ = this->create_subscription( - "input/nav_sat", 1, std::bind(&AutowareIvAdapter::callbackNavSat, this, _1)); - sub_autoware_state_ = this->create_subscription( - "input/autoware_state", 1, std::bind(&AutowareIvAdapter::callbackAutowareState, this, _1)); - sub_control_mode_ = this->create_subscription( - "input/control_mode", 1, std::bind(&AutowareIvAdapter::callbackControlMode, this, _1)); - sub_gate_mode_ = this->create_subscription( - "input/gate_mode", durable_qos, std::bind(&AutowareIvAdapter::callbackGateMode, this, _1)); - sub_emergency_ = this->create_subscription( - "input/emergency_state", 1, std::bind(&AutowareIvAdapter::callbackEmergencyState, this, _1)); - sub_hazard_status_ = - this->create_subscription( - "input/hazard_status", 1, std::bind(&AutowareIvAdapter::callbackHazardStatus, this, _1)); - sub_stop_reason_ = this->create_subscription( - "input/stop_reason", 100, std::bind(&AutowareIvAdapter::callbackStopReason, this, _1)); - sub_v2x_command_ = this->create_subscription( - "input/v2x_command", 100, std::bind(&AutowareIvAdapter::callbackV2XCommand, this, _1)); - sub_v2x_state_ = this->create_subscription( - "input/v2x_state", 100, std::bind(&AutowareIvAdapter::callbackV2XState, this, _1)); - sub_diagnostics_ = this->create_subscription( - "input/diagnostics", 1, std::bind(&AutowareIvAdapter::callbackDiagnostics, this, _1)); - sub_global_rpt_ = this->create_subscription( - "input/global_rpt", 1, std::bind(&AutowareIvAdapter::callbackGlobalRpt, this, _1)); - sub_lane_change_available_ = - this->create_subscription( - "input/lane_change_available", 1, - std::bind(&AutowareIvAdapter::callbackLaneChangeAvailable, this, _1)); - sub_lane_change_ready_ = this->create_subscription( - "input/lane_change_ready", 1, std::bind(&AutowareIvAdapter::callbackLaneChangeReady, this, _1)); - sub_lane_change_candidate_ = this->create_subscription( - "input/lane_change_candidate_path", 1, - std::bind(&AutowareIvAdapter::callbackLaneChangeCandidatePath, this, _1)); - sub_obstacle_avoid_ready_ = - this->create_subscription( - "input/obstacle_avoid_ready", durable_qos, - std::bind(&AutowareIvAdapter::callbackLaneObstacleAvoidReady, this, _1)); - sub_obstacle_avoid_candidate_ = - this->create_subscription( - "input/obstacle_avoid_candidate_path", durable_qos, - std::bind(&AutowareIvAdapter::callbackLaneObstacleAvoidCandidatePath, this, _1)); - sub_max_velocity_ = this->create_subscription( - "input/max_velocity", 1, std::bind(&AutowareIvAdapter::callbackMaxVelocity, this, _1)); - sub_current_max_velocity_ = this->create_subscription( - "input/current_max_velocity", durable_qos, - std::bind(&AutowareIvAdapter::callbackCurrentMaxVelocity, this, _1)); - sub_temporary_stop_ = this->create_subscription( - "input/temporary_stop", 1, std::bind(&AutowareIvAdapter::callbackTemporaryStop, this, _1)); - sub_autoware_traj_ = this->create_subscription( - "input/autoware_trajectory", 1, - std::bind(&AutowareIvAdapter::callbackAutowareTrajectory, this, _1)); - sub_door_control_ = this->create_subscription( - "input/door_control", 1, std::bind(&AutowareIvAdapter::callbackDoorControl, this, _1)); - sub_door_status_ = this->create_subscription( - "input/door_status", 1, std::bind(&AutowareIvAdapter::callbackDoorStatus, this, _1)); - - // timer - auto timer_callback = std::bind(&AutowareIvAdapter::timerCallback, this); - auto period = std::chrono::duration_cast( - std::chrono::duration(1.0 / status_pub_hz_)); - timer_ = std::make_shared>( - this->get_clock(), period, std::move(timer_callback), - this->get_node_base_interface()->get_context()); - this->get_node_timers_interface()->add_timer(timer_, nullptr); -} - -void AutowareIvAdapter::emergencyParamCheck(const bool emergency_stop_param) -{ - if (!emergency_stop_param) { - RCLCPP_WARN_STREAM(get_logger(), "parameter[use_external_emergency_stop] is false."); - RCLCPP_WARN_STREAM(get_logger(), "autoware/put/emergency is not valid"); - } -} - -void AutowareIvAdapter::timerCallback() -{ - // get current pose - getCurrentPose(); - - // publish vehicle state - vehicle_state_publisher_->statePublisher(aw_info_); - - // publish autoware state - autoware_state_publisher_->statePublisher(aw_info_); - - // publish lane change state - lane_change_state_publisher_->statePublisher(aw_info_); - - // publish obstacle_avoidance state - obstacle_avoidance_state_publisher_->statePublisher(aw_info_); - - // publish pacmod door status - pub_door_status_->publish(pacmod_util::getDoorStatusMsg(aw_info_.door_state_ptr)); - - // publish v2x command and state - if (aw_info_.v2x_command_ptr) { - pub_v2x_command_->publish(*aw_info_.v2x_command_ptr); - } - if (aw_info_.v2x_state_ptr) { - pub_v2x_state_->publish(*aw_info_.v2x_state_ptr); - } -} - -void AutowareIvAdapter::callbackSteer( - const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr msg_ptr) -{ - aw_info_.steer_ptr = msg_ptr; -} - -void AutowareIvAdapter::callbackVehicleCmd( - const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg_ptr) -{ - aw_info_.vehicle_cmd_ptr = msg_ptr; -} - -void AutowareIvAdapter::callbackTurnIndicators( - const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr msg_ptr) -{ - aw_info_.turn_indicators_ptr = msg_ptr; -} - -void AutowareIvAdapter::callbackHazardLights( - const autoware_auto_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr msg_ptr) -{ - aw_info_.hazard_lights_ptr = msg_ptr; -} - -void AutowareIvAdapter::callbackTwist(const nav_msgs::msg::Odometry::ConstSharedPtr msg_ptr) -{ - aw_info_.odometry_ptr = msg_ptr; -} - -void AutowareIvAdapter::callbackGear( - const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg_ptr) -{ - aw_info_.gear_ptr = msg_ptr; -} - -void AutowareIvAdapter::callbackBattery( - const tier4_vehicle_msgs::msg::BatteryStatus::ConstSharedPtr msg_ptr) -{ - aw_info_.battery_ptr = msg_ptr; -} - -void AutowareIvAdapter::callbackNavSat(const sensor_msgs::msg::NavSatFix::ConstSharedPtr msg_ptr) -{ - aw_info_.nav_sat_ptr = msg_ptr; -} - -void AutowareIvAdapter::getCurrentPose() -{ - try { - auto transform = tf_buffer_.lookupTransform("map", "base_link", tf2::TimePointZero); - geometry_msgs::msg::PoseStamped ps; - ps.header = transform.header; - ps.pose.position.x = transform.transform.translation.x; - ps.pose.position.y = transform.transform.translation.y; - ps.pose.position.z = transform.transform.translation.z; - ps.pose.orientation = transform.transform.rotation; - aw_info_.current_pose_ptr = std::make_shared(ps); - } catch (tf2::TransformException & ex) { - RCLCPP_DEBUG_STREAM_THROTTLE( - get_logger(), *this->get_clock(), 2000 /* ms */, "cannot get self pose"); - } -} - -void AutowareIvAdapter::callbackAutowareState( - const autoware_auto_system_msgs::msg::AutowareState::ConstSharedPtr msg_ptr) -{ - aw_info_.autoware_state_ptr = msg_ptr; -} -void AutowareIvAdapter::callbackControlMode( - const autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg_ptr) -{ - aw_info_.control_mode_ptr = msg_ptr; -} - -void AutowareIvAdapter::callbackGateMode( - const tier4_control_msgs::msg::GateMode::ConstSharedPtr msg_ptr) -{ - aw_info_.gate_mode_ptr = msg_ptr; -} - -void AutowareIvAdapter::callbackEmergencyState( - const autoware_auto_system_msgs::msg::EmergencyState::ConstSharedPtr msg_ptr) -{ - aw_info_.emergency_state_ptr = msg_ptr; -} - -void AutowareIvAdapter::callbackHazardStatus( - const autoware_auto_system_msgs::msg::HazardStatusStamped::ConstSharedPtr msg_ptr) -{ - aw_info_.hazard_status_ptr = msg_ptr; -} - -void AutowareIvAdapter::callbackStopReason( - const tier4_planning_msgs::msg::StopReasonArray::ConstSharedPtr msg_ptr) -{ - aw_info_.stop_reason_ptr = stop_reason_aggregator_->updateStopReasonArray(msg_ptr, aw_info_); -} - -void AutowareIvAdapter::callbackV2XCommand( - const tier4_v2x_msgs::msg::InfrastructureCommandArray::ConstSharedPtr msg_ptr) -{ - aw_info_.v2x_command_ptr = v2x_aggregator_->updateV2XCommand(msg_ptr); -} - -void AutowareIvAdapter::callbackV2XState( - const tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr msg_ptr) -{ - aw_info_.v2x_state_ptr = v2x_aggregator_->updateV2XState(msg_ptr); -} - -void AutowareIvAdapter::callbackDiagnostics( - const diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr msg_ptr) -{ - aw_info_.diagnostic_ptr = msg_ptr; -} - -void AutowareIvAdapter::callbackGlobalRpt( - const pacmod3_msgs::msg::GlobalRpt::ConstSharedPtr msg_ptr) -{ - aw_info_.global_rpt_ptr = msg_ptr; -} - -void AutowareIvAdapter::callbackLaneChangeAvailable( - const tier4_planning_msgs::msg::LaneChangeStatus::ConstSharedPtr msg_ptr) -{ - aw_info_.lane_change_available_ptr = msg_ptr; -} - -void AutowareIvAdapter::callbackLaneChangeReady( - const tier4_planning_msgs::msg::LaneChangeStatus::ConstSharedPtr msg_ptr) -{ - aw_info_.lane_change_ready_ptr = msg_ptr; -} - -void AutowareIvAdapter::callbackLaneChangeCandidatePath( - const autoware_auto_planning_msgs::msg::Path::ConstSharedPtr msg_ptr) -{ - aw_info_.lane_change_candidate_ptr = msg_ptr; -} - -void AutowareIvAdapter::callbackLaneObstacleAvoidReady( - const tier4_planning_msgs::msg::IsAvoidancePossible::ConstSharedPtr msg_ptr) -{ - aw_info_.obstacle_avoid_ready_ptr = msg_ptr; -} - -void AutowareIvAdapter::callbackLaneObstacleAvoidCandidatePath( - const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg_ptr) -{ - aw_info_.obstacle_avoid_candidate_ptr = msg_ptr; -} - -void AutowareIvAdapter::callbackMaxVelocity( - const tier4_api_msgs::msg::VelocityLimit::ConstSharedPtr msg_ptr) -{ - aw_info_.max_velocity_ptr = msg_ptr; - max_velocity_publisher_->statePublisher(aw_info_); -} - -void AutowareIvAdapter::callbackCurrentMaxVelocity( - const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg_ptr) -{ - aw_info_.current_max_velocity_ptr = msg_ptr; -} - -void AutowareIvAdapter::callbackTemporaryStop( - const tier4_api_msgs::msg::StopCommand::ConstSharedPtr msg_ptr) -{ - if (aw_info_.temporary_stop_ptr) { - if (aw_info_.temporary_stop_ptr->stop == msg_ptr->stop) { - // if same value as last time is sent, ignore msg. - return; - } - } - - aw_info_.temporary_stop_ptr = msg_ptr; - max_velocity_publisher_->statePublisher(aw_info_); -} - -void AutowareIvAdapter::callbackAutowareTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg_ptr) -{ - aw_info_.autoware_planning_traj_ptr = msg_ptr; -} - -void AutowareIvAdapter::callbackDoorControl( - const tier4_api_msgs::msg::DoorControlCommand::ConstSharedPtr msg_ptr) -{ - pub_door_control_->publish(pacmod_util::createClearOverrideDoorCommand(this->get_clock())); - rclcpp::Rate(10.0).sleep(); // avoid message loss - pub_door_control_->publish(pacmod_util::createDoorCommand(this->get_clock(), msg_ptr)); -} - -void AutowareIvAdapter::callbackDoorStatus( - const pacmod3_msgs::msg::SystemRptInt::ConstSharedPtr msg_ptr) -{ - aw_info_.door_state_ptr = msg_ptr; -} - -} // namespace autoware_api diff --git a/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_node.cpp b/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_node.cpp deleted file mode 100644 index c3bc000bd2e4c..0000000000000 --- a/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_node.cpp +++ /dev/null @@ -1,28 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "awapi_awiv_adapter/awapi_awiv_adapter_core.hpp" - -#include - -#include - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - - return 0; -} diff --git a/awapi/awapi_awiv_adapter/src/awapi_lane_change_state_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_lane_change_state_publisher.cpp deleted file mode 100644 index cb74bf222417f..0000000000000 --- a/awapi/awapi_awiv_adapter/src/awapi_lane_change_state_publisher.cpp +++ /dev/null @@ -1,88 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "awapi_awiv_adapter/awapi_lane_change_state_publisher.hpp" - -#include "tier4_auto_msgs_converter/tier4_auto_msgs_converter.hpp" - -namespace autoware_api -{ -AutowareIvLaneChangeStatePublisher::AutowareIvLaneChangeStatePublisher(rclcpp::Node & node) -: logger_(node.get_logger().get_child("awapi_awiv_lane_change_state_publisher")), - clock_(node.get_clock()) -{ - // publisher - pub_state_ = - node.create_publisher("output/lane_change_status", 1); -} - -void AutowareIvLaneChangeStatePublisher::statePublisher(const AutowareInfo & aw_info) -{ - tier4_api_msgs::msg::LaneChangeStatus status; - - // input header - status.header.frame_id = "base_link"; - status.header.stamp = clock_->now(); - - // get all info - getLaneChangeAvailableInfo(aw_info.lane_change_available_ptr, &status); - getLaneChangeReadyInfo(aw_info.lane_change_ready_ptr, &status); - getCandidatePathInfo(aw_info.lane_change_candidate_ptr, &status); - - // publish info - pub_state_->publish(status); -} - -void AutowareIvLaneChangeStatePublisher::getLaneChangeAvailableInfo( - const tier4_planning_msgs::msg::LaneChangeStatus::ConstSharedPtr & available_ptr, - tier4_api_msgs::msg::LaneChangeStatus * status) -{ - if (!available_ptr) { - RCLCPP_DEBUG_STREAM_THROTTLE( - logger_, *clock_, 5000 /* ms */, "lane change available is nullptr"); - return; - } - - // get lane change available info - status->force_lane_change_available = available_ptr->status; -} - -void AutowareIvLaneChangeStatePublisher::getLaneChangeReadyInfo( - const tier4_planning_msgs::msg::LaneChangeStatus::ConstSharedPtr & ready_ptr, - tier4_api_msgs::msg::LaneChangeStatus * status) -{ - if (!ready_ptr) { - RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *clock_, 5000 /* ms */, "lane change ready is nullptr"); - return; - } - - // get lane change available info - status->lane_change_ready = ready_ptr->status; -} - -void AutowareIvLaneChangeStatePublisher::getCandidatePathInfo( - const autoware_auto_planning_msgs::msg::Path::ConstSharedPtr & path_ptr, - tier4_api_msgs::msg::LaneChangeStatus * status) -{ - if (!path_ptr) { - RCLCPP_DEBUG_STREAM_THROTTLE( - logger_, *clock_, 5000 /* ms */, "lane_change_candidate_path is nullptr"); - return; - } - - using tier4_auto_msgs_converter::convert; - status->candidate_path = convert(*path_ptr); -} - -} // namespace autoware_api diff --git a/awapi/awapi_awiv_adapter/src/awapi_max_velocity_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_max_velocity_publisher.cpp deleted file mode 100644 index 0866ae2e47d97..0000000000000 --- a/awapi/awapi_awiv_adapter/src/awapi_max_velocity_publisher.cpp +++ /dev/null @@ -1,60 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "awapi_awiv_adapter/awapi_max_velocity_publisher.hpp" - -namespace autoware_api -{ -AutowareIvMaxVelocityPublisher::AutowareIvMaxVelocityPublisher( - rclcpp::Node & node, const double default_max_velocity) -: default_max_velocity_(default_max_velocity) -{ - // publisher - pub_state_ = - node.create_publisher("output/max_velocity", 1); -} - -void AutowareIvMaxVelocityPublisher::statePublisher(const AutowareInfo & aw_info) -{ - tier4_planning_msgs::msg::VelocityLimit max_velocity; - if (calcMaxVelocity( - aw_info.max_velocity_ptr, aw_info.temporary_stop_ptr, - &max_velocity.max_velocity)) // publish info - { - pub_state_->publish(max_velocity); - } -} - -bool AutowareIvMaxVelocityPublisher::calcMaxVelocity( - const tier4_api_msgs::msg::VelocityLimit::ConstSharedPtr & max_velocity_ptr, - const tier4_api_msgs::msg::StopCommand::ConstSharedPtr & temporary_stop_ptr, float * max_velocity) -{ - if (!max_velocity_ptr && !temporary_stop_ptr) { - // receive no max velocity information - return false; - } - - // input max velocity - *max_velocity = - static_cast(max_velocity_ptr ? max_velocity_ptr->max_velocity : default_max_velocity_); - - if (temporary_stop_ptr && temporary_stop_ptr->stop) { - // if temporary_stop is true, max velocity is 0 - *max_velocity = static_cast(0.0); - } - - return true; -} - -} // namespace autoware_api diff --git a/awapi/awapi_awiv_adapter/src/awapi_obstacle_avoidance_state_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_obstacle_avoidance_state_publisher.cpp deleted file mode 100644 index e877a47f18a4e..0000000000000 --- a/awapi/awapi_awiv_adapter/src/awapi_obstacle_avoidance_state_publisher.cpp +++ /dev/null @@ -1,74 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.hpp" - -#include "tier4_auto_msgs_converter/tier4_auto_msgs_converter.hpp" - -namespace autoware_api -{ -AutowareIvObstacleAvoidanceStatePublisher::AutowareIvObstacleAvoidanceStatePublisher( - rclcpp::Node & node) -: logger_(node.get_logger().get_child("awapi_awiv_obstacle_avoidance_state_publisher")), - clock_(node.get_clock()) -{ - // publisher - pub_state_ = node.create_publisher( - "output/obstacle_avoid_status", 1); -} - -void AutowareIvObstacleAvoidanceStatePublisher::statePublisher(const AutowareInfo & aw_info) -{ - tier4_api_msgs::msg::ObstacleAvoidanceStatus status; - - // input header - status.header.frame_id = "base_link"; - status.header.stamp = clock_->now(); - - // get all info - getObstacleAvoidReadyInfo(aw_info.obstacle_avoid_ready_ptr, &status); - getCandidatePathInfo(aw_info.obstacle_avoid_candidate_ptr, &status); - - // publish info - pub_state_->publish(status); -} - -void AutowareIvObstacleAvoidanceStatePublisher::getObstacleAvoidReadyInfo( - const tier4_planning_msgs::msg::IsAvoidancePossible::ConstSharedPtr & ready_ptr, - tier4_api_msgs::msg::ObstacleAvoidanceStatus * status) -{ - if (!ready_ptr) { - RCLCPP_DEBUG_STREAM_THROTTLE( - logger_, *clock_, 5000 /* ms */, "obstacle_avoidance_ready is nullptr"); - return; - } - - status->obstacle_avoidance_ready = ready_ptr->is_avoidance_possible; -} - -void AutowareIvObstacleAvoidanceStatePublisher::getCandidatePathInfo( - const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr & path_ptr, - tier4_api_msgs::msg::ObstacleAvoidanceStatus * status) -{ - if (!path_ptr) { - RCLCPP_DEBUG_STREAM_THROTTLE( - logger_, *clock_, 5000 /* ms */, "obstacle_avoidance_candidate_path is nullptr"); - return; - } - - using tier4_auto_msgs_converter::convert; - status->candidate_path = convert(*path_ptr); -} - -} // namespace autoware_api diff --git a/awapi/awapi_awiv_adapter/src/awapi_pacmod_util.cpp b/awapi/awapi_awiv_adapter/src/awapi_pacmod_util.cpp deleted file mode 100644 index e77b55cb91b77..0000000000000 --- a/awapi/awapi_awiv_adapter/src/awapi_pacmod_util.cpp +++ /dev/null @@ -1,87 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "awapi_awiv_adapter/awapi_pacmod_util.hpp" - -namespace autoware_api -{ -namespace pacmod_util -{ -tier4_api_msgs::msg::DoorStatus getDoorStatusMsg( - const pacmod3_msgs::msg::SystemRptInt::ConstSharedPtr & msg_ptr) -{ - using pacmod3_msgs::msg::SystemRptInt; - using tier4_api_msgs::msg::DoorStatus; - DoorStatus door_status; - - if (!msg_ptr) { - door_status.status = DoorStatus::NOT_APPLICABLE; - return door_status; - } - - door_status.status = DoorStatus::UNKNOWN; - - if (msg_ptr->command == SystemRptInt::DOOR_CLOSE && msg_ptr->output == SystemRptInt::DOOR_OPEN) { - // do not used (command & output are always the same value) - door_status.status = DoorStatus::DOOR_CLOSING; - } else if ( // NOLINT - msg_ptr->command == SystemRptInt::DOOR_OPEN && msg_ptr->output == SystemRptInt::DOOR_CLOSE) { - // do not used (command & output are always the same value) - door_status.status = DoorStatus::DOOR_OPENING; - } else if (msg_ptr->output == SystemRptInt::DOOR_CLOSE) { - door_status.status = DoorStatus::DOOR_CLOSED; - } else if (msg_ptr->output == SystemRptInt::DOOR_OPEN) { - door_status.status = DoorStatus::DOOR_OPENED; - } - - return door_status; -} - -pacmod3_msgs::msg::SystemCmdInt createClearOverrideDoorCommand( - const rclcpp::Clock::SharedPtr & clock) -{ - pacmod3_msgs::msg::SystemCmdInt door_cmd; - door_cmd.header.frame_id = "base_link"; - door_cmd.header.stamp = clock->now(); - door_cmd.clear_override = true; - return door_cmd; -} - -pacmod3_msgs::msg::SystemCmdInt createDoorCommand( - const rclcpp::Clock::SharedPtr & clock, - const tier4_api_msgs::msg::DoorControlCommand::ConstSharedPtr & msg_ptr) -{ - using pacmod3_msgs::msg::SystemCmdInt; - - SystemCmdInt door_cmd; - door_cmd.header.frame_id = "base_link"; - door_cmd.header.stamp = clock->now(); - door_cmd.enable = true; - door_cmd.command = SystemCmdInt::DOOR_NEUTRAL; - - if (!msg_ptr) { - return door_cmd; - } - - if (msg_ptr->open) { - door_cmd.command = SystemCmdInt::DOOR_OPEN; - } else { - door_cmd.command = SystemCmdInt::DOOR_CLOSE; - } - return door_cmd; -} - -} // namespace pacmod_util - -} // namespace autoware_api diff --git a/awapi/awapi_awiv_adapter/src/awapi_stop_reason_aggregator.cpp b/awapi/awapi_awiv_adapter/src/awapi_stop_reason_aggregator.cpp deleted file mode 100644 index a43881c7dd904..0000000000000 --- a/awapi/awapi_awiv_adapter/src/awapi_stop_reason_aggregator.cpp +++ /dev/null @@ -1,186 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "awapi_awiv_adapter/awapi_stop_reason_aggregator.hpp" - -#include -#include - -namespace autoware_api -{ -AutowareIvStopReasonAggregator::AutowareIvStopReasonAggregator( - rclcpp::Node & node, const double timeout, const double thresh_dist_to_stop_pose) -: logger_(node.get_logger().get_child("awapi_awiv_stop_reason_aggregator")), - clock_(node.get_clock()), - timeout_(timeout), - thresh_dist_to_stop_pose_(thresh_dist_to_stop_pose) -{ -} - -tier4_planning_msgs::msg::StopReasonArray::ConstSharedPtr -AutowareIvStopReasonAggregator::updateStopReasonArray( - const tier4_planning_msgs::msg::StopReasonArray::ConstSharedPtr & msg_ptr, - const AutowareInfo & aw_info) -{ - applyUpdate(msg_ptr, aw_info); - applyTimeOut(); - return makeStopReasonArray(aw_info); -} - -void AutowareIvStopReasonAggregator::applyUpdate( - const tier4_planning_msgs::msg::StopReasonArray::ConstSharedPtr & msg_ptr, - [[maybe_unused]] const AutowareInfo & aw_info) -{ - /* remove old stop_reason that matches reason with received msg */ - // make reason-matching msg list - - std::vector remove_idx; - if (!stop_reason_array_vec_.empty()) { - for (int i = stop_reason_array_vec_.size() - 1; i >= 0; i--) { - if (checkMatchingReason(msg_ptr, stop_reason_array_vec_.at(i))) { - remove_idx.emplace_back(i); - } - } - } - - // remove reason matching msg - for (const auto idx : remove_idx) { - stop_reason_array_vec_.erase(stop_reason_array_vec_.begin() + idx); - } - - // add new reason msg - stop_reason_array_vec_.emplace_back(*msg_ptr); -} - -bool AutowareIvStopReasonAggregator::checkMatchingReason( - const tier4_planning_msgs::msg::StopReasonArray::ConstSharedPtr & msg_stop_reason_array, - const tier4_planning_msgs::msg::StopReasonArray & stop_reason_array) -{ - for (const auto & msg_stop_reason : msg_stop_reason_array->stop_reasons) { - for (const auto & stop_reason : stop_reason_array.stop_reasons) { - if (msg_stop_reason.reason == stop_reason.reason) { - // find matching reason - return true; - } - } - } - // cannot find matching reason - return false; -} - -void AutowareIvStopReasonAggregator::applyTimeOut() -{ - const auto current_time = clock_->now(); - - // make timeout-msg list - std::vector remove_idx; - if (!stop_reason_array_vec_.empty()) { - for (int i = stop_reason_array_vec_.size() - 1; i >= 0; i--) { - if ( - (current_time - rclcpp::Time(stop_reason_array_vec_.at(i).header.stamp)).seconds() > - timeout_) { - remove_idx.emplace_back(i); - } - } - } - // remove timeout-msg - for (const auto idx : remove_idx) { - stop_reason_array_vec_.erase(stop_reason_array_vec_.begin() + idx); - } -} - -void AutowareIvStopReasonAggregator::appendStopReasonToArray( - const tier4_planning_msgs::msg::StopReason & stop_reason, - tier4_planning_msgs::msg::StopReasonArray * stop_reason_array, const AutowareInfo & aw_info) -{ - // calculate dist_to_stop_pose - const auto stop_reason_with_dist = inputStopDistToStopReason(stop_reason, aw_info); - - // cut stop reason - const auto near_stop_reason = getNearStopReason(stop_reason_with_dist, aw_info); - - // if stop factors is empty, not append - if (near_stop_reason.stop_factors.empty()) { - return; - } - - // if already exists same reason msg in stop_reason_array_msg, append stop_factors to there - for (auto & base_stop_reasons : stop_reason_array->stop_reasons) { - if (base_stop_reasons.reason == near_stop_reason.reason) { - base_stop_reasons.stop_factors.insert( - base_stop_reasons.stop_factors.end(), near_stop_reason.stop_factors.begin(), - near_stop_reason.stop_factors.end()); - return; - } - } - - // if not exist same reason msg, append new stop reason - stop_reason_array->stop_reasons.emplace_back(near_stop_reason); -} - -tier4_planning_msgs::msg::StopReasonArray::ConstSharedPtr -AutowareIvStopReasonAggregator::makeStopReasonArray(const AutowareInfo & aw_info) -{ - tier4_planning_msgs::msg::StopReasonArray stop_reason_array_msg; - // input header - stop_reason_array_msg.header.frame_id = "map"; - stop_reason_array_msg.header.stamp = clock_->now(); - - // input stop reason - for (const auto & stop_reason_array : stop_reason_array_vec_) { - for (const auto & stop_reason : stop_reason_array.stop_reasons) { - appendStopReasonToArray(stop_reason, &stop_reason_array_msg, aw_info); - } - } - return std::make_shared(stop_reason_array_msg); -} - -tier4_planning_msgs::msg::StopReason AutowareIvStopReasonAggregator::inputStopDistToStopReason( - const tier4_planning_msgs::msg::StopReason & stop_reason, const AutowareInfo & aw_info) -{ - if (!aw_info.autoware_planning_traj_ptr || !aw_info.current_pose_ptr) { - // pass through all stop reason - return stop_reason; - } - - auto stop_reason_with_dist = stop_reason; - for (auto & stop_factor : stop_reason_with_dist.stop_factors) { - const auto & trajectory = *aw_info.autoware_planning_traj_ptr; - const auto & current_pose = aw_info.current_pose_ptr->pose; - stop_factor.dist_to_stop_pose = - planning_util::calcDistanceAlongTrajectory(trajectory, current_pose, stop_factor.stop_pose); - } - return stop_reason_with_dist; -} - -tier4_planning_msgs::msg::StopReason AutowareIvStopReasonAggregator::getNearStopReason( - const tier4_planning_msgs::msg::StopReason & stop_reason, const AutowareInfo & aw_info) -{ - if (!aw_info.autoware_planning_traj_ptr || !aw_info.current_pose_ptr) { - // pass through all stop reason - return stop_reason; - } - - tier4_planning_msgs::msg::StopReason near_stop_reason; - near_stop_reason.reason = stop_reason.reason; - for (const auto & stop_factor : stop_reason.stop_factors) { - if (stop_factor.dist_to_stop_pose < thresh_dist_to_stop_pose_) { - // append only near stop factor - near_stop_reason.stop_factors.emplace_back(stop_factor); - } - } - return near_stop_reason; -} - -} // namespace autoware_api diff --git a/awapi/awapi_awiv_adapter/src/awapi_v2x_aggregator.cpp b/awapi/awapi_awiv_adapter/src/awapi_v2x_aggregator.cpp deleted file mode 100644 index 174ee63aeae21..0000000000000 --- a/awapi/awapi_awiv_adapter/src/awapi_v2x_aggregator.cpp +++ /dev/null @@ -1,104 +0,0 @@ -// Copyright 2021 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "awapi_awiv_adapter/awapi_v2x_aggregator.hpp" - -#include -#include - -namespace autoware_api -{ -namespace -{ -std::string createKey(const Command & command) { return command.type + "-" + command.id; } - -std::string createKey(const State & state) { return state.type + "-" + state.id; } -} // namespace - -AutowareIvV2XAggregator::AutowareIvV2XAggregator(rclcpp::Node & node) -: logger_(node.get_logger().get_child("awapi_awiv_v2x_aggregator")), clock_(node.get_clock()) -{ -} - -CommandArray::ConstSharedPtr AutowareIvV2XAggregator::updateV2XCommand( - const CommandArray::ConstSharedPtr & msg) -{ - // Update data - for (const auto & command : msg->commands) { - const auto key = createKey(command); - command_map_[key] = command; - } - - // Pick valid data - auto output = std::make_shared(); - output->stamp = clock_->now(); - for (const auto & [key, command] : command_map_) { - // Calculate time diff - const auto delay = (clock_->now() - command.stamp).seconds(); - - // Ignore future data considering clock's error - if (delay < -max_clock_error_sec_) { - RCLCPP_DEBUG( - logger_, "future command: delay=%f, max_clock_error=%f", delay, max_clock_error_sec_); - continue; - } - - // Ignore old data - if (delay > max_delay_sec_) { - RCLCPP_DEBUG(logger_, "old command: delay=%f, max_delay_sec=%f", delay, max_delay_sec_); - continue; - } - - output->commands.push_back(command); - } - - return output; -} - -StateArray::ConstSharedPtr AutowareIvV2XAggregator::updateV2XState( - const StateArray::ConstSharedPtr & msg) -{ - // Update data - for (const auto & state : msg->states) { - const auto key = createKey(state); - state_map_[key] = state; - } - - // Pick valid data - auto output = std::make_shared(); - output->stamp = clock_->now(); - for (const auto & [key, state] : state_map_) { - // Calculate time diff - const auto delay = (clock_->now() - state.stamp).seconds(); - - // Ignore future data considering clock's error - if (delay < -max_clock_error_sec_) { - RCLCPP_DEBUG( - logger_, "future state: delay=%f, max_clock_error=%f", delay, max_clock_error_sec_); - continue; - } - - // Ignore old data - if (delay > max_delay_sec_) { - RCLCPP_DEBUG(logger_, "old state: delay=%f, max_delay_sec=%f", delay, max_delay_sec_); - continue; - } - - output->states.push_back(state); - } - - return output; -} - -} // namespace autoware_api diff --git a/awapi/awapi_awiv_adapter/src/awapi_vehicle_state_publisher.cpp b/awapi/awapi_awiv_adapter/src/awapi_vehicle_state_publisher.cpp deleted file mode 100644 index 0de8626df6b38..0000000000000 --- a/awapi/awapi_awiv_adapter/src/awapi_vehicle_state_publisher.cpp +++ /dev/null @@ -1,226 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "awapi_awiv_adapter/awapi_vehicle_state_publisher.hpp" - -#include "tier4_auto_msgs_converter/tier4_auto_msgs_converter.hpp" - -#include -#include - -namespace autoware_api -{ -AutowareIvVehicleStatePublisher::AutowareIvVehicleStatePublisher(rclcpp::Node & node) -: logger_(node.get_logger().get_child("awapi_awiv_vehicle_state_publisher")), - clock_(node.get_clock()), - prev_accel_(0.0) -{ - // publisher - pub_state_ = - node.create_publisher("output/vehicle_status", 1); -} - -void AutowareIvVehicleStatePublisher::statePublisher(const AutowareInfo & aw_info) -{ - tier4_api_msgs::msg::AwapiVehicleStatus status = initVehicleStatus(); - - // input header - status.header.frame_id = "base_link"; - status.header.stamp = clock_->now(); - - // get all info - getPoseInfo(aw_info.current_pose_ptr, &status); - getSteerInfo(aw_info.steer_ptr, &status); - getVehicleCmdInfo(aw_info.vehicle_cmd_ptr, &status); - getTurnSignalInfo(aw_info.turn_indicators_ptr, aw_info.hazard_lights_ptr, &status); - getTwistInfo(aw_info.odometry_ptr, &status); - getGearInfo(aw_info.gear_ptr, &status); - getBatteryInfo(aw_info.battery_ptr, &status); - getGpsInfo(aw_info.nav_sat_ptr, &status); - - // publish info - pub_state_->publish(status); -} - -tier4_api_msgs::msg::AwapiVehicleStatus AutowareIvVehicleStatePublisher::initVehicleStatus() -{ - tier4_api_msgs::msg::AwapiVehicleStatus status; - // set default value - if (std::numeric_limits::has_quiet_NaN) { - status.energy_level = std::numeric_limits::quiet_NaN(); - } - return status; -} - -void AutowareIvVehicleStatePublisher::getPoseInfo( - const std::shared_ptr & pose_ptr, - tier4_api_msgs::msg::AwapiVehicleStatus * status) -{ - if (!pose_ptr) { - RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *clock_, 5000 /* ms */, "current pose is nullptr"); - return; - } - - // get pose - status->pose = pose_ptr->pose; - - // convert quaternion to euler - double yaw, pitch, roll; - tf2::getEulerYPR(pose_ptr->pose.orientation, yaw, pitch, roll); - status->eulerangle.yaw = yaw; - status->eulerangle.pitch = pitch; - status->eulerangle.roll = roll; -} - -void AutowareIvVehicleStatePublisher::getSteerInfo( - const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr & steer_ptr, - tier4_api_msgs::msg::AwapiVehicleStatus * status) -{ - if (!steer_ptr) { - RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *clock_, 5000 /* ms */, "steer is nullptr"); - return; - } - - // get steer - using tier4_auto_msgs_converter::convert; - status->steering = convert(*steer_ptr).data; - - // get steer vel - if (previous_steer_ptr_) { - // calculate steer vel from steer - const double ds = steer_ptr->steering_tire_angle - previous_steer_ptr_->steering_tire_angle; - const double dt = std::max( - (rclcpp::Time(steer_ptr->stamp) - rclcpp::Time(previous_steer_ptr_->stamp)).seconds(), 1e-03); - const double steer_vel = ds / dt; - - // apply lowpass filter - const double lowpass_steer = - lowpass_filter(steer_vel, prev_steer_vel_, steer_vel_lowpass_gain_); - prev_steer_vel_ = lowpass_steer; - status->steering_velocity = lowpass_steer; - } - previous_steer_ptr_ = steer_ptr; -} -void AutowareIvVehicleStatePublisher::getVehicleCmdInfo( - const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr & vehicle_cmd_ptr, - tier4_api_msgs::msg::AwapiVehicleStatus * status) -{ - if (!vehicle_cmd_ptr) { - RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *clock_, 5000 /* ms */, "vehicle cmd is nullptr"); - return; - } - - // get command - status->target_acceleration = vehicle_cmd_ptr->longitudinal.acceleration; - status->target_velocity = vehicle_cmd_ptr->longitudinal.speed; - status->target_steering = vehicle_cmd_ptr->lateral.steering_tire_angle; - status->target_steering_velocity = vehicle_cmd_ptr->lateral.steering_tire_rotation_rate; -} - -void AutowareIvVehicleStatePublisher::getTurnSignalInfo( - const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr & turn_indicators_ptr, - const autoware_auto_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr & hazard_lights_ptr, - tier4_api_msgs::msg::AwapiVehicleStatus * status) -{ - if (!turn_indicators_ptr) { - RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *clock_, 5000 /* ms */, "turn indicators is nullptr"); - return; - } - - if (!hazard_lights_ptr) { - RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *clock_, 5000 /* ms */, "hazard lights is nullptr"); - return; - } - - // get turn signal - using tier4_auto_msgs_converter::convert; - status->turn_signal = convert(*turn_indicators_ptr, *hazard_lights_ptr).data; -} - -void AutowareIvVehicleStatePublisher::getTwistInfo( - const nav_msgs::msg::Odometry::ConstSharedPtr & odometry_ptr, - tier4_api_msgs::msg::AwapiVehicleStatus * status) -{ - if (!odometry_ptr) { - RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *clock_, 5000 /* ms */, "odometry is nullptr"); - return; - } - - // get twist - status->velocity = odometry_ptr->twist.twist.linear.x; - status->angular_velocity = odometry_ptr->twist.twist.angular.z; - - // get accel - if (previous_odometry_ptr_) { - // calculate acceleration from velocity - const double dv = - odometry_ptr->twist.twist.linear.x - previous_odometry_ptr_->twist.twist.linear.x; - const double dt = std::max( - (rclcpp::Time(odometry_ptr->header.stamp) - - rclcpp::Time(previous_odometry_ptr_->header.stamp)) - .seconds(), - 1e-03); - const double accel = dv / dt; - - // apply lowpass filter - const double lowpass_accel = lowpass_filter(accel, prev_accel_, accel_lowpass_gain_); - prev_accel_ = lowpass_accel; - status->acceleration = lowpass_accel; - } - previous_odometry_ptr_ = odometry_ptr; -} - -void AutowareIvVehicleStatePublisher::getGearInfo( - const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr & gear_ptr, - tier4_api_msgs::msg::AwapiVehicleStatus * status) -{ - if (!gear_ptr) { - RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *clock_, 5000 /* ms */, "gear is nullptr"); - return; - } - - // get gear (shift) - using tier4_auto_msgs_converter::convert; - status->gear = convert(*gear_ptr).shift.data; -} - -void AutowareIvVehicleStatePublisher::getBatteryInfo( - const tier4_vehicle_msgs::msg::BatteryStatus::ConstSharedPtr & battery_ptr, - tier4_api_msgs::msg::AwapiVehicleStatus * status) -{ - if (!battery_ptr) { - RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *clock_, 5000 /* ms */, "battery is nullptr"); - return; - } - - // get battery - status->energy_level = battery_ptr->energy_level; -} - -void AutowareIvVehicleStatePublisher::getGpsInfo( - const sensor_msgs::msg::NavSatFix::ConstSharedPtr & nav_sat_ptr, - tier4_api_msgs::msg::AwapiVehicleStatus * status) -{ - if (!nav_sat_ptr) { - RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *clock_, 5000 /* ms */, "nav_sat(gps) is nullptr"); - return; - } - - // get geo_point - status->geo_point.latitude = nav_sat_ptr->latitude; - status->geo_point.longitude = nav_sat_ptr->longitude; - status->geo_point.altitude = nav_sat_ptr->altitude; -} - -} // namespace autoware_api