fix spell, remove debug

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>

Author: yuki-takagi-66

launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml

@@ -166,8 +166,8 @@
       </load_composable_node>
     </group>
 
-    <!-- First Obstacle Cruise -->
-    <group if="$(eval &quot;'$(var motion_stop_planner_type)' == 'obstacle_cruise_planner_with_pseudo_occulusion'&quot;)">
+    <!-- First Obstacle Cruise (original) -->
+    <group if="$(eval &quot;'$(var motion_stop_planner_type)' == 'obstacle_cruise_planner_with_pseudo_occlusion'&quot;)">
       <load_composable_node target="/planning/scenario_planning/lane_driving/motion_planning/motion_planning_container">
         <composable_node pkg="obstacle_cruise_planner" plugin="motion_planning::ObstacleCruisePlannerNode" name="obstacle_cruise_planner" namespace="">
           <!-- topic remap -->
@@ -191,10 +191,10 @@
       </load_composable_node>
     </group>
 
-    <!-- Second Obstacle Cruise -->
-    <group if="$(eval &quot;'$(var motion_stop_planner_type)' == 'obstacle_cruise_planner_with_pseudo_occulusion'&quot;)">
+    <!-- Second Obstacle Cruise (pseudo occlusion)-->
+    <group if="$(eval &quot;'$(var motion_stop_planner_type)' == 'obstacle_cruise_planner_with_pseudo_occlusion'&quot;)">
       <load_composable_node target="/planning/scenario_planning/lane_driving/motion_planning/motion_planning_container">
-        <composable_node pkg="obstacle_cruise_planner" plugin="motion_planning::ObstacleCruisePlannerNode" name="obstacle_cruise_planner_pseudo_occulusion" namespace="">
+        <composable_node pkg="obstacle_cruise_planner" plugin="motion_planning::ObstacleCruisePlannerNode" name="obstacle_cruise_planner_pseudo_occlusion" namespace="">
           <!-- topic remap -->
           <remap from="~/input/trajectory" to="obstacle_cruise/trajectory"/>
           <remap from="~/input/odometry" to="/localization/kinematic_state"/>
@@ -209,7 +209,7 @@
           <param from="$(var common_param_path)"/>
           <param from="$(var vehicle_param_file)"/>
           <param from="$(var nearest_search_param_path)"/>
-          <param from="$(var obstacle_cruise_planner_pseudo_occulusion_param_path)"/>
+          <param from="$(var obstacle_cruise_planner_pseudo_occlusion_param_path)"/>
           <!-- composable node config -->
           <extra_arg name="use_intra_process_comms" value="false"/>
         </composable_node>

planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml

@@ -6,7 +6,7 @@
       enable_debug_info: false
       enable_calculation_time_info: false
 
-      enable_slow_down_planning: true
+      enable_slow_down_planning: false
 
       # longitudinal info
       idling_time: 2.0 # idling time to detect front vehicle starting deceleration [s]

planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp

@@ -203,9 +203,9 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
     // consideration for the current ego pose
     bool enable_to_consider_current_pose{false};
     double time_to_convergence{1.5};
-    bool work_as_pseudo_occulusion{false};
-    double max_obj_vel_for_pseudo_occulusion{0.0};
-    std::vector<lanelet::Id> focus_intersections_for_pseudo_occulusion{};
+    bool work_as_pseudo_occlusion{false};
+    double max_obj_vel_for_pseudo_occlusion{0.0};
+    std::vector<lanelet::Id> focus_intersections_for_pseudo_occlusion{};
   };
   BehaviorDeterminationParam behavior_determination_param_;
 

planning/obstacle_cruise_planner/src/node.cpp

@@ -27,52 +27,17 @@
 #include <lanelet2_extension/utility/message_conversion.hpp>
 #include <lanelet2_extension/utility/query.hpp>
 #include <lanelet2_extension/utility/utilities.hpp>
-#include <tier4_autoware_utils/geometry/boost_polygon_utils.hpp>
 
 #include <boost/format.hpp>
-#include <boost/geometry/algorithms/correct.hpp>
 #include <boost/geometry/algorithms/intersects.hpp>
+#include <boost/geometry/algorithms/within.hpp>
 
 #include <lanelet2_core/geometry/LineString.h>
 #include <lanelet2_core/geometry/Polygon.h>
 
 #include <algorithm>
 #include <chrono>
 
