Merge pull request #1364 from tier4/fix/static-centerline-generator

fix(static_centerline_generator): cherry-pick several changes

Author: takayuki5168

planning/autoware_static_centerline_generator/CMakeLists.txt

@@ -60,5 +60,7 @@ endif()
 
 install(PROGRAMS
   scripts/app.py
+  scripts/centerline_updater_helper.py
+  scripts/show_lanelet2_map_diff.py
   DESTINATION lib/${PROJECT_NAME}
 )

planning/autoware_static_centerline_generator/config/static_centerline_generator.param.yaml

@@ -1,5 +1,9 @@
 /**:
   ros__parameters:
-    marker_color: ["FF0000", "00FF00", "0000FF"]
+    marker_color: ["FF0000", "FF5A00", "FFFF00"]
     marker_color_dist_thresh : [0.1, 0.2, 0.3]
     output_trajectory_interval: 1.0
+
+    validation:
+      dist_threshold_to_road_border: 0.0
+      max_steer_angle_margin: 0.0 # [rad] NOTE: Positive value makes max steer angle threshold to decrease.

planning/autoware_static_centerline_generator/launch/static_centerline_generator.launch.xml

@@ -3,17 +3,19 @@
   <arg name="vehicle_model"/>
 
   <!-- flag -->
-  <arg name="run_background" default="false"/>
+  <arg name="mode" default="AUTO" description="select from AUTO, GUI, and VMB"/>
   <arg name="rviz" default="true"/>
   <arg name="centerline_source" default="optimization_trajectory_base" description="select from optimization_trajectory_base and bag_ego_trajectory_base"/>
-  <arg name="bag_filename" default="bag.db3"/>
 
-  <!-- mandatory arguments when run_background is true -->
+  <!-- mandatory arguments when mode is AUTO -->
   <arg name="lanelet2_input_file_path" default=""/>
-  <arg name="lanelet2_output_file_path" default="/tmp/static_centerline_generator/lanelet2_map.osm"/>
+  <arg name="lanelet2_output_file_path" default="/tmp/autoware_static_centerline_generator/lanelet2_map.osm"/>
   <arg name="start_lanelet_id" default=""/>
   <arg name="end_lanelet_id" default=""/>
 
+  <!-- mandatory arguments when mode is GUI -->
+  <arg name="bag_filename" default="bag.db3"/>
+
   <!-- topic -->
   <arg name="lanelet2_map_topic" default="/map/vector_map"/>
   <arg name="lanelet2_map_marker_topic" default="/map/vector_map_marker"/>
@@ -28,7 +30,7 @@
     default="$(find-pkg-share autoware_launch)/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml"
   />
   <arg name="path_smoother_param" default="$(find-pkg-share autoware_launch)/config/planning/scenario_planning/lane_driving/motion_planning/path_smoother/elastic_band_smoother.param.yaml"/>
-  <arg name="path_optimizer_param" default="$(find-pkg-share autoware_launch)/config/planning/scenario_planning/lane_driving/motion_planning/path_optimizer/path_optimizer.param.yaml"/>
+  <arg name="path_optimizer_param" default="$(find-pkg-share autoware_launch)/config/planning/scenario_planning/lane_driving/motion_planning/autoware_path_optimizer/path_optimizer.param.yaml"/>
   <arg name="mission_planner_param" default="$(find-pkg-share autoware_launch)/config/planning/mission_planning/mission_planner/mission_planner.param.yaml"/>
 
   <!-- Global parameters (for PathFootprint in tier4_planning_rviz_plugin) -->
@@ -55,12 +57,8 @@
   <node pkg="autoware_static_centerline_generator" exec="main" name="static_centerline_generator">
     <remap from="lanelet2_map_topic" to="$(var lanelet2_map_topic)"/>
     <remap from="input_centerline" to="~/input_centerline"/>
-    <remap from="output_whole_centerline" to="~/output_whole_centerline"/>
-    <remap from="output_centerline" to="~/output_centerline"/>
-    <remap from="debug/raw_centerline" to="~/debug/raw_centerline"/>
-    <remap from="debug/unsafe_footprints" to="~/debug/unsafe_footprints"/>
 
-    <param name="run_background" value="$(var run_background)"/>
+    <param name="mode" value="$(var mode)"/>
     <param name="lanelet2_input_file_path" value="$(var lanelet2_input_file_path)"/>
     <param name="lanelet2_output_file_path" value="$(var lanelet2_output_file_path)"/>
     <param name="start_lanelet_id" value="$(var start_lanelet_id)"/>
@@ -75,12 +73,19 @@
     <param from="$(var path_smoother_param)"/>
     <param from="$(var path_optimizer_param)"/>
     <param from="$(var mission_planner_param)"/>
+    <param name="check_footprint_inside_lanes" value="false"/>
+    <!-- override the mission_planner's parameter -->
     <!-- node param -->
     <param from="$(find-pkg-share autoware_static_centerline_generator)/config/static_centerline_generator.param.yaml"/>
     <param name="centerline_source" value="$(var centerline_source)"/>
     <param name="bag_filename" value="$(var bag_filename)"/>
   </node>
 
+  <!-- GUI to select the range of centerline -->
+  <group if="$(eval &quot;'$(var mode)'=='GUI'&quot;)">
+    <node pkg="autoware_static_centerline_generator" exec="centerline_updater_helper.py" name="centerline_updater_helper"/>
+  </group>
+
   <!-- rviz -->
-  <node pkg="rviz2" exec="rviz2" name="rviz2" output="screen" args="-d $(find-pkg-share autoware_static_centerline_generator)/rviz/static_centerline_generator.param" if="$(var rviz)"/>
+  <node pkg="rviz2" exec="rviz2" name="rviz2" output="screen" args="-d $(find-pkg-share autoware_static_centerline_generator)/rviz/static_centerline_generator.rviz" if="$(var rviz)"/>
 </launch>

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
@@ -179,11 +179,11 @@ Visualization Manager:
         Filter size: 10
         History Policy: Keep Last
         Reliability Policy: Reliable
-        Value: /static_centerline_generator/output_centerline
+        Value: /static_centerline_generator/output/centerline
       Value: true
       View Footprint:
-        Alpha: 1
-        Color: 255; 0; 0
+        Alpha: 0.5
+        Color: 0; 255; 0
         Offset from BaseLink: 0
         Rear Overhang: 1.0299999713897705
         Value: true
@@ -268,9 +268,33 @@ Visualization Manager:
             Durability Policy: Transient Local
             History Policy: Keep Last
             Reliability Policy: Reliable
-            Value: /static_centerline_generator/debug/unsafe_footprints
+            Value: /static_centerline_generator/output/validation_results
           Value: true
-      Enabled: false
+        - Class: rviz_default_plugins/MarkerArray
+          Enabled: true
+          Name: Debug Markers
+          Namespaces:
+            curvature: false
+          Topic:
+            Depth: 5
+            Durability Policy: Transient Local
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /static_centerline_generator/debug/markers
+          Value: true
+        - Class: rviz_default_plugins/MarkerArray
+          Enabled: false
+          Name: MarkerArray
+          Namespaces:
+            {}
+          Topic:
+            Depth: 5
+            Durability Policy: Transient Local
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /static_centerline_generator/debug/ego_footprint_bounds
+          Value: true
+      Enabled: true
       Name: debug
   Enabled: true
   Global Options:

planning/autoware_static_centerline_generator/scripts/centerline_updater_helper.py

@@ -20,18 +20,20 @@
 
 from PyQt5 import QtCore
 from PyQt5.QtWidgets import QApplication
-from PyQt5.QtWidgets import QGridLayout
+from PyQt5.QtWidgets import QGroupBox
 from PyQt5.QtWidgets import QMainWindow
 from PyQt5.QtWidgets import QPushButton
 from PyQt5.QtWidgets import QSizePolicy
 from PyQt5.QtWidgets import QSlider
+from PyQt5.QtWidgets import QVBoxLayout
 from PyQt5.QtWidgets import QWidget
 from autoware_planning_msgs.msg import Trajectory
 import rclpy
 from rclpy.node import Node
 from rclpy.qos import QoSDurabilityPolicy
 from rclpy.qos import QoSProfile
-from std_msgs.msg import Bool
+from std_msgs.msg import Empty
+from std_msgs.msg import Float32
 from std_msgs.msg import Int32
 
 
@@ -46,95 +48,70 @@ def setupUI(self):
         self.setWindowFlags(QtCore.Qt.WindowStaysOnTopHint)
 
         central_widget = QWidget(self)
-        central_widget.setObjectName("central_widget")
-
-        self.grid_layout = QGridLayout(central_widget)
+        self.grid_layout = QVBoxLayout(central_widget)
         self.grid_layout.setContentsMargins(10, 10, 10, 10)
-        self.grid_layout.setObjectName("grid_layout")
-
-        # button to update the trajectory's start and end index
-        self.update_button = QPushButton("update slider")
-        self.update_button.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding)
-        self.update_button.clicked.connect(self.onUpdateButton)
-
-        # button to reset the trajectory's start and end index
-        self.reset_button = QPushButton("reset")
-        self.reset_button.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding)
-        self.reset_button.clicked.connect(self.onResetButton)
-
-        # button to save map
-        self.save_map_button = QPushButton("save map")
-        self.save_map_button.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding)
 
         # slide of the trajectory's start and end index
         self.traj_start_index_slider = QSlider(QtCore.Qt.Horizontal)
         self.traj_end_index_slider = QSlider(QtCore.Qt.Horizontal)
         self.min_traj_start_index = 0
         self.max_traj_end_index = None
 
-        # set layout
-        self.grid_layout.addWidget(self.update_button, 1, 0, 1, -1)
-        self.grid_layout.addWidget(self.reset_button, 2, 0, 1, -1)
-        self.grid_layout.addWidget(self.save_map_button, 3, 0, 1, -1)
-        self.grid_layout.addWidget(self.traj_start_index_slider, 4, 0, 1, -1)
-        self.grid_layout.addWidget(self.traj_end_index_slider, 5, 0, 1, -1)
-        self.setCentralWidget(central_widget)
-
-    def initWithEndIndex(self, max_traj_end_index):
-        self.max_traj_end_index = max_traj_end_index
-
-        # initialize slider
-        self.displayed_min_traj_start_index = self.min_traj_start_index
-        self.displayed_max_traj_end_index = self.max_traj_end_index
-        self.initializeSlider()
-
-    def initializeSlider(self, move_value_to_end=True):
-        self.traj_start_index_slider.setMinimum(0)
-        self.traj_end_index_slider.setMinimum(0)
-        self.traj_start_index_slider.setMaximum(
-            self.displayed_max_traj_end_index - self.displayed_min_traj_start_index
-        )
-        self.traj_end_index_slider.setMaximum(
-            self.displayed_max_traj_end_index - self.displayed_min_traj_start_index
+        # Layout: Range of Centerline
+        centerline_vertical_box = QVBoxLayout(self)
+        centerline_vertical_box.addWidget(self.traj_start_index_slider)
+        centerline_vertical_box.addWidget(self.traj_end_index_slider)
+        centerline_group = QGroupBox("Centerline")
+        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)
+        road_boundary_group.setLayout(road_boundary_vertical_box)
+        self.grid_layout.addWidget(road_boundary_group)
+
+        # 2.1. Slider
+        self.road_boundary_lateral_margin_slider = QSlider(QtCore.Qt.Horizontal)
+        road_boundary_vertical_box.addWidget(self.road_boundary_lateral_margin_slider)
+        self.road_boundary_lateral_margin_ratio = 10
+        self.road_boundary_lateral_margin_slider.setMinimum(0)
+        self.road_boundary_lateral_margin_slider.setMaximum(
+            5 * self.road_boundary_lateral_margin_ratio
         )
+        road_boundary_vertical_box.addWidget(QPushButton("reset"))
+        """
 
-        if move_value_to_end:
-            self.traj_start_index_slider.setValue(0)
-            self.traj_end_index_slider.setValue(self.traj_end_index_slider.maximum())
-
-    def onResetButton(self, event):
-        current_traj_start_index = self.displayed_min_traj_start_index
-        current_traj_end_index = self.displayed_max_traj_end_index
+        # 3. General
+        general_group = QGroupBox("General")
+        general_vertical_box = QVBoxLayout(self)
+        general_group.setLayout(general_vertical_box)
+        self.grid_layout.addWidget(general_group)
 
-        self.displayed_min_traj_start_index = self.min_traj_start_index
-        self.displayed_max_traj_end_index = self.max_traj_end_index
+        # 3.1. Validate Centerline
+        self.validate_button = QPushButton("validate")
+        self.validate_button.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding)
+        general_vertical_box.addWidget(self.validate_button)
 
-        self.initializeSlider(False)
-        self.traj_start_index_slider.setValue(current_traj_start_index)
-        if (
-            current_traj_start_index == self.min_traj_start_index
-            and current_traj_end_index == self.max_traj_end_index
-        ):
-            self.traj_end_index_slider.setValue(self.displayed_max_traj_end_index)
-        else:
-            self.traj_end_index_slider.setValue(
-                current_traj_start_index + self.traj_end_index_slider.value()
-            )
-
-    def onUpdateButton(self, event):
-        current_traj_start_index = self.getCurrentStartIndex()
-        current_traj_end_index = self.getCurrentEndIndex()
+        # 3.2. Save Map
+        self.save_map_button = QPushButton("save map")
+        self.save_map_button.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding)
+        general_vertical_box.addWidget(self.save_map_button)
 
-        self.displayed_min_traj_start_index = current_traj_start_index
-        self.displayed_max_traj_end_index = current_traj_end_index
+        self.setCentralWidget(central_widget)
 
-        self.initializeSlider()
+    def initWithEndIndex(self, max_traj_end_index):
+        self.max_traj_end_index = max_traj_end_index
 
-    def getCurrentStartIndex(self):
-        return self.displayed_min_traj_start_index + self.traj_start_index_slider.value()
+        # initialize sliders
+        self.traj_start_index_slider.setMinimum(self.min_traj_start_index)
+        self.traj_start_index_slider.setMaximum(self.max_traj_end_index)
+        self.traj_start_index_slider.setValue(self.min_traj_start_index)
 
-    def getCurrentEndIndex(self):
-        return self.displayed_min_traj_start_index + self.traj_end_index_slider.value()
+        self.traj_end_index_slider.setMinimum(self.min_traj_start_index)
+        self.traj_end_index_slider.setMaximum(self.max_traj_end_index)
+        self.traj_end_index_slider.setValue(self.max_traj_end_index)
 
 
 class CenterlineUpdaterHelper(Node):
@@ -144,18 +121,32 @@ def __init__(self):
         self.widget = CenterlineUpdaterWidget()
         self.widget.show()
         self.widget.save_map_button.clicked.connect(self.onSaveMapButtonPushed)
+        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(Bool, "~/save_map", 1)
-        self.pub_traj_start_index = self.create_publisher(Int32, "~/traj_start_index", 1)
-        self.pub_traj_end_index = self.create_publisher(Int32, "~/traj_end_index", 1)
+        self.pub_save_map = self.create_publisher(Empty, "/static_centerline_generator/save_map", 1)
+        self.pub_validate = self.create_publisher(Empty, "/static_centerline_generator/validate", 1)
+        self.pub_traj_start_index = self.create_publisher(
+            Int32, "/static_centerline_generator/traj_start_index", 1
+        )
+        self.pub_traj_end_index = self.create_publisher(
+            Int32, "/static_centerline_generator/traj_end_index", 1
+        )
+        self.pub_road_boundary_lateral_margin = self.create_publisher(
+            Float32, "/static_centerline_generator/road_boundary_lateral_margin", 1
+        )
 
         transient_local_qos = QoSProfile(depth=1, durability=QoSDurabilityPolicy.TRANSIENT_LOCAL)
         self.sub_whole_centerline = self.create_subscription(
             Trajectory,
-            "/static_centerline_generator/output_whole_centerline",
+            "/static_centerline_generator/output/whole_centerline",
             self.onWholeCenterline,
             transient_local_qos,
         )
@@ -168,20 +159,38 @@ def onWholeCenterline(self, whole_centerline):
         self.widget.initWithEndIndex(len(whole_centerline.points) - 1)
 
     def onSaveMapButtonPushed(self, event):
-        msg = Bool()
-        msg.data = True
+        msg = Empty()
         self.pub_save_map.publish(msg)
 
+        # NOTE: After saving the map, the generated centerline is written
+        # in original_map_ptr_ in static_centerline_generator_node.
+        # When saving the map again, another centerline is written without
+        # removing the previous centerline.
+        # Therefore, saving the map can be called only once.
+        self.widget.save_map_button.setEnabled(False)
+
+    def onValidateButtonPushed(self, event):
+        msg = Empty()
+        self.pub_validate.publish(msg)
+
     def onStartIndexChanged(self, event):
         msg = Int32()
-        msg.data = self.widget.getCurrentStartIndex()
+        msg.data = self.widget.traj_start_index_slider.value()
         self.pub_traj_start_index.publish(msg)
 
     def onEndIndexChanged(self, event):
         msg = Int32()
-        msg.data = self.widget.getCurrentEndIndex()
+        msg.data = self.widget.traj_end_index_slider.value()
         self.pub_traj_end_index.publish(msg)
 
+    def onRoadBoundaryLateralMargin(self, event):
+        msg = Float32()
+        msg.data = (
+            self.widget.road_boundary_lateral_margin_slider.value()
+            / self.widget.road_boundary_lateral_margin_ratio
+        )
+        self.pub_road_boundary_lateral_margin.publish(msg)
+
 
 def main(args=None):
     app = QApplication(sys.argv)