diff --git a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker_node.hpp b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker_node.hpp index e6421b2af84bb..876797a58df25 100644 --- a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker_node.hpp +++ b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker_node.hpp @@ -100,7 +100,9 @@ class LaneDepartureCheckerNode : public rclcpp::Node // Publisher autoware::universe_utils::DebugPublisher debug_publisher_{this, "~/debug"}; - autoware::universe_utils::ProcessingTimePublisher processing_time_publisher_{this}; + autoware::universe_utils::ProcessingTimePublisher processing_diag_publisher_{ + this, "~/debug/processing_time_ms_diag"}; + rclcpp::Publisher::SharedPtr processing_time_publisher_; // Timer rclcpp::TimerBase::SharedPtr timer_; diff --git a/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp index 427cf0898470e..dde37b03ffecc 100644 --- a/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp +++ b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp @@ -170,6 +170,8 @@ LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & o lane_departure_checker_->setParam(param_, vehicle_info); // Publisher + processing_time_publisher_ = + this->create_publisher("~/debug/processing_time_ms", 1); // Nothing // Diagnostic Updater @@ -347,7 +349,11 @@ void LaneDepartureCheckerNode::onTimer() } processing_time_map["Total"] = stop_watch.toc("Total"); - processing_time_publisher_.publish(processing_time_map); + processing_diag_publisher_.publish(processing_time_map); + tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + processing_time_msg.stamp = get_clock()->now(); + processing_time_msg.data = processing_time_map["Total"]; + processing_time_publisher_->publish(processing_time_msg); } rcl_interfaces::msg::SetParametersResult LaneDepartureCheckerNode::onParameter( diff --git a/launch/tier4_perception_launch/package.xml b/launch/tier4_perception_launch/package.xml index 6ec706a4aad32..57d4b209efeef 100644 --- a/launch/tier4_perception_launch/package.xml +++ b/launch/tier4_perception_launch/package.xml @@ -19,6 +19,7 @@ detected_object_feature_remover detected_object_validation detection_by_tracker + elevation_map_loader euclidean_cluster ground_segmentation image_projection_based_fusion diff --git a/localization/pose_instability_detector/src/pose_instability_detector.cpp b/localization/pose_instability_detector/src/pose_instability_detector.cpp index 28398588809eb..23362dd13c6bc 100644 --- a/localization/pose_instability_detector/src/pose_instability_detector.cpp +++ b/localization/pose_instability_detector/src/pose_instability_detector.cpp @@ -354,6 +354,9 @@ PoseInstabilityDetector::clip_out_necessary_twist( start_twist.header.stamp = start_time; result_deque.push_front(start_twist); } else { + if (result_deque.size() < 2) { + return result_deque; + } // If the first element is earlier than start_time, interpolate the first element rclcpp::Time time0 = rclcpp::Time(result_deque[0].header.stamp); rclcpp::Time time1 = rclcpp::Time(result_deque[1].header.stamp); @@ -380,6 +383,9 @@ PoseInstabilityDetector::clip_out_necessary_twist( end_twist.header.stamp = end_time; result_deque.push_back(end_twist); } else { + if (result_deque.size() < 2) { + return result_deque; + } // If the last element is later than end_time, interpolate the last element rclcpp::Time time0 = rclcpp::Time(result_deque[result_deque.size() - 2].header.stamp); rclcpp::Time time1 = rclcpp::Time(result_deque[result_deque.size() - 1].header.stamp); diff --git a/localization/pose_instability_detector/test/test.cpp b/localization/pose_instability_detector/test/test.cpp index 9300984967d4b..482e659e7a13c 100644 --- a/localization/pose_instability_detector/test/test.cpp +++ b/localization/pose_instability_detector/test/test.cpp @@ -163,6 +163,64 @@ TEST_F(TestPoseInstabilityDetector, output_warn_when_twist_is_too_small) // NOL EXPECT_TRUE(diagnostic_status.level == diagnostic_msgs::msg::DiagnosticStatus::WARN); } +TEST_F(TestPoseInstabilityDetector, does_not_crash_even_if_abnormal_odometry_data_comes) // NOLINT +{ + // [Condition] There is no twist_msg between the two target odometry_msgs. + // Normally this doesn't seem to happen. + // As far as I can think, this happens when the odometry msg stops (so the next timer callback + // will refer to the same odometry msg, and the timestamp difference will be calculated as 0) + // This test case shows that an error occurs when two odometry msgs come in close succession and + // there is no other odometry msg. + // Referring again, this doesn't normally seem to happen in usual operation. + + builtin_interfaces::msg::Time timestamp{}; + + // send the twist message1 + timestamp.sec = 0; + timestamp.nanosec = 0; + helper_->send_twist_message(timestamp, 0.2, 0.0, 0.0); + + // send the first odometry message after the first twist message + timestamp.sec = 0; + timestamp.nanosec = 5e8 + 1; + helper_->send_odometry_message(timestamp, 10.0, 0.0, 0.0); + + // process the above message (by timer_callback) + helper_->received_diagnostic_array_flag = false; + while (!helper_->received_diagnostic_array_flag) { + executor_.spin_some(); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + // send the second odometry message before the second twist message + timestamp.sec = 0; + timestamp.nanosec = 5e8 + 1e7; + helper_->send_odometry_message(timestamp, 12.0, 0.0, 0.0); + + // send the twist message2 + timestamp.sec = 1; + timestamp.nanosec = 0; + helper_->send_twist_message(timestamp, 0.2, 0.0, 0.0); + + // process the above messages (by timer_callback) + helper_->received_diagnostic_array_flag = false; + while (!helper_->received_diagnostic_array_flag) { + executor_.spin_some(); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + // provoke timer callback again + helper_->received_diagnostic_array_flag = false; + while (!helper_->received_diagnostic_array_flag) { + executor_.spin_some(); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + // This test is OK if pose_instability_detector does not crash. The diagnostics status is not + // checked. + SUCCEED(); +} + int main(int argc, char ** argv) { rclcpp::init(argc, argv); diff --git a/perception/euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp b/perception/euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp index 40699fd6e27ab..126f877afddb0 100644 --- a/perception/euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp +++ b/perception/euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp @@ -106,7 +106,7 @@ bool VoxelGridBasedEuclideanCluster::cluster( temporary_cluster.height = pointcloud_msg->height; temporary_cluster.fields = pointcloud_msg->fields; temporary_cluster.point_step = point_step; - temporary_cluster.data.resize(max_cluster_size_ * point_step); + temporary_cluster.data.resize(cluster.indices.size() * point_step); clusters_data_size.push_back(0); } @@ -117,13 +117,17 @@ bool VoxelGridBasedEuclideanCluster::cluster( voxel_grid_.getCentroidIndexAt(voxel_grid_.getGridCoordinates(point.x, point.y, point.z)); if (map.find(index) != map.end()) { auto & cluster_data_size = clusters_data_size.at(map[index]); - if (cluster_data_size + point_step > std::size_t(max_cluster_size_ * point_step)) { + if (cluster_data_size > std::size_t(max_cluster_size_ * point_step)) { continue; } std::memcpy( &temporary_clusters.at(map[index]).data[cluster_data_size], &pointcloud_msg->data[i * point_step], point_step); cluster_data_size += point_step; + if (cluster_data_size == temporary_clusters.at(map[index]).data.size()) { + temporary_clusters.at(map[index]) + .data.resize(temporary_clusters.at(map[index]).data.size() * 2); + } } } diff --git a/perception/ground_segmentation/src/ransac_ground_filter_nodelet.cpp b/perception/ground_segmentation/src/ransac_ground_filter_nodelet.cpp index d7fa777dc58c9..aa224e7adc5bf 100644 --- a/perception/ground_segmentation/src/ransac_ground_filter_nodelet.cpp +++ b/perception/ground_segmentation/src/ransac_ground_filter_nodelet.cpp @@ -292,12 +292,13 @@ void RANSACGroundFilterComponent::filter( no_ground_cloud_msg_ptr->header = input->header; no_ground_cloud_msg_ptr->fields = input->fields; no_ground_cloud_msg_ptr->point_step = point_step; + no_ground_cloud_msg_ptr->data.resize(input->data.size()); size_t output_size = 0; // use not downsampled pointcloud for extract pointcloud that higher than height threshold for (size_t global_size = 0; global_size < input->data.size(); global_size += point_step) { - float x = *reinterpret_cast(input->data[global_size + x_offset]); - float y = *reinterpret_cast(input->data[global_size + y_offset]); - float z = *reinterpret_cast(input->data[global_size + z_offset]); + float x = *reinterpret_cast(&input->data[global_size + x_offset]); + float y = *reinterpret_cast(&input->data[global_size + y_offset]); + float z = *reinterpret_cast(&input->data[global_size + z_offset]); const Eigen::Vector3d transformed_point = plane_affine.inverse() * Eigen::Vector3d(x, y, z); if (std::abs(transformed_point.z()) > height_threshold_) { std::memcpy( diff --git a/planning/autoware_static_centerline_generator/CMakeLists.txt b/planning/autoware_static_centerline_generator/CMakeLists.txt index 08a97c9010008..261e8beb0022f 100644 --- a/planning/autoware_static_centerline_generator/CMakeLists.txt +++ b/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} ) diff --git a/planning/autoware_static_centerline_generator/config/static_centerline_generator.param.yaml b/planning/autoware_static_centerline_generator/config/static_centerline_generator.param.yaml index 24a5536949479..060590803428a 100644 --- a/planning/autoware_static_centerline_generator/config/static_centerline_generator.param.yaml +++ b/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. diff --git a/planning/autoware_static_centerline_generator/launch/static_centerline_generator.launch.xml b/planning/autoware_static_centerline_generator/launch/static_centerline_generator.launch.xml index 3d786ab995e5b..38379a52e4c92 100644 --- a/planning/autoware_static_centerline_generator/launch/static_centerline_generator.launch.xml +++ b/planning/autoware_static_centerline_generator/launch/static_centerline_generator.launch.xml @@ -3,17 +3,19 @@ - + - - + - + + + + @@ -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" /> - + @@ -55,12 +57,8 @@ - - - - - + @@ -75,12 +73,19 @@ + + + + + + + - + diff --git a/planning/autoware_static_centerline_generator/rviz/static_centerline_generator.rviz b/planning/autoware_static_centerline_generator/rviz/static_centerline_generator.rviz index 62b4c9b75ec87..f1bd110783009 100644 --- 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 @@ -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: diff --git a/planning/autoware_static_centerline_generator/scripts/centerline_updater_helper.py b/planning/autoware_static_centerline_generator/scripts/centerline_updater_helper.py index 3672165caed85..f3d908713361d 100755 --- a/planning/autoware_static_centerline_generator/scripts/centerline_updater_helper.py +++ b/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,25 +48,8 @@ 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) @@ -72,69 +57,61 @@ def setupUI(self): 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) diff --git a/planning/autoware_static_centerline_generator/scripts/tmp/bag_ego_trajectory_based_centerline.sh b/planning/autoware_static_centerline_generator/scripts/tmp/bag_ego_trajectory_based_centerline.sh index e7f86062a9d20..c170e24e475d9 100755 --- a/planning/autoware_static_centerline_generator/scripts/tmp/bag_ego_trajectory_based_centerline.sh +++ b/planning/autoware_static_centerline_generator/scripts/tmp/bag_ego_trajectory_based_centerline.sh @@ -1,3 +1,5 @@ #!/bin/bash -ros2 launch autoware_static_centerline_generator static_centerline_generator.launch.xml centerline_source:="bag_ego_trajectory_base" run_background:=false lanelet2_input_file_path:="$HOME/autoware_map/sample_map/lanelet2_map.osm" bag_filename:="$(ros2 pkg prefix autoware_static_centerline_generator --share)/test/data/bag_ego_trajectory.db3" vehicle_model:=lexus +ros2 launch autoware_static_centerline_generator static_centerline_generator.launch.xml centerline_source:="bag_ego_trajectory_base" mode:="GUI" lanelet2_input_file_path:="$HOME/autoware_map/sample_map/lanelet2_map.osm" bag_filename:="$(ros2 pkg prefix autoware_static_centerline_generator --share)/test/data/bag_ego_trajectory_turn-right.db3" vehicle_model:=lexus + +# ros2 launch autoware_static_centerline_generator static_centerline_generator.launch.xml centerline_source:="bag_ego_trajectory_base" mode:="GUI" lanelet2_input_file_path:="$HOME/autoware_map/sample_map/lanelet2_map.osm" bag_filename:="$(ros2 pkg prefix autoware_static_centerline_generator --share)/test/data/bag_ego_trajectory_turn-left-right.db3" vehicle_model:=lexus diff --git a/planning/autoware_static_centerline_generator/src/main.cpp b/planning/autoware_static_centerline_generator/src/main.cpp index 9f52f271cd5e5..5afc7e58ba52b 100644 --- a/planning/autoware_static_centerline_generator/src/main.cpp +++ b/planning/autoware_static_centerline_generator/src/main.cpp @@ -25,11 +25,19 @@ int main(int argc, char * argv[]) node_options); // get ros parameter - const bool run_background = node->declare_parameter("run_background"); + const auto mode = node->declare_parameter("mode"); // process - if (!run_background) { - node->run(); + if (mode == "AUTO") { + node->generate_centerline(); + node->validate(); + node->save_map(); + } else if (mode == "GUI") { + node->generate_centerline(); + } else if (mode == "VMB") { + // Do nothing + } else { + throw std::invalid_argument("The `mode` is invalid."); } // NOTE: spin node to keep showing debug path/trajectory in rviz with transient local 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 index c5ecc48ee209d..de89cf4d12a87 100644 --- a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp +++ b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp @@ -20,6 +20,7 @@ #include "autoware/universe_utils/ros/parameter.hpp" #include "autoware_static_centerline_generator/msg/points_with_lane_id.hpp" #include "centerline_source/bag_ego_trajectory_based_centerline.hpp" +#include "interpolation/spline_interpolation_points_2d.hpp" #include "lanelet2_extension/utility/message_conversion.hpp" #include "lanelet2_extension/utility/query.hpp" #include "lanelet2_extension/utility/utilities.hpp" @@ -34,7 +35,7 @@ #include #include -#include "std_msgs/msg/bool.hpp" +#include "std_msgs/msg/empty.hpp" #include "std_msgs/msg/float32.hpp" #include "std_msgs/msg/int32.hpp" @@ -55,6 +56,10 @@ #include #include +#define RESET_TEXT "\x1B[0m" +#define RED_TEXT "\x1B[31m" +#define BOLD_TEXT "\x1B[1m" + namespace autoware::static_centerline_generator { namespace @@ -115,7 +120,7 @@ geometry_msgs::msg::Pose get_text_pose( const auto & i = vehicle_info; const double x_front = i.front_overhang_m + i.wheel_base_m; - const double y_left = i.wheel_tread_m / 2.0 + i.left_overhang_m + 1.0; + const double y_left = i.wheel_tread_m / 2.0 + i.left_overhang_m + 0.5; return autoware::universe_utils::calcOffsetPose(pose, x_front, y_left, 0.0); } @@ -171,6 +176,32 @@ std::vector resample_trajectory_points( autoware::motion_utils::resampleTrajectory(input_traj, resample_interval); return autoware::motion_utils::convertToTrajectoryPointArray(resampled_input_traj); } + +bool arePointsClose( + const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2, const double epsilon) +{ + return std::abs(p1.x - p2.x) < epsilon && std::abs(p1.y - p2.y) < epsilon; +} + +bool areSameDirection( + const double yaw, const geometry_msgs::msg::Point & start_point, + const geometry_msgs::msg::Point & end_point) +{ + return autoware::universe_utils::normalizeRadian( + yaw - std::atan2(end_point.y - start_point.y, end_point.x - start_point.x)) < M_PI_2; +} + +std::vector convertToGeometryPoints(const LineString2d & lanelet_points) +{ + std::vector points; + for (const auto & lanelet_point : lanelet_points) { + geometry_msgs::msg::Point point; + point.x = lanelet_point.x(); + point.y = lanelet_point.y(); + points.push_back(point); + } + return points; +} } // namespace StaticCenterlineGeneratorNode::StaticCenterlineGeneratorNode( @@ -181,46 +212,48 @@ StaticCenterlineGeneratorNode::StaticCenterlineGeneratorNode( pub_map_bin_ = create_publisher("lanelet2_map_topic", utils::create_transient_local_qos()); pub_whole_centerline_ = - create_publisher("output_whole_centerline", utils::create_transient_local_qos()); + create_publisher("~/output/whole_centerline", utils::create_transient_local_qos()); pub_centerline_ = - create_publisher("output_centerline", utils::create_transient_local_qos()); + create_publisher("~/output/centerline", utils::create_transient_local_qos()); // debug publishers - pub_debug_unsafe_footprints_ = - create_publisher("debug/unsafe_footprints", utils::create_transient_local_qos()); + pub_validation_results_ = + create_publisher("~/validation_results", utils::create_transient_local_qos()); + pub_debug_markers_ = + create_publisher("~/debug/markers", utils::create_transient_local_qos()); + + pub_debug_ego_footprint_bounds_ = create_publisher( + "~/debug/ego_footprint_bounds", utils::create_transient_local_qos()); // subscribers + sub_footprint_margin_for_road_bound_ = create_subscription( + "/static_centerline_generator/road_boundary_lateral_margin", rclcpp::QoS{1}, + [this](const std_msgs::msg::Float32 & msg) { footprint_margin_for_road_bound_ = msg.data; }); sub_traj_start_index_ = create_subscription( - "/centerline_updater_helper/traj_start_index", rclcpp::QoS{1}, + "/static_centerline_generator/traj_start_index", rclcpp::QoS{1}, [this](const std_msgs::msg::Int32 & msg) { - update_centerline_range(msg.data, traj_range_indices_.second); + if (centerline_handler_.update_start_index(msg.data)) { + visualize_selected_centerline(); + } }); sub_traj_end_index_ = create_subscription( - "/centerline_updater_helper/traj_end_index", rclcpp::QoS{1}, + "/static_centerline_generator/traj_end_index", rclcpp::QoS{1}, [this](const std_msgs::msg::Int32 & msg) { - update_centerline_range(traj_range_indices_.first, msg.data); - }); - 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 = - autoware::universe_utils::getOrDeclareParameter( - *this, "lanelet2_output_file_path"); - if (!centerline_with_route_ || msg.data) { - const auto & c = *centerline_with_route_; - const auto selected_centerline = std::vector( - c.centerline.begin() + traj_range_indices_.first, - c.centerline.begin() + traj_range_indices_.second + 1); - const auto selected_route_lane_ids = get_route_lane_ids_from_points(selected_centerline); - save_map( - lanelet2_output_file_path, - CenterlineWithRoute{selected_centerline, selected_route_lane_ids}); + if (centerline_handler_.update_end_index(msg.data)) { + visualize_selected_centerline(); } }); - sub_traj_resample_interval_ = create_subscription( - "/centerline_updater_helper/traj_resample_interval", rclcpp::QoS{1}, - [this]([[maybe_unused]] const std_msgs::msg::Float32 & msg) { - // TODO(murooka) + sub_save_map_ = create_subscription( + "/static_centerline_generator/save_map", rclcpp::QoS{1}, + [this]([[maybe_unused]] const std_msgs::msg::Empty & msg) { + if (!centerline_handler_.is_valid()) { + return; + } + save_map(); }); + sub_validate_ = create_subscription( + "/static_centerline_generator/validate", rclcpp::QoS{1}, + [this]([[maybe_unused]] const std_msgs::msg::Empty & msg) { validate(); }); // services callback_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); @@ -259,43 +292,35 @@ StaticCenterlineGeneratorNode::StaticCenterlineGeneratorNode( }(); } -void StaticCenterlineGeneratorNode::update_centerline_range( - const int traj_start_index, const int traj_end_index) +void StaticCenterlineGeneratorNode::visualize_selected_centerline() { - if (!centerline_with_route_ || traj_range_indices_.second + 1 < traj_start_index) { - return; - } - - traj_range_indices_ = std::make_pair(traj_start_index, traj_end_index); - - const auto & centerline = centerline_with_route_->centerline; - const auto selected_centerline = std::vector( - centerline.begin() + traj_range_indices_.first, - centerline.begin() + traj_range_indices_.second + 1); - + // publish selected centerline + const auto selected_centerline = centerline_handler_.get_selected_centerline(); pub_centerline_->publish( autoware::motion_utils::convertToTrajectory(selected_centerline, create_header(this->now()))); + + // delete markers for validation + pub_validation_results_->publish(utils::create_delete_all_marker_array({}, now())); + pub_debug_markers_->publish(utils::create_delete_all_marker_array( + {"unsafe_footprints", "unsafe_footprints_distance"}, now())); + pub_debug_ego_footprint_bounds_->publish( + utils::create_delete_all_marker_array({"road_bounds"}, now())); } -void StaticCenterlineGeneratorNode::run() +void StaticCenterlineGeneratorNode::generate_centerline() { // declare planning setting parameters const auto lanelet2_input_file_path = declare_parameter("lanelet2_input_file_path"); - const auto lanelet2_output_file_path = - declare_parameter("lanelet2_output_file_path"); // process load_map(lanelet2_input_file_path); - const auto centerline_with_route = generate_centerline_with_route(); - traj_range_indices_ = std::make_pair(0, centerline_with_route.centerline.size() - 1); + const auto whole_centerline_with_route = generate_whole_centerline_with_route(); + centerline_handler_ = CenterlineHandler(whole_centerline_with_route); - evaluate(centerline_with_route.route_lane_ids, centerline_with_route.centerline); - save_map(lanelet2_output_file_path, centerline_with_route); - - centerline_with_route_ = centerline_with_route; + visualize_selected_centerline(); } -CenterlineWithRoute StaticCenterlineGeneratorNode::generate_centerline_with_route() +CenterlineWithRoute StaticCenterlineGeneratorNode::generate_whole_centerline_with_route() { if (!route_handler_ptr_) { RCLCPP_ERROR(get_logger(), "Route handler is not ready. Return empty trajectory."); @@ -307,14 +332,15 @@ CenterlineWithRoute StaticCenterlineGeneratorNode::generate_centerline_with_rout if (centerline_source_ == CenterlineSource::OptimizationTrajectoryBase) { const lanelet::Id start_lanelet_id = declare_parameter("start_lanelet_id"); const lanelet::Id end_lanelet_id = declare_parameter("end_lanelet_id"); - const auto route_lane_ids = plan_route(start_lanelet_id, end_lanelet_id); + const auto route_lane_ids = plan_route_by_lane_ids(start_lanelet_id, end_lanelet_id); const auto optimized_centerline = optimization_trajectory_based_centerline_.generate_centerline_with_optimization( *this, *route_handler_ptr_, route_lane_ids); return CenterlineWithRoute{optimized_centerline, route_lane_ids}; } else if (centerline_source_ == CenterlineSource::BagEgoTrajectoryBase) { const auto bag_centerline = generate_centerline_with_bag(*this); - const auto route_lane_ids = get_route_lane_ids_from_points(bag_centerline); + const auto route_lane_ids = + plan_route(bag_centerline.front().pose, bag_centerline.back().pose); return CenterlineWithRoute{bag_centerline, route_lane_ids}; } throw std::logic_error( @@ -329,30 +355,9 @@ CenterlineWithRoute StaticCenterlineGeneratorNode::generate_centerline_with_rout pub_whole_centerline_->publish(autoware::motion_utils::convertToTrajectory( centerline_with_route.centerline, create_header(this->now()))); - pub_centerline_->publish(autoware::motion_utils::convertToTrajectory( - centerline_with_route.centerline, create_header(this->now()))); - return centerline_with_route; } -std::vector StaticCenterlineGeneratorNode::get_route_lane_ids_from_points( - const std::vector & points) -{ - const auto start_lanelets = route_handler_ptr_->getRoadLaneletsAtPose(points.front().pose); - const auto end_lanelets = route_handler_ptr_->getRoadLaneletsAtPose(points.back().pose); - if (start_lanelets.empty() || end_lanelets.empty()) { - RCLCPP_ERROR(get_logger(), "Nearest lanelets to the bag's points are not found."); - return std::vector{}; - } - - const lanelet::Id start_lanelet_id = start_lanelets.front().id(); - const lanelet::Id end_lanelet_id = end_lanelets.front().id(); - if (start_lanelet_id == end_lanelet_id) { - return std::vector{start_lanelet_id}; - } - return plan_route(start_lanelet_id, end_lanelet_id); -} - void StaticCenterlineGeneratorNode::load_map(const std::string & lanelet2_input_file_path) { // copy the input LL2 map to the temporary file for debugging @@ -426,24 +431,35 @@ void StaticCenterlineGeneratorNode::on_load_map( response->message = "InvalidMapFormat"; } -std::vector StaticCenterlineGeneratorNode::plan_route( +std::vector StaticCenterlineGeneratorNode::plan_route_by_lane_ids( const lanelet::Id start_lanelet_id, const lanelet::Id end_lanelet_id) { - if (!map_bin_ptr_ || !route_handler_ptr_) { + if (!route_handler_ptr_) { RCLCPP_ERROR(get_logger(), "Map or route handler is not ready. Return empty lane ids."); return std::vector{}; } - // calculate check points (= start and goal pose) - const auto check_points = [&]() { - const auto start_center_pose = utils::get_center_pose(*route_handler_ptr_, start_lanelet_id); - const auto end_center_pose = utils::get_center_pose(*route_handler_ptr_, end_lanelet_id); - return std::vector{start_center_pose, end_center_pose}; - }(); - RCLCPP_INFO(get_logger(), "Calculated check points."); + const auto start_center_pose = utils::get_center_pose(*route_handler_ptr_, start_lanelet_id); + const auto end_center_pose = utils::get_center_pose(*route_handler_ptr_, end_lanelet_id); + return plan_route(start_center_pose, end_center_pose); +} + +std::vector StaticCenterlineGeneratorNode::plan_route( + const geometry_msgs::msg::Pose & start_center_pose, + const geometry_msgs::msg::Pose & end_center_pose) +{ + if (!map_bin_ptr_) { + RCLCPP_ERROR(get_logger(), "Map or route handler is not ready. Return empty lane ids."); + return std::vector{}; + } // plan route by the mission_planner package const auto route = [&]() { + // calculate check points + RCLCPP_INFO(get_logger(), "Calculated check points."); + const auto check_points = + std::vector{start_center_pose, end_center_pose}; + // create mission_planner plugin auto plugin_loader = pluginlib::ClassLoader( "autoware_mission_planner", "autoware::mission_planner::PlannerPlugin"); @@ -459,10 +475,16 @@ std::vector StaticCenterlineGeneratorNode::plan_route( return route; }(); - RCLCPP_INFO(get_logger(), "Planned route."); // get lanelets const auto route_lane_ids = get_lane_ids_from_route(route); + + std::string route_lane_ids_str = ""; + for (const lanelet::Id route_lane_id : route_lane_ids) { + route_lane_ids_str += std::to_string(route_lane_id) + ","; + } + RCLCPP_INFO_STREAM(get_logger(), "Planned route. (" << route_lane_ids_str << ")"); + return route_lane_ids; } @@ -479,7 +501,7 @@ void StaticCenterlineGeneratorNode::on_plan_route( const lanelet::Id end_lanelet_id = request->end_lane_id; // plan route - const auto route_lane_ids = plan_route(start_lanelet_id, end_lanelet_id); + const auto route_lane_ids = plan_route_by_lane_ids(start_lanelet_id, end_lanelet_id); const auto route_lanelets = utils::get_lanelets_from_ids(*route_handler_ptr_, route_lane_ids); // extract lane ids @@ -533,8 +555,11 @@ void StaticCenterlineGeneratorNode::on_plan_path( return; } + centerline_handler_ = + CenterlineHandler(CenterlineWithRoute{optimized_traj_points, route_lane_ids}); + // publish unsafe_footprints - evaluate(route_lane_ids, optimized_traj_points); + validate(); // create output data auto target_traj_point = optimized_traj_points.cbegin(); @@ -572,16 +597,132 @@ void StaticCenterlineGeneratorNode::on_plan_path( response->message = ""; } -void StaticCenterlineGeneratorNode::evaluate( - const std::vector & route_lane_ids, - const std::vector & optimized_traj_points) +RoadBounds StaticCenterlineGeneratorNode::update_road_boundary( + const std::vector & centerline) +{ + const double max_ego_lon_offset = vehicle_info_.front_overhang_m + vehicle_info_.wheel_base_m; + const double min_ego_lon_offset = -vehicle_info_.rear_overhang_m; + const double max_ego_lat_offset = + vehicle_info_.wheel_tread_m / 2.0 + vehicle_info_.left_overhang_m; + const double ego_lat_offset = max_ego_lat_offset + footprint_margin_for_road_bound_; + + std::vector ego_left_bound; + std::vector ego_right_bound; + for (size_t i = 0; i < centerline.size(); ++i) { + const auto & centerline_point = centerline.at(i).pose; + if (i == 0) { + // Add the first bound point + ego_left_bound.push_back(autoware::universe_utils::calcOffsetPose( + centerline_point, min_ego_lon_offset, ego_lat_offset, 0.0) + .position); + ego_right_bound.push_back(autoware::universe_utils::calcOffsetPose( + centerline_point, min_ego_lon_offset, -ego_lat_offset, 0.0) + .position); + } + + if (i == centerline.size() - 1) { + // Add the last bound point + const auto ego_left_bound_last_point = + autoware::universe_utils::calcOffsetPose( + centerline_point, max_ego_lon_offset, ego_lat_offset, 0.0) + .position; + if (!arePointsClose(ego_left_bound.back(), ego_left_bound_last_point, 1e-6)) { + ego_left_bound.push_back(ego_left_bound_last_point); + } + const auto ego_right_bound_last_point = + autoware::universe_utils::calcOffsetPose( + centerline_point, max_ego_lon_offset, -ego_lat_offset, 0.0) + .position; + if (!arePointsClose(ego_right_bound.back(), ego_right_bound_last_point, 1e-6)) { + ego_right_bound.push_back(ego_right_bound_last_point); + } + } else { + // Calculate new bound point depending on the orientation + const auto & next_centerline_point = centerline.at(i + 1).pose; + const double diff_yaw = autoware::universe_utils::normalizeRadian( + tf2::getYaw(next_centerline_point.orientation) - tf2::getYaw(centerline_point.orientation)); + const auto [ego_left_bound_new_point, ego_right_bound_new_point] = [&]() { + if (0 < diff_yaw) { + return std::make_pair( + autoware::universe_utils::calcOffsetPose(centerline_point, 0.0, ego_lat_offset, 0.0) + .position, + autoware::universe_utils::calcOffsetPose( + centerline_point, max_ego_lon_offset, -ego_lat_offset, 0.0) + .position); + } + return std::make_pair( + autoware::universe_utils::calcOffsetPose( + centerline_point, max_ego_lon_offset, ego_lat_offset, 0.0) + .position, + autoware::universe_utils::calcOffsetPose(centerline_point, 0.0, -ego_lat_offset, 0.0) + .position); + }(); + + // Check if the bound will be longitudinally monotonic. + if (areSameDirection( + tf2::getYaw(centerline_point.orientation), ego_left_bound.back(), + ego_left_bound_new_point)) { + ego_left_bound.push_back(ego_left_bound_new_point); + } + if (areSameDirection( + tf2::getYaw(centerline_point.orientation), ego_right_bound.back(), + ego_right_bound_new_point)) { + ego_right_bound.push_back(ego_right_bound_new_point); + } + } + } + + // Publish marker + MarkerArray ego_footprint_bounds_marker_array; + { + auto left_bound_marker = autoware::universe_utils::createDefaultMarker( + "map", now(), "road_bounds", 0, Marker::LINE_STRIP, + autoware::universe_utils::createMarkerScale(0.05, 0.0, 0.0), + autoware::universe_utils::createMarkerColor(1.0, 0.5, 0.7, 0.8)); + left_bound_marker.lifetime = rclcpp::Duration(0, 0); + for (const auto & ego_left_bound_point : ego_left_bound) { + left_bound_marker.points.push_back(ego_left_bound_point); + } + ego_footprint_bounds_marker_array.markers.push_back(left_bound_marker); + } + { + auto right_bound_marker = autoware::universe_utils::createDefaultMarker( + "map", now(), "road_bounds", 1, Marker::LINE_STRIP, + autoware::universe_utils::createMarkerScale(0.05, 0.0, 0.0), + autoware::universe_utils::createMarkerColor(1.0, 0.5, 0.7, 0.8)); + right_bound_marker.lifetime = rclcpp::Duration(0, 0); + for (const auto & ego_right_bound_point : ego_right_bound) { + right_bound_marker.points.push_back(ego_right_bound_point); + } + ego_footprint_bounds_marker_array.markers.push_back(right_bound_marker); + } + pub_debug_ego_footprint_bounds_->publish(ego_footprint_bounds_marker_array); + + return RoadBounds{ego_left_bound, ego_right_bound}; +} + +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 " + "##############################################" + << std::endl; + + const auto & centerline = centerline_handler_.get_selected_centerline(); + const auto & route_lane_ids = centerline_handler_.get_route_lane_ids(); const auto route_lanelets = utils::get_lanelets_from_ids(*route_handler_ptr_, route_lane_ids); - const auto dist_thresh_vec = autoware::universe_utils::getOrDeclareParameter>( - *this, "marker_color_dist_thresh"); - const auto marker_color_vec = - autoware::universe_utils::getOrDeclareParameter>( - *this, "marker_color"); + + const double dist_thresh_to_road_border = + getRosParameter("validation.dist_threshold_to_road_border"); + const double max_steer_angle_margin = + getRosParameter("validation.max_steer_angle_margin"); + + // calculate color for distance to road border + const auto dist_thresh_vec = getRosParameter>("marker_color_dist_thresh"); + const auto marker_color_vec = getRosParameter>("marker_color"); const auto get_marker_color = [&](const double dist) -> boost::optional> { for (size_t i = 0; i < dist_thresh_vec.size(); ++i) { const double dist_thresh = dist_thresh_vec.at(i); @@ -593,27 +734,34 @@ void StaticCenterlineGeneratorNode::evaluate( }; // create right/left bound - LineString2d right_bound; - LineString2d left_bound; + LineString2d lanelet_right_bound; + LineString2d lanelet_left_bound; for (const auto & lanelet : route_lanelets) { for (const auto & point : lanelet.rightBound()) { - boost::geometry::append(right_bound, Point2d(point.x(), point.y())); + boost::geometry::append(lanelet_right_bound, Point2d(point.x(), point.y())); } for (const auto & point : lanelet.leftBound()) { - boost::geometry::append(left_bound, Point2d(point.x(), point.y())); + boost::geometry::append(lanelet_left_bound, Point2d(point.x(), point.y())); } } + // calculate curvature + SplineInterpolationPoints2d centerline_spline(centerline); + const auto curvature_vec = centerline_spline.getSplineInterpolatedCurvatures(); + const double curvature_threshold = vehicle_info_.calcCurvatureFromSteerAngle( + vehicle_info_.max_steer_angle_rad - max_steer_angle_margin); + // calculate the distance between footprint and right/left bounds MarkerArray marker_array; double min_dist = std::numeric_limits::max(); - for (size_t i = 0; i < optimized_traj_points.size(); ++i) { - const auto & traj_point = optimized_traj_points.at(i); + double max_curvature = std::numeric_limits::min(); + for (size_t i = 0; i < centerline.size(); ++i) { + const auto & traj_point = centerline.at(i); const auto footprint_poly = create_vehicle_footprint(traj_point.pose, vehicle_info_); - const double dist_to_right = boost::geometry::distance(footprint_poly, right_bound); - const double dist_to_left = boost::geometry::distance(footprint_poly, left_bound); + const double dist_to_right = boost::geometry::distance(footprint_poly, lanelet_right_bound); + const double dist_to_left = boost::geometry::distance(footprint_poly, lanelet_left_bound); const double min_dist_to_bound = std::min(dist_to_right, dist_to_left); if (min_dist_to_bound < min_dist) { @@ -622,46 +770,111 @@ void StaticCenterlineGeneratorNode::evaluate( // create marker const auto marker_color_opt = get_marker_color(min_dist_to_bound); + const auto text_pose = get_text_pose(traj_point.pose, vehicle_info_); if (marker_color_opt) { const auto & marker_color = marker_color_opt.get(); // add footprint marker - const auto footprint_marker = - utils::create_footprint_marker(footprint_poly, marker_color, now(), i); - autoware::universe_utils::appendMarkerArray(footprint_marker, &marker_array); + const auto footprint_marker = utils::create_footprint_marker( + footprint_poly, 0.05, marker_color.at(0), marker_color.at(1), marker_color.at(2), 0.7, + now(), i); + marker_array.markers.push_back(footprint_marker); // add text of distance to bounds marker - const auto text_pose = get_text_pose(traj_point.pose, vehicle_info_); - const auto text_marker = - utils::create_distance_text_marker(text_pose, min_dist_to_bound, marker_color, now(), i); - autoware::universe_utils::appendMarkerArray(text_marker, &marker_array); + const auto text_marker = utils::create_text_marker( + "unsafe_footprints_distance", text_pose, min_dist_to_bound, marker_color.at(0), + marker_color.at(1), marker_color.at(2), 0.999, now(), i); + marker_array.markers.push_back(text_marker); + } + + const double curvature = curvature_vec.at(i); + const auto text_marker = + utils::create_text_marker("curvature", text_pose, curvature, 0.05, 0.05, 0.0, 0.9, now(), i); + marker_array.markers.push_back(text_marker); + + if (max_curvature < std::abs(curvature)) { + max_curvature = std::abs(curvature); } } - pub_debug_unsafe_footprints_->publish(marker_array); + // publish road boundaries + const auto left_bound = convertToGeometryPoints(lanelet_left_bound); + const auto right_bound = convertToGeometryPoints(lanelet_right_bound); + + marker_array.markers.push_back( + utils::create_points_marker("left_bound", left_bound, 0.05, 0.0, 0.6, 0.8, 0.8, now())); + marker_array.markers.push_back( + utils::create_points_marker("right_bound", right_bound, 0.05, 0.0, 0.6, 0.8, 0.8, now())); + pub_debug_markers_->publish(marker_array); + + // show the validation results + // 1. distance from footprints to road boundaries + const bool are_footprints_inside_lanelets = [&]() { + std::cerr << "1. Footprints inside Lanelets:" << std::endl; + if (dist_thresh_to_road_border < min_dist) { + std::cerr << " The generated centerline is inside the lanelet. (threshold:" + << dist_thresh_to_road_border << " < actual:" << min_dist << ")" << std::endl + << " Passed." << std::endl; + return true; + } + std::cerr << RED_TEXT + << " The generated centerline is outside the lanelet. (actual:" << min_dist + << " <= threshold:" << dist_thresh_to_road_border << ")" << std::endl + << " Failed." << RESET_TEXT << std::endl; + return false; + }(); + // 2. centerline's curvature + const bool is_curvature_low = [&]() { + std::cerr << "2. Curvature:" << std::endl; + if (max_curvature < curvature_threshold) { + std::cerr << " The generated centerline has no high curvature. (actual:" << max_curvature + << " < threshold:" << curvature_threshold << ")" + << " Passed." << std::endl; + return true; + } + std::cerr << RED_TEXT << " The generated centerline has a too high curvature. (threshold:" + << curvature_threshold << " <= actual:" << max_curvature << ")" + << " Failed." << RESET_TEXT << std::endl; + return false; + }(); + // 3. result + std::cerr << std::endl << BOLD_TEXT << "Result:" << RESET_TEXT << std::endl; + if (are_footprints_inside_lanelets && is_curvature_low) { + std::cerr << BOLD_TEXT << " Passed!" << RESET_TEXT << std::endl; + } else { + std::cerr << BOLD_TEXT << RED_TEXT << " Failed!" << RESET_TEXT << std::endl; + } - RCLCPP_INFO(get_logger(), "Minimum distance to road is %f [m]", min_dist); + std::cerr << "###################################################################################" + "#############################" + << std::endl + << std::endl; + RCLCPP_INFO(get_logger(), "Validated the generated centerline."); } -void StaticCenterlineGeneratorNode::save_map( - const std::string & lanelet2_output_file_path, const CenterlineWithRoute & centerline_with_route) +void StaticCenterlineGeneratorNode::save_map() { if (!route_handler_ptr_) { return; } - const auto & c = centerline_with_route; - const auto route_lanelets = utils::get_lanelets_from_ids(*route_handler_ptr_, c.route_lane_ids); + const auto & centerline = centerline_handler_.get_selected_centerline(); + const auto & route_lane_ids = centerline_handler_.get_route_lane_ids(); + + const auto lanelet2_output_file_path = getRosParameter("lanelet2_output_file_path"); + + const auto route_lanelets = utils::get_lanelets_from_ids(*route_handler_ptr_, route_lane_ids); // update centerline in map - utils::update_centerline(original_map_ptr_, route_lanelets, c.centerline); + utils::update_centerline(original_map_ptr_, route_lanelets, centerline); RCLCPP_INFO(get_logger(), "Updated centerline in map."); // save map with modified center line - std::filesystem::create_directory("/tmp/static_centerline_generator"); // TODO(murooka) + std::filesystem::create_directory("/tmp/autoware_static_centerline_generator"); const auto map_projector = geography_utils::get_lanelet2_projector(*map_projector_info_); lanelet::write(lanelet2_output_file_path, *original_map_ptr_, *map_projector); - RCLCPP_INFO(get_logger(), "Saved map."); + RCLCPP_INFO( + get_logger(), "Saved map in %s", "/tmp/autoware_static_centerline_generator/lanelet2_map.osm"); // copy the output LL2 map to the temporary file for debugging const std::string debug_output_file_dir{"/tmp/autoware_static_centerline_generator/output/"}; 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 index 2f25a064dca2a..fd2314d42e46f 100644 --- a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.hpp +++ b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.hpp @@ -15,6 +15,7 @@ #ifndef STATIC_CENTERLINE_GENERATOR_NODE_HPP_ #define STATIC_CENTERLINE_GENERATOR_NODE_HPP_ +#include "autoware/universe_utils/ros/parameter.hpp" #include "autoware_static_centerline_generator/srv/load_map.hpp" #include "autoware_static_centerline_generator/srv/plan_path.hpp" #include "autoware_static_centerline_generator/srv/plan_route.hpp" @@ -23,7 +24,7 @@ #include "rclcpp/rclcpp.hpp" #include "type_alias.hpp" -#include "std_msgs/msg/bool.hpp" +#include "std_msgs/msg/empty.hpp" #include "std_msgs/msg/float32.hpp" #include "std_msgs/msg/int32.hpp" #include "tier4_map_msgs/msg/map_projector_info.hpp" @@ -45,12 +46,67 @@ struct CenterlineWithRoute std::vector centerline{}; std::vector route_lane_ids{}; }; +struct CenterlineHandler +{ + CenterlineHandler() = default; + explicit CenterlineHandler(const CenterlineWithRoute & centerline_with_route) + : whole_centerline_with_route(centerline_with_route), + start_index(0), + end_index(centerline_with_route.centerline.size() - 1) + { + } + std::vector get_selected_centerline() const + { + if (!whole_centerline_with_route) { + return std::vector{}; + } + const auto & centerline_begin = whole_centerline_with_route->centerline.begin(); + return std::vector( + centerline_begin + start_index, centerline_begin + end_index + 1); + } + std::vector get_route_lane_ids() const + { + if (!whole_centerline_with_route) { + return std::vector{}; + } + return whole_centerline_with_route->route_lane_ids; + } + bool is_valid() const { return whole_centerline_with_route && start_index < end_index; } + bool update_start_index(const int arg_start_index) + { + if (whole_centerline_with_route && arg_start_index < end_index) { + start_index = arg_start_index; + return true; + } + return false; + } + bool update_end_index(const int arg_end_index) + { + if (whole_centerline_with_route && start_index < arg_end_index) { + end_index = arg_end_index; + return true; + } + return false; + } + + std::optional whole_centerline_with_route{std::nullopt}; + int start_index{}; + int end_index{}; +}; + +struct RoadBounds +{ + std::vector left_bound; + std::vector right_bound; +}; class StaticCenterlineGeneratorNode : public rclcpp::Node { public: explicit StaticCenterlineGeneratorNode(const rclcpp::NodeOptions & node_options); - void run(); + void generate_centerline(); + void validate(); + void save_map(); private: // load map @@ -59,33 +115,40 @@ class StaticCenterlineGeneratorNode : public rclcpp::Node const LoadMap::Request::SharedPtr request, const LoadMap::Response::SharedPtr response); // plan route - std::vector plan_route( + std::vector plan_route_by_lane_ids( const lanelet::Id start_lanelet_id, const lanelet::Id end_lanelet_id); + std::vector plan_route( + const geometry_msgs::msg::Pose & start_center_pose, + const geometry_msgs::msg::Pose & end_center_pose); + void on_plan_route( const PlanRoute::Request::SharedPtr request, const PlanRoute::Response::SharedPtr response); // plan centerline - CenterlineWithRoute generate_centerline_with_route(); + CenterlineWithRoute generate_whole_centerline_with_route(); std::vector get_route_lane_ids_from_points( const std::vector & points); void on_plan_path( const PlanPath::Request::SharedPtr request, const PlanPath::Response::SharedPtr response); - void update_centerline_range(const int traj_start_index, const int traj_end_index); - void evaluate( - const std::vector & route_lane_ids, - const std::vector & optimized_traj_points); - void save_map( - const std::string & lanelet2_output_file_path, - const CenterlineWithRoute & centerline_with_route); + void visualize_selected_centerline(); + RoadBounds update_road_boundary(const std::vector & centerline); + + // parameter + template + T getRosParameter(const std::string & param_name) + { + return autoware::universe_utils::getOrDeclareParameter(*this, param_name); + } lanelet::LaneletMapPtr original_map_ptr_{nullptr}; LaneletMapBin::ConstSharedPtr map_bin_ptr_{nullptr}; std::shared_ptr route_handler_ptr_{nullptr}; std::unique_ptr map_projector_info_{nullptr}; - std::pair traj_range_indices_{0, 0}; - std::optional centerline_with_route_{std::nullopt}; + CenterlineHandler centerline_handler_; + + float footprint_margin_for_road_bound_{0.0}; enum class CenterlineSource { OptimizationTrajectoryBase = 0, @@ -96,15 +159,19 @@ class StaticCenterlineGeneratorNode : public rclcpp::Node // publisher rclcpp::Publisher::SharedPtr pub_map_bin_{nullptr}; - rclcpp::Publisher::SharedPtr pub_debug_unsafe_footprints_{nullptr}; rclcpp::Publisher::SharedPtr pub_whole_centerline_{nullptr}; rclcpp::Publisher::SharedPtr pub_centerline_{nullptr}; + rclcpp::Publisher::SharedPtr pub_validation_results_{nullptr}; + rclcpp::Publisher::SharedPtr pub_debug_ego_footprint_bounds_{nullptr}; + rclcpp::Publisher::SharedPtr pub_debug_markers_{nullptr}; // subscriber rclcpp::Subscription::SharedPtr sub_traj_start_index_; rclcpp::Subscription::SharedPtr sub_traj_end_index_; - rclcpp::Subscription::SharedPtr sub_save_map_; + rclcpp::Subscription::SharedPtr sub_save_map_; + rclcpp::Subscription::SharedPtr sub_validate_; rclcpp::Subscription::SharedPtr sub_traj_resample_interval_; + rclcpp::Subscription::SharedPtr sub_footprint_margin_for_road_bound_; // service rclcpp::Service::SharedPtr srv_load_map_; diff --git a/planning/autoware_static_centerline_generator/src/type_alias.hpp b/planning/autoware_static_centerline_generator/src/type_alias.hpp index c5157acc2b525..2b7b99bfe81be 100644 --- a/planning/autoware_static_centerline_generator/src/type_alias.hpp +++ b/planning/autoware_static_centerline_generator/src/type_alias.hpp @@ -40,6 +40,7 @@ using autoware_planning_msgs::msg::PathPoint; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; using tier4_planning_msgs::msg::PathWithLaneId; +using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; } // namespace autoware::static_centerline_generator diff --git a/planning/autoware_static_centerline_generator/src/utils.cpp b/planning/autoware_static_centerline_generator/src/utils.cpp index de362e3af5d7f..1a8e0eae2fbd9 100644 --- a/planning/autoware_static_centerline_generator/src/utils.cpp +++ b/planning/autoware_static_centerline_generator/src/utils.cpp @@ -23,6 +23,7 @@ #include #include +#include namespace autoware::static_centerline_generator { @@ -151,6 +152,7 @@ void update_centerline( auto & lanelet_ref = lanelets_ref.at(lanelet_idx); const lanelet::BasicPoint2d point(traj_pos.x, traj_pos.y); + // TODO(murooka) This does not work with L-crank map. const bool is_inside = lanelet::geometry::inside(lanelet_ref, point); if (is_inside) { const auto center_point = createPoint3d(traj_pos.x, traj_pos.y, traj_pos.z); @@ -181,20 +183,17 @@ void update_centerline( } } -MarkerArray create_footprint_marker( - const LinearRing2d & footprint_poly, const std::array & marker_color, - const rclcpp::Time & now, const size_t idx) +Marker create_footprint_marker( + const LinearRing2d & footprint_poly, const double width, const double r, const double g, + const double b, const double a, const rclcpp::Time & now, const size_t idx) { - const double r = marker_color.at(0); - const double g = marker_color.at(1); - const double b = marker_color.at(2); - auto marker = autoware::universe_utils::createDefaultMarker( "map", rclcpp::Clock().now(), "unsafe_footprints", idx, visualization_msgs::msg::Marker::LINE_STRIP, - autoware::universe_utils::createMarkerScale(0.1, 0.0, 0.0), - autoware::universe_utils::createMarkerColor(r, g, b, 0.999)); + autoware::universe_utils::createMarkerScale(width, 0.0, 0.0), + autoware::universe_utils::createMarkerColor(r, g, b, a)); marker.header.stamp = now; + // TODO(murooka) Ideally, the following is unnecessary for the topic of transient local. marker.lifetime = rclcpp::Duration(0, 0); for (const auto & point : footprint_poly) { @@ -206,38 +205,79 @@ MarkerArray create_footprint_marker( marker.points.push_back(geom_point); } marker.points.push_back(marker.points.front()); - - visualization_msgs::msg::MarkerArray marker_array; - marker_array.markers.push_back(marker); - - return marker_array; + return marker; } -MarkerArray create_distance_text_marker( - const geometry_msgs::msg::Pose & pose, const double dist, - const std::array & marker_color, const rclcpp::Time & now, const size_t idx) +Marker create_text_marker( + const std::string & ns, const geometry_msgs::msg::Pose & pose, const double value, const double r, + const double g, const double b, const double a, const rclcpp::Time & now, const size_t idx) { - const double r = marker_color.at(0); - const double g = marker_color.at(1); - const double b = marker_color.at(2); - auto marker = autoware::universe_utils::createDefaultMarker( - "map", rclcpp::Clock().now(), "unsafe_footprints_distance", idx, - visualization_msgs::msg::Marker::TEXT_VIEW_FACING, + "map", rclcpp::Clock().now(), ns, idx, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, autoware::universe_utils::createMarkerScale(0.5, 0.5, 0.5), - autoware::universe_utils::createMarkerColor(r, g, b, 0.999)); + autoware::universe_utils::createMarkerColor(r, g, b, a)); marker.pose = pose; marker.header.stamp = now; marker.lifetime = rclcpp::Duration(0, 0); std::stringstream ss; - ss << std::setprecision(2) << dist; + ss << std::setprecision(2) << value; marker.text = ss.str(); - visualization_msgs::msg::MarkerArray marker_array; - marker_array.markers.push_back(marker); + return marker; +} + +Marker create_points_marker( + const std::string & ns, const std::vector & points, const double width, + const double r, const double g, const double b, const double a, const rclcpp::Time & now) +{ + auto marker = autoware::universe_utils::createDefaultMarker( + "map", now, ns, 1, Marker::LINE_STRIP, + autoware::universe_utils::createMarkerScale(width, 0.0, 0.0), + autoware::universe_utils::createMarkerColor(r, g, b, a)); + marker.lifetime = rclcpp::Duration(0, 0); + marker.points = points; + return marker; +} + +MarkerArray create_delete_all_marker_array( + const std::vector & ns_vec, const rclcpp::Time & now) +{ + Marker marker; + marker.header.stamp = now; + marker.action = visualization_msgs::msg::Marker::DELETEALL; + + MarkerArray marker_array; + for (const auto & ns : ns_vec) { + marker.ns = ns; + marker_array.markers.push_back(marker); + } return marker_array; } + +std::pair, std::vector> +calcBoundsFromLanelets(const lanelet::ConstLanelets lanelets) +{ + std::vector left_bound; + std::vector right_bound; + for (const auto & lanelet : lanelets) { + for (const auto & lanelet_left_bound_point : lanelet.leftBound()) { + geometry_msgs::msg::Point left_bound_point; + left_bound_point.x = lanelet_left_bound_point.x(); + left_bound_point.y = lanelet_left_bound_point.y(); + left_bound_point.z = lanelet_left_bound_point.z(); + left_bound.push_back(left_bound_point); + } + for (const auto & lanelet_right_bound_point : lanelet.rightBound()) { + geometry_msgs::msg::Point right_bound_point; + right_bound_point.x = lanelet_right_bound_point.x(); + right_bound_point.y = lanelet_right_bound_point.y(); + right_bound_point.z = lanelet_right_bound_point.z(); + right_bound.push_back(right_bound_point); + } + } + return std::make_pair(left_bound, right_bound); +} } // namespace utils } // namespace autoware::static_centerline_generator diff --git a/planning/autoware_static_centerline_generator/src/utils.hpp b/planning/autoware_static_centerline_generator/src/utils.hpp index 70a7e61e19b18..e7fd51b30e658 100644 --- a/planning/autoware_static_centerline_generator/src/utils.hpp +++ b/planning/autoware_static_centerline_generator/src/utils.hpp @@ -22,6 +22,7 @@ #include #include +#include #include namespace autoware::static_centerline_generator @@ -45,13 +46,23 @@ void update_centerline( lanelet::LaneletMapPtr lanelet_map_ptr, const lanelet::ConstLanelets & lanelets, const std::vector & new_centerline); -MarkerArray create_footprint_marker( - const LinearRing2d & footprint_poly, const std::array & marker_color, - const rclcpp::Time & now, const size_t idx); +Marker create_footprint_marker( + const LinearRing2d & footprint_poly, const double width, const double r, const double g, + const double b, const double a, const rclcpp::Time & now, const size_t idx); -MarkerArray create_distance_text_marker( - const geometry_msgs::msg::Pose & pose, const double dist, - const std::array & marker_color, const rclcpp::Time & now, const size_t idx); +Marker create_text_marker( + const std::string & ns, const geometry_msgs::msg::Pose & pose, const double value, const double r, + const double g, const double b, const double a, const rclcpp::Time & now, const size_t idx); + +Marker create_points_marker( + const std::string & ns, const std::vector & points, const double width, + const double r, const double g, const double b, const double a, const rclcpp::Time & now); + +MarkerArray create_delete_all_marker_array( + const std::vector & ns_vec, const rclcpp::Time & now); + +std::pair, std::vector> +calcBoundsFromLanelets(const lanelet::ConstLanelets lanelets); } // namespace utils } // namespace autoware::static_centerline_generator diff --git a/planning/autoware_static_centerline_generator/test/data/bag_ego_trajectory_turn-left-right.db3 b/planning/autoware_static_centerline_generator/test/data/bag_ego_trajectory_turn-left-right.db3 new file mode 100644 index 0000000000000..ed3448772b794 Binary files /dev/null and b/planning/autoware_static_centerline_generator/test/data/bag_ego_trajectory_turn-left-right.db3 differ diff --git a/planning/autoware_static_centerline_generator/test/data/bag_ego_trajectory.db3 b/planning/autoware_static_centerline_generator/test/data/bag_ego_trajectory_turn-right.db3 similarity index 100% rename from planning/autoware_static_centerline_generator/test/data/bag_ego_trajectory.db3 rename to planning/autoware_static_centerline_generator/test/data/bag_ego_trajectory_turn-right.db3 diff --git a/planning/autoware_static_centerline_generator/test/test_static_centerline_generator.test.py b/planning/autoware_static_centerline_generator/test/test_static_centerline_generator.test.py index 07f8f3e30e659..4bb0630d13942 100644 --- a/planning/autoware_static_centerline_generator/test/test_static_centerline_generator.test.py +++ b/planning/autoware_static_centerline_generator/test/test_static_centerline_generator.test.py @@ -38,7 +38,7 @@ def generate_test_description(): output="screen", parameters=[ {"lanelet2_map_path": lanelet2_map_path}, - {"run_background": False}, + {"mode": "AUTO"}, {"rviz": False}, {"centerline_source": "optimization_trajectory_base"}, {"lanelet2_input_file_path": lanelet2_map_path}, diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md index 1be3e04914cda..3e155aba0af2e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md @@ -382,13 +382,14 @@ In addition, the safety check has a time hysteresis, and if the path is judged " ### Parameters for safety check -| Name | Unit | Type | Description | Default value | -| :----------------------- | :--- | :----- | :------------------------------------------------------------------------------------------------------- | :--------------------------- | -| enable_safety_check | [-] | bool | flag whether to use safety check | true | -| method | [-] | string | method for safety check. `RSS` or `integral_predicted_polygon` | `integral_predicted_polygon` | -| keep_unsafe_time | [s] | double | safety check Hysteresis time. if the path is judged "safe" for the time it is finally treated as "safe". | 3.0 | -| check_all_predicted_path | - | bool | Flag to check all predicted paths | true | -| publish_debug_marker | - | bool | Flag to publish debug markers | false | +| Name | Unit | Type | Description | Default value | +| :----------------------------------- | :---- | :----- | :------------------------------------------------------------------------------------------------------- | :--------------------------- | +| enable_safety_check | [-] | bool | flag whether to use safety check | true | +| method | [-] | string | method for safety check. `RSS` or `integral_predicted_polygon` | `integral_predicted_polygon` | +| keep_unsafe_time | [s] | double | safety check Hysteresis time. if the path is judged "safe" for the time it is finally treated as "safe". | 3.0 | +| check_all_predicted_path | - | bool | Flag to check all predicted paths | true | +| publish_debug_marker | - | bool | Flag to publish debug markers | false | +| `collision_check_yaw_diff_threshold` | [rad] | double | Maximum yaw difference between ego and object when executing rss-based collision checking | 3.1416 | #### Parameters for RSS safety check diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml index 915d256aba1db..53e06631a81d5 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml @@ -180,6 +180,7 @@ time_horizon: 10.0 # hysteresis factor to expand/shrink polygon with the value hysteresis_factor_expand_rate: 1.0 + collision_check_yaw_diff_threshold: 3.1416 # temporary backward_path_length: 100.0 forward_path_length: 100.0 diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index 2ac0b9493e67a..6caeb411f6680 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -2319,8 +2319,10 @@ std::pair GoalPlannerModule::isSafePath( return autoware::behavior_path_planner::utils::path_safety_checker::checkSafetyWithRSS( pull_over_path, ego_predicted_path, filtered_objects, collision_check, planner_data->parameters, safety_check_params->rss_params, - objects_filtering_params->use_all_predicted_path, hysteresis_factor); - } else if (parameters.safety_check_params.method == "integral_predicted_polygon") { + objects_filtering_params->use_all_predicted_path, hysteresis_factor, + safety_check_params->collision_check_yaw_diff_threshold); + } + if (parameters.safety_check_params.method == "integral_predicted_polygon") { return utils::path_safety_checker::checkSafetyWithIntegralPredictedPolygon( ego_predicted_path, vehicle_info_, filtered_objects, objects_filtering_params->check_all_predicted_path, diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp index 901fe351fc68b..b079db9babf31 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp @@ -357,6 +357,8 @@ void GoalPlannerModuleManager::init(rclcpp::Node * node) p.safety_check_params.method = node->declare_parameter(safety_check_ns + "method"); p.safety_check_params.hysteresis_factor_expand_rate = node->declare_parameter(safety_check_ns + "hysteresis_factor_expand_rate"); + p.safety_check_params.collision_check_yaw_diff_threshold = + node->declare_parameter(safety_check_ns + "collision_check_yaw_diff_threshold"); p.safety_check_params.backward_path_length = node->declare_parameter(safety_check_ns + "backward_path_length"); p.safety_check_params.forward_path_length = @@ -778,6 +780,9 @@ void GoalPlannerModuleManager::updateModuleParams( updateParam( parameters, safety_check_ns + "hysteresis_factor_expand_rate", p->safety_check_params.hysteresis_factor_expand_rate); + updateParam( + parameters, safety_check_ns + "collision_check_yaw_diff_threshold", + p->safety_check_params.collision_check_yaw_diff_threshold); updateParam( parameters, safety_check_ns + "backward_path_length", p->safety_check_params.backward_path_length); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md index a0870fe428c8a..5790be377b7aa 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md @@ -725,6 +725,7 @@ The following parameters are configurable in [lane_change.param.yaml](https://gi | `check_objects_on_current_lanes` | [-] | boolean | If true, the lane change module check objects on current lanes when performing collision assessment. | false | | `check_objects_on_other_lanes` | [-] | boolean | If true, the lane change module include objects on other lanes. when performing collision assessment | false | | `use_all_predicted_path` | [-] | boolean | If false, use only the predicted path that has the maximum confidence. | true | +| `safety_check.collision_check_yaw_diff_threshold` | [rad] | double | Maximum yaw difference between ego and object when executing rss-based collision checking | 3.1416 | #### safety constraints during lane change path is computed diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml index 1ab33514c5f24..d2f695071649a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml @@ -30,6 +30,7 @@ # safety check safety_check: allow_loose_check_for_cancel: true + collision_check_yaw_diff_threshold: 3.1416 execution: expected_front_deceleration: -1.0 expected_rear_deceleration: -1.0 diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp index 0facd8c077505..a65f3baff01d7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp @@ -137,6 +137,7 @@ struct LaneChangeParameters // safety check bool allow_loose_check_for_cancel{true}; + double collision_check_yaw_diff_threshold{3.1416}; utils::path_safety_checker::RSSparams rss_params{}; utils::path_safety_checker::RSSparams rss_params_for_abort{}; utils::path_safety_checker::RSSparams rss_params_for_stuck{}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp index 61ff54e8f99f3..22c93f7d4bfd5 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp @@ -102,6 +102,8 @@ void LaneChangeModuleManager::initParams(rclcpp::Node * node) // safety check p.allow_loose_check_for_cancel = getOrDeclareParameter(*node, parameter("safety_check.allow_loose_check_for_cancel")); + p.collision_check_yaw_diff_threshold = getOrDeclareParameter( + *node, parameter("safety_check.collision_check_yaw_diff_threshold")); p.rss_params.longitudinal_distance_min_threshold = getOrDeclareParameter( *node, parameter("safety_check.execution.longitudinal_distance_min_threshold")); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index 505bb3ef93340..ebfa1e7ac260c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -30,6 +30,7 @@ #include #include +#include #include #include #include @@ -467,8 +468,16 @@ void NormalLaneChange::insertStopPoint( PathWithLaneId NormalLaneChange::getReferencePath() const { - return utils::getCenterLinePathFromLanelet( - status_.lane_change_path.info.target_lanes.front(), planner_data_); + lanelet::ConstLanelet closest_lanelet; + if (!lanelet::utils::query::getClosestLanelet( + status_.lane_change_path.info.target_lanes, getEgoPose(), &closest_lanelet)) { + return prev_module_output_.reference_path; + } + const auto reference_path = utils::getCenterLinePathFromLanelet(closest_lanelet, planner_data_); + if (reference_path.points.empty()) { + return prev_module_output_.reference_path; + } + return reference_path; } std::optional NormalLaneChange::extendPath() @@ -2048,6 +2057,8 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( lane_change_parameters_->lane_expansion_left_offset, lane_change_parameters_->lane_expansion_right_offset); + constexpr double collision_check_yaw_diff_threshold{M_PI}; + for (const auto & obj : collision_check_objects) { auto current_debug_data = utils::path_safety_checker::createObjectDebug(obj); const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( @@ -2056,7 +2067,8 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( for (const auto & obj_path : obj_predicted_paths) { const auto collided_polygons = utils::path_safety_checker::getCollidedPolygons( path, ego_predicted_path, obj, obj_path, common_parameters, rss_params, 1.0, - get_max_velocity_for_safety_check(), current_debug_data.second); + get_max_velocity_for_safety_check(), collision_check_yaw_diff_threshold, + current_debug_data.second); if (collided_polygons.empty()) { utils::path_safety_checker::updateCollisionCheckDebugMap( diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/docs/behavior_path_planner_safety_check.md b/planning/behavior_path_planner/autoware_behavior_path_planner_common/docs/behavior_path_planner_safety_check.md index 4733fe7832da6..5511202c390b1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/docs/behavior_path_planner_safety_check.md +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/docs/behavior_path_planner_safety_check.md @@ -48,7 +48,7 @@ $$ rss_{dist} = v_{rear} (t_{reaction} + t_{margin}) + \frac{v_{rear}^2}{2|a_{rear, decel}|} - \frac{v_{front}^2}{2|a_{front, decel|}} $$ -where $V_{front}$, $v_{rear}$ are front and rear vehicle velocity respectively and $a_{rear, front}$, $a_{rear, decel}$ are front and rear vehicle deceleration. +where $V_{front}$, $v_{rear}$ are front and rear vehicle velocity respectively and $a_{rear, front}$, $a_{rear, decel}$ are front and rear vehicle deceleration. Note that RSS distance is normally used for objects traveling in the same direction, if the yaw difference between a given ego pose and object pose is more than a user-defined yaw difference threshold, the rss collision check will be skipped for that specific pair of poses. #### 5. Create extended ego and target object polygons @@ -56,7 +56,7 @@ In this step, we compute extended ego and target object polygons. The extended p ![extended_polygons](../images/path_safety_checker/extended_polygons.drawio.svg) -As the picture shows, we expand the rear object polygon. For the longitudinal side, we extend it with the RSS distance, and for the lateral side, we extend it by the lateral margin +As the picture shows, we expand the rear object polygon. For the longitudinal side, we extend it with the RSS distance, and for the lateral side, we extend it by the lateral margin. #### 6. Check overlap diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp index 313f9df471938..0e7d1cee65f02 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp @@ -23,6 +23,7 @@ #include +#include #include #include #include @@ -203,7 +204,9 @@ struct SafetyCheckParams /// possible values: ["RSS", "integral_predicted_polygon"] double keep_unsafe_time{0.0}; ///< Time to keep unsafe before changing to safe. double hysteresis_factor_expand_rate{ - 0.0}; ///< Hysteresis factor to expand/shrink polygon with the value. + 0.0}; ///< Hysteresis factor to expand/shrink polygon with the value. + double collision_check_yaw_diff_threshold{ + 3.1416}; ///< threshold yaw difference between ego and object. double backward_path_length{0.0}; ///< Length of the backward lane for path generation. double forward_path_length{0.0}; ///< Length of the forward path lane for path generation. RSSparams rss_params{}; ///< Parameters related to the RSS model. diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp index 3f1094dfd70b6..d0d19b6b8fed2 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp @@ -26,6 +26,7 @@ #include #include +#include #include namespace autoware::behavior_path_planner::utils::path_safety_checker @@ -98,7 +99,8 @@ bool checkSafetyWithRSS( const std::vector & ego_predicted_path, const std::vector & objects, CollisionCheckDebugMap & debug_map, const BehaviorPathPlannerParameters & parameters, const RSSparams & rss_params, - const bool check_all_predicted_path, const double hysteresis_factor); + const bool check_all_predicted_path, const double hysteresis_factor, + const double yaw_difference_th); /** * @brief Iterate the points in the ego and target's predicted path and @@ -111,6 +113,8 @@ bool checkSafetyWithRSS( * @param common_parameters The common parameters used in behavior path planner. * @param front_object_deceleration The deceleration of the object in the front.(used in RSS) * @param rear_object_deceleration The deceleration of the object in the rear.(used in RSS) + * @param yaw_difference_th maximum yaw difference between any given ego path pose and object + * predicted path pose. * @param debug The debug information for collision checking. * @return true if distance is safe. */ @@ -120,7 +124,7 @@ bool checkCollision( const ExtendedPredictedObject & target_object, const PredictedPathWithPolygon & target_object_path, const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters, - const double hysteresis_factor, CollisionCheckDebug & debug); + const double hysteresis_factor, const double yaw_difference_th, CollisionCheckDebug & debug); /** * @brief Iterate the points in the ego and target's predicted path and @@ -142,7 +146,8 @@ std::vector getCollidedPolygons( const ExtendedPredictedObject & target_object, const PredictedPathWithPolygon & target_object_path, const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters, - const double hysteresis_factor, const double max_velocity_limit, CollisionCheckDebug & debug); + const double hysteresis_factor, const double max_velocity_limit, const double yaw_difference_th, + CollisionCheckDebug & debug); bool checkPolygonsIntersects( const std::vector & polys_1, const std::vector & polys_2); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp index 7bb8c13aa65fb..b05d869cca6b5 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp @@ -26,6 +26,9 @@ #include #include +#include + +#include #include namespace autoware::behavior_path_planner::utils::path_safety_checker @@ -482,7 +485,8 @@ bool checkSafetyWithRSS( const std::vector & ego_predicted_path, const std::vector & objects, CollisionCheckDebugMap & debug_map, const BehaviorPathPlannerParameters & parameters, const RSSparams & rss_params, - const bool check_all_predicted_path, const double hysteresis_factor) + const bool check_all_predicted_path, const double hysteresis_factor, + const double yaw_difference_th) { // Check for collisions with each predicted path of the object const bool is_safe = !std::any_of(objects.begin(), objects.end(), [&](const auto & object) { @@ -495,7 +499,7 @@ bool checkSafetyWithRSS( obj_predicted_paths.begin(), obj_predicted_paths.end(), [&](const auto & obj_path) { const bool has_collision = !utils::path_safety_checker::checkCollision( planned_path, ego_predicted_path, object, obj_path, parameters, rss_params, - hysteresis_factor, current_debug_data.second); + hysteresis_factor, yaw_difference_th, current_debug_data.second); utils::path_safety_checker::updateCollisionCheckDebugMap( debug_map, current_debug_data, !has_collision); @@ -559,11 +563,12 @@ bool checkCollision( const ExtendedPredictedObject & target_object, const PredictedPathWithPolygon & target_object_path, const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters, - const double hysteresis_factor, CollisionCheckDebug & debug) + const double hysteresis_factor, const double yaw_difference_th, CollisionCheckDebug & debug) { const auto collided_polygons = getCollidedPolygons( planned_path, predicted_ego_path, target_object, target_object_path, common_parameters, - rss_parameters, hysteresis_factor, std::numeric_limits::max(), debug); + rss_parameters, hysteresis_factor, std::numeric_limits::max(), yaw_difference_th, + debug); return collided_polygons.empty(); } @@ -573,7 +578,8 @@ std::vector getCollidedPolygons( const ExtendedPredictedObject & target_object, const PredictedPathWithPolygon & target_object_path, const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters, - double hysteresis_factor, const double max_velocity_limit, CollisionCheckDebug & debug) + double hysteresis_factor, const double max_velocity_limit, const double yaw_difference_th, + CollisionCheckDebug & debug) { { debug.ego_predicted_path = predicted_ego_path; @@ -604,6 +610,11 @@ std::vector getCollidedPolygons( const auto & ego_polygon = interpolated_data->poly; const auto ego_velocity = std::min(interpolated_data->velocity, max_velocity_limit); + const double ego_yaw = tf2::getYaw(ego_pose.orientation); + const double object_yaw = tf2::getYaw(obj_pose.orientation); + const double yaw_difference = autoware::universe_utils::normalizeRadian(ego_yaw - object_yaw); + if (std::abs(yaw_difference) > yaw_difference_th) continue; + // check overlap if (boost::geometry::overlaps(ego_polygon, obj_polygon)) { debug.unsafe_reason = "overlap_polygon"; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/README.md index 066708891652a..dba4a0cfce5e7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/README.md @@ -436,17 +436,18 @@ Parameters under `target_filtering` are related to filtering target objects for Parameters under `safety_check_params` define the configuration for safety check. -| Name | Unit | Type | Description | Default value | -| :--------------------------------------------- | :--- | :----- | :------------------------------------------ | :------------ | -| enable_safety_check | - | bool | Flag to enable safety check | true | -| check_all_predicted_path | - | bool | Flag to check all predicted paths | true | -| publish_debug_marker | - | bool | Flag to publish debug markers | false | -| rss_params.rear_vehicle_reaction_time | [s] | double | Reaction time for rear vehicles | 2.0 | -| rss_params.rear_vehicle_safety_time_margin | [s] | double | Safety time margin for rear vehicles | 1.0 | -| rss_params.lateral_distance_max_threshold | [m] | double | Maximum lateral distance threshold | 2.0 | -| rss_params.longitudinal_distance_min_threshold | [m] | double | Minimum longitudinal distance threshold | 3.0 | -| rss_params.longitudinal_velocity_delta_time | [s] | double | Delta time for longitudinal velocity | 0.8 | -| hysteresis_factor_expand_rate | - | double | Rate to expand/shrink the hysteresis factor | 1.0 | +| Name | Unit | Type | Description | Default value | +| :--------------------------------------------- | :--- | :----- | :---------------------------------------------------------------------------------------- | :------------ | +| enable_safety_check | - | bool | Flag to enable safety check | true | +| check_all_predicted_path | - | bool | Flag to check all predicted paths | true | +| publish_debug_marker | - | bool | Flag to publish debug markers | false | +| rss_params.rear_vehicle_reaction_time | [s] | double | Reaction time for rear vehicles | 2.0 | +| rss_params.rear_vehicle_safety_time_margin | [s] | double | Safety time margin for rear vehicles | 1.0 | +| rss_params.lateral_distance_max_threshold | [m] | double | Maximum lateral distance threshold | 2.0 | +| rss_params.longitudinal_distance_min_threshold | [m] | double | Minimum longitudinal distance threshold | 3.0 | +| rss_params.longitudinal_velocity_delta_time | [s] | double | Delta time for longitudinal velocity | 0.8 | +| hysteresis_factor_expand_rate | - | double | Rate to expand/shrink the hysteresis factor | 1.0 | +| collision_check_yaw_diff_threshold | - | double | Maximum yaw difference between ego and object when executing rss-based collision checking | 1.578 | ## **Path Generation** diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml index ce7ead87b54c7..d39a320a19e14 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml @@ -143,8 +143,10 @@ lateral_distance_max_threshold: 2.0 longitudinal_distance_min_threshold: 3.0 longitudinal_velocity_delta_time: 0.8 + extended_polygon_policy: "along_path" # [-] select "rectangle" or "along_path" # hysteresis factor to expand/shrink polygon hysteresis_factor_expand_rate: 1.0 + collision_check_yaw_diff_threshold: 1.578 # temporary backward_path_length: 30.0 forward_path_length: 100.0 diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp index 7554098414442..b07d6497463ef 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp @@ -77,7 +77,7 @@ struct PullOutStatus // record if the ego has departed from the start point bool has_departed{false}; - PullOutStatus() {} + PullOutStatus() = default; }; class StartPlannerModule : public SceneModuleInterface @@ -90,7 +90,7 @@ class StartPlannerModule : public SceneModuleInterface std::unordered_map> & objects_of_interest_marker_interface_ptr_map); - ~StartPlannerModule() + ~StartPlannerModule() override { if (freespace_planner_timer_) { freespace_planner_timer_->cancel(); @@ -296,16 +296,16 @@ class StartPlannerModule : public SceneModuleInterface PathWithLaneId getCurrentPath() const; void planWithPriority( const std::vector & start_pose_candidates, const Pose & refined_start_pose, - const Pose & goal_pose, const std::string search_priority); + const Pose & goal_pose, const std::string & search_priority); PathWithLaneId generateStopPath() const; lanelet::ConstLanelets getPathRoadLanes(const PathWithLaneId & path) const; std::vector generateDrivableLanes(const PathWithLaneId & path) const; void updatePullOutStatus(); void updateStatusAfterBackwardDriving(); PredictedObjects filterStopObjectsInPullOutLanes( - const lanelet::ConstLanelets & pull_out_lanes, const geometry_msgs::msg::Point & current_pose, - const double velocity_threshold, const double object_check_backward_distance, - const double object_check_forward_distance) const; + const lanelet::ConstLanelets & pull_out_lanes, const geometry_msgs::msg::Point & current_point, + const double velocity_threshold, const double object_check_forward_distance, + const double object_check_backward_distance) const; bool needToPrepareBlinkerBeforeStartDrivingForward() const; bool hasReachedFreespaceEnd() const; bool hasReachedPullOutEnd() const; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp index 9451bb3870648..873bab1043485 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp @@ -283,6 +283,8 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) node->declare_parameter(safety_check_ns + "forward_path_length"); p.safety_check_params.publish_debug_marker = node->declare_parameter(safety_check_ns + "publish_debug_marker"); + p.safety_check_params.collision_check_yaw_diff_threshold = + node->declare_parameter(safety_check_ns + "collision_check_yaw_diff_threshold"); } // RSSparams @@ -298,6 +300,8 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) node->declare_parameter(rss_ns + "longitudinal_distance_min_threshold"); p.safety_check_params.rss_params.longitudinal_velocity_delta_time = node->declare_parameter(rss_ns + "longitudinal_velocity_delta_time"); + p.safety_check_params.rss_params.extended_polygon_policy = + node->declare_parameter(rss_ns + "extended_polygon_policy"); } // surround moving obstacle check @@ -367,7 +371,6 @@ void StartPlannerModuleManager::updateModuleParams( updateParam( parameters, ns + "extra_width_margin_for_rear_obstacle", p->extra_width_margin_for_rear_obstacle); - updateParam>( parameters, ns + "collision_check_margins", p->collision_check_margins); updateParam( @@ -658,6 +661,9 @@ void StartPlannerModuleManager::updateModuleParams( updateParam( parameters, safety_check_ns + "publish_debug_marker", p->safety_check_params.publish_debug_marker); + updateParam( + parameters, safety_check_ns + "collision_check_yaw_diff_threshold", + p->safety_check_params.collision_check_yaw_diff_threshold); } const std::string rss_ns = safety_check_ns + "rss_params."; { @@ -676,6 +682,9 @@ void StartPlannerModuleManager::updateModuleParams( updateParam( parameters, rss_ns + "longitudinal_velocity_delta_time", p->safety_check_params.rss_params.longitudinal_velocity_delta_time); + updateParam( + parameters, rss_ns + "extended_polygon_policy", + p->safety_check_params.rss_params.extended_polygon_policy); } std::string surround_moving_obstacle_check_ns = ns + "surround_moving_obstacle_check."; { diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp index f134e1645e9a5..86e35aa52095a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp @@ -34,7 +34,9 @@ #include #include +#include #include +#include #include #include #include @@ -65,7 +67,7 @@ StartPlannerModule::StartPlannerModule( { lane_departure_checker_ = std::make_shared(); lane_departure_checker_->setVehicleInfo(vehicle_info_); - autoware::lane_departure_checker::Param lane_departure_checker_params; + autoware::lane_departure_checker::Param lane_departure_checker_params{}; lane_departure_checker_params.footprint_extra_margin = parameters->lane_departure_check_expansion_margin; @@ -104,7 +106,7 @@ void StartPlannerModule::onFreespacePlannerTimer() std::optional current_status_opt{std::nullopt}; std::optional parameters_opt{std::nullopt}; std::optional pull_out_status_opt{std::nullopt}; - bool is_stopped; + bool is_stopped{false}; // making a local copy of thread sensitive data { @@ -501,7 +503,7 @@ bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough() const // Get the closest target obj width in the relevant lanes const auto closest_object_width = std::invoke([&]() -> std::optional { double arc_length_to_closet_object = std::numeric_limits::max(); - double closest_object_width = -1.0; + std::optional closest_object_width = std::nullopt; std::for_each( target_objects_on_lane.on_current_lane.begin(), target_objects_on_lane.on_current_lane.end(), [&](const auto & o) { @@ -512,7 +514,6 @@ bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough() const arc_length_to_closet_object = arc_length; closest_object_width = o.shape.dimensions.y; }); - if (closest_object_width < 0.0) return std::nullopt; return closest_object_width; }); if (!closest_object_width) return false; @@ -837,7 +838,7 @@ PathWithLaneId StartPlannerModule::getCurrentPath() const void StartPlannerModule::planWithPriority( const std::vector & start_pose_candidates, const Pose & refined_start_pose, - const Pose & goal_pose, const std::string search_priority) + const Pose & goal_pose, const std::string & search_priority) { if (start_pose_candidates.empty()) return; @@ -1116,8 +1117,8 @@ void StartPlannerModule::updateStatusAfterBackwardDriving() waitApproval(); // To enable approval of the forward path, the RTC status is removed. removeRTCStatus(); - for (auto itr = uuid_map_.begin(); itr != uuid_map_.end(); ++itr) { - itr->second = generateUUID(); + for (auto & itr : uuid_map_) { + itr.second = generateUUID(); } } @@ -1307,7 +1308,7 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() return ignore_signal_.value() == id; }; - const auto update_ignore_signal = [this](const lanelet::Id & id, const bool is_ignore) { + const auto update_ignore_signal = [](const lanelet::Id & id, const bool is_ignore) { return is_ignore ? std::make_optional(id) : std::nullopt; }; @@ -1426,10 +1427,12 @@ bool StartPlannerModule::isSafePath() const merged_target_object.insert( merged_target_object.end(), target_objects_on_lane.on_shoulder_lane.begin(), target_objects_on_lane.on_shoulder_lane.end()); + return autoware::behavior_path_planner::utils::path_safety_checker::checkSafetyWithRSS( pull_out_path, ego_predicted_path, merged_target_object, debug_data_.collision_check, planner_data_->parameters, safety_check_params_->rss_params, - objects_filtering_params_->use_all_predicted_path, hysteresis_factor); + objects_filtering_params_->use_all_predicted_path, hysteresis_factor, + safety_check_params_->collision_check_yaw_diff_threshold); } bool StartPlannerModule::isGoalBehindOfEgoInSameRouteSegment() const @@ -1697,7 +1700,7 @@ void StartPlannerModule::setDebugData() // safety check if (parameters_->safety_check_params.enable_safety_check) { - if (debug_data_.ego_predicted_path.size() > 0) { + if (!debug_data_.ego_predicted_path.empty()) { const auto & ego_predicted_path = utils::path_safety_checker::convertToPredictedPath( debug_data_.ego_predicted_path, ego_predicted_path_params_->time_resolution); add( @@ -1706,7 +1709,7 @@ void StartPlannerModule::setDebugData() debug_marker_); } - if (debug_data_.filtered_objects.objects.size() > 0) { + if (!debug_data_.filtered_objects.objects.empty()) { add( createObjectsMarkerArray( debug_data_.filtered_objects, "filtered_objects", 0, 0.0, 0.5, 0.9), diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml index 884812d0db716..fb8b02f1f0158 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml @@ -178,6 +178,7 @@ safety_check_backward_distance: 100.0 # [m] hysteresis_factor_expand_rate: 1.5 # [-] hysteresis_factor_safe_count: 3 # [-] + collision_check_yaw_diff_threshold: 3.1416 # [rad] # predicted path parameters min_velocity: 1.38 # [m/s] max_velocity: 50.0 # [m/s] diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp index 53e7c2f341926..5f6a4526af580 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp @@ -226,6 +226,8 @@ struct AvoidanceParameters size_t hysteresis_factor_safe_count; double hysteresis_factor_expand_rate{0.0}; + double collision_check_yaw_diff_threshold{3.1416}; + bool consider_front_overhang{true}; bool consider_rear_overhang{true}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp index 361fe60c21eb1..d7335165e5622 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp @@ -199,6 +199,8 @@ AvoidanceParameters getParameter(rclcpp::Node * node) getOrDeclareParameter(*node, ns + "hysteresis_factor_expand_rate"); p.hysteresis_factor_safe_count = getOrDeclareParameter(*node, ns + "hysteresis_factor_safe_count"); + p.collision_check_yaw_diff_threshold = + getOrDeclareParameter(*node, ns + "collision_check_yaw_diff_threshold"); } // safety check predicted path params diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json index cb150514ac372..ce36cbf949637 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json @@ -931,6 +931,11 @@ "description": "Hysteresis count that be used for chattering prevention.", "default": 10 }, + "collision_check_yaw_diff_threshold": { + "type": "number", + "description": "Max yaw difference between ego and object when doing collision check", + "default": 3.1416 + }, "min_velocity": { "type": "number", "description": "Minimum velocity of the ego vehicle's predicted path.", diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp index bbdb8078aadf9..922883f289de8 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp @@ -780,7 +780,8 @@ bool StaticObstacleAvoidanceModule::isSafePath( for (const auto & obj_path : obj_predicted_paths) { if (!utils::path_safety_checker::checkCollision( shifted_path.path, ego_predicted_path, object, obj_path, p, parameters_->rss_params, - hysteresis_factor, current_debug_data.second)) { + hysteresis_factor, parameters_->collision_check_yaw_diff_threshold, + current_debug_data.second)) { utils::path_safety_checker::updateCollisionCheckDebugMap( debug.collision_check, current_debug_data, false); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp index fde54a7041672..12cb59ac46195 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp @@ -29,6 +29,7 @@ #include #include +#include #include #include #include @@ -48,6 +49,10 @@ void DynamicObstacleStopModule::init(rclcpp::Node & node, const std::string & mo node.create_publisher("~/" + ns_ + "/debug_markers", 1); virtual_wall_publisher_ = node.create_publisher("~/" + ns_ + "/virtual_walls", 1); + processing_diag_publisher_ = std::make_shared( + &node, "~/debug/" + ns_ + "/processing_time_ms_diag"); + processing_time_publisher_ = node.create_publisher( + "~/debug/" + ns_ + "/processing_time_ms", 1); using autoware::universe_utils::getOrDeclareParameter; auto & p = params_; @@ -179,6 +184,16 @@ VelocityPlanningResult DynamicObstacleStopModule::plan( debug_data_.ego_footprints = ego_data.trajectory_footprints; debug_data_.obstacle_footprints = obstacle_forward_footprints; debug_data_.z = ego_data.pose.position.z; + std::map processing_times; + processing_times["preprocessing"] = preprocessing_duration_us / 1000; + processing_times["footprints"] = footprints_duration_us / 1000; + processing_times["collisions"] = collisions_duration_us / 1000; + processing_times["Total"] = total_time_us / 1000; + processing_diag_publisher_->publish(processing_times); + tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + processing_time_msg.stamp = clock_->now(); + processing_time_msg.data = processing_times["Total"]; + processing_time_publisher_->publish(processing_time_msg); return result; } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/footprint.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/footprint.cpp index 9695146a2ad38..73189e732b312 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/footprint.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/footprint.cpp @@ -74,11 +74,14 @@ void make_ego_footprint_rtree(EgoData & ego_data, const PlannerParam & params) for (const auto & p : ego_data.trajectory) ego_data.trajectory_footprints.push_back(autoware::universe_utils::toFootprint( p.pose, params.ego_longitudinal_offset, 0.0, params.ego_lateral_offset * 2.0)); + std::vector rtree_nodes; + rtree_nodes.reserve(ego_data.trajectory_footprints.size()); for (auto i = 0UL; i < ego_data.trajectory_footprints.size(); ++i) { const auto box = boost::geometry::return_envelope( ego_data.trajectory_footprints[i]); - ego_data.rtree.insert(std::make_pair(box, i)); + rtree_nodes.push_back(std::make_pair(box, i)); } + ego_data.rtree = Rtree(rtree_nodes); } } // namespace autoware::motion_velocity_planner::dynamic_obstacle_stop diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp index 2b78a2f1495bf..eb8a33ede2c72 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp @@ -34,6 +34,7 @@ #include #include +#include namespace autoware::motion_velocity_planner { @@ -52,6 +53,10 @@ void ObstacleVelocityLimiterModule::init(rclcpp::Node & node, const std::string node.create_publisher("~/" + ns_ + "/debug_markers", 1); virtual_wall_publisher_ = node.create_publisher("~/" + ns_ + "/virtual_walls", 1); + processing_diag_publisher_ = std::make_shared( + &node, "~/debug/" + ns_ + "/processing_time_ms_diag"); + processing_time_publisher_ = node.create_publisher( + "~/debug/" + ns_ + "/processing_time_ms", 1); const auto vehicle_info = vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo(); vehicle_lateral_offset_ = static_cast(vehicle_info.max_lateral_offset_m); @@ -218,6 +223,16 @@ VelocityPlanningResult ObstacleVelocityLimiterModule::plan( safe_footprint_polygons, obstacle_masks, planner_data->current_odometry.pose.pose.position.z)); } + std::map processing_times; + processing_times["preprocessing"] = preprocessing_us / 1000; + processing_times["obstacles"] = obstacles_us / 1000; + processing_times["slowdowns"] = slowdowns_us / 1000; + processing_times["Total"] = total_us / 1000; + processing_diag_publisher_->publish(processing_times); + tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + processing_time_msg.stamp = clock_->now(); + processing_time_msg.data = processing_times["Total"]; + processing_time_publisher_->publish(processing_time_msg); return result; } } // namespace autoware::motion_velocity_planner diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp index f6d81f6f036d0..8397d0c116090 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp @@ -33,6 +33,7 @@ #include +#include #include #include #include @@ -56,6 +57,10 @@ void OutOfLaneModule::init(rclcpp::Node & node, const std::string & module_name) node.create_publisher("~/" + ns_ + "/debug_markers", 1); virtual_wall_publisher_ = node.create_publisher("~/" + ns_ + "/virtual_walls", 1); + processing_diag_publisher_ = std::make_shared( + &node, "~/debug/" + ns_ + "/processing_time_ms_diag"); + processing_time_publisher_ = node.create_publisher( + "~/debug/" + ns_ + "/processing_time_ms", 1); } void OutOfLaneModule::init_parameters(rclcpp::Node & node) { @@ -207,7 +212,7 @@ VelocityPlanningResult OutOfLaneModule::plan( inputs.ego_data = ego_data; stopwatch.tic("filter_predicted_objects"); inputs.objects = out_of_lane::filter_predicted_objects(planner_data, ego_data, params_); - const auto filter_predicted_objects_ms = stopwatch.toc("filter_predicted_objects"); + const auto filter_predicted_objects_us = stopwatch.toc("filter_predicted_objects"); inputs.route_handler = planner_data->route_handler; inputs.lanelets = other_lanelets; stopwatch.tic("calculate_decisions"); @@ -288,12 +293,26 @@ VelocityPlanningResult OutOfLaneModule::plan( "\tcalc_slowdown_points = %2.0fus\n" "\tinsert_slowdown_points = %2.0fus\n", total_time_us, calculate_lanelets_us, calculate_trajectory_footprints_us, - calculate_overlapping_ranges_us, filter_predicted_objects_ms, calculate_decisions_us, + calculate_overlapping_ranges_us, filter_predicted_objects_us, calculate_decisions_us, calc_slowdown_points_us, insert_slowdown_points_us); debug_publisher_->publish(out_of_lane::debug::create_debug_marker_array(debug_data_)); virtual_wall_marker_creator.add_virtual_walls( out_of_lane::debug::create_virtual_walls(debug_data_, params_)); virtual_wall_publisher_->publish(virtual_wall_marker_creator.create_markers(clock_->now())); + std::map processing_times; + processing_times["calculate_lanelets"] = calculate_lanelets_us / 1000; + processing_times["calculate_trajectory_footprints"] = calculate_trajectory_footprints_us / 1000; + processing_times["calculate_overlapping_ranges"] = calculate_overlapping_ranges_us / 1000; + processing_times["filter_pred_objects"] = filter_predicted_objects_us / 1000; + processing_times["calculate_decision"] = calculate_decisions_us / 1000; + processing_times["calc_slowdown_points"] = calc_slowdown_points_us / 1000; + processing_times["insert_slowdown_points"] = insert_slowdown_points_us / 1000; + processing_times["Total"] = total_time_us / 1000; + processing_diag_publisher_->publish(processing_times); + tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + processing_time_msg.stamp = clock_->now(); + processing_time_msg.data = processing_times["Total"]; + processing_time_publisher_->publish(processing_time_msg); return result; } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/plugin_module_interface.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/plugin_module_interface.hpp index 535510453b8ba..9bd662af697ea 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/plugin_module_interface.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/plugin_module_interface.hpp @@ -19,9 +19,11 @@ #include "velocity_planning_result.hpp" #include +#include #include #include +#include #include #include @@ -44,6 +46,8 @@ class PluginModuleInterface rclcpp::Logger logger_ = rclcpp::get_logger(""); rclcpp::Publisher::SharedPtr debug_publisher_; rclcpp::Publisher::SharedPtr virtual_wall_publisher_; + std::shared_ptr processing_diag_publisher_; + rclcpp::Publisher::SharedPtr processing_time_publisher_; autoware::motion_utils::VirtualWallMarkerCreator virtual_wall_marker_creator{}; }; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml index d2a3e305180d5..e428719f6e32f 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml @@ -27,6 +27,7 @@ lanelet2_extension libboost-dev rclcpp + tier4_debug_msgs tier4_planning_msgs visualization_msgs diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml index c00e0dd856200..aad25d5fb20a1 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml @@ -38,6 +38,7 @@ tf2_eigen tf2_geometry_msgs tf2_ros + tier4_debug_msgs tier4_planning_msgs visualization_msgs diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp index 714b6fd948a32..2bc820d74693b 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp @@ -14,9 +14,11 @@ #include "node.hpp" +#include #include #include #include +#include #include #include #include @@ -31,6 +33,7 @@ #include #include +#include #include #include @@ -80,6 +83,8 @@ MotionVelocityPlannerNode::MotionVelocityPlannerNode(const rclcpp::NodeOptions & velocity_factor_publisher_ = this->create_publisher( "~/output/velocity_factors", 1); + processing_time_publisher_ = this->create_publisher( + "~/debug/total_time/processing_time_ms", 1); // Parameters smooth_velocity_before_planning_ = declare_parameter("smooth_velocity_before_planning"); @@ -103,8 +108,6 @@ MotionVelocityPlannerNode::MotionVelocityPlannerNode(const rclcpp::NodeOptions & std::bind(&MotionVelocityPlannerNode::on_set_param, this, std::placeholders::_1)); logger_configure_ = std::make_unique(this); - published_time_publisher_ = - std::make_unique(this); } void MotionVelocityPlannerNode::on_load_plugin( @@ -252,9 +255,14 @@ void MotionVelocityPlannerNode::on_trajectory( { std::unique_lock lk(mutex_); + autoware::universe_utils::StopWatch stop_watch; + std::map processing_times; + stop_watch.tic("Total"); + if (!update_planner_data()) { return; } + processing_times["update_planner_data"] = stop_watch.toc(true); if (input_trajectory_msg->points.empty()) { RCLCPP_WARN(get_logger(), "Input trajectory message is empty"); @@ -264,6 +272,7 @@ void MotionVelocityPlannerNode::on_trajectory( if (has_received_map_) { planner_data_.route_handler = std::make_shared(*map_ptr_); has_received_map_ = false; + processing_times["make_RouteHandler"] = stop_watch.toc(true); } autoware::motion_velocity_planner::TrajectoryPoints input_trajectory_points{ @@ -271,12 +280,19 @@ void MotionVelocityPlannerNode::on_trajectory( auto output_trajectory_msg = generate_trajectory(input_trajectory_points); output_trajectory_msg.header = input_trajectory_msg->header; + processing_times["generate_trajectory"] = stop_watch.toc(true); lk.unlock(); trajectory_pub_->publish(output_trajectory_msg); - published_time_publisher_->publish_if_subscribed( + published_time_publisher_.publish_if_subscribed( trajectory_pub_, output_trajectory_msg.header.stamp); + processing_times["Total"] = stop_watch.toc("Total"); + processing_diag_publisher_.publish(processing_times); + tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + processing_time_msg.stamp = get_clock()->now(); + processing_time_msg.data = processing_times["Total"]; + processing_time_publisher_->publish(processing_time_msg); } void MotionVelocityPlannerNode::insert_stop( @@ -365,9 +381,11 @@ autoware_planning_msgs::msg::Trajectory MotionVelocityPlannerNode::generate_traj output_trajectory_msg.points = {input_trajectory_points.begin(), input_trajectory_points.end()}; if (smooth_velocity_before_planning_) input_trajectory_points = smooth_trajectory(input_trajectory_points, planner_data_); + const auto resampled_trajectory = + autoware::motion_utils::resampleTrajectory(output_trajectory_msg, 0.5); const auto planning_results = planner_manager_.plan_velocities( - input_trajectory_points, std::make_shared(planner_data_)); + resampled_trajectory.points, std::make_shared(planner_data_)); autoware_adapi_v1_msgs::msg::VelocityFactorArray velocity_factors; velocity_factors.header.frame_id = "map"; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp index 7b771d0b04442..8debb9cbedf00 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp @@ -32,6 +32,7 @@ #include #include #include +#include #include #include @@ -96,6 +97,9 @@ class MotionVelocityPlannerNode : public rclcpp::Node rclcpp::Publisher::SharedPtr debug_viz_pub_; rclcpp::Publisher::SharedPtr velocity_factor_publisher_; + autoware::universe_utils::ProcessingTimePublisher processing_diag_publisher_{this}; + rclcpp::Publisher::SharedPtr processing_time_publisher_; + autoware::universe_utils::PublishedTimePublisher published_time_publisher_{this}; // parameters rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr set_param_callback_; @@ -139,8 +143,6 @@ class MotionVelocityPlannerNode : public rclcpp::Node autoware::motion_velocity_planner::TrajectoryPoints input_trajectory_points); std::unique_ptr logger_configure_; - - std::unique_ptr published_time_publisher_; }; } // namespace autoware::motion_velocity_planner diff --git a/sensing/image_transport_decompressor/CMakeLists.txt b/sensing/image_transport_decompressor/CMakeLists.txt index 2359a327abb8c..2ecd3446290df 100644 --- a/sensing/image_transport_decompressor/CMakeLists.txt +++ b/sensing/image_transport_decompressor/CMakeLists.txt @@ -24,4 +24,5 @@ rclcpp_components_register_node(image_transport_decompressor ament_auto_package(INSTALL_TO_SHARE launch + config ) diff --git a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp index c908ca1a62ee2..7d7deb8c4a504 100644 --- a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp +++ b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp @@ -148,7 +148,8 @@ class MrmHandler : public rclcpp::Node bool isStopped(); bool isDrivingBackwards(); bool isEmergency() const; - bool isAutonomous(); + bool isControlModeAutonomous(); + bool isOperationModeAutonomous(); bool isPullOverStatusAvailable(); bool isComfortableStopStatusAvailable(); bool isEmergencyStopStatusAvailable(); diff --git a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp index 326fbb392fd35..47cbb0ae7d016 100644 --- a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp +++ b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp @@ -396,12 +396,13 @@ void MrmHandler::updateMrmState() } // Get mode - const bool is_auto_mode = isAutonomous(); + const bool is_control_mode_autonomous = isControlModeAutonomous(); + const bool is_operation_mode_autonomous = isOperationModeAutonomous(); // State Machine switch (mrm_state_.state) { case MrmState::NORMAL: - if (is_auto_mode) { + if (is_control_mode_autonomous && is_operation_mode_autonomous) { transitionTo(MrmState::MRM_OPERATING); } return; @@ -537,7 +538,7 @@ bool MrmHandler::isEmergency() const is_operation_mode_availability_timeout; } -bool MrmHandler::isAutonomous() +bool MrmHandler::isControlModeAutonomous() { using autoware_vehicle_msgs::msg::ControlModeReport; auto mode = sub_control_mode_.takeData(); @@ -545,6 +546,14 @@ bool MrmHandler::isAutonomous() return mode->mode == ControlModeReport::AUTONOMOUS; } +bool MrmHandler::isOperationModeAutonomous() +{ + using autoware_adapi_v1_msgs::msg::OperationModeState; + auto state = sub_operation_mode_state_.takeData(); + if (state == nullptr) return false; + return state->mode == OperationModeState::AUTONOMOUS; +} + bool MrmHandler::isPullOverStatusAvailable() { auto status = sub_mrm_pull_over_status_.takeData(); diff --git a/vehicle/autoware_vehicle_info_utils/include/autoware_vehicle_info_utils/vehicle_info.hpp b/vehicle/autoware_vehicle_info_utils/include/autoware_vehicle_info_utils/vehicle_info.hpp index e951f11e49ab7..923fc53055475 100644 --- a/vehicle/autoware_vehicle_info_utils/include/autoware_vehicle_info_utils/vehicle_info.hpp +++ b/vehicle/autoware_vehicle_info_utils/include/autoware_vehicle_info_utils/vehicle_info.hpp @@ -50,6 +50,9 @@ struct VehicleInfo autoware::universe_utils::LinearRing2d createFootprint(const double margin = 0.0) const; autoware::universe_utils::LinearRing2d createFootprint( const double lat_margin, const double lon_margin) const; + + double calcMaxCurvature() const; + double calcCurvatureFromSteerAngle(const double steer_angle) const; }; /// Create vehicle info from base parameters diff --git a/vehicle/autoware_vehicle_info_utils/src/vehicle_info.cpp b/vehicle/autoware_vehicle_info_utils/src/vehicle_info.cpp index db223663f1145..2dd5b19f2f523 100644 --- a/vehicle/autoware_vehicle_info_utils/src/vehicle_info.cpp +++ b/vehicle/autoware_vehicle_info_utils/src/vehicle_info.cpp @@ -85,4 +85,30 @@ VehicleInfo createVehicleInfo( }; } +double VehicleInfo::calcMaxCurvature() const +{ + if (std::abs(wheel_base_m) < 1e-6) { + throw std::invalid_argument("wheel_base_m is 0."); + } + if (std::abs(max_steer_angle_rad) < 1e-6) { + return std::numeric_limits::max(); + } + + const double radius = wheel_base_m / std::tan(max_steer_angle_rad); + const double curvature = 1.0 / radius; + return curvature; +} +double VehicleInfo::calcCurvatureFromSteerAngle(const double steer_angle) const +{ + if (std::abs(wheel_base_m) < 1e-6) { + throw std::invalid_argument("wheel_base_m is 0."); + } + if (std::abs(steer_angle) < 1e-6) { + return std::numeric_limits::max(); + } + + const double radius = wheel_base_m / std::tan(steer_angle); + const double curvature = 1.0 / radius; + return curvature; +} } // namespace autoware::vehicle_info_utils