feat(rtc_interface): add finish distance (#1685)

* feat(rtc_interface): add_finish_distance

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

* fix(behavior_path_planner): remove unnecessary include

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

* fix(behavior_path_planner): fix typo

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

* fix(behavior_path_planner): fix goal pose

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp

@@ -36,8 +36,6 @@ namespace behavior_path_planner
 using tier4_planning_msgs::msg::AvoidanceDebugMsg;
 class AvoidanceModule : public SceneModuleInterface
 {
-  using RegisteredShiftPointArray = std::vector<std::pair<UUID, Pose>>;
-
 public:
   AvoidanceModule(
     const std::string & name, rclcpp::Node & node, std::shared_ptr<AvoidanceParameters> parameters);
@@ -70,6 +68,14 @@ class AvoidanceModule : public SceneModuleInterface
   }
 
 private:
+  struct RegisteredShiftPoint
+  {
+    UUID uuid;
+    Pose start_pose;
+    Pose finish_pose;
+  };
+  using RegisteredShiftPointArray = std::vector<RegisteredShiftPoint>;
+
   std::shared_ptr<AvoidanceParameters> parameters_;
 
   AvoidancePlanningData avoidance_data_;
@@ -89,35 +95,44 @@ class AvoidanceModule : public SceneModuleInterface
   {
     if (candidate.lateral_shift > 0.0) {
       rtc_interface_left_.updateCooperateStatus(
-        uuid_left_, isExecutionReady(), candidate.distance_to_path_change, clock_->now());
+        uuid_left_, isExecutionReady(), candidate.start_distance_to_path_change,
+        candidate.finish_distance_to_path_change, clock_->now());
       candidate_uuid_ = uuid_left_;
       return;
     }
     if (candidate.lateral_shift < 0.0) {
       rtc_interface_right_.updateCooperateStatus(
-        uuid_right_, isExecutionReady(), candidate.distance_to_path_change, clock_->now());
+        uuid_right_, isExecutionReady(), candidate.start_distance_to_path_change,
+        candidate.finish_distance_to_path_change, clock_->now());
       candidate_uuid_ = uuid_right_;
       return;
     }
 
     RCLCPP_WARN_STREAM(
-      getLogger(), "Direction is UNKNOWN, distance = " << candidate.distance_to_path_change);
+      getLogger(),
+      "Direction is UNKNOWN, start_distance = " << candidate.start_distance_to_path_change);
   }
 
   void updateRegisteredRTCStatus(const PathWithLaneId & path)
   {
     const Point ego_position = planner_data_->self_pose->pose.position;
 
     for (const auto & left_shift : left_shift_array_) {
-      const double distance =
-        motion_utils::calcSignedArcLength(path.points, ego_position, left_shift.second.position);
-      rtc_interface_left_.updateCooperateStatus(left_shift.first, true, distance, clock_->now());
+      const double start_distance = motion_utils::calcSignedArcLength(
+        path.points, ego_position, left_shift.start_pose.position);
+      const double finish_distance = motion_utils::calcSignedArcLength(
+        path.points, ego_position, left_shift.finish_pose.position);
+      rtc_interface_left_.updateCooperateStatus(
+        left_shift.uuid, true, start_distance, finish_distance, clock_->now());
     }
 
     for (const auto & right_shift : right_shift_array_) {
-      const double distance =
-        motion_utils::calcSignedArcLength(path.points, ego_position, right_shift.second.position);
-      rtc_interface_right_.updateCooperateStatus(right_shift.first, true, distance, clock_->now());
+      const double start_distance = motion_utils::calcSignedArcLength(
+        path.points, ego_position, right_shift.start_pose.position);
+      const double finish_distance = motion_utils::calcSignedArcLength(
+        path.points, ego_position, right_shift.finish_pose.position);
+      rtc_interface_right_.updateCooperateStatus(
+        right_shift.uuid, true, start_distance, finish_distance, clock_->now());
     }
   }
 

planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp

@@ -118,35 +118,38 @@ class LaneChangeModule : public SceneModuleInterface
 
   bool is_activated_ = false;
 
-  void waitApprovalLeft(const double distance)
+  void waitApprovalLeft(const double start_distance, const double finish_distance)
   {
     rtc_interface_left_.updateCooperateStatus(
-      uuid_left_, isExecutionReady(), distance, clock_->now());
+      uuid_left_, isExecutionReady(), start_distance, finish_distance, clock_->now());
     is_waiting_approval_ = true;
   }
 
