diff --git a/awapi_awiv_adapter/CMakeLists.txt b/awapi_awiv_adapter/CMakeLists.txt index 2405cd3..dab1414 100644 --- a/awapi_awiv_adapter/CMakeLists.txt +++ b/awapi_awiv_adapter/CMakeLists.txt @@ -8,6 +8,7 @@ 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_velocity_factor_converter.cpp src/awapi_autoware_state_publisher.cpp src/awapi_stop_reason_aggregator.cpp src/awapi_v2x_aggregator.cpp diff --git a/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.hpp b/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.hpp index 6bf1001..592b10e 100644 --- a/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.hpp +++ b/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_awiv_adapter_core.hpp @@ -23,6 +23,7 @@ #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 "awapi_awiv_adapter/awapi_velocity_factor_converter.hpp" #include @@ -74,6 +75,8 @@ class AutowareIvAdapter : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_emergency_; rclcpp::Subscription::SharedPtr sub_hazard_status_; + rclcpp::Subscription::SharedPtr + sub_velocity_factor_; rclcpp::Subscription::SharedPtr sub_stop_reason_; rclcpp::Subscription::SharedPtr sub_v2x_command_; rclcpp::Subscription::SharedPtr @@ -123,7 +126,8 @@ class AutowareIvAdapter : public rclcpp::Node void callbackMrmState(const autoware_adapi_v1_msgs::msg::MrmState::ConstSharedPtr msg_ptr); void callbackHazardStatus( const autoware_system_msgs::msg::HazardStatusStamped::ConstSharedPtr msg_ptr); - void callbackStopReason(const tier4_planning_msgs::msg::StopReasonArray::ConstSharedPtr msg_ptr); + void callbackVelocityFactor( + const autoware_adapi_v1_msgs::msg::VelocityFactorArray::ConstSharedPtr msg_ptr); void callbackV2XCommand( const tier4_v2x_msgs::msg::InfrastructureCommandArray::ConstSharedPtr msg_ptr); void callbackV2XState( @@ -156,6 +160,7 @@ class AutowareIvAdapter : public rclcpp::Node AutowareInfo aw_info_; std::unique_ptr vehicle_state_publisher_; std::unique_ptr autoware_state_publisher_; + std::unique_ptr velocity_factor_converter_; std::unique_ptr stop_reason_aggregator_; std::unique_ptr v2x_aggregator_; std::unique_ptr lane_change_state_publisher_; diff --git a/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_velocity_factor_converter.hpp b/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_velocity_factor_converter.hpp new file mode 100644 index 0000000..9175379 --- /dev/null +++ b/awapi_awiv_adapter/include/awapi_awiv_adapter/awapi_velocity_factor_converter.hpp @@ -0,0 +1,81 @@ +// Copyright 2024 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_VELOCITY_FACTOR_CONVERTER_HPP_ +#define AWAPI_AWIV_ADAPTER__AWAPI_VELOCITY_FACTOR_CONVERTER_HPP_ + +#include "awapi_awiv_adapter/awapi_autoware_util.hpp" + +#include + +#include +#include +#include + +#include +#include +#include + +namespace autoware_api +{ + +using autoware_adapi_v1_msgs::msg::PlanningBehavior; +using tier4_planning_msgs::msg::StopReason; + +const std::map conversion_map = { + {PlanningBehavior::AVOIDANCE, StopReason::AVOIDANCE}, + {PlanningBehavior::CROSSWALK, StopReason::CROSSWALK}, + {PlanningBehavior::GOAL_PLANNER, StopReason::GOAL_PLANNER}, + {PlanningBehavior::INTERSECTION, StopReason::INTERSECTION}, + {PlanningBehavior::LANE_CHANGE, StopReason::LANE_CHANGE}, + {PlanningBehavior::MERGE, StopReason::MERGE_FROM_PRIVATE_ROAD}, + {PlanningBehavior::NO_DRIVABLE_LANE, StopReason::NO_DRIVABLE_LANE}, + {PlanningBehavior::NO_STOPPING_AREA, StopReason::NO_STOPPING_AREA}, + {PlanningBehavior::REAR_CHECK, StopReason::BLIND_SPOT}, + {PlanningBehavior::ROUTE_OBSTACLE, StopReason::OBSTACLE_STOP}, + {PlanningBehavior::SIDEWALK, StopReason::WALKWAY}, + {PlanningBehavior::START_PLANNER, StopReason::START_PLANNER}, + {PlanningBehavior::STOP_SIGN, StopReason::STOP_LINE}, + {PlanningBehavior::SURROUNDING_OBSTACLE, StopReason::SURROUND_OBSTACLE_CHECK}, + {PlanningBehavior::TRAFFIC_SIGNAL, StopReason::TRAFFIC_LIGHT}, + {PlanningBehavior::USER_DEFINED_DETECTION_AREA, StopReason::DETECTION_AREA}, + {PlanningBehavior::VIRTUAL_TRAFFIC_LIGHT, StopReason::VIRTUAL_TRAFFIC_LIGHT}, + {PlanningBehavior::RUN_OUT, StopReason::OBSTACLE_STOP}, + {PlanningBehavior::ADAPTIVE_CRUISE, "AdaptiveCruise"}}; + +class AutowareIvVelocityFactorConverter +{ +public: + AutowareIvVelocityFactorConverter(rclcpp::Node & node, const double thresh_dist_to_stop_pose); + + tier4_planning_msgs::msg::StopReasonArray::ConstSharedPtr updateStopReasonArray( + const autoware_adapi_v1_msgs::msg::VelocityFactorArray::ConstSharedPtr & msg_ptr); + +private: + tier4_planning_msgs::msg::StopReasonArray::ConstSharedPtr convert( + const autoware_adapi_v1_msgs::msg::VelocityFactorArray::ConstSharedPtr & msg_ptr); + + tier4_planning_msgs::msg::StopFactor convert( + const autoware_adapi_v1_msgs::msg::VelocityFactor & factor); + + rclcpp::Logger logger_; + + rclcpp::Clock::SharedPtr clock_; + + double thresh_dist_to_stop_pose_; +}; + +} // namespace autoware_api + +#endif // AWAPI_AWIV_ADAPTER__AWAPI_VELOCITY_FACTOR_CONVERTER_HPP_ diff --git a/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml b/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml index e7fae40..12341f1 100644 --- a/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml +++ b/awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml @@ -18,7 +18,7 @@ - + @@ -112,7 +112,7 @@ - + diff --git a/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp b/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp index 02ec938..6396c69 100644 --- a/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp +++ b/awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp @@ -41,6 +41,8 @@ AutowareIvAdapter::AutowareIvAdapter() autoware_state_publisher_ = std::make_unique(*this); stop_reason_aggregator_ = std::make_unique( *this, stop_reason_timeout_, stop_reason_thresh_dist_); + velocity_factor_converter_ = + std::make_unique(*this, stop_reason_thresh_dist_); v2x_aggregator_ = std::make_unique(*this); lane_change_state_publisher_ = std::make_unique(*this); obstacle_avoidance_state_publisher_ = @@ -85,8 +87,10 @@ AutowareIvAdapter::AutowareIvAdapter() "input/mrm_state", 1, std::bind(&AutowareIvAdapter::callbackMrmState, 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_velocity_factor_ = + this->create_subscription( + "input/velocity_factors", 100, + std::bind(&AutowareIvAdapter::callbackVelocityFactor, this, _1)); sub_v2x_command_ = this->create_subscription( "input/v2x_command", 100, std::bind(&AutowareIvAdapter::callbackV2XCommand, this, _1)); sub_v2x_state_ = this->create_subscription( @@ -257,10 +261,10 @@ void AutowareIvAdapter::callbackHazardStatus( aw_info_.hazard_status_ptr = msg_ptr; } -void AutowareIvAdapter::callbackStopReason( - const tier4_planning_msgs::msg::StopReasonArray::ConstSharedPtr msg_ptr) +void AutowareIvAdapter::callbackVelocityFactor( + const autoware_adapi_v1_msgs::msg::VelocityFactorArray::ConstSharedPtr msg_ptr) { - aw_info_.stop_reason_ptr = stop_reason_aggregator_->updateStopReasonArray(msg_ptr, aw_info_); + aw_info_.stop_reason_ptr = velocity_factor_converter_->updateStopReasonArray(msg_ptr); } void AutowareIvAdapter::callbackV2XCommand( diff --git a/awapi_awiv_adapter/src/awapi_velocity_factor_converter.cpp b/awapi_awiv_adapter/src/awapi_velocity_factor_converter.cpp new file mode 100644 index 0000000..dd9dc9f --- /dev/null +++ b/awapi_awiv_adapter/src/awapi_velocity_factor_converter.cpp @@ -0,0 +1,79 @@ +// Copyright 2024 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_velocity_factor_converter.hpp" + +#include +#include + +namespace autoware_api +{ +AutowareIvVelocityFactorConverter::AutowareIvVelocityFactorConverter( + rclcpp::Node & node, const double thresh_dist_to_stop_pose) +: logger_(node.get_logger().get_child("awapi_awiv_velocity_factor_converter")), + clock_(node.get_clock()), + thresh_dist_to_stop_pose_(thresh_dist_to_stop_pose) +{ +} + +tier4_planning_msgs::msg::StopReasonArray::ConstSharedPtr +AutowareIvVelocityFactorConverter::updateStopReasonArray( + const autoware_adapi_v1_msgs::msg::VelocityFactorArray::ConstSharedPtr & msg_ptr) +{ + return convert(msg_ptr); +} + +tier4_planning_msgs::msg::StopReasonArray::ConstSharedPtr +AutowareIvVelocityFactorConverter::convert( + const autoware_adapi_v1_msgs::msg::VelocityFactorArray::ConstSharedPtr & msg_ptr) +{ + tier4_planning_msgs::msg::StopReasonArray stop_reason_array_msg; + // input header + stop_reason_array_msg.header.frame_id = "map"; + stop_reason_array_msg.header.stamp = clock_->now(); + + // input stop reason + for (const auto & factor : msg_ptr->factors) { + // stop reason doesn't has corresponding factor. + if (conversion_map.count(factor.behavior) == 0) { + continue; + } + + // append only near velocity factor. + if (factor.distance > thresh_dist_to_stop_pose_) { + continue; + } + + // TODO(satoshi-ota): it's better to check if it is stop factor. + tier4_planning_msgs::msg::StopReason near_stop_reason; + near_stop_reason.reason = conversion_map.at(factor.behavior); + near_stop_reason.stop_factors.push_back(convert(factor)); + + stop_reason_array_msg.stop_reasons.push_back(near_stop_reason); + } + + return std::make_shared(stop_reason_array_msg); +} + +tier4_planning_msgs::msg::StopFactor AutowareIvVelocityFactorConverter::convert( + const autoware_adapi_v1_msgs::msg::VelocityFactor & factor) +{ + tier4_planning_msgs::msg::StopFactor stop_factor; + stop_factor.stop_pose = factor.pose; + stop_factor.dist_to_stop_pose = factor.distance; + + return stop_factor; +} + +} // namespace autoware_api