-#define debug(var)                                                     \
-  do {                                                                 \
-    std::cerr << __LINE__ << ", " << __func__ << ", " << #var << ": "; \
-    view(var);                                                         \
-  } while (0)
-template <typename T>
-void view(T e)
-{
-  std::cerr << e << std::endl;
-}
-template <typename T>
-void view(const std::vector<T> & v)
-{
-  for (const auto & e : v) {
-    std::cerr << e << " ";
-  }
-  std::cerr << std::endl;
-}
-template <typename T>
-void view(const std::vector<std::vector<T>> & vv)
-{
-  for (const auto & v : vv) {
-    view(v);
-  }
-}
-#define line()                                              \
-  {                                                         \
-    std::cerr << __LINE__ << ", " << __func__ << std::endl; \
-  }
-#define line_with_file()                                                               \
-  {                                                                                    \
-    std::cerr << "(" << __FILE__ << ") " << __func__ << ": " << __LINE__ << std::endl; \
-  }
-
 namespace
 {
 VelocityLimitClearCommand createVelocityLimitClearCommandMessage(
@@ -320,13 +285,13 @@ ObstacleCruisePlannerNode::BehaviorDeterminationParam::BehaviorDeterminationPara
     "behavior_determination.consider_current_pose.enable_to_consider_current_pose");
   time_to_convergence = node.declare_parameter<double>(
     "behavior_determination.consider_current_pose.time_to_convergence");
-  work_as_pseudo_occulusion = node.declare_parameter<bool>(
-    "behavior_determination.slow_down.pseudo_occulusion.enable_function");
-  if (work_as_pseudo_occulusion) {
-    max_obj_vel_for_pseudo_occulusion = node.declare_parameter<double>(
-      "behavior_determination.slow_down.pseudo_occulusion.max_obj_vel");
-    focus_intersections_for_pseudo_occulusion = node.declare_parameter<std::vector<lanelet::Id>>(
-      "behavior_determination.slow_down.pseudo_occulusion.focus_intersections");
+  work_as_pseudo_occlusion = node.declare_parameter<bool>(
+    "behavior_determination.slow_down.pseudo_occlusion.enable_function");
+  if (work_as_pseudo_occlusion) {
+    max_obj_vel_for_pseudo_occlusion = node.declare_parameter<double>(
+      "behavior_determination.slow_down.pseudo_occlusion.max_obj_vel");
+    focus_intersections_for_pseudo_occlusion = node.declare_parameter<std::vector<lanelet::Id>>(
+      "behavior_determination.slow_down.pseudo_occlusion.focus_intersections");
   }
 }
 
@@ -389,15 +354,15 @@ void ObstacleCruisePlannerNode::BehaviorDeterminationParam::onParam(
     parameters, "behavior_determination.consider_current_pose.time_to_convergence",
     time_to_convergence);
   tier4_autoware_utils::updateParam<bool>(
-    parameters, "behavior_determination.slow_down.pseudo_occulusion.enable_function",
-    work_as_pseudo_occulusion);
-  if (work_as_pseudo_occulusion) {
+    parameters, "behavior_determination.slow_down.pseudo_occlusion.enable_function",
+    work_as_pseudo_occlusion);
+  if (work_as_pseudo_occlusion) {
     tier4_autoware_utils::updateParam<double>(
-      parameters, "behavior_determination.slow_down.pseudo_occulusion.max_obj_vel",
-      max_obj_vel_for_pseudo_occulusion);
+      parameters, "behavior_determination.slow_down.pseudo_occlusion.max_obj_vel",
+      max_obj_vel_for_pseudo_occlusion);
     tier4_autoware_utils::updateParam<std::vector<lanelet::Id>>(
-      parameters, "behavior_determination.slow_down.pseudo_occulusion.focus_intersections",
-      focus_intersections_for_pseudo_occulusion);
+      parameters, "behavior_determination.slow_down.pseudo_occlusion.focus_intersections",
+      focus_intersections_for_pseudo_occlusion);
   }
 }
 
@@ -1199,21 +1164,18 @@ std::optional<SlowDownObstacle> ObstacleCruisePlannerNode::createSlowDownObstacl
     return std::nullopt;
   }
 
-  const auto is_occulusion_object = [&]() {
+  const auto is_occlusion_object = [&]() {
     if (
       std::hypot(obstacle.twist.linear.x, obstacle.twist.linear.y) >
-      behavior_determination_param_.max_obj_vel_for_pseudo_occulusion + 1e-6) {
-      line();
+      behavior_determination_param_.max_obj_vel_for_pseudo_occlusion + 1e-6) {
       return false;
     }
 
     if (motion_utils::calcLateralOffset(traj_points, obstacle.pose.position) > 0.0) {
-      line();
       return true;
     }
 
-    for (const auto & id :
-         behavior_determination_param_.focus_intersections_for_pseudo_occulusion) {
+    for (const auto & id : behavior_determination_param_.focus_intersections_for_pseudo_occlusion) {
       if (id == 0) {
         continue;
       }
@@ -1223,15 +1185,13 @@ std::optional<SlowDownObstacle> ObstacleCruisePlannerNode::createSlowDownObstacl
       if (
         boost::geometry::within(obstacle_poly, intersection_poly) ||
         boost::geometry::intersects(obstacle_poly, intersection_poly)) {
-        line();
         return true;
       }
     }
-    line();
     return false;
   };
 
-  if (behavior_determination_param_.work_as_pseudo_occulusion && !is_occulusion_object()) {
+  if (behavior_determination_param_.work_as_pseudo_occlusion && !is_occlusion_object()) {
     return std::nullopt;
   }