-  void waitApprovalRight(const double distance)
+  void waitApprovalRight(const double start_distance, const double finish_distance)
   {
     rtc_interface_right_.updateCooperateStatus(
-      uuid_right_, isExecutionReady(), distance, clock_->now());
+      uuid_right_, isExecutionReady(), start_distance, finish_distance, clock_->now());
     is_waiting_approval_ = true;
   }
 
   void updateRTCStatus(const CandidateOutput & candidate)
   {
     if (candidate.lateral_shift > 0.0) {
       rtc_interface_left_.updateCooperateStatus(
-        uuid_left_, isExecutionReady(), candidate.distance_to_path_change, clock_->now());
+        uuid_left_, isExecutionReady(), candidate.start_distance_to_path_change,
+        candidate.finish_distance_to_path_change, clock_->now());
       return;
     }
     if (candidate.lateral_shift < 0.0) {
       rtc_interface_right_.updateCooperateStatus(
-        uuid_right_, isExecutionReady(), candidate.distance_to_path_change, clock_->now());
+        uuid_right_, isExecutionReady(), candidate.start_distance_to_path_change,
+        candidate.finish_distance_to_path_change, clock_->now());
       return;
     }
 
     RCLCPP_WARN_STREAM(
-      getLogger(), "Direction is UNKNOWN, distance = " << candidate.distance_to_path_change);
+      getLogger(),
+      "Direction is UNKNOWN, start_distance = " << candidate.start_distance_to_path_change);
   }
 
   void removeRTCStatus() override

planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp

@@ -200,7 +200,8 @@ class PullOverModule : public SceneModuleInterface
     const lanelet::ConstLanelets & lanelets, const Pose & goal_pose, const double buffer = 0) const;
   bool isArcPath() const;
   double calcMinimumShiftPathDistance() const;
-  double calcDistanceToPathChange() const;
+  std::pair<double, double> calcDistanceToPathChange() const;
+
   bool planShiftPath();
   bool isStopped();
   bool hasFinishedCurrentPath();

planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp

@@ -85,7 +85,8 @@ struct CandidateOutput
   explicit CandidateOutput(const PathWithLaneId & path) : path_candidate{path} {}
   PathWithLaneId path_candidate{};
   double lateral_shift{0.0};
-  double distance_to_path_change{std::numeric_limits<double>::lowest()};
+  double start_distance_to_path_change{std::numeric_limits<double>::lowest()};
+  double finish_distance_to_path_change{std::numeric_limits<double>::lowest()};
 };
 
 class SceneModuleInterface
@@ -251,12 +252,13 @@ class SceneModuleInterface
   UUID uuid_;
   bool is_waiting_approval_;
 
-  void updateRTCStatus(const double distance)
+  void updateRTCStatus(const double start_distance, const double finish_distance)
   {
     if (!rtc_interface_ptr_) {
       return;
     }
-    rtc_interface_ptr_->updateCooperateStatus(uuid_, isExecutionReady(), distance, clock_->now());
+    rtc_interface_ptr_->updateCooperateStatus(
+      uuid_, isExecutionReady(), start_distance, finish_distance, clock_->now());
   }
 
   virtual void removeRTCStatus()

planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp

@@ -2084,7 +2084,8 @@ CandidateOutput AvoidanceModule::planCandidate() const
       }
     }
     output.lateral_shift = new_shift_points->at(i).getRelativeLength();
-    output.distance_to_path_change = new_shift_points->front().start_longitudinal;
+    output.start_distance_to_path_change = new_shift_points->front().start_longitudinal;
+    output.finish_distance_to_path_change = new_shift_points->back().end_longitudinal;
   }
 
   const size_t ego_idx = findEgoIndex(shifted_path.path.points);
@@ -2101,7 +2102,7 @@ BehaviorModuleOutput AvoidanceModule::planWaitingApproval()
   BehaviorModuleOutput out = plan();
   const auto candidate = planCandidate();
   constexpr double threshold_to_update_status = -1.0e-03;
