diff --git a/awapi/awapi_awiv_adapter/CMakeLists.txt b/awapi/awapi_awiv_adapter/CMakeLists.txt new file mode 100644 index 0000000000000..2f6171b969440 --- /dev/null +++ b/awapi/awapi_awiv_adapter/CMakeLists.txt @@ -0,0 +1,35 @@ +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 new file mode 100644 index 0000000000000..de876912a370a --- /dev/null +++ b/awapi/awapi_awiv_adapter/Readme.md @@ -0,0 +1,299 @@ +# 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 autoware_vehicle_msgs/Shift | | +| | float32 | energy_level | | available only for golf-cart | +| ✓ | int32 | turn_signal | according to autoware_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 autoware_vehicle_msgs/ControlMode | manual/auto (changed by /awapi/autoware/put/engage) | +| | int32 | gate_mode | autoware_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] | | +| | autoware_system_msgs/HazardStatus | hazard_status | | system hazard status | +| ✓ | autoware_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: autoware_planning_msgs/Route + +| ✓ | type | name | unit | note | +| --- | :--------------------------- | :--- | :--- | :--- | +| ✓ | autoware_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: autoware_planning_msgs/StopSpeedExceedStatus + +| ✓ | type | name | unit | note | +| --- | :------------------------------------------- | :--- | :--- | :--- | +| | autoware_planning_msgs/StopSpeedExceedStatus | | - | | + +### /awapi/prediction/get/objects + +- get predicted object +- MessageType: autoware_api_msgs/DynamicObjectArray + +| ✓ | type | name | unit | note | +| --- | :----------------------------------- | :--- | :--- | :--- | +| ✓ | autoware_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 | +| | autoware_planning_msgs/Path | candidate_path | according to autoware_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 | | +| | autoware_planning_msgs/Trajectory | candidate_path | according to autoware_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: autoware_api_msgs/DoorStatus.msg + +| ✓ | type | name | unit | note | +| --- | :--------------------------- | :----- | :--------------------------------------------------------------------------------------- | :------------------------------------------ | +| | autoware_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: autoware_api_msgs/VelocityLimit + +| ✓ | type | name | unit | note | +| --- | :------------------------------ | :--- | :--- | :----------- | +| ✓ | autoware_api_msgs/VelocityLimit | | | max velocity | + +### /awapi/vehicle/put/stop + +- set temporary stop signal +- MessageType: autoware_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 | + | --- | :---------------------------- | :--- | :--- | :--- | + | ✓ | autoware_api_msgs/StopCommand | | | | + +### /awapi/autoware/put/gate_mode + +- send gate mode (auto/remote) +- MessageType: autoware_control_msgs/GateMode + +| ✓ | type | name | unit | note | +| --- | :----------------------------- | :--- | :--- | :--- | +| | autoware_control_msgs/GateMode | | | | + +### /awapi/autoware/put/engage + +- send engage signal (both of autoware/engage and vehicle/engage) +- MessageType: autoware_vehicle_msgs/Engage + +| ✓ | type | name | unit | note | +| --- | :--------------------------- | :--- | :--- | :--- | +| ✓ | autoware_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: autoware_planning_msgs/Route + +| ✓ | type | name | unit | note | +| --- | :--------------------------- | :--- | :--- | :--- | +| ✓ | autoware_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: autoware_planning_msgs/LaneChangeCommand + +| ✓ | type | name | unit | note | +| --- | :------------------------------------------- | :--- | :--- | :--- | +| | autoware_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: autoware_planning_msgs/LaneChangeCommand + +| ✓ | type | name | unit | note | +| --- | :--------------------------------------- | :--- | :--- | :--- | +| | autoware_planning_msgs/LaneChangeCommand | | | | + +### /awapi/object_avoidance/put/approval + +- send object avoidance approval flag +- MessageType: autoware_planning_msgs/EnableAvoidance + +| ✓ | type | name | unit | note | +| --- | :------------------------------------- | :--- | :--- | :--- | +| | autoware_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: autoware_api_msgs/DoorCommand + - send True: open door + - send False: close door + +| ✓ | type | name | unit | note | +| --- | :---------------------------- | :--- | :--- | :------------------------------------------ | +| | autoware_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: autoware_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: autoware_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: autoware_planning_msgs/ExpandStopRange + +| ✓ | type | name | unit | note | +| --- | :------------------------------------- | :--- | :--- | :--- | +| | autoware_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 new file mode 100644 index 0000000000000..f01494fa9a25d --- /dev/null +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_state_publisher.hpp @@ -0,0 +1,81 @@ +// 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, + autoware_api_msgs::msg::AwapiAutowareStatus * status); + void getControlModeInfo( + const autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr & control_mode_ptr, + autoware_api_msgs::msg::AwapiAutowareStatus * status); + void getGateModeInfo( + const autoware_control_msgs::msg::GateMode::ConstSharedPtr & gate_mode_ptr, + autoware_api_msgs::msg::AwapiAutowareStatus * status); + void getEmergencyStateInfo( + const autoware_auto_system_msgs::msg::EmergencyState::ConstSharedPtr & emergency_state_ptr, + autoware_api_msgs::msg::AwapiAutowareStatus * status); + void getCurrentMaxVelInfo( + const autoware_planning_msgs::msg::VelocityLimit::ConstSharedPtr & current_max_velocity_ptr, + autoware_api_msgs::msg::AwapiAutowareStatus * status); + void getHazardStatusInfo( + const AutowareInfo & aw_info, autoware_api_msgs::msg::AwapiAutowareStatus * status); + void getStopReasonInfo( + const autoware_planning_msgs::msg::StopReasonArray::ConstSharedPtr & stop_reason_ptr, + autoware_api_msgs::msg::AwapiAutowareStatus * status); + void getDiagInfo( + const AutowareInfo & aw_info, autoware_api_msgs::msg::AwapiAutowareStatus * status); + void getErrorDiagInfo( + const AutowareInfo & aw_info, autoware_api_msgs::msg::AwapiAutowareStatus * status); + void getGlobalRptInfo( + const pacmod3_msgs::msg::GlobalRpt::ConstSharedPtr & global_rpt_ptr, + autoware_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 new file mode 100644 index 0000000000000..cd4460de7c2d5 --- /dev/null +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_autoware_util.hpp @@ -0,0 +1,148 @@ +// 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; + autoware_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; + autoware_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; + autoware_planning_msgs::msg::StopReasonArray::ConstSharedPtr stop_reason_ptr; + autoware_v2x_msgs::msg::InfrastructureCommandArray::ConstSharedPtr v2x_command_ptr; + autoware_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr v2x_state_ptr; + diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr diagnostic_ptr; + pacmod3_msgs::msg::GlobalRpt::ConstSharedPtr global_rpt_ptr; + autoware_planning_msgs::msg::LaneChangeStatus::ConstSharedPtr lane_change_available_ptr; + autoware_planning_msgs::msg::LaneChangeStatus::ConstSharedPtr lane_change_ready_ptr; + autoware_auto_planning_msgs::msg::Path::ConstSharedPtr lane_change_candidate_ptr; + autoware_planning_msgs::msg::IsAvoidancePossible::ConstSharedPtr obstacle_avoid_ready_ptr; + autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr obstacle_avoid_candidate_ptr; + autoware_api_msgs::msg::VelocityLimit::ConstSharedPtr max_velocity_ptr; + autoware_planning_msgs::msg::VelocityLimit::ConstSharedPtr current_max_velocity_ptr; + autoware_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 new file mode 100644 index 0000000000000..f233b42212428 --- /dev/null +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.hpp @@ -0,0 +1,192 @@ +// 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 autoware_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 autoware_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 autoware_planning_msgs::msg::StopReasonArray::ConstSharedPtr msg_ptr); + void callbackV2XCommand( + const autoware_v2x_msgs::msg::InfrastructureCommandArray::ConstSharedPtr msg_ptr); + void callbackV2XState( + const autoware_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 autoware_planning_msgs::msg::LaneChangeStatus::ConstSharedPtr msg_ptr); + void callbackLaneChangeReady( + const autoware_planning_msgs::msg::LaneChangeStatus::ConstSharedPtr msg_ptr); + void callbackLaneChangeCandidatePath( + const autoware_auto_planning_msgs::msg::Path::ConstSharedPtr msg_ptr); + void callbackLaneObstacleAvoidReady( + const autoware_planning_msgs::msg::IsAvoidancePossible::ConstSharedPtr msg_ptr); + void callbackLaneObstacleAvoidCandidatePath( + const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg_ptr); + void callbackMaxVelocity(const autoware_api_msgs::msg::VelocityLimit::ConstSharedPtr msg_ptr); + void callbackCurrentMaxVelocity( + const autoware_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg_ptr); + void callbackTemporaryStop(const autoware_api_msgs::msg::StopCommand::ConstSharedPtr msg_ptr); + void callbackAutowareTrajectory( + const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg_ptr); + void callbackDoorControl( + const autoware_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 new file mode 100644 index 0000000000000..f4b2803924405 --- /dev/null +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_lane_change_state_publisher.hpp @@ -0,0 +1,52 @@ +// 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 autoware_planning_msgs::msg::LaneChangeStatus::ConstSharedPtr & available_ptr, + autoware_api_msgs::msg::LaneChangeStatus * status); + void getLaneChangeReadyInfo( + const autoware_planning_msgs::msg::LaneChangeStatus::ConstSharedPtr & ready_ptr, + autoware_api_msgs::msg::LaneChangeStatus * status); + void getCandidatePathInfo( + const autoware_auto_planning_msgs::msg::Path::ConstSharedPtr & path_ptr, + autoware_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 new file mode 100644 index 0000000000000..219c1695a48b5 --- /dev/null +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_max_velocity_publisher.hpp @@ -0,0 +1,46 @@ +// 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 autoware_api_msgs::msg::VelocityLimit::ConstSharedPtr & max_velocity_ptr, + const autoware_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 new file mode 100644 index 0000000000000..bb9bc23d71c86 --- /dev/null +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_obstacle_avoidance_state_publisher.hpp @@ -0,0 +1,49 @@ +// 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 autoware_planning_msgs::msg::IsAvoidancePossible::ConstSharedPtr & ready_ptr, + autoware_api_msgs::msg::ObstacleAvoidanceStatus * status); + void getCandidatePathInfo( + const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr & path_ptr, + autoware_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 new file mode 100644 index 0000000000000..c5bbcf8b5600e --- /dev/null +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_pacmod_util.hpp @@ -0,0 +1,40 @@ +// 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 +{ +autoware_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 autoware_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 new file mode 100644 index 0000000000000..8487be93b1725 --- /dev/null +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_stop_reason_aggregator.hpp @@ -0,0 +1,64 @@ +// 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); + autoware_planning_msgs::msg::StopReasonArray::ConstSharedPtr updateStopReasonArray( + const autoware_planning_msgs::msg::StopReasonArray::ConstSharedPtr & msg_ptr, + const AutowareInfo & aw_info); + +private: + void applyUpdate( + const autoware_planning_msgs::msg::StopReasonArray::ConstSharedPtr & msg_ptr, + const AutowareInfo & aw_info); + bool checkMatchingReason( + const autoware_planning_msgs::msg::StopReasonArray::ConstSharedPtr & msg_stop_reason_array, + const autoware_planning_msgs::msg::StopReasonArray & stop_reason_array); + void applyTimeOut(); + void appendStopReasonToArray( + const autoware_planning_msgs::msg::StopReason & stop_reason, + autoware_planning_msgs::msg::StopReasonArray * stop_reason_array, const AutowareInfo & aw_info); + autoware_planning_msgs::msg::StopReasonArray::ConstSharedPtr makeStopReasonArray( + const AutowareInfo & aw_info); + autoware_planning_msgs::msg::StopReason inputStopDistToStopReason( + const autoware_planning_msgs::msg::StopReason & stop_reason, const AutowareInfo & aw_info); + double calcStopDistToStopFactor( + const autoware_planning_msgs::msg::StopFactor & stop_factor, const AutowareInfo & aw_info); + autoware_planning_msgs::msg::StopReason getNearStopReason( + const autoware_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 new file mode 100644 index 0000000000000..663b819af02bd --- /dev/null +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_v2x_aggregator.hpp @@ -0,0 +1,54 @@ +// 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 = autoware_v2x_msgs::msg::InfrastructureCommand; +using CommandArray = autoware_v2x_msgs::msg::InfrastructureCommandArray; +using State = autoware_v2x_msgs::msg::VirtualTrafficLightState; +using StateArray = autoware_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 new file mode 100644 index 0000000000000..6725f26b513a0 --- /dev/null +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_vehicle_state_publisher.hpp @@ -0,0 +1,83 @@ +// 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_; + + autoware_api_msgs::msg::AwapiVehicleStatus initVehicleStatus(); + void getPoseInfo( + const std::shared_ptr & pose_ptr, + autoware_api_msgs::msg::AwapiVehicleStatus * status); + void getSteerInfo( + const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr & steer_ptr, + autoware_api_msgs::msg::AwapiVehicleStatus * status); + void getVehicleCmdInfo( + const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr & + vehicle_cmd_ptr, + autoware_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, + autoware_api_msgs::msg::AwapiVehicleStatus * status); + void getTwistInfo( + const nav_msgs::msg::Odometry::ConstSharedPtr & odometry_ptr, + autoware_api_msgs::msg::AwapiVehicleStatus * status); + void getGearInfo( + const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr & gear_ptr, + autoware_api_msgs::msg::AwapiVehicleStatus * status); + void getBatteryInfo( + const autoware_vehicle_msgs::msg::BatteryStatus::ConstSharedPtr & battery_ptr, + autoware_api_msgs::msg::AwapiVehicleStatus * status); + void getGpsInfo( + const sensor_msgs::msg::NavSatFix::ConstSharedPtr & nav_sat_ptr, + autoware_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 new file mode 100644 index 0000000000000..87e84861b49ee --- /dev/null +++ b/awapi/awapi_awiv_adapter/include/awapi_awiv_adapter/diagnostics_filter.hpp @@ -0,0 +1,118 @@ +// 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 new file mode 100644 index 0000000000000..5038612f87644 --- /dev/null +++ b/awapi/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml @@ -0,0 +1,157 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/awapi/awapi_awiv_adapter/launch/awapi_relay_container.launch.py b/awapi/awapi_awiv_adapter/launch/awapi_relay_container.launch.py new file mode 100644 index 0000000000000..07e9b982c8fa9 --- /dev/null +++ b/awapi/awapi_awiv_adapter/launch/awapi_relay_container.launch.py @@ -0,0 +1,425 @@ +# 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": "autoware_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": "autoware_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": "autoware_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": "autoware_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": "autoware_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": "autoware_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": "autoware_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": "autoware_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": "autoware_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": "autoware_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": "autoware_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": "autoware_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": "autoware_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 new file mode 100644 index 0000000000000..93aa82c06b4c8 --- /dev/null +++ b/awapi/awapi_awiv_adapter/package.xml @@ -0,0 +1,43 @@ + + + awapi_awiv_adapter + 0.1.0 + The awapi_awiv_adapter package + Tomoya Kimura + Tomoya Kimura + Apache License 2.0 + + ament_cmake_auto + + autoware_api_msgs + autoware_auto_control_msgs + autoware_auto_planning_msgs + autoware_auto_system_msgs + autoware_auto_vehicle_msgs + autoware_control_msgs + autoware_external_api_msgs + autoware_iv_auto_msgs_converter + autoware_planning_msgs + autoware_v2x_msgs + autoware_vehicle_msgs + diagnostic_msgs + geometry_msgs + nav_msgs + pacmod3_msgs + rclcpp + sensor_msgs + std_msgs + tf2 + tf2_geometry_msgs + + autoware_auto_perception_msgs + autoware_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 new file mode 100644 index 0000000000000..5df89494b0623 --- /dev/null +++ b/awapi/awapi_awiv_adapter/src/awapi_autoware_state_publisher.cpp @@ -0,0 +1,302 @@ +// 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 "autoware_iv_auto_msgs_converter/autoware_iv_auto_msgs_converter.hpp" +#include "awapi_awiv_adapter/diagnostics_filter.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) +{ + autoware_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, + autoware_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 autoware_iv_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, + autoware_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 autoware_iv_auto_msgs_converter::convert; + status->control_mode = convert(*control_mode_ptr).data; +} + +void AutowareIvAutowareStatePublisher::getGateModeInfo( + const autoware_control_msgs::msg::GateMode::ConstSharedPtr & gate_mode_ptr, + autoware_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, + autoware_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 autoware_planning_msgs::msg::VelocityLimit::ConstSharedPtr & current_max_velocity_ptr, + autoware_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, autoware_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 autoware_iv_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 autoware_planning_msgs::msg::StopReasonArray::ConstSharedPtr & stop_reason_ptr, + autoware_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, autoware_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, autoware_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, + autoware_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 new file mode 100644 index 0000000000000..7d6ccba672996 --- /dev/null +++ b/awapi/awapi_awiv_adapter/src/awapi_autoware_util.cpp @@ -0,0 +1,108 @@ +// 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 new file mode 100644 index 0000000000000..4621e903e208b --- /dev/null +++ b/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp @@ -0,0 +1,384 @@ +// 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 autoware_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 autoware_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 autoware_planning_msgs::msg::StopReasonArray::ConstSharedPtr msg_ptr) +{ + aw_info_.stop_reason_ptr = stop_reason_aggregator_->updateStopReasonArray(msg_ptr, aw_info_); +} + +void AutowareIvAdapter::callbackV2XCommand( + const autoware_v2x_msgs::msg::InfrastructureCommandArray::ConstSharedPtr msg_ptr) +{ + aw_info_.v2x_command_ptr = v2x_aggregator_->updateV2XCommand(msg_ptr); +} + +void AutowareIvAdapter::callbackV2XState( + const autoware_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 autoware_planning_msgs::msg::LaneChangeStatus::ConstSharedPtr msg_ptr) +{ + aw_info_.lane_change_available_ptr = msg_ptr; +} + +void AutowareIvAdapter::callbackLaneChangeReady( + const autoware_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 autoware_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 autoware_api_msgs::msg::VelocityLimit::ConstSharedPtr msg_ptr) +{ + aw_info_.max_velocity_ptr = msg_ptr; + max_velocity_publisher_->statePublisher(aw_info_); +} + +void AutowareIvAdapter::callbackCurrentMaxVelocity( + const autoware_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg_ptr) +{ + aw_info_.current_max_velocity_ptr = msg_ptr; +} + +void AutowareIvAdapter::callbackTemporaryStop( + const autoware_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 autoware_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 new file mode 100644 index 0000000000000..c3bc000bd2e4c --- /dev/null +++ b/awapi/awapi_awiv_adapter/src/awapi_awiv_adapter_node.cpp @@ -0,0 +1,28 @@ +// 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 new file mode 100644 index 0000000000000..522e992abf584 --- /dev/null +++ b/awapi/awapi_awiv_adapter/src/awapi_lane_change_state_publisher.cpp @@ -0,0 +1,88 @@ +// 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 "autoware_iv_auto_msgs_converter/autoware_iv_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) +{ + autoware_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 autoware_planning_msgs::msg::LaneChangeStatus::ConstSharedPtr & available_ptr, + autoware_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 autoware_planning_msgs::msg::LaneChangeStatus::ConstSharedPtr & ready_ptr, + autoware_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, + autoware_api_msgs::msg::LaneChangeStatus * status) +{ + if (!path_ptr) { + RCLCPP_DEBUG_STREAM_THROTTLE( + logger_, *clock_, 5000 /* ms */, "lane_change_candidate_path is nullptr"); + return; + } + + using autoware_iv_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 new file mode 100644 index 0000000000000..632f5fd846cad --- /dev/null +++ b/awapi/awapi_awiv_adapter/src/awapi_max_velocity_publisher.cpp @@ -0,0 +1,61 @@ +// 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) +{ + autoware_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 autoware_api_msgs::msg::VelocityLimit::ConstSharedPtr & max_velocity_ptr, + const autoware_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 new file mode 100644 index 0000000000000..5f6c6e789a747 --- /dev/null +++ b/awapi/awapi_awiv_adapter/src/awapi_obstacle_avoidance_state_publisher.cpp @@ -0,0 +1,74 @@ +// 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 "autoware_iv_auto_msgs_converter/autoware_iv_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) +{ + autoware_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 autoware_planning_msgs::msg::IsAvoidancePossible::ConstSharedPtr & ready_ptr, + autoware_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, + autoware_api_msgs::msg::ObstacleAvoidanceStatus * status) +{ + if (!path_ptr) { + RCLCPP_DEBUG_STREAM_THROTTLE( + logger_, *clock_, 5000 /* ms */, "obstacle_avoidance_candidate_path is nullptr"); + return; + } + + using autoware_iv_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 new file mode 100644 index 0000000000000..fd219d888b9de --- /dev/null +++ b/awapi/awapi_awiv_adapter/src/awapi_pacmod_util.cpp @@ -0,0 +1,87 @@ +// 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 +{ +autoware_api_msgs::msg::DoorStatus getDoorStatusMsg( + const pacmod3_msgs::msg::SystemRptInt::ConstSharedPtr & msg_ptr) +{ + using autoware_api_msgs::msg::DoorStatus; + using pacmod3_msgs::msg::SystemRptInt; + 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 autoware_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 new file mode 100644 index 0000000000000..c5c9994d2d6d6 --- /dev/null +++ b/awapi/awapi_awiv_adapter/src/awapi_stop_reason_aggregator.cpp @@ -0,0 +1,186 @@ +// 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) +{ +} + +autoware_planning_msgs::msg::StopReasonArray::ConstSharedPtr +AutowareIvStopReasonAggregator::updateStopReasonArray( + const autoware_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 autoware_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 autoware_planning_msgs::msg::StopReasonArray::ConstSharedPtr & msg_stop_reason_array, + const autoware_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 autoware_planning_msgs::msg::StopReason & stop_reason, + autoware_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); +} + +autoware_planning_msgs::msg::StopReasonArray::ConstSharedPtr +AutowareIvStopReasonAggregator::makeStopReasonArray(const AutowareInfo & aw_info) +{ + autoware_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); +} + +autoware_planning_msgs::msg::StopReason AutowareIvStopReasonAggregator::inputStopDistToStopReason( + const autoware_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; +} + +autoware_planning_msgs::msg::StopReason AutowareIvStopReasonAggregator::getNearStopReason( + const autoware_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; + } + + autoware_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 new file mode 100644 index 0000000000000..174ee63aeae21 --- /dev/null +++ b/awapi/awapi_awiv_adapter/src/awapi_v2x_aggregator.cpp @@ -0,0 +1,104 @@ +// 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 new file mode 100644 index 0000000000000..d38e4dff171b2 --- /dev/null +++ b/awapi/awapi_awiv_adapter/src/awapi_vehicle_state_publisher.cpp @@ -0,0 +1,226 @@ +// 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 "autoware_iv_auto_msgs_converter/autoware_iv_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) +{ + autoware_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); +} + +autoware_api_msgs::msg::AwapiVehicleStatus AutowareIvVehicleStatePublisher::initVehicleStatus() +{ + autoware_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, + autoware_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, + autoware_api_msgs::msg::AwapiVehicleStatus * status) +{ + if (!steer_ptr) { + RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *clock_, 5000 /* ms */, "steer is nullptr"); + return; + } + + // get steer + using autoware_iv_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, + autoware_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, + autoware_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 autoware_iv_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, + autoware_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, + autoware_api_msgs::msg::AwapiVehicleStatus * status) +{ + if (!gear_ptr) { + RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *clock_, 5000 /* ms */, "gear is nullptr"); + return; + } + + // get gear (shift) + using autoware_iv_auto_msgs_converter::convert; + status->gear = convert(*gear_ptr).shift.data; +} + +void AutowareIvVehicleStatePublisher::getBatteryInfo( + const autoware_vehicle_msgs::msg::BatteryStatus::ConstSharedPtr & battery_ptr, + autoware_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, + autoware_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