feat(static_centerline_optimizer): static centerline optimizer with GUI (#6717)

takayuki5168 · web-flow · commit 6e6e6019b26d · 2024-04-02T15:15:31.000+09:00
feat: add GUI for static centerline optimizer

Signed-off-by: Takayuki Murooka &lt;takayuki5168@gmail.com&gt;
diff --git a/planning/static_centerline_optimizer/include/static_centerline_optimizer/static_centerline_optimizer_node.hpp b/planning/static_centerline_optimizer/include/static_centerline_optimizer/static_centerline_optimizer_node.hpp
@@ -22,6 +22,9 @@
 #include "static_centerline_optimizer/type_alias.hpp"
 #include "vehicle_info_util/vehicle_info_util.hpp"
 
+#include "std_msgs/msg/bool.hpp"
+#include "std_msgs/msg/int32.hpp"
+
 #include <memory>
 #include <string>
 #include <vector>
@@ -67,13 +70,28 @@ class StaticCenterlineOptimizerNode : public rclcpp::Node
   HADMapBin::ConstSharedPtr map_bin_ptr_{nullptr};
   std::shared_ptr<RouteHandler> route_handler_ptr_{nullptr};
 
+  int traj_start_index_{0};
+  int traj_end_index_{0};
+  struct MetaDataToSaveMap
+  {
+    std::vector<TrajectoryPoint> optimized_traj_points{};
+    std::vector<lanelet::Id> route_lane_ids{};
+  };
+  std::optional<MetaDataToSaveMap> meta_data_to_save_map_{std::nullopt};
+
   // publisher
   rclcpp::Publisher<HADMapBin>::SharedPtr pub_map_bin_{nullptr};
   rclcpp::Publisher<PathWithLaneId>::SharedPtr pub_raw_path_with_lane_id_{nullptr};
   rclcpp::Publisher<Path>::SharedPtr pub_raw_path_{nullptr};
   rclcpp::Publisher<MarkerArray>::SharedPtr pub_debug_unsafe_footprints_{nullptr};
+  rclcpp::Publisher<Trajectory>::SharedPtr pub_whole_optimized_centerline_{nullptr};
   rclcpp::Publisher<Trajectory>::SharedPtr pub_optimized_centerline_{nullptr};
 
+  // subscriber
+  rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr sub_traj_start_index_;
+  rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr sub_traj_end_index_;
+  rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr sub_save_map_;
+
   // service
   rclcpp::Service<LoadMap>::SharedPtr srv_load_map_;
   rclcpp::Service<PlanRoute>::SharedPtr srv_plan_route_;
diff --git a/planning/static_centerline_optimizer/launch/static_centerline_optimizer.launch.xml b/planning/static_centerline_optimizer/launch/static_centerline_optimizer.launch.xml
@@ -56,6 +56,7 @@
   <node pkg="static_centerline_optimizer" exec="main" name="static_centerline_optimizer">
     <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"/>
diff --git a/planning/static_centerline_optimizer/scripts/centerline_updater_helper.py b/planning/static_centerline_optimizer/scripts/centerline_updater_helper.py
@@ -0,0 +1,198 @@
+#!/bin/env python3
+
+# Copyright 2024 TIER IV, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+
+import sys
+import time
+
+from PyQt5 import QtCore
+from PyQt5.QtWidgets import QApplication
+from PyQt5.QtWidgets import QGridLayout
+from PyQt5.QtWidgets import QMainWindow
+from PyQt5.QtWidgets import QPushButton
+from PyQt5.QtWidgets import QSizePolicy
+from PyQt5.QtWidgets import QSlider
+from PyQt5.QtWidgets import QWidget
+from autoware_auto_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 Int32
+
+
+class CenterlineUpdaterWidget(QMainWindow):
+    def __init__(self):
+        super(self.__class__, self).__init__()
+        self.setupUI()
+
+    def setupUI(self):
+        self.setObjectName("MainWindow")
+        self.resize(480, 120)
+        self.setWindowFlags(QtCore.Qt.WindowStaysOnTopHint)
+
+        central_widget = QWidget(self)
+        central_widget.setObjectName("central_widget")
+
+        self.grid_layout = QGridLayout(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
+        )
+
+        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
+
+        self.displayed_min_traj_start_index = self.min_traj_start_index
+        self.displayed_max_traj_end_index = self.max_traj_end_index
+
+        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()
+
+        self.displayed_min_traj_start_index = current_traj_start_index
+        self.displayed_max_traj_end_index = current_traj_end_index
+
+        self.initializeSlider()
+
+    def getCurrentStartIndex(self):
+        return self.displayed_min_traj_start_index + self.traj_start_index_slider.value()
+
+    def getCurrentEndIndex(self):
+        return self.displayed_min_traj_start_index + self.traj_end_index_slider.value()
+
+
+class CenterlineUpdaterHelper(Node):
+    def __init__(self):
+        super().__init__("centerline_updater_helper")
+        # Qt
+        self.widget = CenterlineUpdaterWidget()
+        self.widget.show()
+        self.widget.save_map_button.clicked.connect(self.onSaveMapButtonPushed)
+        self.widget.traj_start_index_slider.valueChanged.connect(self.onStartIndexChanged)
+        self.widget.traj_end_index_slider.valueChanged.connect(self.onEndIndexChanged)
+
+        # 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)
+
+        transient_local_qos = QoSProfile(depth=1, durability=QoSDurabilityPolicy.TRANSIENT_LOCAL)
+        self.sub_whole_centerline = self.create_subscription(
+            Trajectory,
+            "/static_centerline_optimizer/output_whole_centerline",
+            self.onWholeCenterline,
+            transient_local_qos,
+        )
+
+        while self.widget.max_traj_end_index is None:
+            rclpy.spin_once(self, timeout_sec=0.01)
+            time.sleep(0.1)
+
+    def onWholeCenterline(self, whole_centerline):
+        self.widget.initWithEndIndex(len(whole_centerline.points) - 1)
+
+    def onSaveMapButtonPushed(self, event):
+        msg = Bool()
+        msg.data = True
+        self.pub_save_map.publish(msg)
+
+    def onStartIndexChanged(self, event):
+        msg = Int32()
+        msg.data = self.widget.getCurrentStartIndex()
+        self.pub_traj_start_index.publish(msg)
+
+    def onEndIndexChanged(self, event):
+        msg = Int32()
+        msg.data = self.widget.getCurrentEndIndex()
+        self.pub_traj_end_index.publish(msg)
+
+
+def main(args=None):
+    app = QApplication(sys.argv)
+
+    rclpy.init(args=args)
+    node = CenterlineUpdaterHelper()
+
+    while True:
+        app.processEvents()
+        rclpy.spin_once(node, timeout_sec=0.01)
+
+
+if __name__ == "__main__":
+    main()
diff --git a/planning/static_centerline_optimizer/scripts/tmp/run.sh b/planning/static_centerline_optimizer/scripts/tmp/run.sh
@@ -1,3 +1,3 @@
 #!/bin/bash
 
-ros2 launch static_centerline_optimizer static_centerline_optimizer.launch.xml run_background:=false lanelet2_input_file_path:="$HOME/AutonomousDrivingScenarios/map/sample_map/lanelet2_map.osm" start_lanelet_id:=125 end_lanelet_id:=132 vehicle_model:=lexus
+ros2 launch static_centerline_optimizer static_centerline_optimizer.launch.xml run_background:=false lanelet2_input_file_path:="$HOME/autoware_map/sample_map/lanelet2_map.osm" start_lanelet_id:=125 end_lanelet_id:=132 vehicle_model:=lexus
diff --git a/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp b/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp
@@ -26,11 +26,14 @@
 #include "static_centerline_optimizer/type_alias.hpp"
 #include "static_centerline_optimizer/utils.hpp"
 #include "tier4_autoware_utils/geometry/geometry.hpp"
+#include "tier4_autoware_utils/ros/parameter.hpp"
 
 #include <mission_planner/mission_planner_plugin.hpp>
 #include <pluginlib/class_loader.hpp>
 #include <tier4_autoware_utils/ros/marker_helper.hpp>
 
+#include "std_msgs/msg/bool.hpp"
+#include "std_msgs/msg/int32.hpp"
 #include <tier4_map_msgs/msg/map_projector_info.hpp>
 
 #include <boost/geometry/algorithms/correct.hpp>
@@ -41,6 +44,7 @@
 #include <lanelet2_io/Io.h>
 #include <lanelet2_projection/UTM.h>
 
+#include <chrono>
 #include <fstream>
 #include <limits>
 #include <memory>
@@ -202,6 +206,14 @@ std::vector<lanelet::Id> check_lanelet_connection(
 
   return unconnected_lane_ids;
 }
+
+std_msgs::msg::Header createHeader(const rclcpp::Time & now)
+{
+  std_msgs::msg::Header header;
+  header.frame_id = "map";
+  header.stamp = now;
+  return header;
+}
 }  // namespace
 
 StaticCenterlineOptimizerNode::StaticCenterlineOptimizerNode(
@@ -213,13 +225,62 @@ StaticCenterlineOptimizerNode::StaticCenterlineOptimizerNode(
   pub_raw_path_with_lane_id_ =
     create_publisher<PathWithLaneId>("input_centerline", create_transient_local_qos());
   pub_raw_path_ = create_publisher<Path>("debug/raw_centerline", create_transient_local_qos());
+  pub_whole_optimized_centerline_ =
+    create_publisher<Trajectory>("output_whole_centerline", create_transient_local_qos());
   pub_optimized_centerline_ =
     create_publisher<Trajectory>("output_centerline", create_transient_local_qos());
 
   // debug publishers
   pub_debug_unsafe_footprints_ =
     create_publisher<MarkerArray>("debug/unsafe_footprints", create_transient_local_qos());
 
+  // subscribers
+  sub_traj_start_index_ = create_subscription<std_msgs::msg::Int32>(
+    "/centerline_updater_helper/traj_start_index", rclcpp::QoS{1},
+    [this](const std_msgs::msg::Int32 & msg) {
+      if (!meta_data_to_save_map_ || traj_end_index_ + 1 < msg.data) {
+        return;
+      }
+      traj_start_index_ = msg.data;
+
+      const auto & d = meta_data_to_save_map_;
+      const auto selected_optimized_traj_points = std::vector<TrajectoryPoint>(
+        d->optimized_traj_points.begin() + traj_start_index_,
+        d->optimized_traj_points.begin() + traj_end_index_ + 1);
+
+      pub_optimized_centerline_->publish(motion_utils::convertToTrajectory(
+        selected_optimized_traj_points, createHeader(this->now())));
+    });
+  sub_traj_end_index_ = create_subscription<std_msgs::msg::Int32>(
+    "/centerline_updater_helper/traj_end_index", rclcpp::QoS{1},
+    [this](const std_msgs::msg::Int32 & msg) {
+      if (!meta_data_to_save_map_ || msg.data + 1 < traj_start_index_) {
+        return;
+      }
+      traj_end_index_ = msg.data;
+
+      const auto & d = meta_data_to_save_map_;
+      const auto selected_optimized_traj_points = std::vector<TrajectoryPoint>(
+        d->optimized_traj_points.begin() + traj_start_index_,
+        d->optimized_traj_points.begin() + traj_end_index_ + 1);
+
+      pub_optimized_centerline_->publish(motion_utils::convertToTrajectory(
+        selected_optimized_traj_points, createHeader(this->now())));
+    });
+  sub_save_map_ = create_subscription<std_msgs::msg::Bool>(
+    "/centerline_updater_helper/save_map", rclcpp::QoS{1}, [this](const std_msgs::msg::Bool & msg) {
+      const auto lanelet2_output_file_path =
+        tier4_autoware_utils::getOrDeclareParameter<std::string>(
+          *this, "lanelet2_output_file_path");
+      if (!meta_data_to_save_map_ || msg.data) {
+        const auto & d = meta_data_to_save_map_;
+        const auto selected_optimized_traj_points = std::vector<TrajectoryPoint>(
+          d->optimized_traj_points.begin() + traj_start_index_,
+          d->optimized_traj_points.begin() + traj_end_index_ + 1);
+        save_map(lanelet2_output_file_path, d->route_lane_ids, selected_optimized_traj_points);
+      }
+    });
+
   // services
   callback_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
   srv_load_map_ = create_service<LoadMap>(
@@ -258,7 +319,11 @@ void StaticCenterlineOptimizerNode::run()
   load_map(lanelet2_input_file_path);
   const auto route_lane_ids = plan_route(start_lanelet_id, end_lanelet_id);
   const auto optimized_traj_points = plan_path(route_lane_ids);
+  traj_start_index_ = 0;
+  traj_end_index_ = optimized_traj_points.size() - 1;
   save_map(lanelet2_output_file_path, route_lane_ids, optimized_traj_points);
+
+  meta_data_to_save_map_ = MetaDataToSaveMap{optimized_traj_points, route_lane_ids};
 }
 
 void StaticCenterlineOptimizerNode::load_map(const std::string & lanelet2_input_file_path)
@@ -450,6 +515,8 @@ std::vector<TrajectoryPoint> StaticCenterlineOptimizerNode::plan_path(
 
   // smooth trajectory and road collision avoidance
   const auto optimized_traj_points = optimize_trajectory(raw_path);
+  pub_whole_optimized_centerline_->publish(
+    motion_utils::convertToTrajectory(optimized_traj_points, raw_path.header));
   pub_optimized_centerline_->publish(
     motion_utils::convertToTrajectory(optimized_traj_points, raw_path.header));
   RCLCPP_INFO(

Original file line number	Diff line number	Diff line change
`@@ -1,3 +1,3 @@`
`1`	`1`	`#!/bin/bash`
`2`	`2`
`3`		`-ros2 launch static_centerline_optimizer static_centerline_optimizer.launch.xml run_background:=false lanelet2_input_file_path:="$HOME/AutonomousDrivingScenarios/map/sample_map/lanelet2_map.osm" start_lanelet_id:=125 end_lanelet_id:=132 vehicle_model:=lexus`
	`3`	`+ros2 launch static_centerline_optimizer static_centerline_optimizer.launch.xml run_background:=false lanelet2_input_file_path:="$HOME/autoware_map/sample_map/lanelet2_map.osm" start_lanelet_id:=125 end_lanelet_id:=132 vehicle_model:=lexus`