-  if (candidate.distance_to_path_change > threshold_to_update_status) {
+  if (candidate.start_distance_to_path_change > threshold_to_update_status) {
     updateCandidateRTCStatus(candidate);
     waitApproval();
   } else {
@@ -2132,9 +2133,11 @@ void AvoidanceModule::addShiftPointIfApproved(const AvoidPointArray & shift_poin
     }
 
     if (shift_points.at(i).getRelativeLength() > 0.0) {
-      left_shift_array_.push_back({uuid_left_, shift_points.front().start});
+      left_shift_array_.push_back(
+        {uuid_left_, shift_points.front().start, shift_points.back().end});
     } else if (shift_points.at(i).getRelativeLength() < 0.0) {
-      right_shift_array_.push_back({uuid_right_, shift_points.front().start});
+      right_shift_array_.push_back(
+        {uuid_right_, shift_points.front().start, shift_points.back().end});
     }
 
     uuid_left_ = generateUUID();

planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp

@@ -55,10 +55,16 @@ BehaviorModuleOutput LaneChangeModule::run()
   is_activated_ = isActivated();
   const auto output = plan();
   const auto turn_signal_info = output.turn_signal_info;
+  const auto current_pose = planner_data_->self_pose->pose;
+  const double start_distance = motion_utils::calcSignedArcLength(
+    output.path->points, current_pose.position,
+    status_.lane_change_path.shift_point.start.position);
+  const double finish_distance = motion_utils::calcSignedArcLength(
+    output.path->points, current_pose.position, status_.lane_change_path.shift_point.end.position);
   if (turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) {
-    waitApprovalLeft(turn_signal_info.signal_distance);
+    waitApprovalLeft(start_distance, finish_distance);
   } else if (turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_RIGHT) {
-    waitApprovalRight(turn_signal_info.signal_distance);
+    waitApprovalRight(start_distance, finish_distance);
   }
   return output;
 }
@@ -195,9 +201,12 @@ CandidateOutput LaneChangeModule::planCandidate() const
   output.path_candidate = selected_path.path;
   output.lateral_shift = selected_path.shifted_path.shift_length.at(end_idx) -
                          selected_path.shifted_path.shift_length.at(start_idx);
-  output.distance_to_path_change = motion_utils::calcSignedArcLength(
+  output.start_distance_to_path_change = motion_utils::calcSignedArcLength(
     selected_path.path.points, planner_data_->self_pose->pose.position,
     selected_path.shift_point.start.position);
+  output.finish_distance_to_path_change = motion_utils::calcSignedArcLength(
+    selected_path.path.points, planner_data_->self_pose->pose.position,
+    selected_path.shift_point.end.position);
 
   return output;
 }

planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp

@@ -85,8 +85,8 @@ void PullOutModule::onEntry()
     if (last_pull_out_start_update_time_ == nullptr) {
       last_pull_out_start_update_time_ = std::make_unique<rclcpp::Time>(clock_->now());
     }
-    const auto elpased_time = (clock_->now() - *last_pull_out_start_update_time_).seconds();
-    if (elpased_time < parameters_.backward_path_update_duration) {
+    const auto elapsed_time = (clock_->now() - *last_pull_out_start_update_time_).seconds();
+    if (elapsed_time < parameters_.backward_path_update_duration) {
       return;
     }
     last_pull_out_start_update_time_ = std::make_unique<rclcpp::Time>(clock_->now());
@@ -173,6 +173,21 @@ BehaviorModuleOutput PullOutModule::plan()
   output.turn_signal_info =
     calcTurnSignalInfo(status_.pull_out_path.start_pose, status_.pull_out_path.end_pose);
 
+  if (status_.back_finished) {
+    const double start_distance = motion_utils::calcSignedArcLength(
+      path.points, planner_data_->self_pose->pose.position,
+      status_.pull_out_path.start_pose.position);
+    const double finish_distance = motion_utils::calcSignedArcLength(
+      path.points, planner_data_->self_pose->pose.position,
+      status_.pull_out_path.end_pose.position);
+    updateRTCStatus(start_distance, finish_distance);
+  } else {
+    const double distance = motion_utils::calcSignedArcLength(
+      path.points, planner_data_->self_pose->pose.position,
+      status_.pull_out_path.start_pose.position);
+    updateRTCStatus(0.0, distance);
+  }
+
   setDebugData();
 
   return output;
@@ -234,8 +249,20 @@ BehaviorModuleOutput PullOutModule::planWaitingApproval()
   output.path_candidate = std::make_shared<PathWithLaneId>(candidate_path);
 
   waitApproval();
-  // requset approval when stopped at the corresponding point, so distance is 0
-  updateRTCStatus(0.0);
+  if (status_.back_finished) {
+    const double start_distance = motion_utils::calcSignedArcLength(
+      candidate_path.points, planner_data_->self_pose->pose.position,
+      status_.pull_out_path.start_pose.position);
+    const double finish_distance = motion_utils::calcSignedArcLength(
+      candidate_path.points, planner_data_->self_pose->pose.position,
+      status_.pull_out_path.end_pose.position);
+    updateRTCStatus(start_distance, finish_distance);
+  } else {
+    const double distance = motion_utils::calcSignedArcLength(
+      candidate_path.points, planner_data_->self_pose->pose.position,
+      status_.pull_out_path.start_pose.position);
+    updateRTCStatus(0.0, distance);
+  }
 
   setDebugData();
   return output;
@@ -421,7 +448,7 @@ std::vector<Pose> PullOutModule::searchBackedPoses()
     p.point.pose = calcOffsetPose(p.point.pose, 0, distance_from_center_line, 0);
   }
 
-  // check collision between footprint and onject at the backed pose
+  // check collision between footprint and object at the backed pose
   const auto local_vehicle_footprint = createVehicleFootprint(vehicle_info_);
   std::vector<Pose> backed_poses;
   for (double back_distance = 0.0; back_distance <= parameters_.max_back_distance;
@@ -489,12 +516,10 @@ void PullOutModule::checkBackFinished()
     RCLCPP_INFO(getLogger(), "back finished");
     status_.back_finished = true;
 
-    // requst pull_out approval
+    // request pull_out approval
     waitApproval();
     removeRTCStatus();
     uuid_ = generateUUID();
-    // requset approval when stopped at the corresponding point, so distance is 0
-    updateRTCStatus(0.0);
     current_state_ = BT::NodeStatus::SUCCESS;  // for breaking loop
   }
 }

planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp

@@ -608,8 +608,8 @@ BehaviorModuleOutput PullOverModule::plan()
     }
   }
 
-  const double distance_to_path_change = calcDistanceToPathChange();
-  updateRTCStatus(distance_to_path_change);
+  const auto distance_to_path_change = calcDistanceToPathChange();
+  updateRTCStatus(distance_to_path_change.first, distance_to_path_change.second);
 
   publishDebugData();
 
@@ -637,22 +637,25 @@ BehaviorModuleOutput PullOverModule::planWaitingApproval()
     out.path_candidate = std::make_shared<PathWithLaneId>(parallel_parking_planner_.getFullPath());
   }
 
-  const double distance_to_path_change = calcDistanceToPathChange();
-  updateRTCStatus(distance_to_path_change);
+  const auto distance_to_path_change = calcDistanceToPathChange();
+  updateRTCStatus(distance_to_path_change.first, distance_to_path_change.second);
   waitApproval();
 
   return out;
 }
 
-double PullOverModule::calcDistanceToPathChange() const
+std::pair<double, double> PullOverModule::calcDistanceToPathChange() const
 {
   const Pose parking_start_pose = getParkingStartPose();
+  const Pose parking_end_pose = modified_goal_pose_;
   const auto dist_to_parking_start_pose = calcSignedArcLength(
     status_.path.points, planner_data_->self_pose->pose, parking_start_pose.position,
     std::numeric_limits<double>::max(), M_PI_2);
-  const double distance_to_path_change =
+  const double dist_to_parking_finish_pose = calcSignedArcLength(
+    status_.path.points, planner_data_->self_pose->pose.position, parking_end_pose.position);
+  const double start_distance_to_path_change =
     dist_to_parking_start_pose ? *dist_to_parking_start_pose : std::numeric_limits<double>::max();
-  return distance_to_path_change;
+  return {start_distance_to_path_change, dist_to_parking_finish_pose};
 }
 
 void PullOverModule::setParameters(const PullOverParameters & parameters)

planning/behavior_velocity_planner/include/scene_module/scene_module_interface.hpp

@@ -354,7 +354,7 @@ class SceneModuleManagerInterfaceWithRTC : public SceneModuleManagerInterface
   void updateRTCStatus(
     const UUID & uuid, const bool safe, const double distance, const Time & stamp)
   {
-    rtc_interface_.updateCooperateStatus(uuid, safe, distance, stamp);
+    rtc_interface_.updateCooperateStatus(uuid, safe, distance, distance, stamp);
   }
 
   void removeRTCStatus(const UUID & uuid) { rtc_interface_.removeCooperateStatus(uuid); }

planning/rtc_interface/README.md

@@ -21,13 +21,14 @@ while (...) {
   const bool safe = ...
 
   // Get distance to the object corresponding to the module id
-  const double distance = ...
+  const double start_distance = ...
+  const double finish_distance = ...
 
   // Get time stamp
   const rclcpp::Time stamp = ...
 
   // Update status
-  rtc_interface.updateCooperateStatus(uuid, safe, distance, stamp);
+  rtc_interface.updateCooperateStatus(uuid, safe, start_distance, finish_distance, stamp);
 
   if (rtc_interface.isActivated(uuid)) {
     // Execute planning
@@ -89,7 +90,7 @@ Nothing
 ### updateCooperateStatus
 
 ```c++
-rtc_interface::updateCooperateStatus(const unique_identifier_msgs::msg::UUID & uuid, const bool safe, const double distance, const rclcpp::Time & stamp)
+rtc_interface::updateCooperateStatus(const unique_identifier_msgs::msg::UUID & uuid, const bool safe, const double start_distance, const double finish_distance, const rclcpp::Time & stamp)
 ```
 
 #### Description
@@ -101,7 +102,8 @@ If cooperate status corresponding to `uuid` is not registered yet, add new coope
 
 - `uuid` : UUID for requesting module
 - `safe` : Safety status of requesting module
-- `distance` : Distance to the object from ego vehicle
+- `start_distance` : Distance to the start object from ego vehicle
+- `finish_distance` : Distance to the finish object from ego vehicle
 - `stamp` : Time stamp
 
 #### Output