feat(mission_planner): add minimum checking length (autowarefoundatio…

…n#3816) * refactor(mission_planner): clean reroute code Signed-off-by: yutaka <purewater0901@gmail.com> * feat(mission_planner): add minimum reroute length Signed-off-by: yutaka <purewater0901@gmail.com> * update Signed-off-by: yutaka <purewater0901@gmail.com> --------- Signed-off-by: yutaka <purewater0901@gmail.com>
tier4 · May 25, 2023 · 41f5bc1 · 41f5bc1
1 parent 7aaaef2
commit 41f5bc1
Show file tree

Hide file tree

Showing 4 changed files with 7 additions and 1 deletion.
diff --git a/planning/mission_planner/README.md b/planning/mission_planner/README.md
@@ -24,6 +24,7 @@ In current Autoware.universe, only Lanelet2 map format is supported.
 | `enable_correct_goal_pose` | bool   | Enabling correction of goal pose according to the closest lanelet orientation                                    |
 | `enable_correct_goal_pose` | bool   | Enabling correction of goal pose according to the closest lanelet orientation                                    |
 | `reroute_time_threshold`   | double | If the time to the rerouting point at the current velocity is greater than this threshold, rerouting is possible |
+| `minimum_reroute_length`   | double | Minimum Length for publishing a new route                                                                        |
 
 ### Services
 

diff --git a/planning/mission_planner/config/mission_planner.param.yaml b/planning/mission_planner/config/mission_planner.param.yaml
@@ -7,3 +7,4 @@
     goal_angle_threshold_deg: 45.0
     enable_correct_goal_pose: false
     reroute_time_threshold: 10.0
+    minimum_reroute_length: 30.0
diff --git a/planning/mission_planner/src/mission_planner/mission_planner.cpp b/planning/mission_planner/src/mission_planner/mission_planner.cpp
@@ -80,6 +80,7 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options)
 {
   map_frame_ = declare_parameter<std::string>("map_frame");
   reroute_time_threshold_ = declare_parameter<double>("reroute_time_threshold");
+  minimum_reroute_length_ = declare_parameter<double>("minimum_reroute_length");
 
   planner_ = plugin_loader_.createSharedInstance("mission_planner::lanelet2::DefaultPlanner");
   planner_->initialize(this);
@@ -492,7 +493,9 @@ bool MissionPlanner::checkRerouteSafety(
 
   // check safety
   const auto current_velocity = odometry_->twist.twist.linear.x;
-  if (accumulated_length > current_velocity * reroute_time_threshold_) {
+  const double safety_length =
+    std::max(current_velocity * reroute_time_threshold_, minimum_reroute_length_);
+  if (accumulated_length > safety_length) {
     return true;
   }
 

diff --git a/planning/mission_planner/src/mission_planner/mission_planner.hpp b/planning/mission_planner/src/mission_planner/mission_planner.hpp
@@ -121,6 +121,7 @@ class MissionPlanner : public rclcpp::Node
     const SetRoutePoints::Service::Response::SharedPtr res);
 
   double reroute_time_threshold_{10.0};
+  double minimum_reroute_length_{30.0};
   bool checkRerouteSafety(const LaneletRoute & original_route, const LaneletRoute & target_route);
 
   std::shared_ptr<LaneletRoute> original_route_{nullptr};