refactor(static_centerline_optimizer): clean up the code (autowarefou…

…ndation#7756) Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
mitukou1109 · Jul 2, 2024 · 9bdd197 · 9bdd197
1 parent de2967d
commit 9bdd197
Show file tree

Hide file tree

Showing 4 changed files with 12 additions and 19 deletions.
diff --git a/planning/autoware_static_centerline_generator/rviz/static_centerline_generator.rviz b/planning/autoware_static_centerline_generator/rviz/static_centerline_generator.rviz
@@ -122,7 +122,7 @@ Visualization Manager:
       Name: Map
     - Class: rviz_plugins/PathWithLaneId
       Color Border Vel Max: 3
-      Enabled: true
+      Enabled: false
       Name: Raw Centerline
       Topic:
         Depth: 5

diff --git a/planning/autoware_static_centerline_generator/scripts/centerline_updater_helper.py b/planning/autoware_static_centerline_generator/scripts/centerline_updater_helper.py
@@ -65,6 +65,7 @@ def setupUI(self):
         centerline_group.setLayout(centerline_vertical_box)
         self.grid_layout.addWidget(centerline_group)
 
+        """
         # 2. Road Boundary
         road_boundary_group = QGroupBox("Road Boundary")
         road_boundary_vertical_box = QVBoxLayout(self)
@@ -79,8 +80,8 @@ def setupUI(self):
         self.road_boundary_lateral_margin_slider.setMaximum(
             5 * self.road_boundary_lateral_margin_ratio
         )
-
         road_boundary_vertical_box.addWidget(QPushButton("reset"))
+        """
 
         # 3. General
         general_group = QGroupBox("General")
@@ -123,9 +124,11 @@ def __init__(self):
         self.widget.validate_button.clicked.connect(self.onValidateButtonPushed)
         self.widget.traj_start_index_slider.valueChanged.connect(self.onStartIndexChanged)
         self.widget.traj_end_index_slider.valueChanged.connect(self.onEndIndexChanged)
+        """
         self.widget.road_boundary_lateral_margin_slider.valueChanged.connect(
             self.onRoadBoundaryLateralMargin
         )
+        """
 
         # ROS
         self.pub_save_map = self.create_publisher(Empty, "/static_centerline_generator/save_map", 1)

diff --git a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp
@@ -251,11 +251,6 @@ StaticCenterlineGeneratorNode::StaticCenterlineGeneratorNode(
       }
       save_map();
     });
-  sub_traj_resample_interval_ = create_subscription<std_msgs::msg::Float32>(
-    "/static_centerline_generator/traj_resample_interval", rclcpp::QoS{1},
-    [this]([[maybe_unused]] const std_msgs::msg::Float32 & msg) {
-      // TODO(murooka)
-    });
   sub_validate_ = create_subscription<std_msgs::msg::Empty>(
     "/static_centerline_generator/validate", rclcpp::QoS{1},
     [this]([[maybe_unused]] const std_msgs::msg::Empty & msg) { validate(); });
@@ -325,14 +320,6 @@ void StaticCenterlineGeneratorNode::generate_centerline()
   visualize_selected_centerline();
 }
 
-void StaticCenterlineGeneratorNode::validate()
-{
-  const auto selected_centerline = centerline_handler_.get_selected_centerline();
-  const auto road_bounds = update_road_boundary(selected_centerline);
-
-  evaluate();
-}
-
 CenterlineWithRoute StaticCenterlineGeneratorNode::generate_whole_centerline_with_route()
 {
   if (!route_handler_ptr_) {
@@ -572,7 +559,7 @@ void StaticCenterlineGeneratorNode::on_plan_path(
     CenterlineHandler(CenterlineWithRoute{optimized_traj_points, route_lane_ids});
 
   // publish unsafe_footprints
-  evaluate();
+  validate();
 
   // create output data
   auto target_traj_point = optimized_traj_points.cbegin();
@@ -714,8 +701,11 @@ RoadBounds StaticCenterlineGeneratorNode::update_road_boundary(
   return RoadBounds{ego_left_bound, ego_right_bound};
 }
 
-void StaticCenterlineGeneratorNode::evaluate()
+void StaticCenterlineGeneratorNode::validate()
 {
+  // const auto selected_centerline = centerline_handler_.get_selected_centerline();
+  // const auto road_bounds = update_road_boundary(selected_centerline);
+
   std::cerr << std::endl
             << "############################################## Validation Results "
                "##############################################"
@@ -730,6 +720,7 @@ void StaticCenterlineGeneratorNode::evaluate()
   const double max_steer_angle_margin =
     getRosParameter<double>("validation.max_steer_angle_margin");
 
+  // calculate color for distance to road border
   const auto dist_thresh_vec = getRosParameter<std::vector<double>>("marker_color_dist_thresh");
   const auto marker_color_vec = getRosParameter<std::vector<std::string>>("marker_color");
   const auto get_marker_color = [&](const double dist) -> boost::optional<std::array<double, 3>> {
@@ -806,7 +797,7 @@ void StaticCenterlineGeneratorNode::evaluate()
     }
   }
 
-  // publish left boundary
+  // publish road boundaries
   const auto left_bound = convertToGeometryPoints(lanelet_left_bound);
   const auto right_bound = convertToGeometryPoints(lanelet_right_bound);
 

diff --git a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.hpp b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.hpp
@@ -133,7 +133,6 @@ class StaticCenterlineGeneratorNode : public rclcpp::Node
 
   void visualize_selected_centerline();
   RoadBounds update_road_boundary(const std::vector<TrajectoryPoint> & centerline);
-  void evaluate();
 
   // parameter
   template <typename T>