From 28f5b18e71c0458e1d27067bfa2f608c2b682012 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Fri, 8 Sep 2023 10:40:40 +0900 Subject: [PATCH 01/11] docs(planning_debug_tools): rename readme to README (#4222) Signed-off-by: Takamasa Horibe --- planning/planning_debug_tools/{Readme.md => README.md} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename planning/planning_debug_tools/{Readme.md => README.md} (100%) diff --git a/planning/planning_debug_tools/Readme.md b/planning/planning_debug_tools/README.md similarity index 100% rename from planning/planning_debug_tools/Readme.md rename to planning/planning_debug_tools/README.md From cd3956e437e8d85c18ed6bde2c826fd6036095af Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Fri, 8 Sep 2023 11:00:38 +0900 Subject: [PATCH 02/11] docs(operation_transition_manager): update readme (#4888) * docs(operation_transition_manager): update readme Signed-off-by: Takamasa Horibe * precommit Signed-off-by: Takamasa Horibe --------- Signed-off-by: Takamasa Horibe --- .../README.md | 22 +- .../transition_detailed_structure.drawio.svg | 610 ++++++++++++++++++ .../transition_rough_structure.drawio.svg | 321 +++++++++ 3 files changed, 952 insertions(+), 1 deletion(-) create mode 100644 control/operation_mode_transition_manager/image/transition_detailed_structure.drawio.svg create mode 100644 control/operation_mode_transition_manager/image/transition_rough_structure.drawio.svg diff --git a/control/operation_mode_transition_manager/README.md b/control/operation_mode_transition_manager/README.md index 73249095c6d5c..3cb07a9bd7821 100644 --- a/control/operation_mode_transition_manager/README.md +++ b/control/operation_mode_transition_manager/README.md @@ -25,7 +25,27 @@ There is also an `In Transition` state that occurs during each mode transitions. ## Design - +A rough design of the relationship between `operation_mode_transition_manager`` and the other nodes is shown below. + +![transition_rough_structure](image/transition_rough_structure.drawio.svg) + +A more detailed structure is below. + +![transition_detailed_structure](image/transition_detailed_structure.drawio.svg) + +Here we see that `operation_mode_transition_manager` has multiple state transitions as follows + +- **AUTOWARE ENABLED <---> DISABLED** + - **ENABLED**: the vehicle is controlled by Autoware. + - **DISABLED**: the vehicle is out of Autoware control, expecting the e.g. manual driving. +- **AUTOWARE ENABLED <---> AUTO/LOCAL/REMOTE/NONE** + - **AUTO**: the vehicle is controlled by Autoware, with the autonomous control command calculated by the planning/control component. + - **LOCAL**: the vehicle is controlled by Autoware, with the locally connected operator, e.g. joystick controller. + - **REMOTE**: the vehicle is controlled by Autoware, with the remotely connected operator. + - **NONE**: the vehicle is not controlled by any operator. +- **IN TRANSITION <---> COMPLETED** + - **IN TRANSITION**: the mode listed above is in the transition process, expecting the former operator to have a responsibility to confirm the transition is completed. + - **COMPLETED**: the mode transition is completed. ## Inputs / Outputs / API diff --git a/control/operation_mode_transition_manager/image/transition_detailed_structure.drawio.svg b/control/operation_mode_transition_manager/image/transition_detailed_structure.drawio.svg new file mode 100644 index 0000000000000..3492dca6dd86b --- /dev/null +++ b/control/operation_mode_transition_manager/image/transition_detailed_structure.drawio.svg @@ -0,0 +1,610 @@ + + + + + + + + + + + + +
+
+
+ Control Command +
+ (Auto) +
+
+
+ + Control Command... + + + + + + +
+
+
+ Trajectory Follower +
+
+
+ + Trajectory Follower + + + + + + + + + +
+
+
+ Control Command +
+
+
+ + Control Command + + + + + + +
+
+
+ Vehicle Cmd Gate +
+
+
+ + Vehicle Cmd Gate + + + + + + +
+
+
+ Vehicle Interface +
+
+
+ + Vehicle Interface + + + + + + + +
+
+
+ Current Mode +
+
+
+ + Current Mode + + + + + + + +
+
+
+ Auto mode +
+ Availability +
+
+
+ + Auto mode... + + + + + + + +
+
+
+ Mode Change +
+ Request +
+
+
+ + Mode Change... + + + + + + +
+
+
+ Operation Mode +
+ Transition Manager +
+
+
+ + Operation Mode... + + + + + + + +
+
+
+ Mode Change +
+ Request +
+
+
+ + Mode Change... + + + + + + +
+
+
+ API +
+
+
+ + API + + + + + + +
+
+
+ Trajectory (From Planning) +
+
+
+ + Trajectory (From Planning) + + + + + + + +
+
+
+ Mode Change +
+ Response +
+
+
+ + Mode Change... + + + + + + + + + + +
+
+
+ Ego pose, twist, acceleration (From Localization) +
+
+
+ + Ego pose, twist, accelera... + + + + + + + +
+
+
+ External Control Command +
+
+
+ + External C... + + + + + + +
+
+
+ AUTOWARE ENABLED +
+
+
+ + AUTOWARE ENABLED + + + + + + +
+
+
+ REMOTE +
+
+
+ + REMOTE + + + + + + + + + +
+
+
+ NONE +
+
+
+ + NONE + + + + + + + + + + + + +
+
+
+ AUTO +
+
+
+ + AUTO + + + + + + + + + + + + +
+
+
+ LOCAL +
+
+
+ + LOCAL + + + + + + + + + +
+
+
+ + AUTOWARE +
+ DISABLED + +
+
+
+ + AUTOWARE... + + + + + + + + + +
+
+
+ TRANSITION +
+
+
+ + TRANSITION + + + + + + +
+
+
+ IN TRANSITION +
+
+
+ + IN TRANSITION + + + + + + +
+
+
+ COMPLETED +
+
+
+ + COMPLETED + + + + + + + + + + + +
+
+
+ + Vehicle Control mode +
+ (From Vehicle) + +
+
+
+ + Vehicle Control mode... + + + + + + +
+
+
+ + Control Component + +
+
+
+ + Control Component + + + + + + +
+
+
+ External Command +
+ Selector +
+
+
+ + External Command... + + + + + + + +
+
+
+ Control Command +
+ (Local) +
+
+
+ + Control Co... + + + + + + + +
+
+
+ Control Command +
+ (Remote) +
+
+
+ + Control Command... + + + + + + + Text is not SVG - cannot display + + + diff --git a/control/operation_mode_transition_manager/image/transition_rough_structure.drawio.svg b/control/operation_mode_transition_manager/image/transition_rough_structure.drawio.svg new file mode 100644 index 0000000000000..073a2315355be --- /dev/null +++ b/control/operation_mode_transition_manager/image/transition_rough_structure.drawio.svg @@ -0,0 +1,321 @@ + + + + + + + + + + + +
+
+
+ Control Command +
+
+
+ + Control Command + + + + + + +
+
+
+ Trajectory Follower +
+
+
+ + Trajectory Follower + + + + + + + +
+
+
+ Control Command +
+
+
+ + Control Command + + + + + + +
+
+
+ Vehicle Cmd Gate +
+
+
+ + Vehicle Cmd Gate + + + + + + +
+
+
+ Vehicle Interface +
+
+
+ + Vehicle Interface + + + + + + + +
+
+
+ Current Mode +
+
+
+ + Current Mode + + + + + + + +
+
+
+ Auto mode +
+ Availability +
+
+
+ + Auto mode... + + + + + + + +
+
+
+ Mode Change +
+ Request +
+
+
+ + Mode Change... + + + + + + +
+
+
+ Operation Mode +
+ Transition Manager +
+
+
+ + Operation Mode... + + + + + + + +
+
+
+ Mode Change +
+ Request +
+
+
+ + Mode Change... + + + + + + +
+
+
+ API +
+
+
+ + API + + + + + + + + +
+
+
+ Change the control mode (e.g. Auto/Manual) for the request, follow the control command in the Auto mode +
+
+
+ + Change the control mode (e.g. Auto/Ma... + + + + + + +
+
+
+ Manage the operation mode transition (e.g. Manual/Auto). +
+ It also rejects the request based on the driving conditrion, checks the transition is complited. +
+
+
+ + Manage the operation mode transition... + + + + + + + + +
+
+
+ Manage the operation mode transition (e.g. Manual/Auto). +
+ It also rejects the request, checks the transition is complited. +
+
+
+ + Manage the operation mode transition... + + + + + + +
+
+
+ Apply filter based on the current operation mode (e.g. Manual/Auto, in the process of the transition) +
+
+
+ + Apply filter based on t... + + + + + + + + + + + Text is not SVG - cannot display + + + From b498c5352390e3637fb723140bf7b541d5bb04a8 Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Fri, 8 Sep 2023 11:47:18 +0900 Subject: [PATCH 03/11] feat(ndt_scan_matcher): add diag lidar_topic_delay_time_sec (#4815) * feat(ndt_scan_matcher): add diag lidar_topic_delay_time_sec Signed-off-by: yamato-ando * fix param Signed-off-by: yamato-ando * fix stoi to stod Signed-off-by: yamato-ando * style(pre-commit): autofix * update readme Signed-off-by: yamato-ando --------- Signed-off-by: yamato-ando Co-authored-by: yamato-ando Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- localization/ndt_scan_matcher/README.md | 1 + .../config/ndt_scan_matcher.param.yaml | 3 ++ .../ndt_scan_matcher_core.hpp | 1 + .../src/ndt_scan_matcher_core.cpp | 30 ++++++++++++++++++- 4 files changed, 34 insertions(+), 1 deletion(-) diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index b30cbaca1e87a..fcbe4804ca7d2 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -64,6 +64,7 @@ One optional function is regularization. Please see the regularization chapter i | `max_iterations` | int | The number of iterations required to calculate alignment | | `converged_param_type` | int | The type of indicators for scan matching score (0: TP, 1: NVTL) | | `converged_param_transform_probability` | double | Threshold for deciding whether to trust the estimation result | +| `lidar_topic_timeout_sec` | double | Tolerance of timestamp difference between current time and sensor pointcloud | | `num_threads` | int | Number of threads used for parallel computing | (TP: Transform Probability, NVTL: Nearest Voxel Transform Probability) diff --git a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml index a5d8142b6616e..b5affd72b2158 100644 --- a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml +++ b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml @@ -37,6 +37,9 @@ # The number of particles to estimate initial pose initial_estimate_particles_num: 100 + # Tolerance of timestamp difference between current time and sensor pointcloud. [sec] + lidar_topic_timeout_sec: 1.0 + # Tolerance of timestamp difference between initial_pose and sensor pointcloud. [sec] initial_pose_timeout_sec: 1.0 diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp index 8657e354c728a..ed3b99019f7d9 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp @@ -179,6 +179,7 @@ class NDTScanMatcher : public rclcpp::Node double converged_param_nearest_voxel_transformation_likelihood_; int initial_estimate_particles_num_; + double lidar_topic_timeout_sec_; double initial_pose_timeout_sec_; double initial_pose_distance_tolerance_m_; float inversion_vector_threshold_; diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index 154fc8526f512..714f8f65bad37 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -91,6 +91,7 @@ NDTScanMatcher::NDTScanMatcher() converged_param_transform_probability_(4.5), converged_param_nearest_voxel_transformation_likelihood_(2.3), initial_estimate_particles_num_(100), + lidar_topic_timeout_sec_(1.0), initial_pose_timeout_sec_(1.0), initial_pose_distance_tolerance_m_(10.0), inversion_vector_threshold_(-0.9), @@ -141,6 +142,9 @@ NDTScanMatcher::NDTScanMatcher() "converged_param_nearest_voxel_transformation_likelihood", converged_param_nearest_voxel_transformation_likelihood_); + lidar_topic_timeout_sec_ = + this->declare_parameter("lidar_topic_timeout_sec", lidar_topic_timeout_sec_); + initial_pose_timeout_sec_ = this->declare_parameter("initial_pose_timeout_sec", initial_pose_timeout_sec_); @@ -268,6 +272,12 @@ void NDTScanMatcher::timer_diagnostic() diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; diag_status_msg.message += "Initializing State. "; } + if ( + state_ptr_->count("lidar_topic_delay_time_sec") && + std::stod((*state_ptr_)["lidar_topic_delay_time_sec"]) > lidar_topic_timeout_sec_) { + diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + diag_status_msg.message += "lidar_topic_delay_time_sec exceed limit. "; + } if ( state_ptr_->count("skipping_publish_num") && std::stoi((*state_ptr_)["skipping_publish_num"]) > 1 && @@ -347,11 +357,29 @@ void NDTScanMatcher::callback_sensor_points( return; } + const rclcpp::Time sensor_ros_time = sensor_points_msg_in_sensor_frame->header.stamp; + const double lidar_topic_delay_time_sec = (this->now() - sensor_ros_time).seconds(); + (*state_ptr_)["lidar_topic_delay_time_sec"] = std::to_string(lidar_topic_delay_time_sec); + + if (lidar_topic_delay_time_sec > lidar_topic_timeout_sec_) { + RCLCPP_WARN( + this->get_logger(), + "The LiDAR topic is experiencing latency. The delay time is %lf[sec] (the tolerance is " + "%lf[sec])", + lidar_topic_delay_time_sec, lidar_topic_timeout_sec_); + + // If the delay time of the LiDAR topic exceeds the delay compensation time of ekf_localizer, + // even if further processing continues, the estimated result will be rejected by ekf_localizer. + // Therefore, it would be acceptable to exit the function here. + // However, for now, we will continue the processing as it is. + + // return; + } + // mutex ndt_ptr_ std::lock_guard lock(ndt_ptr_mtx_); const auto exe_start_time = std::chrono::system_clock::now(); - const rclcpp::Time sensor_ros_time = sensor_points_msg_in_sensor_frame->header.stamp; // preprocess input pointcloud pcl::shared_ptr> sensor_points_in_sensor_frame( From 61fef17c37d25525ce8ce845c21e5ee7a57c2c98 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Fri, 8 Sep 2023 20:08:51 +0900 Subject: [PATCH 04/11] feat(planning_debug_tools): support old auto TrafficSignalArray (#4910) * feat(planning_debug_tools): support old auto TrafficSignalArray Signed-off-by: kosuke55 * use is_auto flag Signed-off-by: kosuke55 --------- Signed-off-by: kosuke55 --- .../perception_replayer/perception_replayer.py | 17 +++++++++++++---- .../perception_replayer_common.py | 17 ++++++++++++++--- .../perception_reproducer.py | 17 +++++++++++++---- 3 files changed, 40 insertions(+), 11 deletions(-) diff --git a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py index 9d8fa4a32a93d..a9abc5be60761 100755 --- a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py +++ b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py @@ -92,13 +92,22 @@ def on_timer(self): self.objects_pub.publish(objects_msg) # traffic signals + # temporary support old auto msgs if traffic_signals_msg: - traffic_signals_msg.stamp = timestamp - self.traffic_signals_pub.publish(traffic_signals_msg) + if self.is_auto_traffic_signals: + traffic_signals_msg.header.stamp = timestamp + self.auto_traffic_signals_pub.publish(traffic_signals_msg) + else: + traffic_signals_msg.stamp = timestamp + self.traffic_signals_pub.publish(traffic_signals_msg) self.prev_traffic_signals_msg = traffic_signals_msg elif self.prev_traffic_signals_msg: - self.prev_traffic_signals_msg.stamp = timestamp - self.traffic_signals_pub.publish(self.prev_traffic_signals_msg) + if self.is_auto_traffic_signals: + self.prev_traffic_signals_msg.header.stamp = timestamp + self.auto_traffic_signals_pub.publish(self.prev_traffic_signals_msg) + else: + self.prev_traffic_signals_msg.stamp = timestamp + self.traffic_signals_pub.publish(self.prev_traffic_signals_msg) def onPushed(self, event): if self.widget.button.isChecked(): diff --git a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py index 7774a8c82daf6..33cf8ca1f16ee 100644 --- a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py +++ b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py @@ -22,6 +22,7 @@ from autoware_auto_perception_msgs.msg import DetectedObjects from autoware_auto_perception_msgs.msg import PredictedObjects from autoware_auto_perception_msgs.msg import TrackedObjects +from autoware_auto_perception_msgs.msg import TrafficSignalArray as AutoTrafficSignalArray from autoware_perception_msgs.msg import TrafficSignalArray from nav_msgs.msg import Odometry import psutil @@ -65,9 +66,6 @@ def __init__(self, args, name): self.pointcloud_pub = self.create_publisher( PointCloud2, "/perception/obstacle_segmentation/pointcloud", 1 ) - self.traffic_signals_pub = self.create_publisher( - TrafficSignalArray, "/perception/traffic_light_recognition/traffic_signals", 1 - ) # load rosbag print("Stared loading rosbag") @@ -78,6 +76,16 @@ def __init__(self, args, name): self.load_rosbag(args.bag) print("Ended loading rosbag") + # temporary support old auto msgs + if self.is_auto_traffic_signals: + self.auto_traffic_signals_pub = self.create_publisher( + AutoTrafficSignalArray, "/perception/traffic_light_recognition/traffic_signals", 1 + ) + else: + self.traffic_signals_pub = self.create_publisher( + TrafficSignalArray, "/perception/traffic_light_recognition/traffic_signals", 1 + ) + # wait for ready to publish/subscribe time.sleep(1.0) @@ -113,6 +121,9 @@ def load_rosbag(self, rosbag2_path: str): self.rosbag_ego_odom_data.append((stamp, msg)) if topic == traffic_signals_topic: self.rosbag_traffic_signals_data.append((stamp, msg)) + self.is_auto_traffic_signals = ( + "autoware_auto_perception_msgs" in type(msg).__module__ + ) def kill_online_perception_node(self): # kill node if required diff --git a/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py b/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py index 5344e622be38e..1b75c86de2709 100755 --- a/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py +++ b/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py @@ -66,13 +66,22 @@ def on_timer(self): self.objects_pub.publish(objects_msg) # traffic signals + # temporary support old auto msgs if traffic_signals_msg: - traffic_signals_msg.stamp = timestamp - self.traffic_signals_pub.publish(traffic_signals_msg) + if self.is_auto_traffic_signals: + traffic_signals_msg.header.stamp = timestamp + self.auto_traffic_signals_pub.publish(traffic_signals_msg) + else: + traffic_signals_msg.stamp = timestamp + self.traffic_signals_pub.publish(traffic_signals_msg) self.prev_traffic_signals_msg = traffic_signals_msg elif self.prev_traffic_signals_msg: - self.prev_traffic_signals_msg.stamp = timestamp - self.traffic_signals_pub.publish(self.prev_traffic_signals_msg) + if self.is_auto_traffic_signals: + self.prev_traffic_signals_msg.header.stamp = timestamp + self.auto_traffic_signals_pub.publish(self.prev_traffic_signals_msg) + else: + self.prev_traffic_signals_msg.stamp = timestamp + self.traffic_signals_pub.publish(self.prev_traffic_signals_msg) def find_nearest_ego_odom_by_observation(self, ego_pose): if self.ego_pose_idx: From 49f3bd6cd28ce1d797af15d27d2b7085374a237f Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Fri, 8 Sep 2023 21:37:24 +0900 Subject: [PATCH 05/11] feat(planning_debug_tools): add recorded ego pose publisher to perception replayer (#4906) Signed-off-by: kosuke55 --- .../perception_replayer.py | 53 +++++++++++++++++++ .../perception_replayer_common.py | 2 + .../time_manager_widget.py | 4 +- 3 files changed, 58 insertions(+), 1 deletion(-) diff --git a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py index a9abc5be60761..f1015be07fd7c 100755 --- a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py +++ b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py @@ -20,6 +20,7 @@ import sys from PyQt5.QtWidgets import QApplication +from geometry_msgs.msg import PoseWithCovarianceStamped from perception_replayer_common import PerceptionReplayerCommon import rclpy from time_manager_widget import TimeManagerWidget @@ -43,6 +44,7 @@ def __init__(self, args): self.widget.button.clicked.connect(self.onPushed) for button in self.widget.rate_button: button.clicked.connect(functools.partial(self.onSetRate, button)) + self.widget.pub_recorded_ego_pose_button.clicked.connect(self.publish_recorded_ego_pose) # start timer callback self.delta_time = 0.1 @@ -118,6 +120,57 @@ def onPushed(self, event): def onSetRate(self, button): self.rate = float(button.text()) + def publish_recorded_ego_pose(self): + ego_odom = self.find_ego_odom_by_timestamp(self.bag_timestamp) + if not ego_odom: + return + + recorded_ego_pose = PoseWithCovarianceStamped() + recorded_ego_pose.header.stamp = self.get_clock().now().to_msg() + recorded_ego_pose.header.frame_id = "map" + recorded_ego_pose.pose.pose = ego_odom.pose.pose + recorded_ego_pose.pose.covariance = [ + 0.25, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.25, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.06853892326654787, + ] + + self.recorded_ego_pub.publish(recorded_ego_pose) + print("Published recorded ego pose as /initialpose") + if __name__ == "__main__": parser = argparse.ArgumentParser() diff --git a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py index 33cf8ca1f16ee..f59270a9740af 100644 --- a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py +++ b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py @@ -24,6 +24,7 @@ from autoware_auto_perception_msgs.msg import TrackedObjects from autoware_auto_perception_msgs.msg import TrafficSignalArray as AutoTrafficSignalArray from autoware_perception_msgs.msg import TrafficSignalArray +from geometry_msgs.msg import PoseWithCovarianceStamped from nav_msgs.msg import Odometry import psutil from rclpy.node import Node @@ -66,6 +67,7 @@ def __init__(self, args, name): self.pointcloud_pub = self.create_publisher( PointCloud2, "/perception/obstacle_segmentation/pointcloud", 1 ) + self.recorded_ego_pub = self.create_publisher(PoseWithCovarianceStamped, "/initialpose", 1) # load rosbag print("Stared loading rosbag") diff --git a/planning/planning_debug_tools/scripts/perception_replayer/time_manager_widget.py b/planning/planning_debug_tools/scripts/perception_replayer/time_manager_widget.py index 382e725b114cd..a1d87a8b06266 100644 --- a/planning/planning_debug_tools/scripts/perception_replayer/time_manager_widget.py +++ b/planning/planning_debug_tools/scripts/perception_replayer/time_manager_widget.py @@ -89,13 +89,15 @@ def setupUI(self): self.button.setCheckable(True) self.button.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding) self.grid_layout.addWidget(self.button, 1, 0, 1, -1) + self.pub_recorded_ego_pose_button = QPushButton("publish recorded ego pose") + self.grid_layout.addWidget(self.pub_recorded_ego_pose_button, 2, 0, 1, -1) # slider self.slider = QJumpSlider(QtCore.Qt.Horizontal, self.max_value) self.slider.setMinimum(0) self.slider.setMaximum(self.max_value) self.slider.setValue(0) - self.grid_layout.addWidget(self.slider, 2, 0, 1, -1) + self.grid_layout.addWidget(self.slider, 3, 0, 1, -1) self.setCentralWidget(self.central_widget) From 85f8db381fee9cd4b5168fa6f15ca9c0bdfd463a Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Sat, 9 Sep 2023 14:13:55 +0900 Subject: [PATCH 06/11] fix(tier4_planning_rviz_plugin): update vehicle info parameters in panel received from global parameter (#4907) Signed-off-by: Takayuki Murooka --- .../tier4_planning_rviz_plugin/include/path/display_base.hpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/common/tier4_planning_rviz_plugin/include/path/display_base.hpp b/common/tier4_planning_rviz_plugin/include/path/display_base.hpp index 55c7a9c53d72d..ccedcceefaaf9 100644 --- a/common/tier4_planning_rviz_plugin/include/path/display_base.hpp +++ b/common/tier4_planning_rviz_plugin/include/path/display_base.hpp @@ -506,6 +506,10 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay vehicle_footprint_info_ = std::make_shared( vehicle_info_->vehicle_length_m, vehicle_info_->vehicle_width_m, vehicle_info_->rear_overhang_m); + + property_vehicle_length_.setValue(vehicle_info_->vehicle_length_m); + property_vehicle_width_.setValue(vehicle_info_->vehicle_width_m); + property_rear_overhang_.setValue(vehicle_info_->rear_overhang_m); } else { const float length{property_vehicle_length_.getFloat()}; const float width{property_vehicle_width_.getFloat()}; From 38b9e1c9d9e9593d379e3dd9882b1a363cfadbb4 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Sat, 9 Sep 2023 14:14:14 +0900 Subject: [PATCH 07/11] feat(surround_obstacle_checker): make parameters more tunable (#4925) * feat(surround_obstacle_checker): make parameters more tunable Signed-off-by: Takayuki Murooka * update Signed-off-by: Takayuki Murooka * update param Signed-off-by: Takayuki Murooka * update Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../surround_obstacle_checker.param.yaml | 59 +++++- .../debug_marker.hpp | 24 ++- .../surround_obstacle_checker/node.hpp | 25 ++- .../src/debug_marker.cpp | 86 +++++---- .../surround_obstacle_checker/src/node.cpp | 180 ++++++++++++++---- 5 files changed, 282 insertions(+), 92 deletions(-) diff --git a/planning/surround_obstacle_checker/config/surround_obstacle_checker.param.yaml b/planning/surround_obstacle_checker/config/surround_obstacle_checker.param.yaml index 10b5c2814e2a8..5ec10572ffe83 100644 --- a/planning/surround_obstacle_checker/config/surround_obstacle_checker.param.yaml +++ b/planning/surround_obstacle_checker/config/surround_obstacle_checker.param.yaml @@ -1,12 +1,61 @@ /**: ros__parameters: - # obstacle check - use_pointcloud: true # use pointcloud as obstacle check - use_dynamic_object: true # use dynamic object as obstacle check - surround_check_distance: 0.5 # if objects exist in this distance, transit to "exist-surrounding-obstacle" status [m] - surround_check_recover_distance: 0.8 # if no object exists in this distance, transit to "non-surrounding-obstacle" status [m] + # surround_check_*_distance: if objects exist in this distance, transit to "exist-surrounding-obstacle" status [m] + # surround_check_hysteresis_distance: if no object exists in this hysteresis distance added to the above distance, transit to "non-surrounding-obstacle" status [m] + pointcloud: + enable_check: false + surround_check_front_distance: 0.5 + surround_check_side_distance: 0.5 + surround_check_back_distance: 0.5 + unknown: + enable_check: true + surround_check_front_distance: 0.5 + surround_check_side_distance: 0.5 + surround_check_back_distance: 0.5 + car: + enable_check: true + surround_check_front_distance: 0.5 + surround_check_side_distance: 0.0 + surround_check_back_distance: 0.5 + truck: + enable_check: true + surround_check_front_distance: 0.5 + surround_check_side_distance: 0.0 + surround_check_back_distance: 0.5 + bus: + enable_check: true + surround_check_front_distance: 0.5 + surround_check_side_distance: 0.0 + surround_check_back_distance: 0.5 + trailer: + enable_check: true + surround_check_front_distance: 0.5 + surround_check_side_distance: 0.0 + surround_check_back_distance: 0.5 + motorcycle: + enable_check: true + surround_check_front_distance: 0.5 + surround_check_side_distance: 0.0 + surround_check_back_distance: 0.5 + bicycle: + enable_check: true + surround_check_front_distance: 0.5 + surround_check_side_distance: 0.5 + surround_check_back_distance: 0.5 + pedestrian: + enable_check: true + surround_check_front_distance: 0.5 + surround_check_side_distance: 0.5 + surround_check_back_distance: 0.5 + + surround_check_hysteresis_distance: 0.3 + state_clear_time: 2.0 + # ego stop state + stop_state_ego_speed: 0.1 #[m/s] + # debug publish_debug_footprints: true # publish vehicle footprint & footprints with surround_check_distance and surround_check_recover_distance offsets + debug_footprint_label: "car" diff --git a/planning/surround_obstacle_checker/include/surround_obstacle_checker/debug_marker.hpp b/planning/surround_obstacle_checker/include/surround_obstacle_checker/debug_marker.hpp index bd0fe48bbda28..590f026942801 100644 --- a/planning/surround_obstacle_checker/include/surround_obstacle_checker/debug_marker.hpp +++ b/planning/surround_obstacle_checker/include/surround_obstacle_checker/debug_marker.hpp @@ -16,6 +16,7 @@ #define SURROUND_OBSTACLE_CHECKER__DEBUG_MARKER_HPP_ #include +#include #include #include @@ -38,6 +39,7 @@ using geometry_msgs::msg::PolygonStamped; using tier4_planning_msgs::msg::StopFactor; using tier4_planning_msgs::msg::StopReason; using tier4_planning_msgs::msg::StopReasonArray; +using vehicle_info_util::VehicleInfo; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; @@ -52,15 +54,19 @@ class SurroundObstacleCheckerDebugNode { public: explicit SurroundObstacleCheckerDebugNode( - const Polygon2d & ego_polygon, const double base_link2front, - const double & surround_check_distance, const double & surround_check_recover_distance, - const geometry_msgs::msg::Pose & self_pose, const rclcpp::Clock::SharedPtr clock, - rclcpp::Node & node); + const vehicle_info_util::VehicleInfo & vehicle_info, const double base_link2front, + const std::string & object_label, const double & surround_check_front_distance, + const double & surround_check_side_distance, const double & surround_check_back_distance, + const double & surround_check_hysteresis_distance, const geometry_msgs::msg::Pose & self_pose, + const rclcpp::Clock::SharedPtr clock, rclcpp::Node & node); bool pushPose(const geometry_msgs::msg::Pose & pose, const PoseType & type); bool pushObstaclePoint(const geometry_msgs::msg::Point & obstacle_point, const PointType & type); void publish(); void publishFootprints(); + void updateFootprintMargin( + const std::string & object_label, const double front_distance, const double side_distance, + const double back_distance); private: rclcpp::Publisher::SharedPtr debug_virtual_wall_pub_; @@ -72,10 +78,13 @@ class SurroundObstacleCheckerDebugNode rclcpp::Publisher::SharedPtr vehicle_footprint_offset_pub_; rclcpp::Publisher::SharedPtr vehicle_footprint_recover_offset_pub_; - Polygon2d ego_polygon_; + vehicle_info_util::VehicleInfo vehicle_info_; double base_link2front_; - double surround_check_distance_; - double surround_check_recover_distance_; + std::string object_label_; + double surround_check_front_distance_; + double surround_check_side_distance_; + double surround_check_back_distance_; + double surround_check_hysteresis_distance_; geometry_msgs::msg::Pose self_pose_; MarkerArray makeVirtualWallMarker(); @@ -83,7 +92,6 @@ class SurroundObstacleCheckerDebugNode StopReasonArray makeStopReasonArray(); VelocityFactorArray makeVelocityFactorArray(); - Polygon2d createSelfPolygonWithOffset(const Polygon2d & base_polygon, const double & offset); PolygonStamped boostPolygonToPolygonStamped(const Polygon2d & boost_polygon, const double & z); std::shared_ptr stop_obstacle_point_ptr_; diff --git a/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp b/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp index de3e4bf120b13..a7c177f202173 100644 --- a/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp +++ b/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp @@ -38,6 +38,7 @@ #include #include +#include #include #include @@ -62,18 +63,27 @@ class SurroundObstacleCheckerNode : public rclcpp::Node struct NodeParam { - bool use_pointcloud; - bool use_dynamic_object; bool is_surround_obstacle; - // For preventing chattering, - // surround_check_recover_distance_ must be bigger than surround_check_distance_ - double surround_check_recover_distance; - double surround_check_distance; + std::unordered_map enable_check_map; + std::unordered_map surround_check_front_distance_map; + std::unordered_map surround_check_side_distance_map; + std::unordered_map surround_check_back_distance_map; + bool pointcloud_enable_check; + double pointcloud_surround_check_front_distance; + double pointcloud_surround_check_side_distance; + double pointcloud_surround_check_back_distance; + double surround_check_hysteresis_distance; double state_clear_time; bool publish_debug_footprints; + std::string debug_footprint_label; }; private: + rcl_interfaces::msg::SetParametersResult onParam( + const std::vector & parameters); + + std::array getCheckDistances(const std::string & str_label) const; + void onTimer(); void onPointCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg); @@ -107,6 +117,9 @@ class SurroundObstacleCheckerNode : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_clear_velocity_limit_; rclcpp::Publisher::SharedPtr pub_velocity_limit_; + // parameter callback result + OnSetParametersCallbackHandle::SharedPtr set_param_res_; + // stop checker std::unique_ptr vehicle_stop_checker_; diff --git a/planning/surround_obstacle_checker/src/debug_marker.cpp b/planning/surround_obstacle_checker/src/debug_marker.cpp index c30f778584fd7..79c2300be3941 100644 --- a/planning/surround_obstacle_checker/src/debug_marker.cpp +++ b/planning/surround_obstacle_checker/src/debug_marker.cpp @@ -26,6 +26,29 @@ namespace surround_obstacle_checker { +namespace +{ +Polygon2d createSelfPolygon( + const VehicleInfo & vehicle_info, const double front_margin = 0.0, const double side_margin = 0.0, + const double rear_margin = 0.0) +{ + const double & front_m = vehicle_info.max_longitudinal_offset_m + front_margin; + const double & width_left_m = vehicle_info.max_lateral_offset_m + side_margin; + const double & width_right_m = vehicle_info.min_lateral_offset_m - side_margin; + const double & rear_m = vehicle_info.min_longitudinal_offset_m - rear_margin; + + Polygon2d ego_polygon; + + ego_polygon.outer().push_back(Point2d(front_m, width_left_m)); + ego_polygon.outer().push_back(Point2d(front_m, width_right_m)); + ego_polygon.outer().push_back(Point2d(rear_m, width_right_m)); + ego_polygon.outer().push_back(Point2d(rear_m, width_left_m)); + + bg::correct(ego_polygon); + + return ego_polygon; +} +} // namespace using motion_utils::createStopVirtualWallMarker; using tier4_autoware_utils::appendMarkerArray; @@ -36,14 +59,18 @@ using tier4_autoware_utils::createMarkerScale; using tier4_autoware_utils::createPoint; SurroundObstacleCheckerDebugNode::SurroundObstacleCheckerDebugNode( - const Polygon2d & ego_polygon, const double base_link2front, - const double & surround_check_distance, const double & surround_check_recover_distance, - const geometry_msgs::msg::Pose & self_pose, const rclcpp::Clock::SharedPtr clock, - rclcpp::Node & node) -: ego_polygon_(ego_polygon), + const vehicle_info_util::VehicleInfo & vehicle_info, const double base_link2front, + const std::string & object_label, const double & surround_check_front_distance, + const double & surround_check_side_distance, const double & surround_check_back_distance, + const double & surround_check_hysteresis_distance, const geometry_msgs::msg::Pose & self_pose, + const rclcpp::Clock::SharedPtr clock, rclcpp::Node & node) +: vehicle_info_(vehicle_info), base_link2front_(base_link2front), - surround_check_distance_(surround_check_distance), - surround_check_recover_distance_(surround_check_recover_distance), + object_label_(object_label), + surround_check_front_distance_(surround_check_front_distance), + surround_check_side_distance_(surround_check_side_distance), + surround_check_back_distance_(surround_check_back_distance), + surround_check_hysteresis_distance_(surround_check_hysteresis_distance), self_pose_(self_pose), clock_(clock) { @@ -86,20 +113,25 @@ bool SurroundObstacleCheckerDebugNode::pushObstaclePoint( void SurroundObstacleCheckerDebugNode::publishFootprints() { + const auto ego_polygon = createSelfPolygon(vehicle_info_); + /* publish vehicle footprint polygon */ - const auto footprint = boostPolygonToPolygonStamped(ego_polygon_, self_pose_.position.z); + const auto footprint = boostPolygonToPolygonStamped(ego_polygon, self_pose_.position.z); vehicle_footprint_pub_->publish(footprint); /* publish vehicle footprint polygon with offset */ - const auto polygon_with_offset = - createSelfPolygonWithOffset(ego_polygon_, surround_check_distance_); + const auto polygon_with_offset = createSelfPolygon( + vehicle_info_, surround_check_front_distance_, surround_check_side_distance_, + surround_check_back_distance_); const auto footprint_with_offset = boostPolygonToPolygonStamped(polygon_with_offset, self_pose_.position.z); vehicle_footprint_offset_pub_->publish(footprint_with_offset); /* publish vehicle footprint polygon with recover offset */ - const auto polygon_with_recover_offset = - createSelfPolygonWithOffset(ego_polygon_, surround_check_recover_distance_); + const auto polygon_with_recover_offset = createSelfPolygon( + vehicle_info_, surround_check_front_distance_ + surround_check_hysteresis_distance_, + surround_check_side_distance_ + surround_check_hysteresis_distance_, + surround_check_back_distance_ + surround_check_hysteresis_distance_); const auto footprint_with_recover_offset = boostPolygonToPolygonStamped(polygon_with_recover_offset, self_pose_.position.z); vehicle_footprint_recover_offset_pub_->publish(footprint_with_recover_offset); @@ -206,26 +238,6 @@ VelocityFactorArray SurroundObstacleCheckerDebugNode::makeVelocityFactorArray() return velocity_factor_array; } -Polygon2d SurroundObstacleCheckerDebugNode::createSelfPolygonWithOffset( - const Polygon2d & base_polygon, const double & offset) -{ - typedef double coordinate_type; - const double buffer_distance = offset; - const int points_per_circle = 36; - boost::geometry::strategy::buffer::distance_symmetric distance_strategy( - buffer_distance); - boost::geometry::strategy::buffer::join_round join_strategy(points_per_circle); - boost::geometry::strategy::buffer::end_round end_strategy(points_per_circle); - boost::geometry::strategy::buffer::point_circle circle_strategy(points_per_circle); - boost::geometry::strategy::buffer::side_straight side_strategy; - boost::geometry::model::multi_polygon result; - // Create the buffer of a multi polygon - boost::geometry::buffer( - base_polygon, result, distance_strategy, side_strategy, join_strategy, end_strategy, - circle_strategy); - return result.front(); -} - PolygonStamped SurroundObstacleCheckerDebugNode::boostPolygonToPolygonStamped( const Polygon2d & boost_polygon, const double & z) { @@ -244,4 +256,14 @@ PolygonStamped SurroundObstacleCheckerDebugNode::boostPolygonToPolygonStamped( return polygon_stamped; } +void SurroundObstacleCheckerDebugNode::updateFootprintMargin( + const std::string & object_label, const double front_distance, const double side_distance, + const double back_distance) +{ + object_label_ = object_label; + surround_check_front_distance_ = front_distance; + surround_check_side_distance_ = side_distance; + surround_check_back_distance_ = back_distance; +} + } // namespace surround_obstacle_checker diff --git a/planning/surround_obstacle_checker/src/node.cpp b/planning/surround_obstacle_checker/src/node.cpp index 17a47be8c0596..4ea3e81d65411 100644 --- a/planning/surround_obstacle_checker/src/node.cpp +++ b/planning/surround_obstacle_checker/src/node.cpp @@ -47,6 +47,7 @@ namespace surround_obstacle_checker namespace bg = boost::geometry; using Point2d = bg::model::d2::point_xy; using Polygon2d = bg::model::polygon; +using autoware_auto_perception_msgs::msg::ObjectClassification; using tier4_autoware_utils::createPoint; using tier4_autoware_utils::pose2transform; @@ -120,12 +121,14 @@ Polygon2d createObjPolygon( return createObjPolygon(pose, polygon); } -Polygon2d createSelfPolygon(const VehicleInfo & vehicle_info) +Polygon2d createSelfPolygon( + const VehicleInfo & vehicle_info, const double front_margin, const double side_margin, + const double rear_margin) { - const double & front_m = vehicle_info.max_longitudinal_offset_m; - const double & width_left_m = vehicle_info.max_lateral_offset_m; - const double & width_right_m = vehicle_info.min_lateral_offset_m; - const double & rear_m = vehicle_info.min_longitudinal_offset_m; + const double & front_m = vehicle_info.max_longitudinal_offset_m + front_margin; + const double & width_left_m = vehicle_info.max_lateral_offset_m + side_margin; + const double & width_right_m = vehicle_info.min_lateral_offset_m - side_margin; + const double & rear_m = vehicle_info.min_longitudinal_offset_m - rear_margin; Polygon2d ego_polygon; @@ -146,13 +149,42 @@ SurroundObstacleCheckerNode::SurroundObstacleCheckerNode(const rclcpp::NodeOptio // Parameters { auto & p = node_param_; - p.use_pointcloud = this->declare_parameter("use_pointcloud"); - p.use_dynamic_object = this->declare_parameter("use_dynamic_object"); - p.surround_check_distance = this->declare_parameter("surround_check_distance"); - p.surround_check_recover_distance = - this->declare_parameter("surround_check_recover_distance"); + + // for object label + std::unordered_map label_map{ + {"unknown", ObjectClassification::UNKNOWN}, {"car", ObjectClassification::CAR}, + {"truck", ObjectClassification::TRUCK}, {"bus", ObjectClassification::BUS}, + {"trailer", ObjectClassification::TRAILER}, {"motorcycle", ObjectClassification::MOTORCYCLE}, + {"bicycle", ObjectClassification::BICYCLE}, {"pedestrian", ObjectClassification::PEDESTRIAN}}; + for (const auto & label_pair : label_map) { + p.enable_check_map.emplace( + label_pair.second, this->declare_parameter(label_pair.first + ".enable_check")); + p.surround_check_front_distance_map.emplace( + label_pair.second, + this->declare_parameter(label_pair.first + ".surround_check_front_distance")); + p.surround_check_side_distance_map.emplace( + label_pair.second, + this->declare_parameter(label_pair.first + ".surround_check_side_distance")); + p.surround_check_back_distance_map.emplace( + label_pair.second, + this->declare_parameter(label_pair.first + ".surround_check_back_distance")); + } + + // for pointcloud + p.pointcloud_enable_check = this->declare_parameter("pointcloud.enable_check"); + p.pointcloud_surround_check_front_distance = + this->declare_parameter("pointcloud.surround_check_front_distance"); + p.pointcloud_surround_check_side_distance = + this->declare_parameter("pointcloud.surround_check_side_distance"); + p.pointcloud_surround_check_back_distance = + this->declare_parameter("pointcloud.surround_check_back_distance"); + + p.surround_check_hysteresis_distance = + this->declare_parameter("surround_check_hysteresis_distance"); + p.state_clear_time = this->declare_parameter("state_clear_time"); p.publish_debug_footprints = this->declare_parameter("publish_debug_footprints"); + p.debug_footprint_label = this->declare_parameter("debug_footprint_label"); } vehicle_info_ = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); @@ -176,6 +208,10 @@ SurroundObstacleCheckerNode::SurroundObstacleCheckerNode(const rclcpp::NodeOptio "~/input/odometry", 1, std::bind(&SurroundObstacleCheckerNode::onOdometry, this, std::placeholders::_1)); + // Parameter callback + set_param_res_ = this->add_on_set_parameters_callback( + std::bind(&SurroundObstacleCheckerNode::onParam, this, std::placeholders::_1)); + using std::chrono_literals::operator""ms; timer_ = rclcpp::create_timer( this, get_clock(), 100ms, std::bind(&SurroundObstacleCheckerNode::onTimer, this)); @@ -184,15 +220,55 @@ SurroundObstacleCheckerNode::SurroundObstacleCheckerNode(const rclcpp::NodeOptio vehicle_stop_checker_ = std::make_unique(this); // Debug - auto const self_polygon = createSelfPolygon(vehicle_info_); odometry_ptr_ = std::make_shared(); + const auto check_distances = getCheckDistances(node_param_.debug_footprint_label); debug_ptr_ = std::make_shared( - self_polygon, vehicle_info_.max_longitudinal_offset_m, node_param_.surround_check_distance, - node_param_.surround_check_recover_distance, odometry_ptr_->pose.pose, this->get_clock(), + vehicle_info_, vehicle_info_.max_longitudinal_offset_m, node_param_.debug_footprint_label, + check_distances.at(0), check_distances.at(1), check_distances.at(2), + node_param_.surround_check_hysteresis_distance, odometry_ptr_->pose.pose, this->get_clock(), *this); } +std::array SurroundObstacleCheckerNode::getCheckDistances( + const std::string & str_label) const +{ + if (str_label == "pointcloud") { + return { + node_param_.pointcloud_surround_check_front_distance, + node_param_.pointcloud_surround_check_side_distance, + node_param_.pointcloud_surround_check_back_distance}; + } + + std::unordered_map label_map{ + {"unknown", ObjectClassification::UNKNOWN}, {"car", ObjectClassification::CAR}, + {"truck", ObjectClassification::TRUCK}, {"bus", ObjectClassification::BUS}, + {"trailer", ObjectClassification::TRAILER}, {"motorcycle", ObjectClassification::MOTORCYCLE}, + {"bicycle", ObjectClassification::BICYCLE}, {"pedestrian", ObjectClassification::PEDESTRIAN}}; + + const int int_label = label_map.at(str_label); + return { + node_param_.surround_check_front_distance_map.at(int_label), + node_param_.surround_check_side_distance_map.at(int_label), + node_param_.surround_check_back_distance_map.at(int_label)}; +} + +rcl_interfaces::msg::SetParametersResult SurroundObstacleCheckerNode::onParam( + const std::vector & parameters) +{ + tier4_autoware_utils::updateParam( + parameters, "debug_footprint_label", node_param_.debug_footprint_label); + const auto check_distances = getCheckDistances(node_param_.debug_footprint_label); + debug_ptr_->updateFootprintMargin( + node_param_.debug_footprint_label, check_distances.at(0), check_distances.at(1), + check_distances.at(2)); + + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + result.reason = "success"; + return result; +} + void SurroundObstacleCheckerNode::onTimer() { if (!odometry_ptr_) { @@ -205,13 +281,22 @@ void SurroundObstacleCheckerNode::onTimer() debug_ptr_->publishFootprints(); } - if (node_param_.use_pointcloud && !pointcloud_ptr_) { + if (node_param_.pointcloud_enable_check && !pointcloud_ptr_) { RCLCPP_INFO_THROTTLE( this->get_logger(), *this->get_clock(), 5000 /* ms */, "waiting for pointcloud info..."); return; } - if (node_param_.use_dynamic_object && !object_ptr_) { + const bool use_dynamic_object = + node_param_.enable_check_map.at(ObjectClassification::UNKNOWN) || + node_param_.enable_check_map.at(ObjectClassification::CAR) || + node_param_.enable_check_map.at(ObjectClassification::TRUCK) || + node_param_.enable_check_map.at(ObjectClassification::BUS) || + node_param_.enable_check_map.at(ObjectClassification::TRAILER) || + node_param_.enable_check_map.at(ObjectClassification::MOTORCYCLE) || + node_param_.enable_check_map.at(ObjectClassification::BICYCLE) || + node_param_.enable_check_map.at(ObjectClassification::PEDESTRIAN); + if (use_dynamic_object && !object_ptr_) { RCLCPP_INFO_THROTTLE( this->get_logger(), *this->get_clock(), 5000 /* ms */, "waiting for dynamic object info..."); return; @@ -220,11 +305,11 @@ void SurroundObstacleCheckerNode::onTimer() const auto nearest_obstacle = getNearestObstacle(); const auto is_vehicle_stopped = vehicle_stop_checker_->isVehicleStopped(); + constexpr double epsilon = 1e-3; switch (state_) { case State::PASS: { const auto is_obstacle_found = - !nearest_obstacle ? false - : nearest_obstacle.get().first < node_param_.surround_check_distance; + !nearest_obstacle ? false : nearest_obstacle.get().first < epsilon; if (!isStopRequired(is_obstacle_found, is_vehicle_stopped)) { break; @@ -250,7 +335,7 @@ void SurroundObstacleCheckerNode::onTimer() const auto is_obstacle_found = !nearest_obstacle ? false - : nearest_obstacle.get().first < node_param_.surround_check_recover_distance; + : nearest_obstacle.get().first < node_param_.surround_check_hysteresis_distance; if (isStopRequired(is_obstacle_found, is_vehicle_stopped)) { break; @@ -304,17 +389,8 @@ void SurroundObstacleCheckerNode::onOdometry(const nav_msgs::msg::Odometry::Cons boost::optional SurroundObstacleCheckerNode::getNearestObstacle() const { - boost::optional nearest_pointcloud{boost::none}; - boost::optional nearest_object{boost::none}; - - if (node_param_.use_pointcloud) { - nearest_pointcloud = getNearestObstacleByPointCloud(); - } - - if (node_param_.use_dynamic_object) { - nearest_object = getNearestObstacleByDynamicObject(); - } - + const auto nearest_pointcloud = getNearestObstacleByPointCloud(); + const auto nearest_object = getNearestObstacleByDynamicObject(); if (!nearest_pointcloud && !nearest_object) { return {}; } @@ -333,15 +409,13 @@ boost::optional SurroundObstacleCheckerNode::getNearestObstacle() cons boost::optional SurroundObstacleCheckerNode::getNearestObstacleByPointCloud() const { - if (pointcloud_ptr_->data.empty()) { + if (!node_param_.pointcloud_enable_check || pointcloud_ptr_->data.empty()) { return boost::none; } + const auto transform_stamped = getTransform("base_link", pointcloud_ptr_->header.frame_id, pointcloud_ptr_->header.stamp, 0.5); - geometry_msgs::msg::Point nearest_point; - auto minimum_distance = std::numeric_limits::max(); - if (!transform_stamped) { return {}; } @@ -352,8 +426,14 @@ boost::optional SurroundObstacleCheckerNode::getNearestObstacleByPoint tier4_autoware_utils::transformPointCloud( transformed_pointcloud, transformed_pointcloud, isometry); - const auto ego_polygon = createSelfPolygon(vehicle_info_); + const double front_margin = node_param_.pointcloud_surround_check_front_distance; + const double side_margin = node_param_.pointcloud_surround_check_side_distance; + const double back_margin = node_param_.pointcloud_surround_check_back_distance; + const auto ego_polygon = createSelfPolygon(vehicle_info_, front_margin, side_margin, back_margin); + geometry_msgs::msg::Point nearest_point; + double minimum_distance = std::numeric_limits::max(); + bool was_minimum_distance_updated = false; for (const auto & p : transformed_pointcloud) { Point2d boost_point(p.x, p.y); @@ -362,10 +442,14 @@ boost::optional SurroundObstacleCheckerNode::getNearestObstacleByPoint if (distance_to_object < minimum_distance) { nearest_point = createPoint(p.x, p.y, p.z); minimum_distance = distance_to_object; + was_minimum_distance_updated = true; } } - return std::make_pair(minimum_distance, nearest_point); + if (was_minimum_distance_updated) { + return std::make_pair(minimum_distance, nearest_point); + } + return boost::none; } boost::optional SurroundObstacleCheckerNode::getNearestObstacleByDynamicObject() const @@ -373,9 +457,6 @@ boost::optional SurroundObstacleCheckerNode::getNearestObstacleByDynam const auto transform_stamped = getTransform(object_ptr_->header.frame_id, "base_link", object_ptr_->header.stamp, 0.5); - geometry_msgs::msg::Point nearest_point; - auto minimum_distance = std::numeric_limits::max(); - if (!transform_stamped) { return {}; } @@ -383,10 +464,23 @@ boost::optional SurroundObstacleCheckerNode::getNearestObstacleByDynam tf2::Transform tf_src2target; tf2::fromMsg(transform_stamped.get().transform, tf_src2target); - const auto ego_polygon = createSelfPolygon(vehicle_info_); - + // TODO(murooka) check computation cost + geometry_msgs::msg::Point nearest_point; + double minimum_distance = std::numeric_limits::max(); + bool was_minimum_distance_updated = false; for (const auto & object : object_ptr_->objects) { const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose; + const int label = object.classification.front().label; + + if (!node_param_.enable_check_map.at(label)) { + continue; + } + + const double front_margin = node_param_.surround_check_front_distance_map.at(label); + const double side_margin = node_param_.surround_check_side_distance_map.at(label); + const double back_margin = node_param_.surround_check_back_distance_map.at(label); + const auto ego_polygon = + createSelfPolygon(vehicle_info_, front_margin, side_margin, back_margin); tf2::Transform tf_src2object; tf2::fromMsg(object_pose, tf_src2object); @@ -404,10 +498,14 @@ boost::optional SurroundObstacleCheckerNode::getNearestObstacleByDynam if (distance_to_object < minimum_distance) { nearest_point = object_pose.position; minimum_distance = distance_to_object; + was_minimum_distance_updated = true; } } - return std::make_pair(minimum_distance, nearest_point); + if (was_minimum_distance_updated) { + return std::make_pair(minimum_distance, nearest_point); + } + return boost::none; } boost::optional SurroundObstacleCheckerNode::getTransform( From 94838a90f71ab8d7db82e8d0bbd2db69eb02ee83 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Sun, 10 Sep 2023 01:22:43 +0900 Subject: [PATCH 08/11] chore(build): remove tier4_autoware_utils.hpp/motion_utils.hpp from freespace/route_handler (#4934) remove tier4_autoware_utils.hpp/motion_utils.hpp from freespace/route_handler Signed-off-by: Mamoru Sobue --- .../include/behavior_path_planner/data_manager.hpp | 1 + .../drivable_area_expansion/drivable_area_expansion.cpp | 3 +++ planning/behavior_path_planner/test/test_turn_signal.cpp | 4 ++++ planning/behavior_velocity_out_of_lane_module/src/debug.cpp | 2 ++ .../behavior_velocity_out_of_lane_module/src/decisions.cpp | 4 ++++ planning/freespace_planner/package.xml | 1 + .../src/freespace_planner/freespace_planner_node.cpp | 3 ++- .../src/abstract_algorithm.cpp | 3 ++- planning/freespace_planning_algorithms/src/astar_search.cpp | 3 ++- .../src/lanelet2_plugins/default_planner.cpp | 6 +++++- .../src/lanelet2_plugins/utility_functions.hpp | 2 -- .../mission_planner/src/mission_planner/arrival_checker.cpp | 4 +++- .../route_handler/include/route_handler/route_handler.hpp | 2 -- planning/route_handler/package.xml | 1 - planning/route_handler/src/route_handler.cpp | 1 + .../include/static_centerline_optimizer/type_alias.hpp | 1 + 16 files changed, 31 insertions(+), 10 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp index 9217e139d218c..a144aa12704a8 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp @@ -19,6 +19,7 @@ #include "behavior_path_planner/turn_signal_decider.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/parameters.hpp" +#include #include #include diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp index 0fcb0acfcb000..52be0ac5a81ef 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp +++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp @@ -22,7 +22,10 @@ #include #include +#include +#include #include +#include #include diff --git a/planning/behavior_path_planner/test/test_turn_signal.cpp b/planning/behavior_path_planner/test/test_turn_signal.cpp index 8c6f8de7ee6a0..a48c8a6b0d976 100644 --- a/planning/behavior_path_planner/test/test_turn_signal.cpp +++ b/planning/behavior_path_planner/test/test_turn_signal.cpp @@ -13,6 +13,10 @@ // limitations under the License. #include "behavior_path_planner/turn_signal_decider.hpp" +#include +#include +#include + #include "autoware_auto_planning_msgs/msg/path_point.hpp" #include diff --git a/planning/behavior_velocity_out_of_lane_module/src/debug.cpp b/planning/behavior_velocity_out_of_lane_module/src/debug.cpp index 475e341d76528..0574a5226dc4a 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/debug.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/debug.cpp @@ -14,6 +14,8 @@ #include "debug.hpp" +#include + #include namespace behavior_velocity_planner::out_of_lane::debug diff --git a/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp b/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp index 46db7a53b5c81..2ebfa4ecbe431 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp @@ -15,6 +15,10 @@ #include "decisions.hpp" #include +#include +#include +#include +#include #include #include diff --git a/planning/freespace_planner/package.xml b/planning/freespace_planner/package.xml index a748b7f80c9ad..f92c0a449970d 100644 --- a/planning/freespace_planner/package.xml +++ b/planning/freespace_planner/package.xml @@ -19,6 +19,7 @@ autoware_auto_planning_msgs freespace_planning_algorithms geometry_msgs + motion_utils nav_msgs planning_test_utils rclcpp diff --git a/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp b/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp index e0d7f86e45245..025cc32d4ed24 100644 --- a/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp +++ b/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp @@ -32,7 +32,8 @@ #include "freespace_planning_algorithms/abstract_algorithm.hpp" -#include +#include +#include #include #include diff --git a/planning/freespace_planning_algorithms/src/abstract_algorithm.cpp b/planning/freespace_planning_algorithms/src/abstract_algorithm.cpp index 159c0df82fcc3..2c7340072eb53 100644 --- a/planning/freespace_planning_algorithms/src/abstract_algorithm.cpp +++ b/planning/freespace_planning_algorithms/src/abstract_algorithm.cpp @@ -14,7 +14,8 @@ #include "freespace_planning_algorithms/abstract_algorithm.hpp" -#include +#include +#include #include diff --git a/planning/freespace_planning_algorithms/src/astar_search.cpp b/planning/freespace_planning_algorithms/src/astar_search.cpp index b2b2321adacd0..1ab5e9d5df487 100644 --- a/planning/freespace_planning_algorithms/src/astar_search.cpp +++ b/planning/freespace_planning_algorithms/src/astar_search.cpp @@ -14,7 +14,8 @@ #include "freespace_planning_algorithms/astar_search.hpp" -#include +#include +#include #include diff --git a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp index d8289824e517c..979800aea0754 100644 --- a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp +++ b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp @@ -20,7 +20,11 @@ #include #include #include -#include +#include +#include +#include +#include +#include #include #include diff --git a/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp b/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp index 0bfb0553e75ef..3ea6237f38501 100644 --- a/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp +++ b/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp @@ -15,8 +15,6 @@ #ifndef LANELET2_PLUGINS__UTILITY_FUNCTIONS_HPP_ #define LANELET2_PLUGINS__UTILITY_FUNCTIONS_HPP_ -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" - #include #include #include diff --git a/planning/mission_planner/src/mission_planner/arrival_checker.cpp b/planning/mission_planner/src/mission_planner/arrival_checker.cpp index ac7a37acd4d5b..1e9f02ff0338d 100644 --- a/planning/mission_planner/src/mission_planner/arrival_checker.cpp +++ b/planning/mission_planner/src/mission_planner/arrival_checker.cpp @@ -14,7 +14,9 @@ #include "arrival_checker.hpp" -#include +#include +#include +#include #include diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp index 847c8db3b16a9..6bd9756098558 100644 --- a/planning/route_handler/include/route_handler/route_handler.hpp +++ b/planning/route_handler/include/route_handler/route_handler.hpp @@ -16,9 +16,7 @@ #define ROUTE_HANDLER__ROUTE_HANDLER_HPP_ #include -#include #include -#include #include #include diff --git a/planning/route_handler/package.xml b/planning/route_handler/package.xml index ef2051d1b244a..6304e21932676 100644 --- a/planning/route_handler/package.xml +++ b/planning/route_handler/package.xml @@ -24,7 +24,6 @@ autoware_planning_msgs geometry_msgs lanelet2_extension - motion_utils rclcpp rclcpp_components tf2_ros diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index 6a95434c81c23..3d9dd8fe7a64a 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include diff --git a/planning/static_centerline_optimizer/include/static_centerline_optimizer/type_alias.hpp b/planning/static_centerline_optimizer/include/static_centerline_optimizer/type_alias.hpp index 5798c19d0d563..1a700d36deaaf 100644 --- a/planning/static_centerline_optimizer/include/static_centerline_optimizer/type_alias.hpp +++ b/planning/static_centerline_optimizer/include/static_centerline_optimizer/type_alias.hpp @@ -15,6 +15,7 @@ #define STATIC_CENTERLINE_OPTIMIZER__TYPE_ALIAS_HPP_ #include "route_handler/route_handler.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" #include "autoware_auto_mapping_msgs/msg/had_map_bin.hpp" #include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" From 1a30fc3d4b01c16824b9f0147ad9d3851aec5cb3 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Sun, 10 Sep 2023 02:03:35 +0900 Subject: [PATCH 09/11] chore(build): remove tier4_autoware_utils.hpp/motion_utils.hpp from map_based_prediction (#4937) --- .../map_based_prediction/map_based_prediction_node.hpp | 2 -- .../map_based_prediction/src/map_based_prediction_node.cpp | 5 ++++- perception/map_based_prediction/src/path_generator.cpp | 4 ++-- 3 files changed, 6 insertions(+), 5 deletions(-) diff --git a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp index 7364e3d11b650..f694892541f4a 100644 --- a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp +++ b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp @@ -20,11 +20,9 @@ #include #include #include -#include #include #include #include -#include #include #include diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp index 3b4e010d0a855..c1cb9d9dd3910 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp @@ -16,7 +16,10 @@ #include #include -#include +#include +#include +#include +#include #include diff --git a/perception/map_based_prediction/src/path_generator.cpp b/perception/map_based_prediction/src/path_generator.cpp index b1f6319801d4a..547132410badf 100644 --- a/perception/map_based_prediction/src/path_generator.cpp +++ b/perception/map_based_prediction/src/path_generator.cpp @@ -15,8 +15,8 @@ #include "map_based_prediction/path_generator.hpp" #include -#include -#include +#include +#include #include From 477ce95a1975773e426458f986b9a8a8c43a89f7 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Sun, 10 Sep 2023 15:37:50 +0900 Subject: [PATCH 10/11] fix(start_planner): prevent approval when stop path (#4932) Signed-off-by: kosuke55 --- .../scene_module/start_planner/start_planner_module.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index e8acc77113959..7be05561aa34a 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -141,6 +141,10 @@ bool StartPlannerModule::isExecutionRequested() const bool StartPlannerModule::isExecutionReady() const { + if (!status_.is_safe_static_objects) { + return false; + } + if (status_.pull_out_path.partial_paths.empty()) { return true; } @@ -185,6 +189,7 @@ BehaviorModuleOutput StartPlannerModule::plan() getLogger(), *clock_, 5000, "Start plan for a backward goal is not supported now"); const auto output = generateStopOutput(); setDebugData(); // use status updated in generateStopOutput() + updateRTCStatus(0, 0); return output; } @@ -202,6 +207,7 @@ BehaviorModuleOutput StartPlannerModule::plan() getLogger(), *clock_, 5000, "Not found safe pull out path, publish stop path"); const auto output = generateStopOutput(); setDebugData(); // use status updated in generateStopOutput() + updateRTCStatus(0, 0); return output; } @@ -298,6 +304,7 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() clearWaitingApproval(); const auto output = generateStopOutput(); setDebugData(); // use status updated in generateStopOutput() + updateRTCStatus(0, 0); return output; } @@ -308,6 +315,7 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() clearWaitingApproval(); const auto output = generateStopOutput(); setDebugData(); // use status updated in generateStopOutput() + updateRTCStatus(0, 0); return output; } From a25cbe07468136f255cb0bdf676c86702698ebf5 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Mon, 11 Sep 2023 01:31:17 +0900 Subject: [PATCH 11/11] chore(build): remove tier4_autoware_utils.hpp/motion_utils.hpp from behavior_path_planner (#4936) Signed-off-by: Mamoru Sobue --- .../include/behavior_path_planner/data_manager.hpp | 2 +- .../include/behavior_path_planner/marker_utils/utils.hpp | 3 +-- .../include/behavior_path_planner/planner_manager.hpp | 1 + .../scene_module/scene_module_interface.hpp | 5 +++++ .../utils/avoidance/avoidance_module_data.hpp | 3 ++- .../include/behavior_path_planner/utils/utils.hpp | 4 ++-- .../src/behavior_path_planner_node.cpp | 2 +- .../src/marker_utils/avoidance/debug.cpp | 1 + planning/behavior_path_planner/src/marker_utils/utils.cpp | 3 ++- planning/behavior_path_planner/src/planner_manager.cpp | 1 + .../src/scene_module/avoidance/avoidance_module.cpp | 5 ++++- .../src/scene_module/avoidance/manager.cpp | 3 +++ .../dynamic_avoidance/dynamic_avoidance_module.cpp | 2 +- .../src/scene_module/dynamic_avoidance/manager.cpp | 2 ++ .../src/scene_module/goal_planner/goal_planner_module.cpp | 3 +-- .../src/scene_module/goal_planner/manager.cpp | 2 +- .../src/scene_module/lane_change/manager.cpp | 3 +++ .../src/scene_module/lane_change/normal.cpp | 1 + .../src/scene_module/side_shift/manager.cpp | 2 ++ .../src/scene_module/start_planner/manager.cpp | 2 ++ .../src/scene_module/start_planner/start_planner_module.cpp | 1 - planning/behavior_path_planner/src/turn_signal_decider.cpp | 6 +++++- .../behavior_path_planner/src/utils/avoidance/utils.cpp | 6 +++++- .../geometric_parallel_parking.cpp | 3 ++- .../src/utils/goal_planner/freespace_pull_over.cpp | 1 + .../src/utils/goal_planner/geometric_pull_over.cpp | 1 + .../src/utils/goal_planner/goal_searcher.cpp | 2 ++ .../src/utils/goal_planner/shift_pull_over.cpp | 1 + .../behavior_path_planner/src/utils/lane_change/utils.cpp | 3 +++ .../occupancy_grid_based_collision_detector.cpp | 2 +- .../src/utils/path_safety_checker/objects_filtering.cpp | 4 ++++ .../src/utils/path_safety_checker/safety_check.cpp | 2 ++ .../src/utils/path_shifter/path_shifter.cpp | 2 +- planning/behavior_path_planner/src/utils/path_utils.cpp | 4 +++- .../src/utils/start_goal_planner_common/utils.cpp | 2 ++ .../src/utils/start_planner/geometric_pull_out.cpp | 1 + .../src/utils/start_planner/shift_pull_out.cpp | 1 + .../behavior_path_planner/src/utils/start_planner/util.cpp | 1 + planning/behavior_path_planner/src/utils/utils.cpp | 3 +++ .../behavior_path_planner/test/test_lane_change_utils.cpp | 4 +++- planning/behavior_path_planner/test/test_safety_check.cpp | 3 ++- planning/behavior_path_planner/test/test_turn_signal.cpp | 2 ++ planning/behavior_path_planner/test/test_utils.cpp | 1 + 43 files changed, 84 insertions(+), 22 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp index a144aa12704a8..78172c6fc0750 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp @@ -18,8 +18,8 @@ #include "behavior_path_planner/parameters.hpp" #include "behavior_path_planner/turn_signal_decider.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/parameters.hpp" +#include "motion_utils/trajectory/trajectory.hpp" -#include #include #include diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp index ba8ddd9b19c46..c53735486efb0 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp @@ -17,9 +17,8 @@ #include "behavior_path_planner/data_manager.hpp" #include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" -#include +#include #include #include diff --git a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp index b77ac16a9db61..4bdfc807aa7c4 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp @@ -19,6 +19,7 @@ #include "behavior_path_planner/scene_module/scene_module_manager_interface.hpp" #include "behavior_path_planner/scene_module/scene_module_visitor.hpp" #include "behavior_path_planner/utils/lane_following/module_data.hpp" +#include "tier4_autoware_utils/system/stop_watch.hpp" #include #include diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp index 6acf5b945308f..077f7a3a763d5 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp @@ -22,10 +22,15 @@ #include #include #include +#include +#include +#include #include #include #include +#include #include +#include #include #include diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp index 3264ec1e9ddfd..215a8bfd97cc0 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp @@ -20,10 +20,11 @@ #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" #include -#include +#include #include #include +#include #include #include diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp index 0caa79ba5d062..d5e1d22fdcc88 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp @@ -22,11 +22,11 @@ #include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" #include "behavior_path_planner/utils/start_planner/pull_out_path.hpp" -#include "motion_utils/motion_utils.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include "object_recognition_utils/predicted_path_utils.hpp" #include -#include +#include #include #include diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index a9b137e54e90f..41df95378c52f 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -19,7 +19,7 @@ #include "behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp" #include "behavior_path_planner/utils/path_utils.hpp" -#include +#include #include #include diff --git a/planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp b/planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp index bea1447b0d95a..b2c326fda7a69 100644 --- a/planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp +++ b/planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp @@ -16,6 +16,7 @@ #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/utils.hpp" +#include "motion_utils/trajectory/path_with_lane_id.hpp" #include diff --git a/planning/behavior_path_planner/src/marker_utils/utils.cpp b/planning/behavior_path_planner/src/marker_utils/utils.cpp index d6a237079448f..b51fc585ce935 100644 --- a/planning/behavior_path_planner/src/marker_utils/utils.cpp +++ b/planning/behavior_path_planner/src/marker_utils/utils.cpp @@ -19,8 +19,9 @@ #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/utils.hpp" +#include #include - +#include namespace marker_utils { using behavior_path_planner::ShiftLine; diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp index 7708fd8ac116e..ee9c445e9fd2f 100644 --- a/planning/behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/src/planner_manager.cpp @@ -16,6 +16,7 @@ #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/utils.hpp" +#include "tier4_autoware_utils/system/stop_watch.hpp" #include #include diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 3b005c1b2242d..e5374efd0a5b5 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -23,7 +23,10 @@ #include #include -#include +#include +#include +#include +#include #include #include diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp index 8900e2b3e827c..912f867fd5701 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -14,6 +14,9 @@ #include "behavior_path_planner/scene_module/avoidance/manager.hpp" +#include "tier4_autoware_utils/ros/parameter.hpp" +#include "tier4_autoware_utils/ros/update_param.hpp" + #include #include diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp index da98c0478e580..b5ecffbcc9791 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp @@ -20,7 +20,7 @@ #include #include -#include +#include #include #include diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp index 82ebf59ea5bb2..616194ea66ac7 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp @@ -14,6 +14,8 @@ #include "behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp" +#include "tier4_autoware_utils/ros/update_param.hpp" + #include #include diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index cfdc968656353..d55b3c173cc83 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -21,13 +21,12 @@ #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" #include "behavior_path_planner/utils/utils.hpp" +#include "tier4_autoware_utils/math/unit_conversion.hpp" #include #include #include -#include #include -#include #include #include diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp index 7e2975a73aa44..ba147a28173c3 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp @@ -15,8 +15,8 @@ #include "behavior_path_planner/scene_module/goal_planner/manager.hpp" #include "behavior_path_planner/utils/goal_planner/util.hpp" +#include "tier4_autoware_utils/ros/update_param.hpp" -#include #include #include diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp index 27f44433c6d4e..f4a75daa6c764 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp @@ -14,6 +14,9 @@ #include "behavior_path_planner/scene_module/lane_change/manager.hpp" +#include "tier4_autoware_utils/ros/parameter.hpp" +#include "tier4_autoware_utils/ros/update_param.hpp" + #include #include diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index 8deee76656dca..a81f6227e4064 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -23,6 +23,7 @@ #include #include +#include #include #include diff --git a/planning/behavior_path_planner/src/scene_module/side_shift/manager.cpp b/planning/behavior_path_planner/src/scene_module/side_shift/manager.cpp index 5a039a98f2e52..288d9d44aee54 100644 --- a/planning/behavior_path_planner/src/scene_module/side_shift/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/side_shift/manager.cpp @@ -14,6 +14,8 @@ #include "behavior_path_planner/scene_module/side_shift/manager.hpp" +#include "tier4_autoware_utils/ros/update_param.hpp" + #include #include diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp index 5f4b57d9c6367..09166cd72a83e 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp @@ -14,6 +14,8 @@ #include "behavior_path_planner/scene_module/start_planner/manager.hpp" +#include "tier4_autoware_utils/ros/update_param.hpp" + #include #include diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index 7be05561aa34a..afea96a8aa883 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -24,7 +24,6 @@ #include #include #include -#include #include #include diff --git a/planning/behavior_path_planner/src/turn_signal_decider.cpp b/planning/behavior_path_planner/src/turn_signal_decider.cpp index fa661aef06681..65e3e8343f49c 100644 --- a/planning/behavior_path_planner/src/turn_signal_decider.cpp +++ b/planning/behavior_path_planner/src/turn_signal_decider.cpp @@ -18,9 +18,13 @@ #include #include +#include #include +#include #include -#include +#include +#include +#include #include #include diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index b60abbddf6632..4afe5381daa64 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -17,10 +17,14 @@ #include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" #include "behavior_path_planner/utils/avoidance/utils.hpp" #include "behavior_path_planner/utils/path_utils.hpp" +#include "motion_utils/trajectory/path_with_lane_id.hpp" +#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include #include -#include +#include +#include +#include #include diff --git a/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp b/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp index 99d109dd0bece..5b054de43ef74 100644 --- a/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp +++ b/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp @@ -17,12 +17,13 @@ #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/start_planner/util.hpp" #include "behavior_path_planner/utils/utils.hpp" +#include "motion_utils/trajectory/path_with_lane_id.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/math/unit_conversion.hpp" #include #include #include -#include #ifdef ROS_DISTRO_GALACTIC #include diff --git a/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp b/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp index 9322350877ad1..5bb4e924b6d89 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp @@ -17,6 +17,7 @@ #include "behavior_path_planner/utils/goal_planner/util.hpp" #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" +#include "tier4_autoware_utils/geometry/path_with_lane_id_geometry.hpp" #include #include diff --git a/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp b/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp index ed0106d891bb4..f350262cf4b80 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp @@ -16,6 +16,7 @@ #include "behavior_path_planner/utils/goal_planner/util.hpp" #include "behavior_path_planner/utils/path_utils.hpp" +#include "motion_utils/trajectory/path_with_lane_id.hpp" #include #include diff --git a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp index 19dec4d3c97d1..2dd81eba7e6c7 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp @@ -18,7 +18,9 @@ #include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/path_utils.hpp" #include "lanelet2_extension/utility/utilities.hpp" +#include "motion_utils/trajectory/path_with_lane_id.hpp" +#include #include #include diff --git a/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp b/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp index aeb28d74e5f95..b7ece14035265 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp @@ -16,6 +16,7 @@ #include "behavior_path_planner/utils/goal_planner/util.hpp" #include "behavior_path_planner/utils/path_utils.hpp" +#include "motion_utils/trajectory/path_with_lane_id.hpp" #include #include diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp index f3a1260eecadc..ffd5116a5d2dd 100644 --- a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp +++ b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp @@ -25,8 +25,11 @@ #include #include #include +#include +#include #include #include +#include #include #include diff --git a/planning/behavior_path_planner/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp b/planning/behavior_path_planner/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp index d244d08abf99b..0f87c68e38289 100644 --- a/planning/behavior_path_planner/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp +++ b/planning/behavior_path_planner/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp @@ -15,7 +15,7 @@ #include "behavior_path_planner/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" #include -#include +#include #include diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp index 23016b01cb9ab..7205b273ff281 100644 --- a/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp +++ b/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp @@ -16,6 +16,10 @@ #include "behavior_path_planner/utils/utils.hpp" +#include +#include +#include + namespace behavior_path_planner::utils::path_safety_checker { diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp index 1838dc6bdc07b..dddcc672e7107 100644 --- a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp @@ -16,8 +16,10 @@ #include "behavior_path_planner/marker_utils/utils.hpp" #include "interpolation/linear_interpolation.hpp" +#include "motion_utils/trajectory/path_with_lane_id.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include "object_recognition_utils/predicted_path_utils.hpp" +#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" namespace behavior_path_planner::utils::path_safety_checker { diff --git a/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp b/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp index 9ca083af35c92..44ba6bd364bc4 100644 --- a/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp +++ b/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp @@ -19,7 +19,7 @@ #include #include -#include +#include #include #include diff --git a/planning/behavior_path_planner/src/utils/path_utils.cpp b/planning/behavior_path_planner/src/utils/path_utils.cpp index c4bfb62276d42..112c7e49c7c86 100644 --- a/planning/behavior_path_planner/src/utils/path_utils.cpp +++ b/planning/behavior_path_planner/src/utils/path_utils.cpp @@ -15,11 +15,13 @@ #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/utils.hpp" +#include "motion_utils/trajectory/path_with_lane_id.hpp" #include #include #include -#include +#include +#include #include diff --git a/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp b/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp index 830860de484b6..d07a2f54a5e8a 100644 --- a/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp +++ b/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp @@ -14,6 +14,8 @@ #include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" +#include + namespace behavior_path_planner::utils::start_goal_planner_common { diff --git a/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp index 628f1cfd7e421..0d75391f9d4a6 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp @@ -19,6 +19,7 @@ #include "behavior_path_planner/utils/utils.hpp" #include +#include using lanelet::utils::getArcCoordinates; using motion_utils::findNearestIndex; diff --git a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp index f76aca793d571..f86f9d3d90a07 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp @@ -18,6 +18,7 @@ #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/start_planner/util.hpp" #include "behavior_path_planner/utils/utils.hpp" +#include "motion_utils/trajectory/path_with_lane_id.hpp" #include diff --git a/planning/behavior_path_planner/src/utils/start_planner/util.cpp b/planning/behavior_path_planner/src/utils/start_planner/util.cpp index 9ee0491ca017e..f5b4c9b979ee3 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/util.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/util.cpp @@ -20,6 +20,7 @@ #include "behavior_path_planner/utils/utils.hpp" #include +#include #include #include diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index da23d818cf59a..617d4a224fdd5 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -20,6 +20,9 @@ #include #include #include +#include +#include +#include #include "autoware_auto_perception_msgs/msg/predicted_object.hpp" #include "autoware_auto_perception_msgs/msg/predicted_path.hpp" diff --git a/planning/behavior_path_planner/test/test_lane_change_utils.cpp b/planning/behavior_path_planner/test/test_lane_change_utils.cpp index 3a809509641f4..ad64a7ae3feb5 100644 --- a/planning/behavior_path_planner/test/test_lane_change_utils.cpp +++ b/planning/behavior_path_planner/test/test_lane_change_utils.cpp @@ -12,7 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. #include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" + +#include +#include #include #include diff --git a/planning/behavior_path_planner/test/test_safety_check.cpp b/planning/behavior_path_planner/test/test_safety_check.cpp index 9dfcef487cc81..39e831bd5565b 100644 --- a/planning/behavior_path_planner/test/test_safety_check.cpp +++ b/planning/behavior_path_planner/test/test_safety_check.cpp @@ -15,7 +15,8 @@ #include "behavior_path_planner/marker_utils/utils.hpp" #include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" + +#include #include diff --git a/planning/behavior_path_planner/test/test_turn_signal.cpp b/planning/behavior_path_planner/test/test_turn_signal.cpp index a48c8a6b0d976..4917443064e78 100644 --- a/planning/behavior_path_planner/test/test_turn_signal.cpp +++ b/planning/behavior_path_planner/test/test_turn_signal.cpp @@ -12,12 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. #include "behavior_path_planner/turn_signal_decider.hpp" +#include "motion_utils/trajectory/path_with_lane_id.hpp" #include #include #include #include "autoware_auto_planning_msgs/msg/path_point.hpp" +#include #include #include diff --git a/planning/behavior_path_planner/test/test_utils.cpp b/planning/behavior_path_planner/test/test_utils.cpp index b9e2727ebaa13..1452e45980793 100644 --- a/planning/behavior_path_planner/test/test_utils.cpp +++ b/planning/behavior_path_planner/test/test_utils.cpp @@ -16,6 +16,7 @@ #include "lanelet2_core/Attribute.h" #include "lanelet2_core/geometry/Point.h" #include "lanelet2_core/primitives/Lanelet.h" +#include "motion_utils/trajectory/path_with_lane_id.hpp" #include #include