feat(awapi): add velocity factor converter

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

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 <rclcpp/rclcpp.hpp>
 
@@ -74,6 +75,8 @@ class AutowareIvAdapter : public rclcpp::Node
   rclcpp::Subscription<autoware_adapi_v1_msgs::msg::MrmState>::SharedPtr sub_emergency_;
   rclcpp::Subscription<autoware_system_msgs::msg::HazardStatusStamped>::SharedPtr
     sub_hazard_status_;
+  rclcpp::Subscription<autoware_adapi_v1_msgs::msg::VelocityFactorArray>::SharedPtr
+    sub_velocity_factor_;
   rclcpp::Subscription<tier4_planning_msgs::msg::StopReasonArray>::SharedPtr sub_stop_reason_;
   rclcpp::Subscription<tier4_v2x_msgs::msg::InfrastructureCommandArray>::SharedPtr sub_v2x_command_;
   rclcpp::Subscription<tier4_v2x_msgs::msg::VirtualTrafficLightStateArray>::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<AutowareIvVehicleStatePublisher> vehicle_state_publisher_;
   std::unique_ptr<AutowareIvAutowareStatePublisher> autoware_state_publisher_;
+  std::unique_ptr<AutowareIvVelocityFactorConverter> velocity_factor_converter_;
   std::unique_ptr<AutowareIvStopReasonAggregator> stop_reason_aggregator_;
   std::unique_ptr<AutowareIvV2XAggregator> v2x_aggregator_;
   std::unique_ptr<AutowareIvLaneChangeStatePublisher> lane_change_state_publisher_;

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 <rclcpp/rclcpp.hpp>
+
+#include <autoware_adapi_v1_msgs/msg/planning_behavior.hpp>
+#include <autoware_adapi_v1_msgs/msg/velocity_factor.hpp>
+#include <autoware_adapi_v1_msgs/msg/velocity_factor_array.hpp>
+
+#include <map>
+#include <string>
+#include <vector>
+
+namespace autoware_api
+{
+
+using autoware_adapi_v1_msgs::msg::PlanningBehavior;
+using tier4_planning_msgs::msg::StopReason;
+
+const std::map<std::string, std::string> 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_

awapi_awiv_adapter/launch/awapi_awiv_adapter.launch.xml

@@ -18,7 +18,7 @@
   <arg name="input_gate_mode" default="/control/current_gate_mode"/>
   <arg name="input_mrm_state" default="/system/fail_safe/mrm_state"/>
   <arg name="input_hazard_status" default="/system/emergency/hazard_status"/>
-  <arg name="input_stop_reason" default="/planning/scenario_planning/status/stop_reasons"/>
+  <arg name="input_velocity_factors" default="/api/planning/velocity_factors"/>
   <arg name="input_v2x_command" default="/planning/scenario_planning/status/infrastructure_commands"/>
   <arg name="input_v2x_state" default="/system/v2x/virtual_traffic_light_states"/>
   <arg name="input_diagnostics" default="/diagnostics_agg"/>
@@ -112,7 +112,7 @@
       <remap from="input/gate_mode" to="$(var input_gate_mode)"/>
       <remap from="input/mrm_state" to="$(var input_mrm_state)"/>
       <remap from="input/hazard_status" to="$(var input_hazard_status)"/>
-      <remap from="input/stop_reason" to="$(var input_stop_reason)"/>
+      <remap from="input/velocity_factors" to="$(var input_velocity_factors)"/>
       <remap from="input/v2x_command" to="$(var input_v2x_command)"/>
       <remap from="input/v2x_state" to="$(var input_v2x_state)"/>
       <remap from="input/diagnostics" to="$(var input_diagnostics)"/>

awapi_awiv_adapter/src/awapi_awiv_adapter_core.cpp

@@ -41,6 +41,8 @@ AutowareIvAdapter::AutowareIvAdapter()
   autoware_state_publisher_ = std::make_unique<AutowareIvAutowareStatePublisher>(*this);
   stop_reason_aggregator_ = std::make_unique<AutowareIvStopReasonAggregator>(
     *this, stop_reason_timeout_, stop_reason_thresh_dist_);
+  velocity_factor_converter_ =
+    std::make_unique<AutowareIvVelocityFactorConverter>(*this, stop_reason_thresh_dist_);
   v2x_aggregator_ = std::make_unique<AutowareIvV2XAggregator>(*this);
   lane_change_state_publisher_ = std::make_unique<AutowareIvLaneChangeStatePublisher>(*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<autoware_system_msgs::msg::HazardStatusStamped>(
     "input/hazard_status", 1, std::bind(&AutowareIvAdapter::callbackHazardStatus, this, _1));
-  sub_stop_reason_ = this->create_subscription<tier4_planning_msgs::msg::StopReasonArray>(
-    "input/stop_reason", 100, std::bind(&AutowareIvAdapter::callbackStopReason, this, _1));
+  sub_velocity_factor_ =
+    this->create_subscription<autoware_adapi_v1_msgs::msg::VelocityFactorArray>(
+      "input/velocity_factors", 100,
+      std::bind(&AutowareIvAdapter::callbackVelocityFactor, this, _1));
   sub_v2x_command_ = this->create_subscription<tier4_v2x_msgs::msg::InfrastructureCommandArray>(
     "input/v2x_command", 100, std::bind(&AutowareIvAdapter::callbackV2XCommand, this, _1));
   sub_v2x_state_ = this->create_subscription<tier4_v2x_msgs::msg::VirtualTrafficLightStateArray>(
@@ -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(

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 <memory>
+#include <vector>
+
+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<tier4_planning_msgs::msg::StopReasonArray>(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