feat(bpp): support new rtc cooperate status

Signed-off-by: satoshi-ota <[email protected]>
autowarefoundation · May 10, 2024 · 2877834 · 2877834
1 parent 6cc85b8
commit 2877834
Show file tree

Hide file tree

Showing 4 changed files with 18 additions and 8 deletions.
diff --git a/planning/behavior_path_avoidance_by_lane_change_module/src/interface.cpp b/planning/behavior_path_avoidance_by_lane_change_module/src/interface.cpp
@@ -57,7 +57,10 @@ void AvoidanceByLaneChangeInterface::updateRTCStatus(
     return (dir == Direction::LEFT) ? "left" : "right";
   });
 
+  const auto state = isWaitingApproval() ? State::WAITING_FOR_EXECUTION : State::RUNNING;
+
   rtc_interface_ptr_map_.at(direction)->updateCooperateStatus(
-    uuid_map_.at(direction), isExecutionReady(), start_distance, finish_distance, clock_->now());
+    uuid_map_.at(direction), isExecutionReady(), state, start_distance, finish_distance,
+    clock_->now());
 }
 }  // namespace behavior_path_planner
diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp
@@ -77,15 +77,17 @@ class AvoidanceModule : public SceneModuleInterface
   {
     if (candidate.lateral_shift > 0.0) {
       rtc_interface_ptr_map_.at("left")->updateCooperateStatus(
-        uuid_map_.at("left"), isExecutionReady(), candidate.start_distance_to_path_change,
-        candidate.finish_distance_to_path_change, clock_->now());
+        uuid_map_.at("left"), isExecutionReady(), State::WAITING_FOR_EXECUTION,
+        candidate.start_distance_to_path_change, candidate.finish_distance_to_path_change,
+        clock_->now());
       candidate_uuid_ = uuid_map_.at("left");
       return;
     }
     if (candidate.lateral_shift < 0.0) {
       rtc_interface_ptr_map_.at("right")->updateCooperateStatus(
-        uuid_map_.at("right"), isExecutionReady(), candidate.start_distance_to_path_change,
-        candidate.finish_distance_to_path_change, clock_->now());
+        uuid_map_.at("right"), isExecutionReady(), State::WAITING_FOR_EXECUTION,
+        candidate.start_distance_to_path_change, candidate.finish_distance_to_path_change,
+        clock_->now());
       candidate_uuid_ = uuid_map_.at("right");
       return;
     }
@@ -108,7 +110,7 @@ class AvoidanceModule : public SceneModuleInterface
         motion_utils::calcSignedArcLength(path.points, ego_idx, left_shift.start_pose.position);
       const double finish_distance = start_distance + left_shift.relative_longitudinal;
       rtc_interface_ptr_map_.at("left")->updateCooperateStatus(
-        left_shift.uuid, true, start_distance, finish_distance, clock_->now());
+        left_shift.uuid, true, State::RUNNING, start_distance, finish_distance, clock_->now());
       if (finish_distance > -1.0e-03) {
         steering_factor_interface_ptr_->updateSteeringFactor(
           {left_shift.start_pose, left_shift.finish_pose}, {start_distance, finish_distance},
@@ -121,7 +123,7 @@ class AvoidanceModule : public SceneModuleInterface
         motion_utils::calcSignedArcLength(path.points, ego_idx, right_shift.start_pose.position);
       const double finish_distance = start_distance + right_shift.relative_longitudinal;
       rtc_interface_ptr_map_.at("right")->updateCooperateStatus(
-        right_shift.uuid, true, start_distance, finish_distance, clock_->now());
+        right_shift.uuid, true, State::RUNNING, start_distance, finish_distance, clock_->now());
       if (finish_distance > -1.0e-03) {
         steering_factor_interface_ptr_->updateSteeringFactor(
           {right_shift.start_pose, right_shift.finish_pose}, {start_distance, finish_distance},

diff --git a/...ning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/type_alias.hpp b/...ning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/type_alias.hpp
@@ -28,6 +28,7 @@
 #include <autoware_auto_vehicle_msgs/msg/turn_indicators_command.hpp>
 #include <tier4_planning_msgs/msg/avoidance_debug_msg.hpp>
 #include <tier4_planning_msgs/msg/avoidance_debug_msg_array.hpp>
+#include <tier4_rtc_msgs/msg/state.hpp>
 #include <visualization_msgs/msg/marker_array.hpp>
 
 namespace behavior_path_planner
@@ -49,6 +50,7 @@ using visualization_msgs::msg::MarkerArray;
 // tier4 msgs
 using tier4_planning_msgs::msg::AvoidanceDebugMsg;
 using tier4_planning_msgs::msg::AvoidanceDebugMsgArray;
+using tier4_rtc_msgs::msg::State;
 
 // tier4 utils functions
 using tier4_autoware_utils::appendMarkerArray;

diff --git a/..._planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp b/..._planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp
@@ -41,6 +41,7 @@
 #include <tier4_planning_msgs/msg/stop_factor.hpp>
 #include <tier4_planning_msgs/msg/stop_reason.hpp>
 #include <tier4_planning_msgs/msg/stop_reason_array.hpp>
+#include <tier4_rtc_msgs/msg/state.hpp>
 #include <unique_identifier_msgs/msg/uuid.hpp>
 #include <visualization_msgs/msg/detail/marker_array__struct.hpp>
 
@@ -67,6 +68,7 @@ using tier4_planning_msgs::msg::AvoidanceDebugMsgArray;
 using tier4_planning_msgs::msg::StopFactor;
 using tier4_planning_msgs::msg::StopReason;
 using tier4_planning_msgs::msg::StopReasonArray;
+using tier4_rtc_msgs::msg::State;
 using unique_identifier_msgs::msg::UUID;
 using visualization_msgs::msg::MarkerArray;
 using PlanResult = PathWithLaneId::SharedPtr;
@@ -490,8 +492,9 @@ class SceneModuleInterface
   {
     for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) {
       if (ptr) {
+        const auto state = isWaitingApproval() ? State::WAITING_FOR_EXECUTION : State::RUNNING;
         ptr->updateCooperateStatus(
-          uuid_map_.at(module_name), isExecutionReady(), start_distance, finish_distance,
+          uuid_map_.at(module_name), isExecutionReady(), state, start_distance, finish_distance,
           clock_->now());
       }
     }