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 index fa7043de42f5c..f25a6771aa0cb 100644 --- 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 #include #include @@ -67,13 +70,28 @@ class StaticCenterlineOptimizerNode : public rclcpp::Node HADMapBin::ConstSharedPtr map_bin_ptr_{nullptr}; std::shared_ptr route_handler_ptr_{nullptr}; + int traj_start_index_{0}; + int traj_end_index_{0}; + struct MetaDataToSaveMap + { + std::vector optimized_traj_points{}; + std::vector route_lane_ids{}; + }; + std::optional meta_data_to_save_map_{std::nullopt}; + // publisher rclcpp::Publisher::SharedPtr pub_map_bin_{nullptr}; rclcpp::Publisher::SharedPtr pub_raw_path_with_lane_id_{nullptr}; rclcpp::Publisher::SharedPtr pub_raw_path_{nullptr}; rclcpp::Publisher::SharedPtr pub_debug_unsafe_footprints_{nullptr}; + rclcpp::Publisher::SharedPtr pub_whole_optimized_centerline_{nullptr}; rclcpp::Publisher::SharedPtr pub_optimized_centerline_{nullptr}; + // subscriber + rclcpp::Subscription::SharedPtr sub_traj_start_index_; + rclcpp::Subscription::SharedPtr sub_traj_end_index_; + rclcpp::Subscription::SharedPtr sub_save_map_; + // service rclcpp::Service::SharedPtr srv_load_map_; rclcpp::Service::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 index 37a9abc47bfeb..2e163e2eb93eb 100644 --- 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 @@ + diff --git a/planning/static_centerline_optimizer/scripts/centerline_updater_helper.py b/planning/static_centerline_optimizer/scripts/centerline_updater_helper.py new file mode 100755 index 0000000000000..00a646406f14f --- /dev/null +++ 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 index 07857a9d923a1..9e475b9d8759b 100755 --- 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 index d98341ecb2e23..844352f577432 100644 --- 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 #include #include +#include "std_msgs/msg/bool.hpp" +#include "std_msgs/msg/int32.hpp" #include #include @@ -41,6 +44,7 @@ #include #include +#include #include #include #include @@ -202,6 +206,14 @@ std::vector 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,6 +225,8 @@ StaticCenterlineOptimizerNode::StaticCenterlineOptimizerNode( pub_raw_path_with_lane_id_ = create_publisher("input_centerline", create_transient_local_qos()); pub_raw_path_ = create_publisher("debug/raw_centerline", create_transient_local_qos()); + pub_whole_optimized_centerline_ = + create_publisher("output_whole_centerline", create_transient_local_qos()); pub_optimized_centerline_ = create_publisher("output_centerline", create_transient_local_qos()); @@ -220,6 +234,53 @@ StaticCenterlineOptimizerNode::StaticCenterlineOptimizerNode( pub_debug_unsafe_footprints_ = create_publisher("debug/unsafe_footprints", create_transient_local_qos()); + // subscribers + sub_traj_start_index_ = create_subscription( + "/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( + 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( + "/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( + 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( + "/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( + *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( + 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( @@ -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 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(