From 01a61e5e4b7fd1bb88656744a0b9d69ba35c8074 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Thu, 16 Jan 2025 17:27:46 +0900 Subject: [PATCH 01/25] feat(velocity_smoother): introduce diagnostics (#9933) * feat(velocity_smoother): introduce diagnostics Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka Signed-off-by: TetsuKawa --- .../include/autoware/velocity_smoother/node.hpp | 11 ++++++----- .../autoware_velocity_smoother/src/node.cpp | 17 +++++++++-------- 2 files changed, 15 insertions(+), 13 deletions(-) diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp index fc0066b1a84f3..d519356c7aa18 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp @@ -20,6 +20,7 @@ #include "autoware/osqp_interface/osqp_interface.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" +#include "autoware/universe_utils/ros/diagnostics_interface.hpp" #include "autoware/universe_utils/ros/logger_level_configure.hpp" #include "autoware/universe_utils/ros/polling_subscriber.hpp" #include "autoware/universe_utils/ros/self_pose_listener.hpp" @@ -45,8 +46,7 @@ #include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" -#include "tier4_planning_msgs/msg/stop_speed_exceeded.hpp" // temporary -#include "tier4_planning_msgs/msg/velocity_limit.hpp" // temporary +#include "tier4_planning_msgs/msg/velocity_limit.hpp" // temporary #include "visualization_msgs/msg/marker_array.hpp" #include @@ -61,6 +61,7 @@ namespace autoware::velocity_smoother using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; using TrajectoryPoints = std::vector; +using autoware::universe_utils::DiagnosticsInterface; using autoware_adapi_v1_msgs::msg::OperationModeState; using autoware_internal_debug_msgs::msg::Float32Stamped; using autoware_internal_debug_msgs::msg::Float64Stamped; @@ -68,8 +69,7 @@ using geometry_msgs::msg::AccelWithCovarianceStamped; using geometry_msgs::msg::Pose; using geometry_msgs::msg::PoseStamped; using nav_msgs::msg::Odometry; -using tier4_planning_msgs::msg::StopSpeedExceeded; // temporary -using tier4_planning_msgs::msg::VelocityLimit; // temporary +using tier4_planning_msgs::msg::VelocityLimit; // temporary using visualization_msgs::msg::MarkerArray; struct Motion @@ -89,7 +89,6 @@ class VelocitySmootherNode : public rclcpp::Node private: rclcpp::Publisher::SharedPtr pub_trajectory_; rclcpp::Publisher::SharedPtr pub_virtual_wall_; - rclcpp::Publisher::SharedPtr pub_over_stop_velocity_; rclcpp::Subscription::SharedPtr sub_current_trajectory_; autoware::universe_utils::InterProcessPollingSubscriber sub_current_odometry_{ this, "/localization/kinematic_state"}; @@ -290,6 +289,8 @@ class VelocitySmootherNode : public rclcpp::Node std::unique_ptr published_time_publisher_; mutable std::shared_ptr time_keeper_{nullptr}; + + std::unique_ptr diagnostics_interface_{nullptr}; }; } // namespace autoware::velocity_smoother diff --git a/planning/autoware_velocity_smoother/src/node.cpp b/planning/autoware_velocity_smoother/src/node.cpp index 1e0f6119ac21e..1f2b1cad6e42a 100644 --- a/planning/autoware_velocity_smoother/src/node.cpp +++ b/planning/autoware_velocity_smoother/src/node.cpp @@ -36,7 +36,8 @@ namespace autoware::velocity_smoother { VelocitySmootherNode::VelocitySmootherNode(const rclcpp::NodeOptions & node_options) -: Node("velocity_smoother", node_options) +: Node("velocity_smoother", node_options), + diagnostics_interface_(std::make_unique(this, "velocity_smoother")) { using std::placeholders::_1; @@ -63,7 +64,6 @@ VelocitySmootherNode::VelocitySmootherNode(const rclcpp::NodeOptions & node_opti pub_velocity_limit_ = create_publisher( "~/output/current_velocity_limit_mps", rclcpp::QoS{1}.transient_local()); pub_dist_to_stopline_ = create_publisher("~/distance_to_stopline", 1); - pub_over_stop_velocity_ = create_publisher("~/stop_speed_exceeded", 1); sub_current_trajectory_ = create_subscription( "~/input/trajectory", 1, std::bind(&VelocitySmootherNode::onCurrentTrajectory, this, _1)); @@ -444,6 +444,7 @@ void VelocitySmootherNode::onCurrentTrajectory(const Trajectory::ConstSharedPtr RCLCPP_DEBUG(get_logger(), "========================= run start ========================="); stop_watch_.tic(); + diagnostics_interface_->clear(); base_traj_raw_ptr_ = msg; // receive data @@ -524,6 +525,10 @@ void VelocitySmootherNode::onCurrentTrajectory(const Trajectory::ConstSharedPtr // Publish Calculation Time publishStopWatchTime(); + + // Publish diagnostics + diagnostics_interface_->publish(now()); + RCLCPP_DEBUG(get_logger(), "========================== run() end ==========================\n\n"); } @@ -906,12 +911,8 @@ void VelocitySmootherNode::overwriteStopPoint( input_stop_vel, output_stop_vel, over_stop_velocity_warn_thr_); } - { - StopSpeedExceeded msg{}; - msg.stamp = this->now(); - msg.stop_speed_exceeded = is_stop_velocity_exceeded; - pub_over_stop_velocity_->publish(msg); - } + diagnostics_interface_->add_key_value( + "The velocity on the stop point is larger than 0.", is_stop_velocity_exceeded); } void VelocitySmootherNode::applyExternalVelocityLimit(TrajectoryPoints & traj) const From 6b4eb62dcd591b312410169090c97e40a4e5adfe Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara <32741405+kyoichi-sugahara@users.noreply.github.com> Date: Thu, 16 Jan 2025 17:36:00 +0900 Subject: [PATCH 02/25] test(start_planner): disable GenerateValidFreespacePullOutPath test (#9937) Signed-off-by: kyoichi-sugahara Signed-off-by: TetsuKawa --- .../test/test_freespace_pull_out.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_freespace_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_freespace_pull_out.cpp index 79c58c37f0dcf..5078c463d7c36 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_freespace_pull_out.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_freespace_pull_out.cpp @@ -78,7 +78,7 @@ class TestFreespacePullOut : public ::testing::Test } }; -TEST_F(TestFreespacePullOut, GenerateValidFreespacePullOutPath) +TEST_F(TestFreespacePullOut, DISABLED_GenerateValidFreespacePullOutPath) { const auto start_pose = geometry_msgs::build() From 3b3263cadb33c01c6fddcd19a11a04871555f82b Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Thu, 16 Jan 2025 18:30:31 +0900 Subject: [PATCH 03/25] feat(planning_factor)!: remove velocity_factor, steering_factor and introduce planning_factor (#9927) Signed-off-by: satoshi-ota Signed-off-by: Mamoru Sobue Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> Co-authored-by: satoshi-ota Signed-off-by: TetsuKawa --- .../planner_interface.hpp | 11 +- .../obstacle_cruise_planner/type_alias.hpp | 5 - .../obstacle_cruise_planner/utils.hpp | 3 - .../package.xml | 1 - .../src/node.cpp | 1 + .../pid_based_planner/pid_based_planner.cpp | 7 +- .../src/planner_interface.cpp | 18 +- .../src/utils.cpp | 21 -- .../package.xml | 1 - .../src/debug_marker.cpp | 33 +-- .../src/debug_marker.hpp | 14 +- .../src/interface.cpp | 4 +- .../src/interface.hpp | 3 +- .../src/manager.cpp | 2 +- .../manager.hpp | 2 +- .../scene.hpp | 3 +- .../src/scene.cpp | 5 +- .../src/manager.cpp | 4 +- .../goal_planner_module.hpp | 5 +- .../src/goal_planner_module.cpp | 26 +-- .../src/manager.cpp | 2 +- .../interface.hpp | 1 + .../src/interface.cpp | 48 ++-- .../src/manager.cpp | 2 +- .../behavior_path_planner_node.hpp | 7 +- .../src/behavior_path_planner_node.cpp | 39 ++-- .../src/planner_manager.cpp | 3 +- .../interface/scene_module_interface.hpp | 44 ++-- .../scene_module_manager_interface.hpp | 50 +--- .../scene_module_manager_interface.cpp | 9 +- .../manager.hpp | 2 +- .../sampling_planner_module.hpp | 3 +- .../src/sampling_planner_module.cpp | 5 +- .../manager.hpp | 2 +- .../behavior_path_side_shift_module/scene.hpp | 3 +- .../src/scene.cpp | 5 +- .../manager.hpp | 2 +- .../start_planner_module.hpp | 11 +- .../src/start_planner_module.cpp | 39 ++-- .../manager.hpp | 2 +- .../scene.hpp | 15 +- .../src/scene.cpp | 20 +- .../scene.hpp | 4 +- .../src/decisions.cpp | 14 +- .../src/manager.cpp | 2 +- .../src/scene.cpp | 7 +- .../src/manager.cpp | 4 +- .../src/scene_crosswalk.cpp | 23 +- .../src/scene_crosswalk.hpp | 4 +- .../src/manager.cpp | 3 +- .../src/scene.cpp | 6 +- .../src/scene.hpp | 4 +- .../src/manager.cpp | 9 +- .../src/scene_intersection.cpp | 167 ++++++++----- .../src/scene_intersection.hpp | 4 +- .../src/scene_merge_from_private_road.cpp | 13 +- .../src/scene_merge_from_private_road.hpp | 4 +- .../src/manager.cpp | 3 +- .../src/scene.cpp | 8 +- .../src/scene.hpp | 4 +- .../src/manager.cpp | 2 +- .../src/scene_no_stopping_area.cpp | 7 +- .../src/scene_no_stopping_area.hpp | 4 +- .../src/manager.cpp | 4 +- .../src/scene_occlusion_spot.cpp | 7 +- .../src/scene_occlusion_spot.hpp | 4 +- .../scene_module_interface.hpp | 22 +- .../src/scene_module_interface.cpp | 7 +- .../scene_module_interface_with_rtc.hpp | 4 +- .../src/scene_module_interface_with_rtc.cpp | 6 +- .../src/manager.cpp | 3 +- .../src/scene.cpp | 22 +- .../src/scene.hpp | 4 +- .../src/manager.cpp | 2 +- .../src/scene.cpp | 7 +- .../src/scene.hpp | 5 +- .../src/manager.cpp | 2 +- .../src/scene.cpp | 7 +- .../src/scene.hpp | 5 +- .../test/test_scene.cpp | 5 +- .../src/manager.cpp | 5 +- .../src/scene.cpp | 7 +- .../src/scene.hpp | 6 +- .../src/manager.cpp | 3 +- .../src/scene.cpp | 6 +- .../src/scene.hpp | 4 +- .../src/manager.cpp | 3 +- .../src/scene.cpp | 7 +- .../src/scene.hpp | 5 +- .../src/manager.cpp | 3 +- .../src/scene_walkway.cpp | 16 +- .../src/scene_walkway.hpp | 4 +- .../src/dynamic_obstacle_stop_module.cpp | 16 +- .../src/dynamic_obstacle_stop_module.hpp | 1 + .../src/obstacle_velocity_limiter_module.cpp | 1 - .../src/out_of_lane_module.cpp | 16 +- .../src/out_of_lane_module.hpp | 1 + .../plugin_module_interface.hpp | 10 +- .../velocity_planning_result.hpp | 2 - .../README.md | 8 +- .../src/node.cpp | 10 - .../src/node.hpp | 2 - .../src/planner_manager.cpp | 2 + system/autoware_default_adapi/package.xml | 1 + .../autoware_default_adapi/src/planning.cpp | 221 ++++++++++++++---- .../autoware_default_adapi/src/planning.hpp | 20 +- 106 files changed, 731 insertions(+), 544 deletions(-) diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp index ef6ae1662dcee..823771e173c48 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp @@ -15,6 +15,7 @@ #ifndef AUTOWARE__OBSTACLE_CRUISE_PLANNER__PLANNER_INTERFACE_HPP_ #define AUTOWARE__OBSTACLE_CRUISE_PLANNER__PLANNER_INTERFACE_HPP_ +#include "autoware/motion_utils/factor/planning_factor_interface.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/obstacle_cruise_planner/common_structs.hpp" #include "autoware/obstacle_cruise_planner/stop_planning_debug_info.hpp" @@ -47,15 +48,15 @@ class PlannerInterface rclcpp::Node & node, const LongitudinalInfo & longitudinal_info, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const EgoNearestParam & ego_nearest_param, const std::shared_ptr debug_data_ptr) - : longitudinal_info_(longitudinal_info), + : planning_factor_interface_{std::make_unique( + &node, "obstacle_cruise_planner")}, + longitudinal_info_(longitudinal_info), vehicle_info_(vehicle_info), ego_nearest_param_(ego_nearest_param), debug_data_ptr_(debug_data_ptr), slow_down_param_(SlowDownParam(node)), stop_param_(StopParam(node, longitudinal_info)) { - velocity_factors_pub_ = - node.create_publisher("/planning/velocity_factors/obstacle_cruise", 1); stop_speed_exceeded_pub_ = node.create_publisher("~/output/stop_speed_exceeded", 1); metrics_pub_ = node.create_publisher("~/metrics", 10); @@ -101,6 +102,7 @@ class PlannerInterface const std::optional & stop_pose = std::nullopt, const std::optional & stop_obstacle = std::nullopt); void publishMetrics(const rclcpp::Time & current_time); + void publishPlanningFactors() { planning_factor_interface_->publish(); } void clearMetrics(); void onParam(const std::vector & parameters) @@ -128,6 +130,8 @@ class PlannerInterface double getSafeDistanceMargin() const { return longitudinal_info_.safe_distance_margin; } protected: + std::unique_ptr planning_factor_interface_; + // Parameters bool enable_debug_info_{false}; bool enable_calculation_time_info_{false}; @@ -145,7 +149,6 @@ class PlannerInterface stop_watch_; // Publishers - rclcpp::Publisher::SharedPtr velocity_factors_pub_; rclcpp::Publisher::SharedPtr stop_speed_exceeded_pub_; rclcpp::Publisher::SharedPtr metrics_pub_; diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/type_alias.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/type_alias.hpp index 04badd2a956ef..31344903b6809 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/type_alias.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/type_alias.hpp @@ -17,8 +17,6 @@ #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" -#include "autoware_adapi_v1_msgs/msg/planning_behavior.hpp" -#include "autoware_adapi_v1_msgs/msg/velocity_factor_array.hpp" #include "autoware_perception_msgs/msg/predicted_object.hpp" #include "autoware_perception_msgs/msg/predicted_objects.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" @@ -39,9 +37,6 @@ #include using autoware::vehicle_info_utils::VehicleInfo; -using autoware_adapi_v1_msgs::msg::PlanningBehavior; -using autoware_adapi_v1_msgs::msg::VelocityFactor; -using autoware_adapi_v1_msgs::msg::VelocityFactorArray; using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/utils.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/utils.hpp index d929647d9bcd0..ebabd96a54608 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/utils.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/utils.hpp @@ -94,9 +94,6 @@ size_t getIndexWithLongitudinalOffset( return 0; } -VelocityFactorArray makeVelocityFactorArray( - const rclcpp::Time & time, const std::string & behavior = PlanningBehavior::ROUTE_OBSTACLE, - const std::optional pose = std::nullopt); } // namespace obstacle_cruise_utils #endif // AUTOWARE__OBSTACLE_CRUISE_PLANNER__UTILS_HPP_ diff --git a/planning/autoware_obstacle_cruise_planner/package.xml b/planning/autoware_obstacle_cruise_planner/package.xml index 8583d0639e5ce..7c6d0b286c9b0 100644 --- a/planning/autoware_obstacle_cruise_planner/package.xml +++ b/planning/autoware_obstacle_cruise_planner/package.xml @@ -18,7 +18,6 @@ ament_cmake_auto autoware_cmake - autoware_adapi_v1_msgs autoware_interpolation autoware_lanelet2_extension autoware_motion_utils diff --git a/planning/autoware_obstacle_cruise_planner/src/node.cpp b/planning/autoware_obstacle_cruise_planner/src/node.cpp index 6931ff58e8457..4242f0d559946 100644 --- a/planning/autoware_obstacle_cruise_planner/src/node.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/node.cpp @@ -726,6 +726,7 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms // 8. Publish debug data published_time_publisher_->publish_if_subscribed(trajectory_pub_, output_traj.header.stamp); planner_ptr_->publishMetrics(now()); + planner_ptr_->publishPlanningFactors(); publishDebugMarker(); publishDebugInfo(); objects_of_interest_marker_interface_.publishMarkerArray(); diff --git a/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp b/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp index 544597f05ff75..20b564addf149 100644 --- a/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp @@ -335,9 +335,10 @@ std::vector PIDBasedPlanner::planCruise( debug_data_ptr_->obstacles_to_cruise.push_back(cruise_obstacle_info->obstacle); debug_data_ptr_->cruise_metrics = makeMetrics("PIDBasedPlanner", "cruise", planner_data); - velocity_factors_pub_->publish(obstacle_cruise_utils::makeVelocityFactorArray( - planner_data.current_time, PlanningBehavior::ADAPTIVE_CRUISE, - stop_traj_points.at(wall_idx).pose)); + planning_factor_interface_->add( + stop_traj_points, planner_data.ego_pose, stop_traj_points.at(wall_idx).pose, + tier4_planning_msgs::msg::PlanningFactor::NONE, + tier4_planning_msgs::msg::SafetyFactorArray{}); } // do cruise planning diff --git a/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp b/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp index b08bb10ef69cf..b32d7f3a1bcbb 100644 --- a/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp @@ -182,8 +182,6 @@ std::vector PlannerInterface::generateStopTrajectory( : std::abs(vehicle_info_.min_longitudinal_offset_m); if (stop_obstacles.empty()) { - velocity_factors_pub_->publish( - obstacle_cruise_utils::makeVelocityFactorArray(planner_data.current_time)); // delete marker const auto markers = autoware::motion_utils::createDeletedStopVirtualWallMarker(planner_data.current_time, 0); @@ -336,8 +334,10 @@ std::vector PlannerInterface::generateStopTrajectory( // Publish Stop Reason const auto stop_pose = output_traj_points.at(*zero_vel_idx).pose; - velocity_factors_pub_->publish(obstacle_cruise_utils::makeVelocityFactorArray( - planner_data.current_time, PlanningBehavior::ROUTE_OBSTACLE, stop_pose)); + planning_factor_interface_->add( + output_traj_points, planner_data.ego_pose, stop_pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}); // Store stop reason debug data debug_data_ptr_->stop_metrics = makeMetrics("PlannerInterface", "stop", planner_data, stop_pose, *determined_stop_obstacle); @@ -590,10 +590,14 @@ std::vector PlannerInterface::generateSlowDownTrajectory( const auto markers = autoware::motion_utils::createSlowDownVirtualWallMarker( slow_down_traj_points.at(slow_down_wall_idx).pose, "obstacle slow down", planner_data.current_time, i, abs_ego_offset, "", planner_data.is_driving_forward); - velocity_factors_pub_->publish(obstacle_cruise_utils::makeVelocityFactorArray( - planner_data.current_time, PlanningBehavior::ROUTE_OBSTACLE, - slow_down_traj_points.at(slow_down_wall_idx).pose)); autoware::universe_utils::appendMarkerArray(markers, &debug_data_ptr_->slow_down_wall_marker); + planning_factor_interface_->add( + slow_down_traj_points, planner_data.ego_pose, + slow_down_traj_points.at(*slow_down_start_idx).pose, + slow_down_traj_points.at(*slow_down_end_idx).pose, + tier4_planning_msgs::msg::PlanningFactor::SLOW_DOWN, + tier4_planning_msgs::msg::SafetyFactorArray{}, planner_data.is_driving_forward, + stable_slow_down_vel); } // add debug virtual wall diff --git a/planning/autoware_obstacle_cruise_planner/src/utils.cpp b/planning/autoware_obstacle_cruise_planner/src/utils.cpp index 82f4e6978181f..b27def0bfcbe5 100644 --- a/planning/autoware_obstacle_cruise_planner/src/utils.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/utils.cpp @@ -118,25 +118,4 @@ std::vector getClosestStopObstacles(const std::vector pose) -{ - VelocityFactorArray velocity_factor_array; - velocity_factor_array.header.frame_id = "map"; - velocity_factor_array.header.stamp = time; - - if (pose) { - using distance_type = VelocityFactor::_distance_type; - VelocityFactor velocity_factor; - velocity_factor.behavior = behavior; - velocity_factor.pose = pose.value(); - velocity_factor.distance = std::numeric_limits::quiet_NaN(); - velocity_factor.status = VelocityFactor::UNKNOWN; - velocity_factor.detail = std::string(); - velocity_factor_array.factors.push_back(velocity_factor); - } - return velocity_factor_array; -} - } // namespace obstacle_cruise_utils diff --git a/planning/autoware_surround_obstacle_checker/package.xml b/planning/autoware_surround_obstacle_checker/package.xml index 4a6f946b8eb90..8bbc0b6bf4e02 100644 --- a/planning/autoware_surround_obstacle_checker/package.xml +++ b/planning/autoware_surround_obstacle_checker/package.xml @@ -14,7 +14,6 @@ autoware_cmake eigen3_cmake_module - autoware_adapi_v1_msgs autoware_motion_utils autoware_perception_msgs autoware_planning_msgs diff --git a/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp b/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp index bbd1cac04ed89..2acf6ba1c92f5 100644 --- a/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp +++ b/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp @@ -67,7 +67,9 @@ SurroundObstacleCheckerDebugNode::SurroundObstacleCheckerDebugNode( 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), +: planning_factor_interface_{std::make_unique( + &node, "surround_obstacle_checker")}, + vehicle_info_(vehicle_info), object_label_(object_label), surround_check_front_distance_(surround_check_front_distance), surround_check_side_distance_(surround_check_side_distance), @@ -77,8 +79,6 @@ SurroundObstacleCheckerDebugNode::SurroundObstacleCheckerDebugNode( clock_(clock) { debug_viz_pub_ = node.create_publisher("~/debug/marker", 1); - velocity_factor_pub_ = - node.create_publisher("/planning/velocity_factors/surround_obstacle", 1); vehicle_footprint_pub_ = node.create_publisher("~/debug/footprint", 1); vehicle_footprint_offset_pub_ = node.create_publisher("~/debug/footprint_offset", 1); @@ -143,8 +143,12 @@ void SurroundObstacleCheckerDebugNode::publish() debug_viz_pub_->publish(visualization_msg); /* publish stop reason for autoware api */ - const auto velocity_factor_msg = makeVelocityFactorArray(); - velocity_factor_pub_->publish(velocity_factor_msg); + if (stop_pose_ptr_ != nullptr) { + planning_factor_interface_->add( + 0.0, *stop_pose_ptr_, tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}); + } + planning_factor_interface_->publish(); /* reset variables */ stop_pose_ptr_ = nullptr; @@ -170,25 +174,6 @@ MarkerArray SurroundObstacleCheckerDebugNode::makeVisualizationMarker() return msg; } -VelocityFactorArray SurroundObstacleCheckerDebugNode::makeVelocityFactorArray() -{ - VelocityFactorArray velocity_factor_array; - velocity_factor_array.header.frame_id = "map"; - velocity_factor_array.header.stamp = clock_->now(); - - if (stop_pose_ptr_) { - using distance_type = VelocityFactor::_distance_type; - VelocityFactor velocity_factor; - velocity_factor.behavior = PlanningBehavior::SURROUNDING_OBSTACLE; - velocity_factor.pose = *stop_pose_ptr_; - velocity_factor.distance = std::numeric_limits::quiet_NaN(); - velocity_factor.status = VelocityFactor::UNKNOWN; - velocity_factor.detail = std::string(); - velocity_factor_array.factors.push_back(velocity_factor); - } - return velocity_factor_array; -} - PolygonStamped SurroundObstacleCheckerDebugNode::boostPolygonToPolygonStamped( const Polygon2d & boost_polygon, const double & z) { diff --git a/planning/autoware_surround_obstacle_checker/src/debug_marker.hpp b/planning/autoware_surround_obstacle_checker/src/debug_marker.hpp index b2c350c1b4698..c49e277f2dc6c 100644 --- a/planning/autoware_surround_obstacle_checker/src/debug_marker.hpp +++ b/planning/autoware_surround_obstacle_checker/src/debug_marker.hpp @@ -15,13 +15,13 @@ #ifndef DEBUG_MARKER_HPP_ #define DEBUG_MARKER_HPP_ +#include #include #include -#include -#include #include #include +#include #include #include @@ -34,10 +34,10 @@ namespace autoware::surround_obstacle_checker { using autoware::vehicle_info_utils::VehicleInfo; -using autoware_adapi_v1_msgs::msg::PlanningBehavior; -using autoware_adapi_v1_msgs::msg::VelocityFactor; -using autoware_adapi_v1_msgs::msg::VelocityFactorArray; using geometry_msgs::msg::PolygonStamped; +using tier4_planning_msgs::msg::ControlPoint; +using tier4_planning_msgs::msg::PlanningFactor; +using tier4_planning_msgs::msg::PlanningFactorArray; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; @@ -65,12 +65,13 @@ class SurroundObstacleCheckerDebugNode private: rclcpp::Publisher::SharedPtr debug_viz_pub_; - rclcpp::Publisher::SharedPtr velocity_factor_pub_; rclcpp::Publisher::SharedPtr vehicle_footprint_pub_; rclcpp::Publisher::SharedPtr vehicle_footprint_offset_pub_; rclcpp::Publisher::SharedPtr vehicle_footprint_recover_offset_pub_; + std::unique_ptr planning_factor_interface_; + autoware::vehicle_info_utils::VehicleInfo vehicle_info_; std::string object_label_; double surround_check_front_distance_; @@ -80,7 +81,6 @@ class SurroundObstacleCheckerDebugNode geometry_msgs::msg::Pose self_pose_; MarkerArray makeVisualizationMarker(); - VelocityFactorArray makeVelocityFactorArray(); PolygonStamped boostPolygonToPolygonStamped(const Polygon2d & boost_polygon, const double & z); diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp index 3080ba1045b41..37bee1a178f90 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp @@ -32,13 +32,15 @@ AvoidanceByLaneChangeInterface::AvoidanceByLaneChangeInterface( const std::shared_ptr & avoidance_by_lane_change_parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map) + objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr & planning_factor_interface) : LaneChangeInterface{ name, node, parameters, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map, + planning_factor_interface, std::make_unique(parameters, avoidance_by_lane_change_parameters)} { } diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.hpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.hpp index c7fbba34b1adb..6e524f3c91341 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.hpp @@ -40,7 +40,8 @@ class AvoidanceByLaneChangeInterface : public LaneChangeInterface const std::shared_ptr & avoidance_by_lane_change_parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map); + objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr & planning_factor_interface); bool isExecutionRequested() const override; diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp index c54774a575332..2bf8a60676d72 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp @@ -190,7 +190,7 @@ SMIPtr AvoidanceByLaneChangeModuleManager::createNewSceneModuleInstance() { return std::make_unique( name_, *node_, parameters_, avoidance_parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_); + objects_of_interest_marker_interface_ptr_map_, planning_factor_interface_); } } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/manager.hpp index f09196b2cc8e1..ea8f09c4f9336 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/manager.hpp @@ -42,7 +42,7 @@ class DynamicObstacleAvoidanceModuleManager : public SceneModuleManagerInterface { return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_); + objects_of_interest_marker_interface_ptr_map_, planning_factor_interface_); } void updateModuleParams(const std::vector & parameters) override; diff --git a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/scene.hpp index 44b56f4ea0990..8987e7c0446ee 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/scene.hpp @@ -351,7 +351,8 @@ class DynamicObstacleAvoidanceModule : public SceneModuleInterface std::shared_ptr parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map); + objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr planning_factor_interface); void updateModuleParams(const std::any & parameters) override { diff --git a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp index dc7efebe88fba..145e0df2675b4 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp @@ -331,8 +331,9 @@ DynamicObstacleAvoidanceModule::DynamicObstacleAvoidanceModule( std::shared_ptr parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map) -: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT + objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map, planning_factor_interface}, // NOLINT parameters_{std::move(parameters)}, target_objects_manager_{TargetObjectsManager( parameters_->successive_num_to_entry_dynamic_avoidance_condition, diff --git a/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/src/manager.cpp index 45c997f364633..0b438d1a1758e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/src/manager.cpp @@ -28,7 +28,7 @@ ExternalRequestLaneChangeRightModuleManager::createNewSceneModuleInstance() { return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_, + objects_of_interest_marker_interface_ptr_map_, planning_factor_interface_, std::make_unique(parameters_, direction_)); } @@ -37,7 +37,7 @@ ExternalRequestLaneChangeLeftModuleManager::createNewSceneModuleInstance() { return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_, + objects_of_interest_marker_interface_ptr_map_, planning_factor_interface_, std::make_unique(parameters_, direction_)); } diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp index 41524b1b8193c..84367fdd91b80 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -240,7 +240,8 @@ class GoalPlannerModule : public SceneModuleInterface const std::shared_ptr & parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map); + objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr planning_factor_interface); ~GoalPlannerModule() { @@ -455,7 +456,7 @@ class GoalPlannerModule : public SceneModuleInterface // steering factor void updateSteeringFactor( const PullOverContextData & context_data, const std::array & pose, - const std::array distance, const uint16_t type); + const std::array distance); // rtc std::pair calcDistanceToPathChange( diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index 0a6185127fcf0..c7e32a47a59f9 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -66,8 +66,9 @@ GoalPlannerModule::GoalPlannerModule( const std::shared_ptr & parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map) -: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT + objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map, planning_factor_interface}, // NOLINT parameters_{parameters}, vehicle_info_{autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()}, is_lane_parking_cb_running_{false}, @@ -120,9 +121,6 @@ GoalPlannerModule::GoalPlannerModule( initializeSafetyCheckParameters(); } - steering_factor_interface_.init(PlanningBehavior::GOAL_PLANNER); - velocity_factor_interface_.init(PlanningBehavior::GOAL_PLANNER); - /** * NOTE: Add `universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);` to functions called * from the main thread only. @@ -1344,19 +1342,20 @@ void GoalPlannerModule::setTurnSignalInfo( void GoalPlannerModule::updateSteeringFactor( const PullOverContextData & context_data, const std::array & pose, - const std::array distance, const uint16_t type) + const std::array distance) { - const uint16_t steering_factor_direction = std::invoke([&]() { + const uint16_t planning_factor_direction = std::invoke([&]() { const auto turn_signal = calcTurnSignalInfo(context_data); if (turn_signal.turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) { - return SteeringFactor::LEFT; + return PlanningFactor::SHIFT_LEFT; } else if (turn_signal.turn_signal.command == TurnIndicatorsCommand::ENABLE_RIGHT) { - return SteeringFactor::RIGHT; + return PlanningFactor::SHIFT_RIGHT; } - return SteeringFactor::STRAIGHT; + return PlanningFactor::NONE; }); - steering_factor_interface_.set(pose, distance, steering_factor_direction, type, ""); + planning_factor_interface_->add( + distance[0], distance[1], pose[0], pose[1], planning_factor_direction, SafetyFactorArray{}); } void GoalPlannerModule::decideVelocity(PullOverPath & pull_over_path) @@ -1568,10 +1567,9 @@ void GoalPlannerModule::postProcess() updateSteeringFactor( context_data, {pull_over_path.start_pose(), pull_over_path.modified_goal_pose()}, - {distance_to_path_change.first, distance_to_path_change.second}, - has_decided_path ? SteeringFactor::TURNING : SteeringFactor::APPROACHING); + {distance_to_path_change.first, distance_to_path_change.second}); - setVelocityFactor(pull_over_path.full_path()); + set_longitudinal_planning_factor(pull_over_path.full_path()); } BehaviorModuleOutput GoalPlannerModule::planWaitingApproval() diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp index 6bbc34a495080..a99252c923328 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp @@ -31,7 +31,7 @@ std::unique_ptr GoalPlannerModuleManager::createNewSceneMo { return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_); + objects_of_interest_marker_interface_ptr_map_, planning_factor_interface_); } GoalPlannerParameters GoalPlannerModuleManager::initGoalPlannerParameters( diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp index 4b97fb0d069a0..82b042d1135d2 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp @@ -53,6 +53,7 @@ class LaneChangeInterface : public SceneModuleInterface const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr & planning_factor_interface, std::unique_ptr && module_type); LaneChangeInterface(const LaneChangeInterface &) = delete; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp index ebcd4eb4809fc..4519b20d4644d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp @@ -40,15 +40,14 @@ LaneChangeInterface::LaneChangeInterface( const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr & planning_factor_interface, std::unique_ptr && module_type) -: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT +: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map, planning_factor_interface}, // NOLINT parameters_{std::move(parameters)}, module_type_{std::move(module_type)} { module_type_->setTimeKeeper(getTimeKeeper()); logger_ = utils::lane_change::getLogger(module_type_->getModuleTypeStr()); - steering_factor_interface_.init(PlanningBehavior::LANE_CHANGE); - velocity_factor_interface_.init(PlanningBehavior::LANE_CHANGE); } void LaneChangeInterface::processOnExit() @@ -145,7 +144,7 @@ BehaviorModuleOutput LaneChangeInterface::plan() } } - setVelocityFactor(output.path); + set_longitudinal_planning_factor(output.path); return output; } @@ -179,7 +178,7 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() isExecutionReady(), State::WAITING_FOR_EXECUTION); is_abort_path_approved_ = false; - setVelocityFactor(out.path); + set_longitudinal_planning_factor(out.path); return out; } @@ -384,15 +383,6 @@ MarkerArray LaneChangeInterface::getModuleVirtualWall() void LaneChangeInterface::updateSteeringFactorPtr(const BehaviorModuleOutput & output) { universe_utils::ScopedTimeTrack st(__func__, *getTimeKeeper()); - const auto steering_factor_direction = std::invoke([&]() { - if (module_type_->getDirection() == Direction::LEFT) { - return SteeringFactor::LEFT; - } - if (module_type_->getDirection() == Direction::RIGHT) { - return SteeringFactor::RIGHT; - } - return SteeringFactor::UNKNOWN; - }); const auto current_position = module_type_->getEgoPosition(); const auto status = module_type_->getLaneChangeStatus(); @@ -401,24 +391,34 @@ void LaneChangeInterface::updateSteeringFactorPtr(const BehaviorModuleOutput & o const auto finish_distance = autoware::motion_utils::calcSignedArcLength( output.path.points, current_position, status.lane_change_path.info.shift_line.end.position); - steering_factor_interface_.set( - {status.lane_change_path.info.shift_line.start, status.lane_change_path.info.shift_line.end}, - {start_distance, finish_distance}, steering_factor_direction, SteeringFactor::TURNING, ""); + const auto planning_factor_direction = std::invoke([&]() { + if (module_type_->getDirection() == Direction::LEFT) { + return PlanningFactor::SHIFT_LEFT; + } + if (module_type_->getDirection() == Direction::RIGHT) { + return PlanningFactor::SHIFT_RIGHT; + } + return PlanningFactor::UNKNOWN; + }); + + planning_factor_interface_->add( + start_distance, finish_distance, status.lane_change_path.info.shift_line.start, + status.lane_change_path.info.shift_line.end, planning_factor_direction, SafetyFactorArray{}); } void LaneChangeInterface::updateSteeringFactorPtr( const CandidateOutput & output, const LaneChangePath & selected_path) const { - const uint16_t steering_factor_direction = std::invoke([&output]() { + const uint16_t planning_factor_direction = std::invoke([&output]() { if (output.lateral_shift > 0.0) { - return SteeringFactor::LEFT; + return PlanningFactor::SHIFT_LEFT; } - return SteeringFactor::RIGHT; + return PlanningFactor::SHIFT_RIGHT; }); - steering_factor_interface_.set( - {selected_path.info.shift_line.start, selected_path.info.shift_line.end}, - {output.start_distance_to_path_change, output.finish_distance_to_path_change}, - steering_factor_direction, SteeringFactor::APPROACHING, ""); + planning_factor_interface_->add( + output.start_distance_to_path_change, output.finish_distance_to_path_change, + selected_path.info.shift_line.start, selected_path.info.shift_line.end, + planning_factor_direction, SafetyFactorArray{}); } } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp index f26cf79e8b120..450497fb2ca29 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp @@ -302,7 +302,7 @@ std::unique_ptr LaneChangeModuleManager::createNewSceneMod { return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_, + objects_of_interest_marker_interface_ptr_map_, planning_factor_interface_, std::make_unique(parameters_, LaneChangeModuleType::NORMAL, direction_)); } diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp index ebe7c47e6fab2..7f7ecd7326b08 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp @@ -20,7 +20,7 @@ #include "autoware/universe_utils/ros/logger_level_configure.hpp" #include "planner_manager.hpp" -#include +#include #include #include @@ -51,7 +51,7 @@ namespace autoware::behavior_path_planner { -using autoware::motion_utils::SteeringFactorInterface; +using autoware::motion_utils::PlanningFactorInterface; using autoware_adapi_v1_msgs::msg::OperationModeState; using autoware_map_msgs::msg::LaneletMapBin; using autoware_perception_msgs::msg::PredictedObject; @@ -121,7 +121,6 @@ class BehaviorPathPlannerNode : public rclcpp::Node rclcpp::Publisher::SharedPtr bound_publisher_; rclcpp::Publisher::SharedPtr modified_goal_publisher_; rclcpp::Publisher::SharedPtr reroute_availability_publisher_; - rclcpp::Publisher::SharedPtr pub_steering_factors_; rclcpp::TimerBase::SharedPtr timer_; std::map::SharedPtr> path_candidate_publishers_; @@ -136,7 +135,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node std::shared_ptr planner_manager_; - SteeringFactorInterface steering_factor_interface_; + std::unique_ptr planning_factor_interface_; std::mutex mutex_pd_; // mutex for planner_data_ std::mutex mutex_manager_; // mutex for bt_manager_ or planner_manager_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp index 7758ab530e88c..568d432e1faa3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp @@ -32,7 +32,9 @@ using autoware::vehicle_info_utils::VehicleInfoUtils; using tier4_planning_msgs::msg::PathChangeModuleId; BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & node_options) -: Node("behavior_path_planner", node_options) +: Node("behavior_path_planner", node_options), + planning_factor_interface_{ + std::make_unique(this, "behavior_path_planner")} { using std::placeholders::_1; using std::chrono_literals::operator""ms; @@ -51,8 +53,6 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod const auto durable_qos = rclcpp::QoS(1).transient_local(); modified_goal_publisher_ = create_publisher("~/output/modified_goal", durable_qos); - pub_steering_factors_ = - create_publisher("/planning/steering_factor/intersection", 1); reroute_availability_publisher_ = create_publisher("~/output/is_reroute_available", 1); debug_avoidance_msg_array_publisher_ = @@ -115,8 +115,6 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod turn_signal_search_time, turn_signal_intersection_angle_threshold_deg); } - steering_factor_interface_.init(PlanningBehavior::INTERSECTION); - // Start timer { const auto planning_hz = declare_parameter("planning_hz"); @@ -463,33 +461,22 @@ void BehaviorPathPlannerNode::publish_steering_factor( const auto [intersection_flag, approaching_intersection_flag] = planner_data->turn_signal_decider.getIntersectionTurnSignalFlag(); if (intersection_flag || approaching_intersection_flag) { - const uint16_t steering_factor_direction = std::invoke([&turn_signal]() { - if (turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) { - return SteeringFactor::LEFT; - } - return SteeringFactor::RIGHT; - }); - const auto [intersection_pose, intersection_distance] = planner_data->turn_signal_decider.getIntersectionPoseAndDistance(); - steering_factor_interface_.set( - {intersection_pose, intersection_pose}, {intersection_distance, intersection_distance}, - steering_factor_direction, SteeringFactor::TURNING, ""); - } else { - steering_factor_interface_.reset(); - } - - autoware_adapi_v1_msgs::msg::SteeringFactorArray steering_factor_array; - steering_factor_array.header.frame_id = "map"; - steering_factor_array.header.stamp = this->now(); + const uint16_t planning_factor_direction = std::invoke([&turn_signal]() { + if (turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) { + return PlanningFactor::TURN_LEFT; + } + return PlanningFactor::TURN_RIGHT; + }); - const auto steering_factor = steering_factor_interface_.get(); - if (steering_factor.behavior != PlanningBehavior::UNKNOWN) { - steering_factor_array.factors.emplace_back(steering_factor); + planning_factor_interface_->add( + intersection_distance, intersection_distance, intersection_pose, intersection_pose, + planning_factor_direction, SafetyFactorArray{}); } - pub_steering_factors_->publish(steering_factor_array); + planning_factor_interface_->publish(); } void BehaviorPathPlannerNode::publish_reroute_availability() const diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp index 041b7ad20a107..322209d46732b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp @@ -182,8 +182,7 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr & da std::for_each(manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) { m->updateObserver(); m->publishRTCStatus(); - m->publishSteeringFactor(); - m->publishVelocityFactor(); + m->publish_planning_factors(); }); generateCombinedDrivableArea(result_output.valid_output, data); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp index 391ac92411220..c96bc601b6403 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp @@ -21,8 +21,7 @@ #include "autoware/behavior_path_planner_common/utils/utils.hpp" #include -#include -#include +#include #include #include #include @@ -36,7 +35,6 @@ #include #include -#include #include #include #include @@ -54,18 +52,16 @@ namespace autoware::behavior_path_planner { -using autoware::motion_utils::SteeringFactorInterface; -using autoware::motion_utils::VelocityFactorInterface; +using autoware::motion_utils::PlanningFactorInterface; using autoware::objects_of_interest_marker_interface::ColorName; using autoware::objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface; using autoware::rtc_interface::RTCInterface; using autoware::universe_utils::calcOffsetPose; using autoware::universe_utils::generateUUID; -using autoware_adapi_v1_msgs::msg::PlanningBehavior; -using autoware_adapi_v1_msgs::msg::SteeringFactor; -using autoware_adapi_v1_msgs::msg::VelocityFactor; using tier4_planning_msgs::msg::AvoidanceDebugMsgArray; using tier4_planning_msgs::msg::PathWithLaneId; +using tier4_planning_msgs::msg::PlanningFactor; +using tier4_planning_msgs::msg::SafetyFactorArray; using tier4_rtc_msgs::msg::State; using unique_identifier_msgs::msg::UUID; using visualization_msgs::msg::MarkerArray; @@ -86,13 +82,15 @@ class SceneModuleInterface const std::string & name, rclcpp::Node & node, std::unordered_map> rtc_interface_ptr_map, std::unordered_map> - objects_of_interest_marker_interface_ptr_map) + objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr planning_factor_interface) : name_{name}, logger_{node.get_logger().get_child(name)}, clock_{node.get_clock()}, rtc_interface_ptr_map_(std::move(rtc_interface_ptr_map)), objects_of_interest_marker_interface_ptr_map_( std::move(objects_of_interest_marker_interface_ptr_map)), + planning_factor_interface_{planning_factor_interface}, time_keeper_(std::make_shared()) { for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) { @@ -184,8 +182,6 @@ class SceneModuleInterface unlockNewModuleLaunch(); unlockOutputPath(); - reset_factor(); - processOnExit(); } @@ -258,16 +254,6 @@ class SceneModuleInterface ModuleStatus getCurrentStatus() const { return current_state_; } - void reset_factor() - { - steering_factor_interface_.reset(); - velocity_factor_interface_.reset(); - } - - auto get_steering_factor() const -> SteeringFactor { return steering_factor_interface_.get(); } - - auto get_velocity_factor() const -> VelocityFactor { return velocity_factor_interface_.get(); } - std::string name() const { return name_; } PoseWithDetailOpt getStopPose() const @@ -558,11 +544,12 @@ class SceneModuleInterface } } - void setVelocityFactor(const PathWithLaneId & path) + void set_longitudinal_planning_factor(const PathWithLaneId & path) { if (stop_pose_.has_value()) { - velocity_factor_interface_.set( - path.points, getEgoPose(), stop_pose_.value().pose, VelocityFactor::APPROACHING, "stop"); + planning_factor_interface_->add( + path.points, getEgoPose(), stop_pose_.value().pose, PlanningFactor::STOP, + SafetyFactorArray{}); return; } @@ -570,8 +557,9 @@ class SceneModuleInterface return; } - velocity_factor_interface_.set( - path.points, getEgoPose(), slow_pose_.value().pose, VelocityFactor::APPROACHING, "slow down"); + planning_factor_interface_->add( + path.points, getEgoPose(), slow_pose_.value().pose, PlanningFactor::SLOW_DOWN, + SafetyFactorArray{}); } void setDrivableLanes(const std::vector & drivable_lanes); @@ -627,9 +615,7 @@ class SceneModuleInterface std::unordered_map> objects_of_interest_marker_interface_ptr_map_; - mutable SteeringFactorInterface steering_factor_interface_; - - mutable VelocityFactorInterface velocity_factor_interface_; + mutable std::shared_ptr planning_factor_interface_; mutable PoseWithDetailOpt stop_pose_{std::nullopt}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp index 73b133952935b..446829d1d0f9d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp @@ -24,8 +24,6 @@ #include #include -#include -#include #include #include @@ -42,8 +40,6 @@ using autoware::motion_utils::createDeadLineVirtualWallMarker; using autoware::motion_utils::createSlowDownVirtualWallMarker; using autoware::motion_utils::createStopVirtualWallMarker; using autoware::universe_utils::toHexString; -using autoware_adapi_v1_msgs::msg::SteeringFactorArray; -using autoware_adapi_v1_msgs::msg::VelocityFactorArray; using unique_identifier_msgs::msg::UUID; using SceneModulePtr = std::shared_ptr; using SceneModuleObserver = std::weak_ptr; @@ -106,45 +102,7 @@ class SceneModuleManagerInterface } } - void publishSteeringFactor() - { - SteeringFactorArray steering_factor_array; - steering_factor_array.header.frame_id = "map"; - steering_factor_array.header.stamp = node_->now(); - - for (const auto & m : observers_) { - if (m.expired()) { - continue; - } - - const auto steering_factor = m.lock()->get_steering_factor(); - if (steering_factor.behavior != PlanningBehavior::UNKNOWN) { - steering_factor_array.factors.emplace_back(steering_factor); - } - } - - pub_steering_factors_->publish(steering_factor_array); - } - - void publishVelocityFactor() - { - VelocityFactorArray velocity_factor_array; - velocity_factor_array.header.frame_id = "map"; - velocity_factor_array.header.stamp = node_->now(); - - for (const auto & m : observers_) { - if (m.expired()) { - continue; - } - - const auto velocity_factor = m.lock()->get_velocity_factor(); - if (velocity_factor.behavior != PlanningBehavior::UNKNOWN) { - velocity_factor_array.factors.emplace_back(velocity_factor); - } - } - - pub_velocity_factors_->publish(velocity_factor_array); - } + void publish_planning_factors() { planning_factor_interface_->publish(); } void publishVirtualWall() const { @@ -304,10 +262,6 @@ class SceneModuleManagerInterface rclcpp::Publisher::SharedPtr pub_drivable_lanes_; - rclcpp::Publisher::SharedPtr pub_steering_factors_; - - rclcpp::Publisher::SharedPtr pub_velocity_factors_; - rclcpp::Publisher::SharedPtr pub_processing_time_; std::string name_; @@ -318,6 +272,8 @@ class SceneModuleManagerInterface std::unique_ptr idle_module_ptr_; + std::shared_ptr planning_factor_interface_; + std::unordered_map> rtc_interface_ptr_map_; std::unordered_map> diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/interface/scene_module_manager_interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/interface/scene_module_manager_interface.cpp index b91db3a740cfe..11be3b28a7f94 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/interface/scene_module_manager_interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/interface/scene_module_manager_interface.cpp @@ -61,10 +61,11 @@ void SceneModuleManagerInterface::initInterface( pub_drivable_lanes_ = node->create_publisher("~/drivable_lanes/" + name_, 20); pub_processing_time_ = node->create_publisher( "~/processing_time/" + name_, 20); - pub_steering_factors_ = - node->create_publisher("/planning/steering_factor/" + name_, 1); - pub_velocity_factors_ = - node->create_publisher("/planning/velocity_factors/" + name_, 1); + } + + // planning factor + { + planning_factor_interface_ = std::make_shared(node, name_); } // misc diff --git a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/manager.hpp index 28c310ae20ec2..f25c073e879da 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/manager.hpp @@ -38,7 +38,7 @@ class SamplingPlannerModuleManager : public SceneModuleManagerInterface { return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_); + objects_of_interest_marker_interface_ptr_map_, planning_factor_interface_); } void updateModuleParams( diff --git a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/sampling_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/sampling_planner_module.hpp index c9b75f7fa96de..f84fe45d08ca4 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/sampling_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/sampling_planner_module.hpp @@ -86,7 +86,8 @@ class SamplingPlannerModule : public SceneModuleInterface const std::shared_ptr & parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map); + objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr planning_factor_interface); bool isExecutionRequested() const override; bool isExecutionReady() const override; diff --git a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/sampling_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/sampling_planner_module.cpp index fac3cc92de1d6..5bb57f17ce813 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/sampling_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/sampling_planner_module.cpp @@ -42,8 +42,9 @@ SamplingPlannerModule::SamplingPlannerModule( const std::shared_ptr & parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map) -: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT + objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map, planning_factor_interface}, // NOLINT vehicle_info_{autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()} { internal_params_ = std::make_shared(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/manager.hpp index b495e6619a1c7..2b4a20ddc8cca 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/manager.hpp @@ -39,7 +39,7 @@ class SideShiftModuleManager : public SceneModuleManagerInterface { return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_); + objects_of_interest_marker_interface_ptr_map_, planning_factor_interface_); } void updateModuleParams(const std::vector & parameters) override; diff --git a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/scene.hpp index 15d81ff45abcf..c8a5e47287477 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/scene.hpp @@ -45,7 +45,8 @@ class SideShiftModule : public SceneModuleInterface const std::shared_ptr & parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map); + objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr planning_factor_interface); bool isExecutionRequested() const override; bool isExecutionReady() const override; diff --git a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/scene.cpp index 22ee7ab77e077..9536c1ba17336 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/scene.cpp @@ -42,8 +42,9 @@ SideShiftModule::SideShiftModule( const std::shared_ptr & parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map) -: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT + objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map, planning_factor_interface}, // NOLINT parameters_{parameters} { } diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/manager.hpp index 5d3d224673124..68b1e67e484c3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/manager.hpp @@ -38,7 +38,7 @@ class StartPlannerModuleManager : public SceneModuleManagerInterface { return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_); + objects_of_interest_marker_interface_ptr_map_, planning_factor_interface_); } void updateModuleParams(const std::vector & parameters) override; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp index cf9227c2387f8..9b1186cb5fc66 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp @@ -86,7 +86,8 @@ class StartPlannerModule : public SceneModuleInterface const std::shared_ptr & parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map); + objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr planning_factor_interface); ~StartPlannerModule() override { @@ -198,18 +199,18 @@ class StartPlannerModule : public SceneModuleInterface bool requiresDynamicObjectsCollisionDetection() const; - uint16_t getSteeringFactorDirection( + uint16_t getPlanningFactorDirection( const autoware::behavior_path_planner::BehaviorModuleOutput & output) const { switch (output.turn_signal_info.turn_signal.command) { case TurnIndicatorsCommand::ENABLE_LEFT: - return SteeringFactor::LEFT; + return PlanningFactor::SHIFT_LEFT; case TurnIndicatorsCommand::ENABLE_RIGHT: - return SteeringFactor::RIGHT; + return PlanningFactor::SHIFT_RIGHT; default: - return SteeringFactor::STRAIGHT; + return PlanningFactor::NONE; } }; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp index 0bac50a37fdbf..ae05bcf4c084e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp @@ -62,8 +62,9 @@ StartPlannerModule::StartPlannerModule( const std::shared_ptr & parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map) -: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT + objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map, planning_factor_interface}, // NOLINT parameters_{parameters}, vehicle_info_{autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()}, is_freespace_planner_cb_running_{false} @@ -89,9 +90,6 @@ StartPlannerModule::StartPlannerModule( std::bind(&StartPlannerModule::onFreespacePlannerTimer, this), freespace_planner_timer_cb_group_); } - - steering_factor_interface_.init(PlanningBehavior::START_PLANNER); - velocity_factor_interface_.init(PlanningBehavior::START_PLANNER); } void StartPlannerModule::onFreespacePlannerTimer() @@ -732,9 +730,9 @@ BehaviorModuleOutput StartPlannerModule::plan() setDrivableAreaInfo(output); - setVelocityFactor(output.path); + set_longitudinal_planning_factor(output.path); - const auto steering_factor_direction = getSteeringFactorDirection(output); + const auto planning_factor_direction = getPlanningFactorDirection(output); if (status_.driving_forward) { const double start_distance = autoware::motion_utils::calcSignedArcLength( @@ -744,9 +742,9 @@ BehaviorModuleOutput StartPlannerModule::plan() path.points, planner_data_->self_odometry->pose.pose.position, status_.pull_out_path.end_pose.position); updateRTCStatus(start_distance, finish_distance); - steering_factor_interface_.set( - {status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, - {start_distance, finish_distance}, steering_factor_direction, SteeringFactor::TURNING, ""); + planning_factor_interface_->add( + start_distance, finish_distance, status_.pull_out_path.start_pose, + status_.pull_out_path.end_pose, planning_factor_direction, SafetyFactorArray{}); setDebugData(); return output; } @@ -754,9 +752,9 @@ BehaviorModuleOutput StartPlannerModule::plan() path.points, planner_data_->self_odometry->pose.pose.position, status_.pull_out_path.start_pose.position); updateRTCStatus(0.0, distance); - steering_factor_interface_.set( - {status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, {0.0, distance}, - steering_factor_direction, SteeringFactor::TURNING, ""); + planning_factor_interface_->add( + 0.0, distance, status_.pull_out_path.start_pose, status_.pull_out_path.end_pose, + planning_factor_direction, SafetyFactorArray{}); setDebugData(); @@ -839,7 +837,7 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() setDrivableAreaInfo(output); - const auto steering_factor_direction = getSteeringFactorDirection(output); + const auto planning_factor_direction = getPlanningFactorDirection(output); if (status_.driving_forward) { const double start_distance = autoware::motion_utils::calcSignedArcLength( @@ -849,10 +847,9 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() stop_path.points, planner_data_->self_odometry->pose.pose.position, status_.pull_out_path.end_pose.position); updateRTCStatus(start_distance, finish_distance); - steering_factor_interface_.set( - {status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, - {start_distance, finish_distance}, steering_factor_direction, SteeringFactor::APPROACHING, - ""); + planning_factor_interface_->add( + start_distance, finish_distance, status_.pull_out_path.start_pose, + status_.pull_out_path.end_pose, planning_factor_direction, SafetyFactorArray{}); setDebugData(); return output; @@ -861,9 +858,9 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() stop_path.points, planner_data_->self_odometry->pose.pose.position, status_.pull_out_path.start_pose.position); updateRTCStatus(0.0, distance); - steering_factor_interface_.set( - {status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, {0.0, distance}, - steering_factor_direction, SteeringFactor::APPROACHING, ""); + planning_factor_interface_->add( + 0.0, distance, status_.pull_out_path.start_pose, status_.pull_out_path.end_pose, + planning_factor_direction, SafetyFactorArray{}); setDebugData(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/manager.hpp index 895390f91cc16..6296acd8e71f7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/manager.hpp @@ -41,7 +41,7 @@ class StaticObstacleAvoidanceModuleManager : public SceneModuleManagerInterface { return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_); + objects_of_interest_marker_interface_ptr_map_, planning_factor_interface_); } void updateModuleParams(const std::vector & parameters) override; diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp index 5848cc75f148e..247e22e1aedad 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp @@ -46,7 +46,8 @@ class StaticObstacleAvoidanceModule : public SceneModuleInterface const std::string & name, rclcpp::Node & node, std::shared_ptr parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map); + objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr & planning_factor_interface); CandidateOutput planCandidate() const override; BehaviorModuleOutput plan() override; @@ -131,9 +132,9 @@ class StaticObstacleAvoidanceModule : public SceneModuleInterface } if (finish_distance > -1.0e-03) { - steering_factor_interface_.set( - {left_shift.start_pose, left_shift.finish_pose}, {start_distance, finish_distance}, - SteeringFactor::LEFT, SteeringFactor::TURNING, ""); + planning_factor_interface_->add( + start_distance, finish_distance, left_shift.start_pose, left_shift.finish_pose, + PlanningFactor::SHIFT_LEFT, SafetyFactorArray{}); } } @@ -151,9 +152,9 @@ class StaticObstacleAvoidanceModule : public SceneModuleInterface } if (finish_distance > -1.0e-03) { - steering_factor_interface_.set( - {right_shift.start_pose, right_shift.finish_pose}, {start_distance, finish_distance}, - SteeringFactor::RIGHT, SteeringFactor::TURNING, ""); + planning_factor_interface_->add( + start_distance, finish_distance, right_shift.start_pose, right_shift.finish_pose, + PlanningFactor::SHIFT_RIGHT, SafetyFactorArray{}); } } } diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp index 48729c9c4fa0c..739b5b21f59dc 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp @@ -79,14 +79,13 @@ StaticObstacleAvoidanceModule::StaticObstacleAvoidanceModule( const std::string & name, rclcpp::Node & node, std::shared_ptr parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map) -: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT + objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr & planning_factor_interface) +: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map, planning_factor_interface}, // NOLINT helper_{std::make_shared(parameters)}, parameters_{parameters}, generator_{parameters} { - steering_factor_interface_.init(PlanningBehavior::AVOIDANCE); - velocity_factor_interface_.init(PlanningBehavior::AVOIDANCE); } bool StaticObstacleAvoidanceModule::isExecutionRequested() const @@ -776,7 +775,7 @@ void StaticObstacleAvoidanceModule::updateEgoBehavior( insertReturnDeadLine(isBestEffort(parameters_->policy_deceleration), path); - setVelocityFactor(path.path); + set_longitudinal_planning_factor(path.path); } bool StaticObstacleAvoidanceModule::isSafePath( @@ -1209,13 +1208,12 @@ CandidateOutput StaticObstacleAvoidanceModule::planCandidate() const output.start_distance_to_path_change = sl_front.start_longitudinal; output.finish_distance_to_path_change = sl_back.end_longitudinal; - const uint16_t steering_factor_direction = std::invoke([&output]() { - return output.lateral_shift > 0.0 ? SteeringFactor::LEFT : SteeringFactor::RIGHT; + const uint16_t planning_factor_direction = std::invoke([&output]() { + return output.lateral_shift > 0.0 ? PlanningFactor::SHIFT_LEFT : PlanningFactor::SHIFT_RIGHT; }); - steering_factor_interface_.set( - {sl_front.start, sl_back.end}, - {output.start_distance_to_path_change, output.finish_distance_to_path_change}, - steering_factor_direction, SteeringFactor::APPROACHING, ""); + planning_factor_interface_->add( + output.start_distance_to_path_change, output.finish_distance_to_path_change, sl_front.start, + sl_back.end, planning_factor_direction, SafetyFactorArray{}, true, 0.0, output.lateral_shift); output.path_candidate = shifted_path.path; return output; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/scene.hpp index 5e8ef9fc8a063..91eff0b01346a 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/scene.hpp @@ -74,7 +74,9 @@ class BlindSpotModule : public SceneModuleInterfaceWithRTC BlindSpotModule( const int64_t module_id, const int64_t lane_id, const TurnDirection turn_direction, const std::shared_ptr planner_data, const PlannerParam & planner_param, - const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock); + const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface); /** * @brief plan go-stop velocity at traffic crossing with collision check between reference path diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/decisions.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/decisions.cpp index 64e1435167a89..a978c074e0018 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/decisions.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/decisions.cpp @@ -101,8 +101,11 @@ void BlindSpotModule::reactRTCApprovalByDecision( decision.stop_line_idx, planner_data_->vehicle_info_.max_longitudinal_offset_m, *path); const auto stop_pose = path->points.at(decision.stop_line_idx).point.pose; - velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN); + planning_factor_interface_->add( + path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, + "blind_spot(module is judging as UNSAFE)"); } return; } @@ -132,8 +135,11 @@ void BlindSpotModule::reactRTCApprovalByDecision( decision.stop_line_idx, planner_data_->vehicle_info_.max_longitudinal_offset_m, *path); const auto stop_pose = path->points.at(decision.stop_line_idx).point.pose; - velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN); + planning_factor_interface_->add( + path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, + "blind_spot(module is judging as SAFE and RTC is not approved)"); } return; } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/manager.cpp index d6cae9004600a..ee6516f025709 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/manager.cpp @@ -59,7 +59,7 @@ void BlindSpotModuleManager::launchNewModules(const tier4_planning_msgs::msg::Pa registerModule(std::make_shared( module_id, lane_id, turn_direction, planner_data_, planner_param_, - logger_.get_child("blind_spot_module"), clock_)); + logger_.get_child("blind_spot_module"), clock_, time_keeper_, planning_factor_interface_)); generateUUID(module_id); updateRTCStatus( getUUID(module_id), true, State::WAITING_FOR_EXECUTION, std::numeric_limits::lowest(), diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp index 8a9401646aaea..4fca5f821f934 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp @@ -40,14 +40,15 @@ namespace bg = boost::geometry; BlindSpotModule::BlindSpotModule( const int64_t module_id, const int64_t lane_id, const TurnDirection turn_direction, const std::shared_ptr planner_data, const PlannerParam & planner_param, - const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterfaceWithRTC(module_id, logger, clock), + const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterfaceWithRTC(module_id, logger, clock, time_keeper, planning_factor_interface), lane_id_(lane_id), planner_param_{planner_param}, turn_direction_(turn_direction), is_over_pass_judge_line_(false) { - velocity_factor_.init(PlanningBehavior::REAR_CHECK); sibling_straight_lanelet_ = getSiblingStraightLanelet( planner_data->route_handler_->getLaneletMapPtr()->laneletLayer.get(lane_id_), planner_data->route_handler_->getRoutingGraphPtr()); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp index cdaff7225a7d2..5bfd31b75976a 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp @@ -192,8 +192,8 @@ void CrosswalkModuleManager::launchNewModules(const PathWithLaneId & path) // NOTE: module_id is always a lane id so that isModuleRegistered works correctly in the case // where both regulatory element and non-regulatory element crosswalks exist. registerModule(std::make_shared( - node_, road_lanelet_id, crosswalk_lanelet_id, reg_elem_id, lanelet_map_ptr, p, logger, - clock_)); + node_, road_lanelet_id, crosswalk_lanelet_id, reg_elem_id, lanelet_map_ptr, p, logger, clock_, + time_keeper_, planning_factor_interface_)); generateUUID(crosswalk_lanelet_id); updateRTCStatus( getUUID(crosswalk_lanelet_id), true, State::WAITING_FOR_EXECUTION, diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 64f7e9e14dcd1..844647e33f8c0 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -34,12 +34,12 @@ #include #include #include +#include #include #include #include #include #include - namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; @@ -175,13 +175,14 @@ CrosswalkModule::CrosswalkModule( rclcpp::Node & node, const int64_t lane_id, const int64_t module_id, const std::optional & reg_elem_id, const lanelet::LaneletMapPtr & lanelet_map_ptr, const PlannerParam & planner_param, const rclcpp::Logger & logger, - const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterfaceWithRTC(module_id, logger, clock), + const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterfaceWithRTC(module_id, logger, clock, time_keeper, planning_factor_interface), module_id_(module_id), planner_param_(planner_param), use_regulatory_element_(reg_elem_id) { - velocity_factor_.init(PlanningBehavior::CROSSWALK); passed_safety_slow_point_ = false; if (use_regulatory_element_) { @@ -898,9 +899,11 @@ void CrosswalkModule::applySlowDown( } } if (slowdown_pose) - velocity_factor_.set( - output.points, planner_data_->current_odometry->pose, *slowdown_pose, - VelocityFactor::APPROACHING); + planning_factor_interface_->add( + output.points, planner_data_->current_odometry->pose, *slowdown_pose, *slowdown_pose, + tier4_planning_msgs::msg::PlanningFactor::SLOW_DOWN, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, + safety_slow_down_speed, 0.0 /*shift distance*/, "crosswalk_safety_slowdown_for_approaching"); } void CrosswalkModule::applySlowDownByLanelet2Map( @@ -1377,9 +1380,11 @@ void CrosswalkModule::planStop( // Plan stop insertDecelPointWithDebugInfo(stop_factor->stop_pose.position, 0.0, ego_path); - velocity_factor_.set( + planning_factor_interface_->add( ego_path.points, planner_data_->current_odometry->pose, stop_factor->stop_pose, - VelocityFactor::UNKNOWN); + stop_factor->stop_pose, tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0 /*velocity*/, + 0.0 /*shift distance*/, "crosswalk_stop"); } bool CrosswalkModule::checkRestartSuppression( diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index d5a9e463c730b..f32ee96623822 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -334,7 +334,9 @@ class CrosswalkModule : public SceneModuleInterfaceWithRTC rclcpp::Node & node, const int64_t lane_id, const int64_t module_id, const std::optional & reg_elem_id, const lanelet::LaneletMapPtr & lanelet_map_ptr, const PlannerParam & planner_param, const rclcpp::Logger & logger, - const rclcpp::Clock::SharedPtr clock); + const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface); bool modifyPathVelocity(PathWithLaneId * path) override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.cpp index fb2c17d9e3745..f231d7adc3326 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.cpp @@ -64,7 +64,8 @@ void DetectionAreaModuleManager::launchNewModules( if (!isModuleRegistered(module_id)) { registerModule(std::make_shared( module_id, lane_id, *detection_area_with_lane_id.first, planner_param_, - logger_.get_child("detection_area_module"), clock_)); + logger_.get_child("detection_area_module"), clock_, time_keeper_, + planning_factor_interface_)); } } } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp index 031c45753022e..59f6f20937f41 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp @@ -36,8 +36,10 @@ DetectionAreaModule::DetectionAreaModule( const int64_t module_id, const int64_t lane_id, const lanelet::autoware::DetectionArea & detection_area_reg_elem, const PlannerParam & planner_param, const rclcpp::Logger & logger, - const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), + const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), lane_id_(lane_id), detection_area_reg_elem_(detection_area_reg_elem), state_(State::GO), diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp index 9224cf4624687..1d3e5f3540ff3 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp @@ -68,7 +68,9 @@ class DetectionAreaModule : public SceneModuleInterface const int64_t module_id, const int64_t lane_id, const lanelet::autoware::DetectionArea & detection_area_reg_elem, const PlannerParam & planner_param, const rclcpp::Logger & logger, - const rclcpp::Clock::SharedPtr clock); + const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface); bool modifyPathVelocity(PathWithLaneId * path) override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp index 41bdbe3e43c8b..d6f7dfbac92d4 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp @@ -338,7 +338,8 @@ void IntersectionModuleManager::launchNewModules( } const auto new_module = std::make_shared( module_id, lane_id, planner_data_, intersection_param_, associative_ids, turn_direction, - has_traffic_light, node_, logger_.get_child("intersection_module"), clock_); + has_traffic_light, node_, logger_.get_child("intersection_module"), clock_, time_keeper_, + planning_factor_interface_); generateUUID(module_id); /* set RTC status as non_occluded status initially */ const UUID uuid = getUUID(new_module->getModuleId()); @@ -526,7 +527,8 @@ void MergeFromPrivateModuleManager::launchNewModules( planning_utils::getAssociativeIntersectionLanelets(ll, lanelet_map, routing_graph); registerModule(std::make_shared( module_id, lane_id, planner_data_, merge_from_private_area_param_, associative_ids, - logger_.get_child("merge_from_private_road_module"), clock_)); + logger_.get_child("merge_from_private_road_module"), clock_, time_keeper_, + planning_factor_interface_)); continue; } } else { @@ -540,7 +542,8 @@ void MergeFromPrivateModuleManager::launchNewModules( planning_utils::getAssociativeIntersectionLanelets(ll, lanelet_map, routing_graph); registerModule(std::make_shared( module_id, lane_id, planner_data_, merge_from_private_area_param_, associative_ids, - logger_.get_child("merge_from_private_road_module"), clock_)); + logger_.get_child("merge_from_private_road_module"), clock_, time_keeper_, + planning_factor_interface_)); continue; } } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp index 93a13a4d62737..2d5da0f070ed9 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -18,7 +18,6 @@ #include // for toGeomPoly #include -#include #include #include // for toPolygon2d #include @@ -52,8 +51,10 @@ IntersectionModule::IntersectionModule( [[maybe_unused]] std::shared_ptr planner_data, const PlannerParam & planner_param, const std::set & associative_ids, const std::string & turn_direction, const bool has_traffic_light, rclcpp::Node & node, - const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterfaceWithRTC(module_id, logger, clock), + const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterfaceWithRTC(module_id, logger, clock, time_keeper, planning_factor_interface), planner_param_(planner_param), lane_id_(lane_id), associative_ids_(associative_ids), @@ -61,8 +62,6 @@ IntersectionModule::IntersectionModule( has_traffic_light_(has_traffic_light), occlusion_uuid_(autoware::universe_utils::generateUUID()) { - velocity_factor_.init(PlanningBehavior::INTERSECTION); - { collision_state_machine_.setMarginTime( planner_param_.collision_detection.collision_detection_hold_time); @@ -705,7 +704,7 @@ void reactRTCApprovalByDecisionResult( [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, [[maybe_unused]] const double baselink2front, [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path, - [[maybe_unused]] VelocityFactorInterface * velocity_factor, + [[maybe_unused]] motion_utils::PlanningFactorInterface * planning_factor_interface, [[maybe_unused]] IntersectionModule::DebugData * debug_data) { static_assert("Unsupported type passed to reactRTCByDecisionResult"); @@ -720,7 +719,7 @@ void reactRTCApprovalByDecisionResult( [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, [[maybe_unused]] const double baselink2front, [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path, - [[maybe_unused]] VelocityFactorInterface * velocity_factor, + [[maybe_unused]] motion_utils::PlanningFactorInterface * planning_factor_interface, [[maybe_unused]] IntersectionModule::DebugData * debug_data) { return; @@ -734,7 +733,7 @@ void reactRTCApprovalByDecisionResult( [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, [[maybe_unused]] const double baselink2front, [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path, - [[maybe_unused]] VelocityFactorInterface * velocity_factor, + [[maybe_unused]] motion_utils::PlanningFactorInterface * planning_factor_interface, [[maybe_unused]] IntersectionModule::DebugData * debug_data) { return; @@ -746,7 +745,8 @@ void reactRTCApprovalByDecisionResult( const StuckStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, - VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) + motion_utils::PlanningFactorInterface * planning_factor_interface, + IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -760,9 +760,12 @@ void reactRTCApprovalByDecisionResult( debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(pure StuckStop)"); } } if (!rtc_occlusion_approved && decision_result.occlusion_stopline_idx) { @@ -771,9 +774,13 @@ void reactRTCApprovalByDecisionResult( debug_data->occlusion_stop_wall_pose = planning_utils::getAheadPose(occlusion_stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(closest_idx).point.pose, - path->points.at(occlusion_stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(occlusion_stopline_idx).point.pose, + path->points.at(occlusion_stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(StuckStop with occlusion)"); } } return; @@ -785,7 +792,8 @@ void reactRTCApprovalByDecisionResult( const YieldStuckStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, - VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) + motion_utils::PlanningFactorInterface * planning_factor_interface, + IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -799,9 +807,12 @@ void reactRTCApprovalByDecisionResult( debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(Yield Stuck)"); } } return; @@ -813,7 +824,8 @@ void reactRTCApprovalByDecisionResult( const NonOccludedCollisionStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, - VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) + motion_utils::PlanningFactorInterface * planning_factor_interface, + IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -825,9 +837,12 @@ void reactRTCApprovalByDecisionResult( debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(CollisionStop)"); } } if (!rtc_occlusion_approved) { @@ -836,9 +851,12 @@ void reactRTCApprovalByDecisionResult( debug_data->occlusion_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(CollisionStop with occlusion)"); } } return; @@ -849,7 +867,8 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const FirstWaitBeforeOcclusion & decision_result, const IntersectionModule::PlannerParam & planner_param, const double baselink2front, - tier4_planning_msgs::msg::PathWithLaneId * path, VelocityFactorInterface * velocity_factor, + tier4_planning_msgs::msg::PathWithLaneId * path, + motion_utils::PlanningFactorInterface * planning_factor_interface, IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( @@ -862,9 +881,12 @@ void reactRTCApprovalByDecisionResult( debug_data->occlusion_first_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(FirstWaitBeforeOcclusion with collision)"); } } if (!rtc_occlusion_approved) { @@ -881,9 +903,12 @@ void reactRTCApprovalByDecisionResult( debug_data->occlusion_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(FirstWaitBeforeOcclusion with occlusion)"); } } return; @@ -894,7 +919,8 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const PeekingTowardOcclusion & decision_result, const IntersectionModule::PlannerParam & planner_param, const double baselink2front, - tier4_planning_msgs::msg::PathWithLaneId * path, VelocityFactorInterface * velocity_factor, + tier4_planning_msgs::msg::PathWithLaneId * path, + motion_utils::PlanningFactorInterface * planning_factor_interface, IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( @@ -920,9 +946,13 @@ void reactRTCApprovalByDecisionResult( debug_data->static_occlusion_with_traffic_light_timeout = decision_result.static_occlusion_timeout; { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(occlusion_peeking_stopline).point.pose, VelocityFactor::UNKNOWN); + path->points.at(occlusion_peeking_stopline).point.pose, + path->points.at(occlusion_peeking_stopline).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(PeekingToOcclusion)"); } } if (!rtc_default_approved) { @@ -931,9 +961,12 @@ void reactRTCApprovalByDecisionResult( debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(PeekingToOcclusion while stopping for collision)"); } } return; @@ -945,7 +978,8 @@ void reactRTCApprovalByDecisionResult( const OccludedCollisionStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, - VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) + motion_utils::PlanningFactorInterface * planning_factor_interface, + IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -957,9 +991,12 @@ void reactRTCApprovalByDecisionResult( debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(CollisionStop with occlusion)"); } } if (!rtc_occlusion_approved) { @@ -972,9 +1009,12 @@ void reactRTCApprovalByDecisionResult( debug_data->static_occlusion_with_traffic_light_timeout = decision_result.static_occlusion_timeout; { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(CollisionStop with occlusion)"); } } return; @@ -986,7 +1026,8 @@ void reactRTCApprovalByDecisionResult( const OccludedAbsenceTrafficLight & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, - VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) + motion_utils::PlanningFactorInterface * planning_factor_interface, + IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -998,9 +1039,13 @@ void reactRTCApprovalByDecisionResult( debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, + "intersection(Occlusion without traffic light, collision detected)"); } } if (!rtc_occlusion_approved && decision_result.temporal_stop_before_attention_required) { @@ -1009,9 +1054,12 @@ void reactRTCApprovalByDecisionResult( debug_data->occlusion_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(Occlusion without traffic light)"); } } if (!rtc_occlusion_approved && !decision_result.temporal_stop_before_attention_required) { @@ -1032,7 +1080,8 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const Safe & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, - VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) + motion_utils::PlanningFactorInterface * planning_factor_interface, + IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -1043,9 +1092,12 @@ void reactRTCApprovalByDecisionResult( debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(Safe, but RTC interrupted for collision)"); } } if (!rtc_occlusion_approved) { @@ -1054,9 +1106,12 @@ void reactRTCApprovalByDecisionResult( debug_data->occlusion_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(Safe, but RTC interrupted for occlusion)"); } } return; @@ -1068,7 +1123,8 @@ void reactRTCApprovalByDecisionResult( const FullyPrioritized & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, - VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) + motion_utils::PlanningFactorInterface * planning_factor_interface, + IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -1080,9 +1136,12 @@ void reactRTCApprovalByDecisionResult( debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(FullyPrioritized, collision detected)"); } } if (!rtc_occlusion_approved) { @@ -1091,9 +1150,13 @@ void reactRTCApprovalByDecisionResult( debug_data->occlusion_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, + "intersection(FullyPrioritized, RTC for occlusion is interrupting)"); } } return; @@ -1107,7 +1170,7 @@ void IntersectionModule::reactRTCApproval( VisitorSwitch{[&](const auto & decision) { reactRTCApprovalByDecisionResult( activated_, occlusion_activated_, decision, planner_param_, baselink2front, path, - &velocity_factor_, &debug_data_); + planning_factor_interface_.get(), &debug_data_); }}, decision_result); return; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp index 6c31be2ce83b9..db0cfbe98d53c 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -304,7 +304,9 @@ class IntersectionModule : public SceneModuleInterfaceWithRTC const int64_t module_id, const int64_t lane_id, std::shared_ptr planner_data, const PlannerParam & planner_param, const std::set & associative_ids, const std::string & turn_direction, const bool has_traffic_light, rclcpp::Node & node, - const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock); + const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface); /** *********************************************************** diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp index 3dfdcc36c0cff..fc442337198af 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp @@ -41,12 +41,13 @@ MergeFromPrivateRoadModule::MergeFromPrivateRoadModule( const int64_t module_id, const int64_t lane_id, [[maybe_unused]] std::shared_ptr planner_data, const PlannerParam & planner_param, const std::set & associative_ids, - const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), + const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), lane_id_(lane_id), associative_ids_(associative_ids) { - velocity_factor_.init(PlanningBehavior::MERGE); planner_param_ = planner_param; state_machine_.setState(StateMachine::State::STOP); } @@ -153,8 +154,10 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path) /* get stop point and stop factor */ const auto & stop_pose = path->points.at(stopline_idx).point.pose; - velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN); + planning_factor_interface_->add( + path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, "merge_from_private"); const double signed_arc_dist_to_stop_point = autoware::motion_utils::calcSignedArcLength( path->points, current_pose.position, path->points.at(stopline_idx).point.pose.position); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp index dc8bf1a96a7a2..fe446d304d9e0 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp @@ -61,7 +61,9 @@ class MergeFromPrivateRoadModule : public SceneModuleInterface MergeFromPrivateRoadModule( const int64_t module_id, const int64_t lane_id, std::shared_ptr planner_data, const PlannerParam & planner_param, const std::set & associative_ids, - const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock); + const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface); /** * @brief plan go-stop velocity at traffic crossing with collision check between reference path diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/manager.cpp index 9cb1153a8edd2..517568c93bd35 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/manager.cpp @@ -51,7 +51,8 @@ void NoDrivableLaneModuleManager::launchNewModules( } registerModule(std::make_shared( - module_id, lane_id, planner_param_, logger_.get_child("no_drivable_lane_module"), clock_)); + module_id, lane_id, planner_param_, logger_.get_child("no_drivable_lane_module"), clock_, + time_keeper_, planning_factor_interface_)); } } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp index 8365251904b18..f32210bc502bf 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp @@ -21,14 +21,18 @@ #include #include +#include + namespace autoware::behavior_velocity_planner { using autoware::universe_utils::createPoint; NoDrivableLaneModule::NoDrivableLaneModule( const int64_t module_id, const int64_t lane_id, const PlannerParam & planner_param, - const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), + const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), lane_id_(lane_id), planner_param_(planner_param), debug_data_(), diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.hpp index d8c5e3e0425d1..835e946443e90 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.hpp @@ -55,7 +55,9 @@ class NoDrivableLaneModule : public SceneModuleInterface NoDrivableLaneModule( const int64_t module_id, const int64_t lane_id, const PlannerParam & planner_param, - const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock); + const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface); bool modifyPathVelocity(PathWithLaneId * path) override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.cpp index dca2dde33b693..55b6c0dffd1a4 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.cpp @@ -60,7 +60,7 @@ void NoStoppingAreaModuleManager::launchNewModules( // assign 1 no stopping area for each module registerModule(std::make_shared( module_id, lane_id, *m.first, planner_param_, logger_.get_child("no_stopping_area_module"), - clock_)); + clock_, time_keeper_, planning_factor_interface_)); generateUUID(module_id); updateRTCStatus( getUUID(module_id), true, State::WAITING_FOR_EXECUTION, diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp index 5c79ec69a9d98..bcb8b365f6205 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp @@ -30,6 +30,7 @@ #include #include +#include #include namespace autoware::behavior_velocity_planner @@ -40,8 +41,10 @@ NoStoppingAreaModule::NoStoppingAreaModule( const int64_t module_id, const int64_t lane_id, const lanelet::autoware::NoStoppingArea & no_stopping_area_reg_elem, const PlannerParam & planner_param, const rclcpp::Logger & logger, - const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterfaceWithRTC(module_id, logger, clock), + const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterfaceWithRTC(module_id, logger, clock, time_keeper, planning_factor_interface), lane_id_(lane_id), no_stopping_area_reg_elem_(no_stopping_area_reg_elem), planner_param_(planner_param), diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp index 1eafcf157623d..0d53441924805 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp @@ -57,7 +57,9 @@ class NoStoppingAreaModule : public SceneModuleInterfaceWithRTC const int64_t module_id, const int64_t lane_id, const lanelet::autoware::NoStoppingArea & no_stopping_area_reg_elem, const PlannerParam & planner_param, const rclcpp::Logger & logger, - const rclcpp::Clock::SharedPtr clock); + const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface); bool modifyPathVelocity(PathWithLaneId * path) override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/manager.cpp index c4dace9d49244..d28cff55a7a8d 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/manager.cpp @@ -123,8 +123,8 @@ void OcclusionSpotModuleManager::launchNewModules( // general if (!isModuleRegistered(module_id_)) { registerModule(std::make_shared( - module_id_, planner_data_, planner_param_, logger_.get_child("occlusion_spot_module"), - clock_)); + module_id_, planner_data_, planner_param_, logger_.get_child("occlusion_spot_module"), clock_, + time_keeper_, planning_factor_interface_)); } } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp index c5d1cf61614b2..1163b00e08ccb 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp @@ -64,8 +64,11 @@ namespace utils = occlusion_spot_utils; OcclusionSpotModule::OcclusionSpotModule( const int64_t module_id, const std::shared_ptr & planner_data, const PlannerParam & planner_param, const rclcpp::Logger & logger, - const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), param_(planner_param) + const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), + param_(planner_param) { velocity_factor_.init(PlanningBehavior::UNKNOWN); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp index 35c409a9c459b..22b2a91be66b0 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp @@ -48,7 +48,9 @@ class OcclusionSpotModule : public SceneModuleInterface OcclusionSpotModule( const int64_t module_id, const std::shared_ptr & planner_data, const PlannerParam & planner_param, const rclcpp::Logger & logger, - const rclcpp::Clock::SharedPtr clock); + const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface); /** * @brief plan occlusion spot velocity at unknown area in occupancy grid diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp index a891b011dbab8..ad1ae1d47664a 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp @@ -17,6 +17,7 @@ #include #include +#include #include #include #include @@ -79,7 +80,9 @@ class SceneModuleInterface { public: explicit SceneModuleInterface( - const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock); + const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface); virtual ~SceneModuleInterface() = default; virtual bool modifyPathVelocity(PathWithLaneId * path) = 0; @@ -94,13 +97,6 @@ class SceneModuleInterface planner_data_ = planner_data; } - void setTimeKeeper(const std::shared_ptr & time_keeper) - { - time_keeper_ = time_keeper; - } - - std::shared_ptr getTimeKeeper() { return time_keeper_; } - void resetVelocityFactor() { velocity_factor_.reset(); } VelocityFactor getVelocityFactor() const { return velocity_factor_.get(); } std::vector getObjectsOfInterestData() const { return objects_of_interest_; } @@ -111,9 +107,10 @@ class SceneModuleInterface rclcpp::Logger logger_; rclcpp::Clock::SharedPtr clock_; std::shared_ptr planner_data_; - autoware::motion_utils::VelocityFactorInterface velocity_factor_; + autoware::motion_utils::VelocityFactorInterface velocity_factor_; // TODO(soblin): remove this std::vector objects_of_interest_; mutable std::shared_ptr time_keeper_; + std::shared_ptr planning_factor_interface_; void setObjectsOfInterestData( const geometry_msgs::msg::Pose & pose, const autoware_perception_msgs::msg::Shape & shape, @@ -148,6 +145,8 @@ class SceneModuleManagerInterface std::string("~/virtual_wall/") + module_name, 5); pub_velocity_factor_ = node.create_publisher( std::string("/planning/velocity_factors/") + module_name, 1); + planning_factor_interface_ = + std::make_shared(&node, module_name); processing_time_publisher_ = std::make_shared(&node, "~/debug"); @@ -191,6 +190,7 @@ class SceneModuleManagerInterface scene_module->modifyPathVelocity(path); // The velocity factor must be called after modifyPathVelocity. + // TODO(soblin): remove this const auto velocity_factor = scene_module->getVelocityFactor(); if (velocity_factor.behavior != PlanningBehavior::UNKNOWN) { velocity_factor_array.factors.emplace_back(velocity_factor); @@ -204,6 +204,7 @@ class SceneModuleManagerInterface } pub_velocity_factor_->publish(velocity_factor_array); + planning_factor_interface_->publish(); pub_debug_->publish(debug_marker_array); if (is_publish_debug_path_) { tier4_planning_msgs::msg::PathWithLaneId debug_path; @@ -247,7 +248,6 @@ class SceneModuleManagerInterface logger_, "register task: module = %s, id = %lu", getModuleName(), scene_module->getModuleId()); registered_module_id_set_.emplace(scene_module->getModuleId()); - scene_module->setTimeKeeper(time_keeper_); scene_modules_.insert(scene_module); } @@ -282,6 +282,8 @@ class SceneModuleManagerInterface rclcpp::Publisher::SharedPtr pub_processing_time_detail_; std::shared_ptr time_keeper_; + + std::shared_ptr planning_factor_interface_; }; extern template SceneModuleManagerInterface::SceneModuleManagerInterface( rclcpp::Node & node, [[maybe_unused]] const char * module_name); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp index ffd454012d13e..db2a274272f9f 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp @@ -25,11 +25,14 @@ namespace autoware::behavior_velocity_planner { SceneModuleInterface::SceneModuleInterface( - const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock) + const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface) : module_id_(module_id), logger_(logger), clock_(clock), - time_keeper_(std::shared_ptr()) + time_keeper_(time_keeper), + planning_factor_interface_(planning_factor_interface) { } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/include/autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/include/autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp index 4e30ab019aa4e..a955538f4f9fe 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/include/autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/include/autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp @@ -52,7 +52,9 @@ class SceneModuleInterfaceWithRTC : public SceneModuleInterface { public: explicit SceneModuleInterfaceWithRTC( - const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock); + const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface); virtual ~SceneModuleInterfaceWithRTC() = default; void setActivation(const bool activated) { activated_ = activated; } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/src/scene_module_interface_with_rtc.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/src/scene_module_interface_with_rtc.cpp index abac509fd2b2b..2e73b35e20ed9 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/src/scene_module_interface_with_rtc.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/src/scene_module_interface_with_rtc.cpp @@ -27,8 +27,10 @@ namespace autoware::behavior_velocity_planner { SceneModuleInterfaceWithRTC::SceneModuleInterfaceWithRTC( - const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), + const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), activated_(false), safe_(false), distance_(std::numeric_limits::lowest()) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/manager.cpp index 4a16f263d0a54..4af7882461643 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/manager.cpp @@ -156,7 +156,8 @@ void RunOutModuleManager::launchNewModules(const tier4_planning_msgs::msg::PathW if (!isModuleRegistered(module_id)) { registerModule(std::make_shared( module_id, planner_data_, planner_param_, logger_.get_child("run_out_module"), - std::move(dynamic_obstacle_creator_), debug_ptr_, clock_)); + std::move(dynamic_obstacle_creator_), debug_ptr_, clock_, time_keeper_, + planning_factor_interface_)); } } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp index efc2d458ae2a7..193ba5eab0553 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp @@ -45,15 +45,15 @@ RunOutModule::RunOutModule( const int64_t module_id, const std::shared_ptr & planner_data, const PlannerParam & planner_param, const rclcpp::Logger logger, std::unique_ptr dynamic_obstacle_creator, - const std::shared_ptr & debug_ptr, const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), + const std::shared_ptr & debug_ptr, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), planner_param_(planner_param), dynamic_obstacle_creator_(std::move(dynamic_obstacle_creator)), debug_ptr_(debug_ptr), state_machine_(std::make_unique(planner_param.approaching.state)) { - velocity_factor_.init(PlanningBehavior::RUN_OUT); - if (planner_param.run_out.use_partition_lanelet) { const lanelet::LaneletMapConstPtr & ll = planner_data->route_handler_->getLaneletMapPtr(); planning_utils::getAllPartitionLanelets(ll, partition_lanelets_); @@ -770,9 +770,10 @@ bool RunOutModule::insertStopPoint( stop_point_with_lane_id.point.pose = *stop_point; planning_utils::insertVelocity(path, stop_point_with_lane_id, 0.0, insert_idx); - velocity_factor_.set( - path.points, planner_data_->current_odometry->pose, stop_point.value(), VelocityFactor::UNKNOWN, - "run_out"); + planning_factor_interface_->add( + path.points, planner_data_->current_odometry->pose, stop_point.value(), stop_point.value(), + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0 /*velocity*/, 0.0 /*shift_distance*/, "run_out_stop"); return true; } @@ -876,8 +877,11 @@ void RunOutModule::insertApproachingVelocity( return; } - velocity_factor_.set( - output_path.points, current_pose, stop_point.value(), VelocityFactor::UNKNOWN, "run_out"); + planning_factor_interface_->add( + output_path.points, planner_data_->current_odometry->pose, stop_point.value(), + stop_point.value(), tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0 /*velocity*/, + 0.0 /*shift_distance*/, "run_out_approaching_velocity"); // debug debug_ptr_->pushStopPose(autoware::universe_utils::calcOffsetPose( diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.hpp index 83123c71f461e..49de16753a869 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.hpp @@ -47,7 +47,9 @@ class RunOutModule : public SceneModuleInterface const int64_t module_id, const std::shared_ptr & planner_data, const PlannerParam & planner_param, const rclcpp::Logger logger, std::unique_ptr dynamic_obstacle_creator, - const std::shared_ptr & debug_ptr, const rclcpp::Clock::SharedPtr clock); + const std::shared_ptr & debug_ptr, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface); bool modifyPathVelocity(PathWithLaneId * path) override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/manager.cpp index 4677d49b78f4b..a69e33e9e0391 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/manager.cpp @@ -63,7 +63,7 @@ void SpeedBumpModuleManager::launchNewModules(const tier4_planning_msgs::msg::Pa if (!isModuleRegistered(module_id)) { registerModule(std::make_shared( module_id, lane_id, *speed_bump_with_lane_id.first, planner_param_, - logger_.get_child("speed_bump_module"), clock_)); + logger_.get_child("speed_bump_module"), clock_, time_keeper_, planning_factor_interface_)); } } } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.cpp index 54ea807f3268b..4af7801e50ba3 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.cpp @@ -21,6 +21,7 @@ #include #include +#include #include namespace autoware::behavior_velocity_planner @@ -33,8 +34,10 @@ using geometry_msgs::msg::Point32; SpeedBumpModule::SpeedBumpModule( const int64_t module_id, const int64_t lane_id, const lanelet::autoware::SpeedBump & speed_bump_reg_elem, const PlannerParam & planner_param, - const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), + const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), module_id_(module_id), lane_id_(lane_id), speed_bump_reg_elem_(std::move(speed_bump_reg_elem)), diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.hpp index 176a01d5112c5..e3ee2c0566381 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.hpp @@ -21,6 +21,7 @@ #include #include +#include #include #include @@ -54,7 +55,9 @@ class SpeedBumpModule : public SceneModuleInterface SpeedBumpModule( const int64_t module_id, const int64_t lane_id, const lanelet::autoware::SpeedBump & speed_bump_reg_elem, const PlannerParam & planner_param, - const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock); + const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface); bool modifyPathVelocity(PathWithLaneId * path) override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.cpp index b305a1d2aa404..e289779df34b6 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.cpp @@ -81,7 +81,7 @@ void StopLineModuleManager::launchNewModules(const tier4_planning_msgs::msg::Pat if (!isModuleRegistered(module_id)) { registerModule(std::make_shared( module_id, stop_line_with_lane_id.first, planner_param_, - logger_.get_child("stop_line_module"), clock_)); + logger_.get_child("stop_line_module"), clock_, time_keeper_, planning_factor_interface_)); } } } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp index 2a9c5dab1ebd9..71a5ea3b9e807 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp @@ -20,6 +20,7 @@ #include +#include #include #include @@ -28,8 +29,10 @@ namespace autoware::behavior_velocity_planner StopLineModule::StopLineModule( const int64_t module_id, lanelet::ConstLineString3d stop_line, const PlannerParam & planner_param, - const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), + const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), stop_line_(std::move(stop_line)), planner_param_(planner_param), state_(State::APPROACH), diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp index 9ac1463da8618..aac830b0e9f24 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp @@ -30,6 +30,7 @@ #include +#include #include #include @@ -68,7 +69,9 @@ class StopLineModule : public SceneModuleInterface StopLineModule( const int64_t module_id, lanelet::ConstLineString3d stop_line, const PlannerParam & planner_param, const rclcpp::Logger & logger, - const rclcpp::Clock::SharedPtr clock); + const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface); bool modifyPathVelocity(PathWithLaneId * path) override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_scene.cpp index 1eafb71eb403c..e304b479a005d 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_scene.cpp @@ -74,7 +74,10 @@ class StopLineModuleTest : public ::testing::Test clock_ = std::make_shared(); module_ = std::make_shared( - 1, stop_line_, planner_param_, rclcpp::get_logger("test_logger"), clock_); + 1, stop_line_, planner_param_, rclcpp::get_logger("test_logger"), clock_, + std::make_shared(), + std::make_shared( + node_.get(), "test_stopline")); module_->setPlannerData(planner_data_); } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/manager.cpp index 4325cc1d5aaf9..a004f5b823138 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/manager.cpp @@ -42,8 +42,9 @@ void TemplateModuleManager::launchNewModules( { int64_t module_id = 0; if (!isModuleRegistered(module_id)) { - registerModule( - std::make_shared(module_id, logger_.get_child(getModuleName()), clock_)); + registerModule(std::make_shared( + module_id, logger_.get_child(getModuleName()), clock_, time_keeper_, + planning_factor_interface_)); } } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.cpp index 3ce8502baaf63..d8eea337e7186 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.cpp @@ -19,14 +19,17 @@ #include +#include #include namespace autoware::behavior_velocity_planner { TemplateModule::TemplateModule( - const int64_t module_id, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock) + const int64_t module_id, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface) { } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.hpp index 70cd5cb1235e7..ba078f3fd6421 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.hpp @@ -18,9 +18,9 @@ #include #include +#include #include #include - namespace autoware::behavior_velocity_planner { using tier4_planning_msgs::msg::PathWithLaneId; @@ -29,7 +29,9 @@ class TemplateModule : public SceneModuleInterface { public: TemplateModule( - const int64_t module_id, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock); + const int64_t module_id, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface); /** * @brief Modify the velocity of path points. diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp index 1b66824d04623..9a402f31af0e4 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp @@ -114,7 +114,8 @@ void TrafficLightModuleManager::launchNewModules( if (!isModuleRegisteredFromExistingAssociatedModule(lane_id)) { registerModule(std::make_shared( lane_id, *(traffic_light_reg_elem.first), traffic_light_reg_elem.second, planner_param_, - logger_.get_child("traffic_light_module"), clock_)); + logger_.get_child("traffic_light_module"), clock_, time_keeper_, + planning_factor_interface_)); generateUUID(lane_id); updateRTCStatus( getUUID(lane_id), true, State::WAITING_FOR_EXECUTION, std::numeric_limits::lowest(), diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp index 458d8e1588a5b..b051d5ff7eb76 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp @@ -43,8 +43,10 @@ namespace autoware::behavior_velocity_planner TrafficLightModule::TrafficLightModule( const int64_t lane_id, const lanelet::TrafficLight & traffic_light_reg_elem, lanelet::ConstLanelet lane, const PlannerParam & planner_param, const rclcpp::Logger logger, - const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterfaceWithRTC(lane_id, logger, clock), + const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterfaceWithRTC(lane_id, logger, clock, time_keeper, planning_factor_interface), lane_id_(lane_id), traffic_light_reg_elem_(traffic_light_reg_elem), lane_(lane), diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp index 3af13bc1927ce..c30cbbdfc1477 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp @@ -69,7 +69,9 @@ class TrafficLightModule : public SceneModuleInterfaceWithRTC TrafficLightModule( const int64_t lane_id, const lanelet::TrafficLight & traffic_light_reg_elem, lanelet::ConstLanelet lane, const PlannerParam & planner_param, const rclcpp::Logger logger, - const rclcpp::Clock::SharedPtr clock); + const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface); bool modifyPathVelocity(PathWithLaneId * path) override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp index 3815c83d3b6ab..dade98dfc1133 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp @@ -94,7 +94,8 @@ void VirtualTrafficLightModuleManager::launchNewModules( ego_path_linestring, lanelet::utils::to2D(stop_line_opt.value()).basicLineString())) { registerModule(std::make_shared( module_id, lane_id, *m.first, m.second, planner_param_, - logger_.get_child("virtual_traffic_light_module"), clock_)); + logger_.get_child("virtual_traffic_light_module"), clock_, time_keeper_, + planning_factor_interface_)); } } } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp index 86239816ed6f2..9ba7b0bccf1b9 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp @@ -20,6 +20,7 @@ #include #include +#include #include #include @@ -38,8 +39,10 @@ VirtualTrafficLightModule::VirtualTrafficLightModule( const int64_t module_id, const int64_t lane_id, const lanelet::autoware::VirtualTrafficLight & reg_elem, lanelet::ConstLanelet lane, const PlannerParam & planner_param, const rclcpp::Logger logger, - const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), + const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), lane_id_(lane_id), reg_elem_(reg_elem), lane_(lane), diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp index 7f37e7078a431..9e7a1192d9869 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp @@ -31,6 +31,7 @@ #include #include +#include #include #include @@ -81,7 +82,9 @@ class VirtualTrafficLightModule : public SceneModuleInterface const int64_t module_id, const int64_t lane_id, const lanelet::autoware::VirtualTrafficLight & reg_elem, lanelet::ConstLanelet lane, const PlannerParam & planner_param, const rclcpp::Logger logger, - const rclcpp::Clock::SharedPtr clock); + const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface); bool modifyPathVelocity(PathWithLaneId * path) override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/manager.cpp index 0f1a7509043b5..bfa8a96531090 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/manager.cpp @@ -61,7 +61,8 @@ void WalkwayModuleManager::launchNewModules(const PathWithLaneId & path) const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); registerModule(std::make_shared( - lanelet.id(), lanelet_map_ptr, p, use_regulatory_element, logger, clock_)); + lanelet.id(), lanelet_map_ptr, p, use_regulatory_element, logger, clock_, time_keeper_, + planning_factor_interface_)); }; const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath( diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp index 6a7505930a334..94421ae27e077 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp @@ -18,6 +18,7 @@ #include #include +#include #include namespace autoware::behavior_velocity_planner @@ -32,15 +33,15 @@ using autoware::universe_utils::getPose; WalkwayModule::WalkwayModule( const int64_t module_id, const lanelet::LaneletMapPtr & lanelet_map_ptr, const PlannerParam & planner_param, const bool use_regulatory_element, - const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), + const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), module_id_(module_id), state_(State::APPROACH), planner_param_(planner_param), use_regulatory_element_(use_regulatory_element) { - velocity_factor_.init(PlanningBehavior::SIDEWALK); - if (use_regulatory_element_) { const auto reg_elem_ptr = std::dynamic_pointer_cast( lanelet_map_ptr->regulatoryElementLayer.get(module_id)); @@ -119,9 +120,10 @@ bool WalkwayModule::modifyPathVelocity(PathWithLaneId * path) } /* get stop point and stop factor */ - velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, stop_pose.value(), - VelocityFactor::UNKNOWN); + planning_factor_interface_->add( + path->points, planner_data_->current_odometry->pose, stop_pose.value(), stop_pose.value(), + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0 /*velocity*/, 0.0 /*shift distance*/, "walkway_stop"); // use arc length to identify if ego vehicle is in front of walkway stop or not. const double signed_arc_dist_to_stop_point = diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.hpp index df54eafd11cc2..1bd5003498d34 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.hpp @@ -44,7 +44,9 @@ class WalkwayModule : public SceneModuleInterface WalkwayModule( const int64_t module_id, const lanelet::LaneletMapPtr & lanelet_map_ptr, const PlannerParam & planner_param, const bool use_regulatory_element, - const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock); + const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr planning_factor_interface); bool modifyPathVelocity(PathWithLaneId * path) override; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp index 7fd5834ba2cfa..2000615b51e2c 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp @@ -43,7 +43,9 @@ void DynamicObstacleStopModule::init(rclcpp::Node & node, const std::string & mo module_name_ = module_name; logger_ = node.get_logger().get_child(ns_); clock_ = node.get_clock(); - velocity_factor_interface_.init(autoware::motion_utils::PlanningBehavior::ROUTE_OBSTACLE); + + planning_factor_interface_ = std::make_unique( + &node, "dynamic_obstacle_stop"); debug_publisher_ = node.create_publisher("~/" + ns_ + "/debug_markers", 1); @@ -160,15 +162,9 @@ VelocityPlanningResult DynamicObstacleStopModule::plan( debug_data_.stop_pose = stop_pose; if (stop_pose) { result.stop_points.push_back(stop_pose->position); - const auto stop_pose_reached = - planner_data->current_odometry.twist.twist.linear.x < 1e-3 && - autoware::universe_utils::calcDistance2d(ego_data.pose, *stop_pose) < 1e-3; - velocity_factor_interface_.set( - ego_trajectory_points, ego_data.pose, *stop_pose, - stop_pose_reached ? autoware::motion_utils::VelocityFactor::STOPPED - : autoware::motion_utils::VelocityFactor::APPROACHING, - "dynamic_obstacle_stop"); - result.velocity_factor = velocity_factor_interface_.get(); + planning_factor_interface_->add( + ego_trajectory_points, ego_data.pose, *stop_pose, PlanningFactor::STOP, + SafetyFactorArray{}); create_virtual_walls(); } } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.hpp index 1843d26a22a29..82a6d790bebe7 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.hpp @@ -33,6 +33,7 @@ class DynamicObstacleStopModule : public PluginModuleInterface { public: void init(rclcpp::Node & node, const std::string & module_name) override; + void publish_planning_factor() override { planning_factor_interface_->publish(); }; void update_parameters(const std::vector & parameters) override; VelocityPlanningResult plan( const std::vector & ego_trajectory_points, diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp index 1923f023552e3..eb89c027a3b48 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp @@ -48,7 +48,6 @@ void ObstacleVelocityLimiterModule::init(rclcpp::Node & node, const std::string projection_params_ = obstacle_velocity_limiter::ProjectionParameters(node); obstacle_params_ = obstacle_velocity_limiter::ObstacleParameters(node); velocity_params_ = obstacle_velocity_limiter::VelocityParameters(node); - velocity_factor_interface_.init(autoware::motion_utils::PlanningBehavior::ROUTE_OBSTACLE); debug_publisher_ = node.create_publisher("~/" + ns_ + "/debug_markers", 1); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp index bd85546cb6c56..532cc1e8c7d51 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp @@ -58,7 +58,9 @@ void OutOfLaneModule::init(rclcpp::Node & node, const std::string & module_name) logger_ = node.get_logger(); clock_ = node.get_clock(); init_parameters(node); - velocity_factor_interface_.init(motion_utils::PlanningBehavior::ROUTE_OBSTACLE); + + planning_factor_interface_ = + std::make_unique(&node, "out_of_lane"); debug_publisher_ = node.create_publisher("~/" + ns_ + "/debug_markers", 1); @@ -310,15 +312,9 @@ VelocityPlanningResult OutOfLaneModule::plan( slowdown_pose->position, slowdown_pose->position, slowdown_velocity); } - const auto is_approaching = - motion_utils::calcSignedArcLength( - ego_trajectory_points, ego_data.pose.position, slowdown_pose->position) > 0.1 && - planner_data->current_odometry.twist.twist.linear.x > 0.1; - const auto status = is_approaching ? motion_utils::VelocityFactor::APPROACHING - : motion_utils::VelocityFactor::STOPPED; - velocity_factor_interface_.set( - ego_trajectory_points, ego_data.pose, *slowdown_pose, status, "out_of_lane"); - result.velocity_factor = velocity_factor_interface_.get(); + planning_factor_interface_->add( + ego_trajectory_points, ego_data.pose, *slowdown_pose, PlanningFactor::SLOW_DOWN, + SafetyFactorArray{}); virtual_wall_marker_creator.add_virtual_walls( out_of_lane::debug::create_virtual_walls(*slowdown_pose, slowdown_velocity == 0.0, params_)); virtual_wall_publisher_->publish(virtual_wall_marker_creator.create_markers(clock_->now())); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp index 72f20f1c63d96..0e73994dd73a7 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp @@ -38,6 +38,7 @@ class OutOfLaneModule : public PluginModuleInterface { public: void init(rclcpp::Node & node, const std::string & module_name) override; + void publish_planning_factor() override { planning_factor_interface_->publish(); }; void update_parameters(const std::vector & parameters) override; VelocityPlanningResult plan( const std::vector & ego_trajectory_points, diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/plugin_module_interface.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/plugin_module_interface.hpp index 53860c390b4db..6d683acff667f 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/plugin_module_interface.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/plugin_module_interface.hpp @@ -18,7 +18,7 @@ #include "planner_data.hpp" #include "velocity_planning_result.hpp" -#include +#include #include #include @@ -32,6 +32,9 @@ namespace autoware::motion_velocity_planner { +using tier4_planning_msgs::msg::PlanningFactor; +using tier4_planning_msgs::msg::SafetyFactorArray; + class PluginModuleInterface { public: @@ -42,7 +45,7 @@ class PluginModuleInterface const std::vector & ego_trajectory_points, const std::shared_ptr planner_data) = 0; virtual std::string get_module_name() const = 0; - autoware::motion_utils::VelocityFactorInterface velocity_factor_interface_; + virtual void publish_planning_factor() {} rclcpp::Logger logger_ = rclcpp::get_logger(""); rclcpp::Publisher::SharedPtr debug_publisher_; rclcpp::Publisher::SharedPtr virtual_wall_publisher_; @@ -50,6 +53,9 @@ class PluginModuleInterface rclcpp::Publisher::SharedPtr processing_time_publisher_; autoware::motion_utils::VirtualWallMarkerCreator virtual_wall_marker_creator{}; + +protected: + std::unique_ptr planning_factor_interface_; }; } // namespace autoware::motion_velocity_planner diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/velocity_planning_result.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/velocity_planning_result.hpp index 1288884a37979..7b41bf0ee9e3e 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/velocity_planning_result.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/velocity_planning_result.hpp @@ -17,7 +17,6 @@ #include -#include #include #include @@ -43,7 +42,6 @@ struct VelocityPlanningResult { std::vector stop_points{}; std::vector slowdown_intervals{}; - std::optional velocity_factor{}; }; } // namespace autoware::motion_velocity_planner diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/README.md b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/README.md index 01062f81e77a2..c65593497b8d5 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/README.md +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/README.md @@ -30,10 +30,10 @@ This means that to stop before a wall, a stop point is inserted in the trajector ## Output topics -| Name | Type | Description | -| --------------------------- | ------------------------------------------------- | -------------------------------------------------- | -| `~/output/trajectory` | autoware_planning_msgs::msg::Trajectory | Ego trajectory with updated velocity profile | -| `~/output/velocity_factors` | autoware_adapi_v1_msgs::msg::VelocityFactorsArray | factors causing change in the ego velocity profile | +| Name | Type | Description | +| ----------------------------------------- | ---------------------------------------------- | -------------------------------------------------- | +| `~/output/trajectory` | autoware_planning_msgs::msg::Trajectory | Ego trajectory with updated velocity profile | +| `~/output/planning_factors/` | tier4_planning_msgs::msg::PlanningFactorsArray | factors causing change in the ego velocity profile | ## Services diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp index a78ab1489e080..8f5aa761573b7 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp @@ -82,9 +82,6 @@ MotionVelocityPlannerNode::MotionVelocityPlannerNode(const rclcpp::NodeOptions & // Publishers trajectory_pub_ = this->create_publisher("~/output/trajectory", 1); - velocity_factor_publisher_ = - this->create_publisher( - "~/output/velocity_factors", 1); processing_time_publisher_ = this->create_publisher( "~/debug/processing_time_ms", 1); @@ -422,20 +419,13 @@ autoware_planning_msgs::msg::Trajectory MotionVelocityPlannerNode::generate_traj resampled_trajectory, std::make_shared(planner_data_)); processing_times["plan_velocities"] = stop_watch.toc("plan_velocities"); - autoware_adapi_v1_msgs::msg::VelocityFactorArray velocity_factors; - velocity_factors.header.frame_id = "map"; - velocity_factors.header.stamp = get_clock()->now(); - for (const auto & planning_result : planning_results) { for (const auto & stop_point : planning_result.stop_points) insert_stop(output_trajectory_msg, stop_point); for (const auto & slowdown_interval : planning_result.slowdown_intervals) insert_slowdown(output_trajectory_msg, slowdown_interval); - if (planning_result.velocity_factor) - velocity_factors.factors.push_back(*planning_result.velocity_factor); } - velocity_factor_publisher_->publish(velocity_factors); return output_trajectory_msg; } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp index 18d41f5d7fe5d..4235971e76fca 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp @@ -93,8 +93,6 @@ class MotionVelocityPlannerNode : public rclcpp::Node // publishers rclcpp::Publisher::SharedPtr trajectory_pub_; rclcpp::Publisher::SharedPtr debug_viz_pub_; - rclcpp::Publisher::SharedPtr - velocity_factor_publisher_; autoware::universe_utils::ProcessingTimePublisher processing_diag_publisher_{ this, "~/debug/processing_time_ms_diag"}; rclcpp::Publisher::SharedPtr diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp index b08798cbef772..5bdf386479283 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp @@ -102,6 +102,8 @@ std::vector MotionVelocityPlannerManager::plan_velocitie VelocityPlanningResult res = plugin->plan(ego_trajectory_points, planner_data); results.push_back(res); + plugin->publish_planning_factor(); + if (res.stop_points.size() > 0) { const auto stop_decision_metric = make_decision_metric(plugin->get_module_name(), "stop"); metrics_.push_back(stop_decision_metric); diff --git a/system/autoware_default_adapi/package.xml b/system/autoware_default_adapi/package.xml index a24d0c8a30b62..e0d3fc0f9e21b 100644 --- a/system/autoware_default_adapi/package.xml +++ b/system/autoware_default_adapi/package.xml @@ -32,6 +32,7 @@ std_srvs tier4_api_msgs tier4_control_msgs + tier4_planning_msgs tier4_system_msgs python3-flask diff --git a/system/autoware_default_adapi/src/planning.cpp b/system/autoware_default_adapi/src/planning.cpp index d89ef6c221666..e11b01115caad 100644 --- a/system/autoware_default_adapi/src/planning.cpp +++ b/system/autoware_default_adapi/src/planning.cpp @@ -18,6 +18,7 @@ #include +#include #include #include #include @@ -25,6 +26,34 @@ namespace autoware::default_adapi { +const std::map direction_map = { + {PlanningFactor::SHIFT_RIGHT, SteeringFactor::RIGHT}, + {PlanningFactor::SHIFT_LEFT, SteeringFactor::LEFT}, + {PlanningFactor::TURN_RIGHT, SteeringFactor::RIGHT}, + {PlanningFactor::TURN_LEFT, SteeringFactor::LEFT}}; + +const std::map conversion_map = { + {"behavior_path_planner", PlanningBehavior::INTERSECTION}, + {"static_obstacle_avoidance", PlanningBehavior::AVOIDANCE}, + {"crosswalk", PlanningBehavior::CROSSWALK}, + {"goal_planner", PlanningBehavior::GOAL_PLANNER}, + {"intersection", PlanningBehavior::INTERSECTION}, + {"lane_change_left", PlanningBehavior::LANE_CHANGE}, + {"lane_change_right", PlanningBehavior::LANE_CHANGE}, + {"merge_from_private", PlanningBehavior::MERGE}, + {"no_stopping_area", PlanningBehavior::NO_STOPPING_AREA}, + {"blind_spot", PlanningBehavior::REAR_CHECK}, + {"obstacle_cruise_planner", PlanningBehavior::ROUTE_OBSTACLE}, + {"motion_velocity_planner", PlanningBehavior::ROUTE_OBSTACLE}, + {"walkway", PlanningBehavior::SIDEWALK}, + {"start_planner", PlanningBehavior::START_PLANNER}, + {"stop_line", PlanningBehavior::STOP_SIGN}, + {"surround_obstacle_checker", PlanningBehavior::SURROUNDING_OBSTACLE}, + {"traffic_light", PlanningBehavior::TRAFFIC_SIGNAL}, + {"detection_area", PlanningBehavior::USER_DEFINED_DETECTION_AREA}, + {"virtual_traffic_light", PlanningBehavior::VIRTUAL_TRAFFIC_LIGHT}, + {"run_out", PlanningBehavior::RUN_OUT}}; + template void concat(std::vector & v1, const std::vector & v2) { @@ -49,16 +78,122 @@ std::vector::SharedPtr> init_factors( } template -T merge_factors(const rclcpp::Time stamp, const std::vector & factors) +std::vector convert([[maybe_unused]] const std::vector & factors) +{ + static_assert(sizeof(T) == 0, "Only specializations of convert can be used."); + throw std::logic_error("Only specializations of convert can be used."); +} + +template <> +std::vector convert(const std::vector & factors) +{ + std::vector velocity_factors; + + for (const auto & factor : factors) { + if (factor.behavior != PlanningFactor::SLOW_DOWN && factor.behavior != PlanningFactor::STOP) { + continue; + } + + if (factor.control_points.empty()) { + continue; + } + + if (conversion_map.count(factor.module) == 0) { + continue; + } + + VelocityFactor velocity_factor; + velocity_factor.behavior = conversion_map.at(factor.module); + velocity_factor.pose = factor.control_points.front().pose; + velocity_factor.distance = factor.control_points.front().distance; + + velocity_factors.push_back(velocity_factor); + } + + return velocity_factors; +} + +template <> +std::vector convert(const std::vector & factors) { - T message; + std::vector steering_factors; + + for (const auto & factor : factors) { + if ( + factor.behavior != PlanningFactor::SHIFT_RIGHT && + factor.behavior != PlanningFactor::SHIFT_LEFT && + factor.behavior != PlanningFactor::TURN_RIGHT && + factor.behavior != PlanningFactor::TURN_LEFT) { + continue; + } + + if (factor.control_points.size() < 2) { + continue; + } + + if (conversion_map.count(factor.module) == 0) { + continue; + } + + if (direction_map.count(factor.behavior) == 0) { + continue; + } + + SteeringFactor steering_factor; + steering_factor.behavior = conversion_map.at(factor.module); + steering_factor.direction = direction_map.at(factor.behavior); + steering_factor.pose = std::array{ + factor.control_points.front().pose, factor.control_points.back().pose}; + steering_factor.distance = std::array{ + factor.control_points.front().distance, factor.control_points.back().distance}; + + steering_factors.push_back(steering_factor); + } + + return steering_factors; +} + +template +T merge_factors( + [[maybe_unused]] const rclcpp::Time stamp, + [[maybe_unused]] const std::vector & factors) +{ + static_assert(sizeof(T) == 0, "Only specializations of merge_factors can be used."); + throw std::logic_error("Only specializations of merge_factors can be used."); +} + +template <> +VelocityFactorArray merge_factors( + const rclcpp::Time stamp, const std::vector & factors) +{ + VelocityFactorArray message; message.header.stamp = stamp; message.header.frame_id = "map"; for (const auto & factor : factors) { - if (factor) { - concat(message.factors, factor->factors); + if (!factor) { + continue; } + + concat(message.factors, convert(factor->factors)); + } + return message; +} + +template <> +SteeringFactorArray merge_factors( + const rclcpp::Time stamp, const std::vector & factors) +{ + SteeringFactorArray message; + message.header.stamp = stamp; + message.header.frame_id = "map"; + + for (const auto & factor : factors) { + if (!factor) { + continue; + } + + concat(message.factors, convert(factor->factors)); } return message; } @@ -70,46 +205,33 @@ PlanningNode::PlanningNode(const rclcpp::NodeOptions & options) : Node("planning stop_duration_ = declare_parameter("stop_duration", 1.0); stop_checker_ = std::make_unique(this, stop_duration_ + 1.0); - std::vector velocity_factor_topics = { - "/planning/velocity_factors/blind_spot", - "/planning/velocity_factors/crosswalk", - "/planning/velocity_factors/detection_area", - "/planning/velocity_factors/dynamic_obstacle_stop", - "/planning/velocity_factors/intersection", - "/planning/velocity_factors/merge_from_private", - "/planning/velocity_factors/no_stopping_area", - "/planning/velocity_factors/obstacle_stop", - "/planning/velocity_factors/obstacle_cruise", - "/planning/velocity_factors/occlusion_spot", - "/planning/velocity_factors/run_out", - "/planning/velocity_factors/stop_line", - "/planning/velocity_factors/surround_obstacle", - "/planning/velocity_factors/traffic_light", - "/planning/velocity_factors/virtual_traffic_light", - "/planning/velocity_factors/walkway", - "/planning/velocity_factors/motion_velocity_planner", - "/planning/velocity_factors/static_obstacle_avoidance", - "/planning/velocity_factors/dynamic_obstacle_avoidance", - "/planning/velocity_factors/avoidance_by_lane_change", - "/planning/velocity_factors/lane_change_left", - "/planning/velocity_factors/lane_change_right", - "/planning/velocity_factors/start_planner", - "/planning/velocity_factors/goal_planner"}; - - std::vector steering_factor_topics = { - "/planning/steering_factor/static_obstacle_avoidance", - "/planning/steering_factor/dynamic_obstacle_avoidance", - "/planning/steering_factor/avoidance_by_lane_change", - "/planning/steering_factor/intersection", - "/planning/steering_factor/lane_change_left", - "/planning/steering_factor/lane_change_right", - "/planning/steering_factor/start_planner", - "/planning/steering_factor/goal_planner"}; - - sub_velocity_factors_ = - init_factors(this, velocity_factors_, velocity_factor_topics); - sub_steering_factors_ = - init_factors(this, steering_factors_, steering_factor_topics); + std::vector factor_topics = { + "/planning/planning_factors/behavior_path_planner", + "/planning/planning_factors/blind_spot", + "/planning/planning_factors/crosswalk", + "/planning/planning_factors/detection_area", + "/planning/planning_factors/dynamic_obstacle_stop", + "/planning/planning_factors/intersection", + "/planning/planning_factors/merge_from_private", + "/planning/planning_factors/no_stopping_area", + "/planning/planning_factors/obstacle_cruise_planner", + "/planning/planning_factors/occlusion_spot", + "/planning/planning_factors/run_out", + "/planning/planning_factors/stop_line", + "/planning/planning_factors/surround_obstacle_checker", + "/planning/planning_factors/traffic_light", + "/planning/planning_factors/virtual_traffic_light", + "/planning/planning_factors/walkway", + "/planning/planning_factors/motion_velocity_planner", + "/planning/planning_factors/static_obstacle_avoidance", + "/planning/planning_factors/dynamic_obstacle_avoidance", + "/planning/planning_factors/avoidance_by_lane_change", + "/planning/planning_factors/lane_change_left", + "/planning/planning_factors/lane_change_right", + "/planning/planning_factors/start_planner", + "/planning/planning_factors/goal_planner"}; + + sub_factors_ = init_factors(this, factors_, factor_topics); const auto adaptor = autoware::component_interface_utils::NodeAdaptor(this); adaptor.init_pub(pub_velocity_factors_); @@ -139,8 +261,8 @@ void PlanningNode::on_kinematic_state(const KinematicState::ConstSharedPtr msg) void PlanningNode::on_timer() { using autoware_adapi_v1_msgs::msg::VelocityFactor; - auto velocity = merge_factors(now(), velocity_factors_); - auto steering = merge_factors(now(), steering_factors_); + auto velocity = merge_factors(now(), factors_); + auto steering = merge_factors(now(), factors_); // Set the distance if it is nan. if (trajectory_ && kinematic_state_) { @@ -164,6 +286,13 @@ void PlanningNode::on_timer() } } + for (auto & factor : steering.factors) { + if ((factor.status == SteeringFactor::UNKNOWN) && (!std::isnan(factor.distance.front()))) { + const auto is_turning = factor.distance.front() < 0.0; + factor.status = is_turning ? SteeringFactor::TURNING : SteeringFactor::APPROACHING; + } + } + pub_velocity_factors_->publish(velocity); pub_steering_factors_->publish(steering); } diff --git a/system/autoware_default_adapi/src/planning.hpp b/system/autoware_default_adapi/src/planning.hpp index 3a9b99e33a997..49584475734f6 100644 --- a/system/autoware_default_adapi/src/planning.hpp +++ b/system/autoware_default_adapi/src/planning.hpp @@ -21,7 +21,11 @@ #include #include +#include +#include + #include +#include #include // This file should be included after messages. @@ -30,22 +34,26 @@ namespace autoware::default_adapi { +using autoware_adapi_v1_msgs::msg::PlanningBehavior; +using autoware_adapi_v1_msgs::msg::SteeringFactor; +using autoware_adapi_v1_msgs::msg::SteeringFactorArray; +using autoware_adapi_v1_msgs::msg::VelocityFactor; +using autoware_adapi_v1_msgs::msg::VelocityFactorArray; +using tier4_planning_msgs::msg::PlanningFactor; +using tier4_planning_msgs::msg::PlanningFactorArray; + class PlanningNode : public rclcpp::Node { public: explicit PlanningNode(const rclcpp::NodeOptions & options); private: - using VelocityFactorArray = autoware_adapi_v1_msgs::msg::VelocityFactorArray; - using SteeringFactorArray = autoware_adapi_v1_msgs::msg::SteeringFactorArray; Pub pub_velocity_factors_; Pub pub_steering_factors_; Sub sub_trajectory_; Sub sub_kinematic_state_; - std::vector::SharedPtr> sub_velocity_factors_; - std::vector::SharedPtr> sub_steering_factors_; - std::vector velocity_factors_; - std::vector steering_factors_; + std::vector::SharedPtr> sub_factors_; + std::vector factors_; rclcpp::TimerBase::SharedPtr timer_; using VehicleStopChecker = autoware::motion_utils::VehicleStopCheckerBase; From dea525e157d49a3118e1d098788138c6d7698586 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Thu, 16 Jan 2025 22:45:07 +0900 Subject: [PATCH 04/25] feat(motion_utils): add detail and pass type to VirtualWall (#9940) Signed-off-by: Mamoru Sobue Signed-off-by: TetsuKawa --- .../motion_utils/marker/marker_helper.hpp | 5 ++ .../marker/virtual_wall_marker_creator.hpp | 3 +- .../src/marker/marker_helper.cpp | 49 ++++++++++++++++++- .../marker/virtual_wall_marker_creator.cpp | 10 +++- 4 files changed, 63 insertions(+), 4 deletions(-) diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/marker/marker_helper.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/marker/marker_helper.hpp index ee642c93a378f..2b89fc19d1878 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/marker/marker_helper.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/marker/marker_helper.hpp @@ -40,6 +40,11 @@ visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker( const double longitudinal_offset = 0.0, const std::string & ns_prefix = "", const bool is_driving_forward = true); +visualization_msgs::msg::MarkerArray createIntendedPassVirtualMarker( + const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now, + const int32_t id, const double longitudinal_offset, const std::string & ns_prefix, + const bool is_driving_forward); + visualization_msgs::msg::MarkerArray createDeletedStopVirtualWallMarker( const rclcpp::Time & now, const int32_t id); diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/marker/virtual_wall_marker_creator.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/marker/virtual_wall_marker_creator.hpp index 07fcbd9840c88..fd86c54d65be9 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/marker/virtual_wall_marker_creator.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/marker/virtual_wall_marker_creator.hpp @@ -29,12 +29,13 @@ namespace autoware::motion_utils { /// @brief type of virtual wall associated with different marker styles and namespace -enum VirtualWallType { stop, slowdown, deadline }; +enum VirtualWallType { stop, slowdown, deadline, pass }; /// @brief virtual wall to be visualized in rviz struct VirtualWall { geometry_msgs::msg::Pose pose{}; std::string text{}; + std::string detail{}; std::string ns{}; VirtualWallType style = stop; double longitudinal_offset{}; diff --git a/common/autoware_motion_utils/src/marker/marker_helper.cpp b/common/autoware_motion_utils/src/marker/marker_helper.cpp index 91ad1c41eecc3..44b621a53c5f1 100644 --- a/common/autoware_motion_utils/src/marker/marker_helper.cpp +++ b/common/autoware_motion_utils/src/marker/marker_helper.cpp @@ -54,7 +54,7 @@ inline visualization_msgs::msg::MarkerArray createVirtualWallMarkerArray( { auto marker = createDefaultMarker( "map", now, ns_prefix + "factor_text", id, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, - createMarkerScale(0.0, 0.0, 1.0), createMarkerColor(1.0, 1.0, 1.0, 1.0)); + createMarkerScale(0.0, 0.0, 1.0 /*font size*/), createMarkerColor(1.0, 1.0, 1.0, 1.0)); marker.pose = vehicle_front_pose; marker.pose.position.z += 2.0; @@ -85,6 +85,41 @@ inline visualization_msgs::msg::MarkerArray createDeletedVirtualWallMarkerArray( return marker_array; } + +inline visualization_msgs::msg::MarkerArray createIntendedPassArrowMarkerArray( + const geometry_msgs::msg::Pose & vehicle_front_pose, const std::string & module_name, + const std::string & ns_prefix, const rclcpp::Time & now, const int32_t id, + const std_msgs::msg::ColorRGBA & color) +{ + visualization_msgs::msg::MarkerArray marker_array; + + // Arrow + { + auto marker = createDefaultMarker( + "map", now, ns_prefix + "direction", id, visualization_msgs::msg::Marker::ARROW, + createMarkerScale(2.5 /*length*/, 0.15 /*width*/, 1.0 /*height*/), color); + + marker.pose = vehicle_front_pose; + + marker_array.markers.push_back(marker); + } + + // Factor Text + { + auto marker = createDefaultMarker( + "map", now, ns_prefix + "factor_text", id, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, + createMarkerScale(0.0, 0.0, 0.4 /*font size*/), createMarkerColor(1.0, 1.0, 1.0, 1.0)); + + marker.pose = vehicle_front_pose; + marker.pose.position.z += 2.0; + marker.text = module_name; + + marker_array.markers.push_back(marker); + } + + return marker_array; +} + } // namespace namespace autoware::motion_utils @@ -125,6 +160,18 @@ visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker( createMarkerColor(0.0, 1.0, 0.0, 0.5)); } +visualization_msgs::msg::MarkerArray createIntendedPassVirtualMarker( + const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now, + const int32_t id, const double longitudinal_offset, const std::string & ns_prefix, + const bool is_driving_forward) +{ + const auto pose_with_offset = autoware::universe_utils::calcOffsetPose( + pose, longitudinal_offset * (is_driving_forward ? 1.0 : -1.0), 0.0, 0.0); + return createIntendedPassArrowMarkerArray( + pose_with_offset, module_name, ns_prefix + "intended_pass_", now, id, + createMarkerColor(0.77, 0.77, 0.77, 0.5)); +} + visualization_msgs::msg::MarkerArray createDeletedStopVirtualWallMarker( const rclcpp::Time & now, const int32_t id) { diff --git a/common/autoware_motion_utils/src/marker/virtual_wall_marker_creator.cpp b/common/autoware_motion_utils/src/marker/virtual_wall_marker_creator.cpp index 4fecaea1bb838..fb8708b5b7baa 100644 --- a/common/autoware_motion_utils/src/marker/virtual_wall_marker_creator.cpp +++ b/common/autoware_motion_utils/src/marker/virtual_wall_marker_creator.cpp @@ -63,10 +63,16 @@ visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::create_markers( case deadline: create_fn = autoware::motion_utils::createDeadLineVirtualWallMarker; break; + case pass: + create_fn = autoware::motion_utils::createIntendedPassVirtualMarker; + break; } + const auto wall_text = virtual_wall.detail.empty() + ? virtual_wall.text + : virtual_wall.text + "(" + virtual_wall.detail + ")"; auto markers = create_fn( - virtual_wall.pose, virtual_wall.text, now, 0, virtual_wall.longitudinal_offset, - virtual_wall.ns, virtual_wall.is_driving_forward); + virtual_wall.pose, wall_text, now, 0, virtual_wall.longitudinal_offset, virtual_wall.ns, + virtual_wall.is_driving_forward); for (auto & marker : markers.markers) { marker.id = static_cast(marker_count_per_namespace_[marker.ns].current++); marker_array.markers.push_back(marker); From 8a90cbf083295637b347ad5a0a61352b78ab7cb5 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara <32741405+kyoichi-sugahara@users.noreply.github.com> Date: Thu, 16 Jan 2025 23:12:30 +0900 Subject: [PATCH 05/25] fix(start_planner, goal_planner): refactor lane departure checker initialization (#9944) Signed-off-by: TetsuKawa --- .../src/pull_over_planner/geometric_pull_over.cpp | 8 ++++++-- .../src/pull_over_planner/shift_pull_over.cpp | 8 ++++++-- .../src/geometric_pull_out.cpp | 6 ++++-- .../src/shift_pull_out.cpp | 6 ++++-- 4 files changed, 20 insertions(+), 8 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp index 44dca13ccfd66..b7b4ad83de362 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp @@ -29,8 +29,12 @@ GeometricPullOver::GeometricPullOver( rclcpp::Node & node, const GoalPlannerParameters & parameters, const bool is_forward) : PullOverPlannerBase{node, parameters}, parallel_parking_parameters_{parameters.parallel_parking_parameters}, - lane_departure_checker_{ - lane_departure_checker::Param{parameters.lane_departure_check_expansion_margin}, vehicle_info_}, + lane_departure_checker_{[&]() { + auto lane_departure_checker_params = lane_departure_checker::Param{}; + lane_departure_checker_params.footprint_extra_margin = + parameters.lane_departure_check_expansion_margin; + return LaneDepartureChecker{lane_departure_checker_params, vehicle_info_}; + }()}, is_forward_{is_forward}, left_side_parking_{parameters.parking_policy == ParkingPolicy::LEFT_SIDE} { diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp index 8350252369cc6..c8e0788e39b6f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp @@ -32,8 +32,12 @@ namespace autoware::behavior_path_planner { ShiftPullOver::ShiftPullOver(rclcpp::Node & node, const GoalPlannerParameters & parameters) : PullOverPlannerBase{node, parameters}, - lane_departure_checker_{ - lane_departure_checker::Param{parameters.lane_departure_check_expansion_margin}, vehicle_info_}, + lane_departure_checker_{[&]() { + auto lane_departure_checker_params = lane_departure_checker::Param{}; + lane_departure_checker_params.footprint_extra_margin = + parameters.lane_departure_check_expansion_margin; + return LaneDepartureChecker{lane_departure_checker_params, vehicle_info_}; + }()}, left_side_parking_{parameters.parking_policy == ParkingPolicy::LEFT_SIDE} { } diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp index 69c5c41405932..ed09e0c0a33d6 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp @@ -40,10 +40,12 @@ GeometricPullOut::GeometricPullOut( : PullOutPlannerBase{node, parameters, time_keeper}, parallel_parking_parameters_{parameters.parallel_parking_parameters} { + auto lane_departure_checker_params = autoware::lane_departure_checker::Param{}; + lane_departure_checker_params.footprint_extra_margin = + parameters.lane_departure_check_expansion_margin; lane_departure_checker_ = std::make_shared( - autoware::lane_departure_checker::Param{parameters.lane_departure_check_expansion_margin}, - vehicle_info_, time_keeper_); + lane_departure_checker_params, vehicle_info_); planner_.setParameters(parallel_parking_parameters_); } diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp index 8f4de1d3636de..16a3d4ffd842a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp @@ -44,10 +44,12 @@ ShiftPullOut::ShiftPullOut( std::shared_ptr time_keeper) : PullOutPlannerBase{node, parameters, time_keeper} { + autoware::lane_departure_checker::Param lane_departure_checker_params; + lane_departure_checker_params.footprint_extra_margin = + parameters.lane_departure_check_expansion_margin; lane_departure_checker_ = std::make_shared( - autoware::lane_departure_checker::Param{parameters.lane_departure_check_expansion_margin}, - vehicle_info_, time_keeper_); + lane_departure_checker_params, vehicle_info_, time_keeper_); } std::optional ShiftPullOut::plan( From 581f31121c6820db155e77249c248289479eda11 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Thu, 16 Jan 2025 23:39:33 +0900 Subject: [PATCH 06/25] refactor(lane_change): add missing safety check parameter (#9928) * refactor(lane_change): parameterize incoming object yaw threshold Signed-off-by: Zulfaqar Azmi * Readme Signed-off-by: Zulfaqar Azmi * Update planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> * Add missing parameters Signed-off-by: Zulfaqar Azmi * missing dot Signed-off-by: Zulfaqar Azmi * Update planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md Co-authored-by: mkquda <168697710+mkquda@users.noreply.github.com> * update readme Signed-off-by: Zulfaqar Azmi --------- Signed-off-by: Zulfaqar Azmi Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Co-authored-by: mkquda <168697710+mkquda@users.noreply.github.com> Signed-off-by: TetsuKawa --- .../README.md | 7 +++- .../config/lane_change.param.yaml | 7 +++- .../structs/parameters.hpp | 1 + .../src/manager.cpp | 32 ++++++++++++++++++- .../src/scene.cpp | 8 +++-- 5 files changed, 49 insertions(+), 6 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md index 20fd564049133..601fb893f82c6 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md @@ -1149,7 +1149,8 @@ The following parameters are used to configure terminal lane change path feature | `collision_check.check_other_lanes` | [-] | boolean | If true, the lane change module includes objects in other lanes when performing collision assessment. | false | | `collision_check.use_all_predicted_paths` | [-] | boolean | If false, use only the predicted path that has the maximum confidence. | true | | `collision_check.prediction_time_resolution` | [s] | double | Time resolution for object's path interpolation and collision check. | 0.5 | -| `collision_check.yaw_diff_threshold` | [rad] | double | Maximum yaw difference between ego and object when executing rss-based collision checking | 3.1416 | +| `collision_check.yaw_diff_threshold` | [rad] | double | Maximum yaw difference between predicted ego pose and predicted object pose when executing rss-based collision checking | 3.1416 | +| `collision_check.th_incoming_object_yaw` | [rad] | double | Maximum yaw difference between current ego pose and current object pose. Objects with a yaw difference exceeding this value are excluded from the safety check. | 2.3562 | #### safety constraints during lane change path is computed @@ -1162,6 +1163,7 @@ The following parameters are used to configure terminal lane change path feature | `safety_check.execution.lateral_distance_max_threshold` | [m] | double | The lateral distance threshold that is used to determine whether lateral distance between two object is enough and whether lane change is safe. | 2.0 | | `safety_check.execution.longitudinal_distance_min_threshold` | [m] | double | The longitudinal distance threshold that is used to determine whether longitudinal distance between two object is enough and whether lane change is safe. | 3.0 | | `safety_check.execution.longitudinal_velocity_delta_time` | [m] | double | The time multiplier that is used to compute the actual gap between vehicle at each predicted points (not RSS distance) | 0.8 | +| `safety_check.execution.extended_polygon_policy` | [-] | string | Policy used to determine the polygon shape for the safety check. Available options are: `rectangle` or `along-path`. | `rectangle` | #### safety constraints specifically for stopped or parked vehicles @@ -1174,6 +1176,7 @@ The following parameters are used to configure terminal lane change path feature | `safety_check.parked.lateral_distance_max_threshold` | [m] | double | The lateral distance threshold that is used to determine whether lateral distance between two object is enough and whether lane change is safe. | 1.0 | | `safety_check.parked.longitudinal_distance_min_threshold` | [m] | double | The longitudinal distance threshold that is used to determine whether longitudinal distance between two object is enough and whether lane change is safe. | 3.0 | | `safety_check.parked.longitudinal_velocity_delta_time` | [m] | double | The time multiplier that is used to compute the actual gap between vehicle at each predicted points (not RSS distance) | 0.8 | +| `safety_check.parked.extended_polygon_policy` | [-] | string | Policy used to determine the polygon shape for the safety check. Available options are: `rectangle` or `along-path`. | `rectangle` | ##### safety constraints to cancel lane change path @@ -1186,6 +1189,7 @@ The following parameters are used to configure terminal lane change path feature | `safety_check.cancel.lateral_distance_max_threshold` | [m] | double | The lateral distance threshold that is used to determine whether lateral distance between two object is enough and whether lane change is safe. | 1.0 | | `safety_check.cancel.longitudinal_distance_min_threshold` | [m] | double | The longitudinal distance threshold that is used to determine whether longitudinal distance between two object is enough and whether lane change is safe. | 2.5 | | `safety_check.cancel.longitudinal_velocity_delta_time` | [m] | double | The time multiplier that is used to compute the actual gap between vehicle at each predicted points (not RSS distance) | 0.6 | +| `safety_check.cancel.extended_polygon_policy` | [-] | string | Policy used to determine the polygon shape for the safety check. Available options are: `rectangle` or `along-path`. | `rectangle` | ##### safety constraints used during lane change path is computed when ego is stuck @@ -1198,6 +1202,7 @@ The following parameters are used to configure terminal lane change path feature | `safety_check.stuck.lateral_distance_max_threshold` | [m] | double | The lateral distance threshold that is used to determine whether lateral distance between two object is enough and whether lane change is safe. | 2.0 | | `safety_check.stuck.longitudinal_distance_min_threshold` | [m] | double | The longitudinal distance threshold that is used to determine whether longitudinal distance between two object is enough and whether lane change is safe. | 3.0 | | `safety_check.stuck.longitudinal_velocity_delta_time` | [m] | double | The time multiplier that is used to compute the actual gap between vehicle at each predicted points (not RSS distance) | 0.8 | +| `safety_check.stuck.extended_polygon_policy` | [-] | string | Policy used to determine the polygon shape for the safety check. Available options are: `rectangle` or `along-path`. | `rectangle` | (\*1) the value must be negative. diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml index 15eb23daecf95..71fac1246598f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml @@ -58,6 +58,7 @@ lateral_distance_max_threshold: 2.0 longitudinal_distance_min_threshold: 3.0 longitudinal_velocity_delta_time: 0.0 + extended_polygon_policy: "rectangle" parked: expected_front_deceleration: -1.0 expected_rear_deceleration: -2.0 @@ -66,6 +67,7 @@ lateral_distance_max_threshold: 1.0 longitudinal_distance_min_threshold: 3.0 longitudinal_velocity_delta_time: 0.0 + extended_polygon_policy: "rectangle" cancel: expected_front_deceleration: -1.0 expected_rear_deceleration: -2.0 @@ -74,6 +76,7 @@ lateral_distance_max_threshold: 1.0 longitudinal_distance_min_threshold: 2.5 longitudinal_velocity_delta_time: 0.0 + extended_polygon_policy: "rectangle" stuck: expected_front_deceleration: -1.0 expected_rear_deceleration: -1.0 @@ -82,6 +85,7 @@ lateral_distance_max_threshold: 2.0 longitudinal_distance_min_threshold: 3.0 longitudinal_velocity_delta_time: 0.0 + extended_polygon_policy: "rectangle" # lane expansion for object filtering lane_expansion: @@ -123,7 +127,8 @@ intersection: true turns: true prediction_time_resolution: 0.5 - yaw_diff_threshold: 3.1416 + th_incoming_object_yaw: 2.3562 # [rad] + yaw_diff_threshold: 3.1416 # [rad] check_current_lanes: false check_other_lanes: false use_all_predicted_paths: false diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/parameters.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/parameters.hpp index 60386f535fc64..358f60f3193d5 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/parameters.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/parameters.hpp @@ -94,6 +94,7 @@ struct CollisionCheckParameters bool check_current_lane{true}; bool check_other_lanes{true}; bool use_all_predicted_paths{false}; + double th_incoming_object_yaw{2.3562}; double th_yaw_diff{3.1416}; double prediction_time_resolution{0.5}; }; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp index 450497fb2ca29..589338d7b3ace 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp @@ -152,6 +152,8 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s getOrDeclareParameter(*node, parameter("collision_check.prediction_time_resolution")); p.safety.collision_check.th_yaw_diff = getOrDeclareParameter(*node, parameter("collision_check.yaw_diff_threshold")); + p.safety.collision_check.th_incoming_object_yaw = + getOrDeclareParameter(*node, parameter("collision_check.th_incoming_object_yaw")); // rss check auto set_rss_params = [&](auto & params, const std::string & prefix) { @@ -169,6 +171,8 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s *node, parameter(prefix + ".rear_vehicle_safety_time_margin")); params.lateral_distance_max_threshold = getOrDeclareParameter(*node, parameter(prefix + ".lateral_distance_max_threshold")); + params.extended_polygon_policy = + getOrDeclareParameter(*node, parameter(prefix + ".extended_polygon_policy")); }; set_rss_params(p.safety.rss_params, "safety_check.execution"); set_rss_params(p.safety.rss_params_for_parked, "safety_check.parked"); @@ -454,6 +458,19 @@ void LaneChangeModuleManager::updateModuleParams(const std::vectorsafety.collision_check.prediction_time_resolution); updateParam( parameters, ns + "yaw_diff_threshold", p->safety.collision_check.th_yaw_diff); + + auto th_incoming_object_yaw = p->safety.collision_check.th_incoming_object_yaw; + updateParam(parameters, ns + "th_incoming_object_yaw", th_incoming_object_yaw); + if (th_incoming_object_yaw >= M_PI_2) { + p->safety.collision_check.th_incoming_object_yaw = th_incoming_object_yaw; + } else { + RCLCPP_WARN_THROTTLE( + node_->get_logger(), *node_->get_clock(), 5000, + "The value of th_incoming_object_yaw (%.3f rad) is less than the minimum possible value " + "(%.3f " + "rad).", + th_incoming_object_yaw, M_PI_2); + } } { @@ -483,7 +500,7 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector(parameters, ns + "stop_time", p->th_stop_time); } - auto update_rss_params = [¶meters](const std::string & prefix, auto & params) { + auto update_rss_params = [¶meters, this](const std::string & prefix, auto & params) { using autoware::universe_utils::updateParam; updateParam( parameters, prefix + "longitudinal_distance_min_threshold", @@ -502,6 +519,19 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector( parameters, prefix + "lateral_distance_max_threshold", params.lateral_distance_max_threshold); + + auto extended_polygon_policy = params.extended_polygon_policy; + updateParam( + parameters, prefix + "extended_polygon_policy", extended_polygon_policy); + if (extended_polygon_policy == "rectangle" || extended_polygon_policy == "along_path") { + params.extended_polygon_policy = extended_polygon_policy; + } else { + RCLCPP_WARN_THROTTLE( + node_->get_logger(), *node_->get_clock(), 1000, + "Policy %s not supported or there's typo. Make sure you choose either 'rectangle' or " + "'along_path'", + extended_polygon_policy.c_str()); + } }; update_rss_params("lane_change.safety_check.execution.", p->safety.rss_params); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index 45fcd6fa99086..611a0b60a4366 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -1051,7 +1051,9 @@ void NormalLaneChange::filterOncomingObjects(PredictedObjects & objects) const const auto is_same_direction = [&](const PredictedObject & object) { const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose; - return !utils::path_safety_checker::isTargetObjectOncoming(current_pose, object_pose); + return !utils::path_safety_checker::isTargetObjectOncoming( + current_pose, object_pose, + common_data_ptr_->lc_param_ptr->safety.collision_check.th_incoming_object_yaw); }; // Perception noise could make stationary objects seem opposite the ego vehicle; check the @@ -1792,7 +1794,6 @@ bool NormalLaneChange::is_colliding( constexpr auto is_safe{true}; auto current_debug_data = utils::path_safety_checker::createObjectDebug(obj); - constexpr auto collision_check_yaw_diff_threshold{M_PI}; constexpr auto hysteresis_factor{1.0}; const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( obj, lane_change_parameters_->safety.collision_check.use_all_predicted_paths); @@ -1817,7 +1818,8 @@ bool NormalLaneChange::is_colliding( const auto collided_polygons = utils::path_safety_checker::get_collided_polygons( lane_change_path.path, ego_predicted_path, obj, predicted_obj_path, bpp_param.vehicle_info, selected_rss_param, hysteresis_factor, safety_check_max_vel, - collision_check_yaw_diff_threshold, current_debug_data.second); + common_data_ptr_->lc_param_ptr->safety.collision_check.th_yaw_diff, + current_debug_data.second); if (collided_polygons.empty()) { utils::path_safety_checker::updateCollisionCheckDebugMap( From adf60479b0f2d36a71522fa687cf8de2f17bf7d9 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Fri, 17 Jan 2025 00:05:32 +0900 Subject: [PATCH 07/25] feat(behavior_velocity_planner)!: remove velocity_factor completely (#9943) * feat(behavior_velocity_planner)!: remove velocity_factor completely Signed-off-by: Mamoru Sobue * minimize diff Signed-off-by: Mamoru Sobue --------- Signed-off-by: Mamoru Sobue Signed-off-by: TetsuKawa --- .../src/scene.cpp | 8 +++--- .../src/scene_intersection.cpp | 2 -- .../src/scene.cpp | 19 +++++++++----- .../src/scene_no_stopping_area.cpp | 9 ++++--- .../src/scene_occlusion_spot.cpp | 2 -- .../scene_module_interface.hpp | 22 ---------------- .../src/scene.cpp | 26 +++++-------------- .../src/scene.hpp | 5 ---- .../src/manager.cpp | 12 --------- .../src/scene.cpp | 7 ++--- .../src/scene.cpp | 15 ++++++----- .../src/scene.hpp | 4 --- 12 files changed, 39 insertions(+), 92 deletions(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp index 59f6f20937f41..7e6a320f3ddad 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp @@ -46,7 +46,6 @@ DetectionAreaModule::DetectionAreaModule( planner_param_(planner_param), debug_data_() { - velocity_factor_.init(PlanningBehavior::USER_DEFINED_DETECTION_AREA); } bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path) @@ -180,9 +179,10 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path) // Create StopReason { - velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, stop_point->second, - VelocityFactor::UNKNOWN); + planning_factor_interface_->add( + path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, ""); } return true; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp index 2d5da0f070ed9..5b8aca1c8ec9d 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -44,8 +44,6 @@ namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; -using autoware::motion_utils::VelocityFactorInterface; - IntersectionModule::IntersectionModule( const int64_t module_id, const int64_t lane_id, [[maybe_unused]] std::shared_ptr planner_data, diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp index f32210bc502bf..0e646e92035cf 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp @@ -38,7 +38,6 @@ NoDrivableLaneModule::NoDrivableLaneModule( debug_data_(), state_(State::INIT) { - velocity_factor_.init(PlanningBehavior::NO_DRIVABLE_LANE); } bool NoDrivableLaneModule::modifyPathVelocity(PathWithLaneId * path) @@ -166,8 +165,10 @@ void NoDrivableLaneModule::handle_approaching_state(PathWithLaneId * path) // Get stop point and stop factor { const auto & stop_pose = op_stop_pose.value(); - velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::APPROACHING); + planning_factor_interface_->add( + path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, ""); const auto virtual_wall_pose = autoware::motion_utils::calcLongitudinalOffsetPose( path->points, stop_pose.position, debug_data_.base_link2front); @@ -214,8 +215,10 @@ void NoDrivableLaneModule::handle_inside_no_drivable_lane_state(PathWithLaneId * // Get stop point and stop factor { const auto & stop_pose = autoware::universe_utils::getPose(path->points.at(0)); - velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::APPROACHING); + planning_factor_interface_->add( + path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, ""); const auto & virtual_wall_pose = autoware::motion_utils::calcLongitudinalOffsetPose( path->points, stop_pose.position, debug_data_.base_link2front); @@ -252,8 +255,10 @@ void NoDrivableLaneModule::handle_stopped_state(PathWithLaneId * path) // Get stop point and stop factor { const auto & stop_pose = ego_pos_on_path.pose; - velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::STOPPED); + planning_factor_interface_->add( + path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, ""); const auto virtual_wall_pose = autoware::motion_utils::calcLongitudinalOffsetPose( path->points, stop_pose.position, debug_data_.base_link2front); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp index bcb8b365f6205..c788a54073ed6 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp @@ -50,7 +50,6 @@ NoStoppingAreaModule::NoStoppingAreaModule( planner_param_(planner_param), debug_data_() { - velocity_factor_.init(PlanningBehavior::NO_STOPPING_AREA); state_machine_.setState(StateMachine::State::GO); state_machine_.setMarginTime(planner_param_.state_clear_time); } @@ -144,9 +143,11 @@ bool NoStoppingAreaModule::modifyPathVelocity(PathWithLaneId * path) // Create StopReason { - velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, stop_point->second, - VelocityFactor::UNKNOWN); + planning_factor_interface_->add( + path->points, planner_data_->current_odometry->pose, stop_point->second, stop_point->second, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, ""); } } else if (state_machine_.getState() == StateMachine::State::GO) { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp index 1163b00e08ccb..fa259b2bea9aa 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp @@ -70,8 +70,6 @@ OcclusionSpotModule::OcclusionSpotModule( : SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), param_(planner_param) { - velocity_factor_.init(PlanningBehavior::UNKNOWN); - if (param_.detection_method == utils::DETECTION_METHOD::OCCUPANCY_GRID) { debug_data_.detection_type = "occupancy"; //! occupancy grid limitation( 100 * 100 ) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp index ad1ae1d47664a..5bff3ca343e72 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp @@ -18,7 +18,6 @@ #include #include #include -#include #include #include #include @@ -28,8 +27,6 @@ #include #include -#include -#include #include #include #include @@ -51,8 +48,6 @@ namespace autoware::behavior_velocity_planner { -using autoware::motion_utils::PlanningBehavior; -using autoware::motion_utils::VelocityFactor; using autoware::objects_of_interest_marker_interface::ColorName; using autoware::objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface; using autoware::universe_utils::DebugPublisher; @@ -97,8 +92,6 @@ class SceneModuleInterface planner_data_ = planner_data; } - void resetVelocityFactor() { velocity_factor_.reset(); } - VelocityFactor getVelocityFactor() const { return velocity_factor_.get(); } std::vector getObjectsOfInterestData() const { return objects_of_interest_; } void clearObjectsOfInterestData() { objects_of_interest_.clear(); } @@ -107,7 +100,6 @@ class SceneModuleInterface rclcpp::Logger logger_; rclcpp::Clock::SharedPtr clock_; std::shared_ptr planner_data_; - autoware::motion_utils::VelocityFactorInterface velocity_factor_; // TODO(soblin): remove this std::vector objects_of_interest_; mutable std::shared_ptr time_keeper_; std::shared_ptr planning_factor_interface_; @@ -143,8 +135,6 @@ class SceneModuleManagerInterface } pub_virtual_wall_ = node.create_publisher( std::string("~/virtual_wall/") + module_name, 5); - pub_velocity_factor_ = node.create_publisher( - std::string("/planning/velocity_factors/") + module_name, 1); planning_factor_interface_ = std::make_shared(&node, module_name); @@ -180,21 +170,12 @@ class SceneModuleManagerInterface StopWatch stop_watch; stop_watch.tic("Total"); visualization_msgs::msg::MarkerArray debug_marker_array; - autoware_adapi_v1_msgs::msg::VelocityFactorArray velocity_factor_array; - velocity_factor_array.header.frame_id = "map"; - velocity_factor_array.header.stamp = clock_->now(); for (const auto & scene_module : scene_modules_) { - scene_module->resetVelocityFactor(); scene_module->setPlannerData(planner_data_); scene_module->modifyPathVelocity(path); // The velocity factor must be called after modifyPathVelocity. - // TODO(soblin): remove this - const auto velocity_factor = scene_module->getVelocityFactor(); - if (velocity_factor.behavior != PlanningBehavior::UNKNOWN) { - velocity_factor_array.factors.emplace_back(velocity_factor); - } for (const auto & marker : scene_module->createDebugMarkerArray().markers) { debug_marker_array.markers.push_back(marker); @@ -203,7 +184,6 @@ class SceneModuleManagerInterface virtual_wall_marker_creator_.add_virtual_walls(scene_module->createVirtualWalls()); } - pub_velocity_factor_->publish(velocity_factor_array); planning_factor_interface_->publish(); pub_debug_->publish(debug_marker_array); if (is_publish_debug_path_) { @@ -274,8 +254,6 @@ class SceneModuleManagerInterface rclcpp::Publisher::SharedPtr pub_virtual_wall_; rclcpp::Publisher::SharedPtr pub_debug_; rclcpp::Publisher::SharedPtr pub_debug_path_; - rclcpp::Publisher::SharedPtr - pub_velocity_factor_; std::shared_ptr processing_time_publisher_; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp index 71a5ea3b9e807..e2ddbc75c7b57 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp @@ -38,7 +38,6 @@ StopLineModule::StopLineModule( state_(State::APPROACH), debug_data_() { - velocity_factor_.init(PlanningBehavior::STOP_SIGN); } bool StopLineModule::modifyPathVelocity(PathWithLaneId * path) @@ -62,7 +61,12 @@ bool StopLineModule::modifyPathVelocity(PathWithLaneId * path) path->points = trajectory->restore(); - updateVelocityFactor(&velocity_factor_, state_, *stop_point - ego_s); + // TODO(soblin): PlanningFactorInterface use trajectory class + planning_factor_interface_->add( + path->points, trajectory->compute(*stop_point).point.pose, + planner_data_->current_odometry->pose, planner_data_->current_odometry->pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, "stopline"); updateStateAndStoppedTime( &state_, &stopped_time_, clock_->now(), *stop_point - ego_s, planner_data_->isVehicleStopped()); @@ -152,24 +156,6 @@ void StopLineModule::updateStateAndStoppedTime( } } -void StopLineModule::updateVelocityFactor( - autoware::motion_utils::VelocityFactorInterface * velocity_factor, const State & state, - const double & distance_to_stop_point) -{ - switch (state) { - case State::APPROACH: { - velocity_factor->set(distance_to_stop_point, VelocityFactor::APPROACHING); - break; - } - case State::STOPPED: { - velocity_factor->set(distance_to_stop_point, VelocityFactor::STOPPED); - break; - } - case State::START: - break; - } -} - void StopLineModule::updateDebugData( DebugData * debug_data, const geometry_msgs::msg::Pose & stop_pose, const State & state) const { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp index aac830b0e9f24..7d430463c18b9 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp @@ -18,7 +18,6 @@ #include "autoware/behavior_velocity_planner_common/scene_module_interface.hpp" #include "autoware/behavior_velocity_planner_common/utilization/util.hpp" -#include "autoware/motion_utils/factor/velocity_factor_interface.hpp" #include "autoware/trajectory/path_point_with_lane_id.hpp" #include @@ -98,10 +97,6 @@ class StopLineModule : public SceneModuleInterface State * state, std::optional * stopped_time, const rclcpp::Time & now, const double & distance_to_stop_point, const bool & is_vehicle_stopped) const; - static void updateVelocityFactor( - autoware::motion_utils::VelocityFactorInterface * velocity_factor, const State & state, - const double & distance_to_stop_point); - void updateDebugData( DebugData * debug_data, const geometry_msgs::msg::Pose & stop_pose, const State & state) const; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp index 9a402f31af0e4..1d1ee7fc996b2 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp @@ -53,24 +53,13 @@ void TrafficLightModuleManager::modifyPathVelocity(tier4_planning_msgs::msg::Pat autoware_perception_msgs::msg::TrafficLightGroup tl_state; - autoware_adapi_v1_msgs::msg::VelocityFactorArray velocity_factor_array; - velocity_factor_array.header.frame_id = "map"; - velocity_factor_array.header.stamp = clock_->now(); - nearest_ref_stop_path_point_index_ = static_cast(path->points.size() - 1); for (const auto & scene_module : scene_modules_) { std::shared_ptr traffic_light_scene_module( std::dynamic_pointer_cast(scene_module)); - traffic_light_scene_module->resetVelocityFactor(); traffic_light_scene_module->setPlannerData(planner_data_); traffic_light_scene_module->modifyPathVelocity(path); - // The velocity factor must be called after modifyPathVelocity. - const auto velocity_factor = traffic_light_scene_module->getVelocityFactor(); - if (velocity_factor.behavior != PlanningBehavior::UNKNOWN) { - velocity_factor_array.factors.emplace_back(velocity_factor); - } - if ( traffic_light_scene_module->getFirstRefStopPathPointIndex() < nearest_ref_stop_path_point_index_) { @@ -88,7 +77,6 @@ void TrafficLightModuleManager::modifyPathVelocity(tier4_planning_msgs::msg::Pat virtual_wall_marker_creator_.add_virtual_walls( traffic_light_scene_module->createVirtualWalls()); } - pub_velocity_factor_->publish(velocity_factor_array); pub_debug_->publish(debug_marker_array); pub_virtual_wall_->publish(virtual_wall_marker_creator_.create_markers(clock_->now())); pub_tl_state_->publish(tl_state); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp index b051d5ff7eb76..becbaf5ee05f2 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp @@ -54,7 +54,6 @@ TrafficLightModule::TrafficLightModule( debug_data_(), is_prev_state_stop_(false) { - velocity_factor_.init(PlanningBehavior::TRAFFIC_SIGNAL); planner_param_ = planner_param; } @@ -293,9 +292,11 @@ tier4_planning_msgs::msg::PathWithLaneId TrafficLightModule::insertStopPose( size_t insert_index = insert_target_point_idx; planning_utils::insertVelocity(modified_path, target_point_with_lane_id, 0.0, insert_index); - velocity_factor_.set( + planning_factor_interface_->add( modified_path.points, planner_data_->current_odometry->pose, - target_point_with_lane_id.point.pose, VelocityFactor::UNKNOWN); + target_point_with_lane_id.point.pose, target_point_with_lane_id.point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, ""); return modified_path; } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp index 9ba7b0bccf1b9..a5779cfc0712a 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp @@ -48,8 +48,6 @@ VirtualTrafficLightModule::VirtualTrafficLightModule( lane_(lane), planner_param_(planner_param) { - velocity_factor_.init(PlanningBehavior::VIRTUAL_TRAFFIC_LIGHT); - // Get map data const auto instrument = reg_elem_.getVirtualTrafficLight(); const auto instrument_bottom_line = toAutowarePoints(instrument); @@ -420,9 +418,10 @@ void VirtualTrafficLightModule::insertStopVelocityAtStopLine( } // Set StopReason - velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN, - command_.type); + planning_factor_interface_->add( + path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, ""); // Set data for visualization module_data_.stop_head_pose_at_stop_line = @@ -453,8 +452,10 @@ void VirtualTrafficLightModule::insertStopVelocityAtEndLine( } // Set StopReason - velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN); + planning_factor_interface_->add( + path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, ""); // Set data for visualization module_data_.stop_head_pose_at_end_line = diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp index 9e7a1192d9869..0661267b3d361 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp @@ -113,10 +113,6 @@ class VirtualTrafficLightModule : public SceneModuleInterface void updateInfrastructureCommand(); - void setVelocityFactor( - const geometry_msgs::msg::Pose & stop_pose, - autoware_adapi_v1_msgs::msg::VelocityFactor * velocity_factor); - std::optional getPathIndexOfFirstEndLine(); bool isBeforeStartLine(const size_t end_line_idx); From 6383df232c86410f94f45592d5f502046e23ed87 Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Thu, 9 Jan 2025 16:19:01 +0900 Subject: [PATCH 08/25] feat: add node base Signed-off-by: TetsuKawa --- .../CMakeLists.txt | 20 ++++++++++++ .../autoware_topic_relay_controller/README.md | 15 +++++++++ .../config/topic_relay_controller.param.yaml | 4 +++ .../launch/topic_relay_controller.launch.xml | 4 +++ .../package.xml | 22 +++++++++++++ .../schema/topic_relay_controller.schema.json | 30 +++++++++++++++++ .../src/topic_relay_controller_node.cpp | 25 +++++++++++++++ .../src/topic_relay_controller_node.hpp | 32 +++++++++++++++++++ 8 files changed, 152 insertions(+) create mode 100644 system/autoware_topic_relay_controller/CMakeLists.txt create mode 100644 system/autoware_topic_relay_controller/README.md create mode 100644 system/autoware_topic_relay_controller/config/topic_relay_controller.param.yaml create mode 100644 system/autoware_topic_relay_controller/launch/topic_relay_controller.launch.xml create mode 100644 system/autoware_topic_relay_controller/package.xml create mode 100644 system/autoware_topic_relay_controller/schema/topic_relay_controller.schema.json create mode 100644 system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp create mode 100644 system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp diff --git a/system/autoware_topic_relay_controller/CMakeLists.txt b/system/autoware_topic_relay_controller/CMakeLists.txt new file mode 100644 index 0000000000000..7b035b2b62c23 --- /dev/null +++ b/system/autoware_topic_relay_controller/CMakeLists.txt @@ -0,0 +1,20 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_topic_relay_controller) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/topic_relay_controller_node.cpp +) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::topic_relay_controller::TopicRelayController" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR MultiThreadedExecutor +) + +ament_auto_package(INSTALL_TO_SHARE + launch + config +) diff --git a/system/autoware_topic_relay_controller/README.md b/system/autoware_topic_relay_controller/README.md new file mode 100644 index 0000000000000..1976a75dc6d48 --- /dev/null +++ b/system/autoware_topic_relay_controller/README.md @@ -0,0 +1,15 @@ +# topic_relay_controller + +## Purpose + +## Inputs / Outputs + +### Input + +### Output + +## Parameters + +## Assumptions / Known limits + +TBD. diff --git a/system/autoware_topic_relay_controller/config/topic_relay_controller.param.yaml b/system/autoware_topic_relay_controller/config/topic_relay_controller.param.yaml new file mode 100644 index 0000000000000..3a506213f054d --- /dev/null +++ b/system/autoware_topic_relay_controller/config/topic_relay_controller.param.yaml @@ -0,0 +1,4 @@ +--- +/**: + ros__parameters: + tmp: 0 \ No newline at end of file diff --git a/system/autoware_topic_relay_controller/launch/topic_relay_controller.launch.xml b/system/autoware_topic_relay_controller/launch/topic_relay_controller.launch.xml new file mode 100644 index 0000000000000..3f152f40d12df --- /dev/null +++ b/system/autoware_topic_relay_controller/launch/topic_relay_controller.launch.xml @@ -0,0 +1,4 @@ + + + + diff --git a/system/autoware_topic_relay_controller/package.xml b/system/autoware_topic_relay_controller/package.xml new file mode 100644 index 0000000000000..c67a52d52e84c --- /dev/null +++ b/system/autoware_topic_relay_controller/package.xml @@ -0,0 +1,22 @@ + + + + autoware_topic_relay_controller + 0.1.0 + The topic_relay_controller ROS 2 package + Tetsuhiro Kawaguchi + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + rclcpp + rclcpp_components + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/system/autoware_topic_relay_controller/schema/topic_relay_controller.schema.json b/system/autoware_topic_relay_controller/schema/topic_relay_controller.schema.json new file mode 100644 index 0000000000000..d51b0ae4ed1b6 --- /dev/null +++ b/system/autoware_topic_relay_controller/schema/topic_relay_controller.schema.json @@ -0,0 +1,30 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for topic relay controller", + "type": "object", + "definitions": { + "topic_rely_controller": { + "type": "object", + "properties": { + }, + "required": [ + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/topic_rely_controller" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false + } + \ No newline at end of file diff --git a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp new file mode 100644 index 0000000000000..74370ca306aac --- /dev/null +++ b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp @@ -0,0 +1,25 @@ +// Copyright 2025 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR +// CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language +// governing permissions and limitations under the License. + +#include "topic_relay_controller_node.hpp" + +namespace autoware::topic_relay_controller +{ +TopicRelayController::TopicRelayController(const rclcpp::NodeOptions & options) : Node("topic_relay_controller", options) +{ + RCLCPP_INFO(get_logger(), "topic_relay_controller_node started."); +} +} // namespace autoware::topic_relay_controller + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::topic_relay_controller::TopicRelayController) diff --git a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp new file mode 100644 index 0000000000000..c5ea006b13966 --- /dev/null +++ b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp @@ -0,0 +1,32 @@ +// Copyright 2025 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE_TOPIC_RELAY_CONTROLLER__TOPIC_RELAY_CONTROLLER_HPP_ +#define AUTOWARE_TOPIC_RELAY_CONTROLLER__TOPIC_RELAY_CONTROLLER_HPP_ + +// ROS 2 core +#include + +namespace autoware::topic_relay_controller +{ +class TopicRelayController : public rclcpp::Node +{ +public: + explicit TopicRelayController(const rclcpp::NodeOptions & options); + +private: +}; +} // namespace autoware::topic_relay_controller + +#endif // AUTOWARE_TOPIC_RELAY_CONTROLLER__TOPIC_RELAY_CONTROLLER_HPP_ \ No newline at end of file From fd504062af90acc755af8f8ce1db840fd6ff2c70 Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Fri, 10 Jan 2025 18:11:45 +0900 Subject: [PATCH 09/25] modify: include guard Signed-off-by: TetsuKawa --- .../src/topic_relay_controller_node.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp index c5ea006b13966..fca4dfdec31b4 100644 --- a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp +++ b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_TOPIC_RELAY_CONTROLLER__TOPIC_RELAY_CONTROLLER_HPP_ -#define AUTOWARE_TOPIC_RELAY_CONTROLLER__TOPIC_RELAY_CONTROLLER_HPP_ +#ifndef TOPIC_RELAY_CONTROLLER_HPP_ +#define TOPIC_RELAY_CONTROLLER_HPP_ // ROS 2 core #include @@ -29,4 +29,4 @@ class TopicRelayController : public rclcpp::Node }; } // namespace autoware::topic_relay_controller -#endif // AUTOWARE_TOPIC_RELAY_CONTROLLER__TOPIC_RELAY_CONTROLLER_HPP_ \ No newline at end of file +#endif // TOPIC_RELAY_CONTROLLER_HPP_ \ No newline at end of file From 4497e7016c439afec9062a095869fad81fa2b434 Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Fri, 10 Jan 2025 15:45:47 +0900 Subject: [PATCH 10/25] feat: delete schema Signed-off-by: TetsuKawa --- .../schema/topic_relay_controller.schema.json | 30 ------------------- 1 file changed, 30 deletions(-) delete mode 100644 system/autoware_topic_relay_controller/schema/topic_relay_controller.schema.json diff --git a/system/autoware_topic_relay_controller/schema/topic_relay_controller.schema.json b/system/autoware_topic_relay_controller/schema/topic_relay_controller.schema.json deleted file mode 100644 index d51b0ae4ed1b6..0000000000000 --- a/system/autoware_topic_relay_controller/schema/topic_relay_controller.schema.json +++ /dev/null @@ -1,30 +0,0 @@ -{ - "$schema": "http://json-schema.org/draft-07/schema#", - "title": "Parameters for topic relay controller", - "type": "object", - "definitions": { - "topic_rely_controller": { - "type": "object", - "properties": { - }, - "required": [ - ], - "additionalProperties": false - } - }, - "properties": { - "/**": { - "type": "object", - "properties": { - "ros__parameters": { - "$ref": "#/definitions/topic_rely_controller" - } - }, - "required": ["ros__parameters"], - "additionalProperties": false - } - }, - "required": ["/**"], - "additionalProperties": false - } - \ No newline at end of file From 4c4255e901230188d48562bf0e443be60e63c69c Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Fri, 10 Jan 2025 15:44:27 +0900 Subject: [PATCH 11/25] feat: delete config Signed-off-by: TetsuKawa --- system/autoware_topic_relay_controller/CMakeLists.txt | 1 - .../config/topic_relay_controller.param.yaml | 4 ---- 2 files changed, 5 deletions(-) delete mode 100644 system/autoware_topic_relay_controller/config/topic_relay_controller.param.yaml diff --git a/system/autoware_topic_relay_controller/CMakeLists.txt b/system/autoware_topic_relay_controller/CMakeLists.txt index 7b035b2b62c23..f244bf8122f82 100644 --- a/system/autoware_topic_relay_controller/CMakeLists.txt +++ b/system/autoware_topic_relay_controller/CMakeLists.txt @@ -16,5 +16,4 @@ rclcpp_components_register_node(${PROJECT_NAME} ament_auto_package(INSTALL_TO_SHARE launch - config ) diff --git a/system/autoware_topic_relay_controller/config/topic_relay_controller.param.yaml b/system/autoware_topic_relay_controller/config/topic_relay_controller.param.yaml deleted file mode 100644 index 3a506213f054d..0000000000000 --- a/system/autoware_topic_relay_controller/config/topic_relay_controller.param.yaml +++ /dev/null @@ -1,4 +0,0 @@ ---- -/**: - ros__parameters: - tmp: 0 \ No newline at end of file From 8b1445ac8d7429dd636ac7a85622eac99699eb70 Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Thu, 9 Jan 2025 16:29:05 +0900 Subject: [PATCH 12/25] feat: add subscriber Signed-off-by: TetsuKawa --- .../launch/topic_relay_controller.launch.xml | 16 ++++++- .../topic_relay_controller_tf.launch.xml | 18 ++++++++ .../package.xml | 1 + .../src/topic_relay_controller_node.cpp | 45 ++++++++++++++++++- .../src/topic_relay_controller_node.hpp | 21 +++++++++ 5 files changed, 99 insertions(+), 2 deletions(-) create mode 100644 system/autoware_topic_relay_controller/launch/topic_relay_controller_tf.launch.xml diff --git a/system/autoware_topic_relay_controller/launch/topic_relay_controller.launch.xml b/system/autoware_topic_relay_controller/launch/topic_relay_controller.launch.xml index 3f152f40d12df..49672c0a044ab 100644 --- a/system/autoware_topic_relay_controller/launch/topic_relay_controller.launch.xml +++ b/system/autoware_topic_relay_controller/launch/topic_relay_controller.launch.xml @@ -1,4 +1,18 @@ - + + + + + + + + + + + + + + + diff --git a/system/autoware_topic_relay_controller/launch/topic_relay_controller_tf.launch.xml b/system/autoware_topic_relay_controller/launch/topic_relay_controller_tf.launch.xml new file mode 100644 index 0000000000000..6dfd1464715bc --- /dev/null +++ b/system/autoware_topic_relay_controller/launch/topic_relay_controller_tf.launch.xml @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + diff --git a/system/autoware_topic_relay_controller/package.xml b/system/autoware_topic_relay_controller/package.xml index c67a52d52e84c..101c40b373048 100644 --- a/system/autoware_topic_relay_controller/package.xml +++ b/system/autoware_topic_relay_controller/package.xml @@ -12,6 +12,7 @@ rclcpp rclcpp_components + tf2_msgs ament_lint_auto autoware_lint_common diff --git a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp index 74370ca306aac..4dd51cf821e1d 100644 --- a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp +++ b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp @@ -17,7 +17,50 @@ namespace autoware::topic_relay_controller { TopicRelayController::TopicRelayController(const rclcpp::NodeOptions & options) : Node("topic_relay_controller", options) { - RCLCPP_INFO(get_logger(), "topic_relay_controller_node started."); + // Parameter + node_param_.topic = declare_parameter("topic"); + node_param_.remap_topic = declare_parameter("remap_topic"); + node_param_.qos = declare_parameter("qos", 1); + node_param_.transient_local = declare_parameter("transient_local", false); + node_param_.best_effort = declare_parameter("best_effort", false); + node_param_.is_transform = (node_param_.topic == "/tf" || node_param_.topic == "/tf_static"); + + if (node_param_.is_transform) { + node_param_.frame_id = declare_parameter("frame_id"); + node_param_.child_frame_id = declare_parameter("child_frame_id"); + } else { + node_param_.topic_type = declare_parameter("topic_type"); + } + + // Subscriber + rclcpp::QoS qos = rclcpp::QoS{node_param_.qos}; + if (node_param_.transient_local) { + qos.transient_local(); + } + if (node_param_.best_effort) { + qos.best_effort(); + } + + if (node_param_.is_transform) { + sub_transform_ = this->create_subscription( + node_param_.topic, qos, [this](tf2_msgs::msg::TFMessage::ConstSharedPtr msg) { + for (const auto & transform : msg->transforms) { + if ( + transform.header.frame_id == node_param_.frame_id && + transform.child_frame_id == node_param_.child_frame_id) { + RCLCPP_INFO ( + this->get_logger(), "Received transform from %s to %s", + transform.header.frame_id.c_str(), transform.child_frame_id.c_str()); + } + } + }); + } else { + sub_topic_ = this->create_generic_subscription( + node_param_.topic, node_param_.topic_type, qos, + [this]([[maybe_unused]] std::shared_ptr msg) { + RCLCPP_INFO(this->get_logger(), "Received message"); + }); + } } } // namespace autoware::topic_relay_controller diff --git a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp index fca4dfdec31b4..fe4a789f6c5c4 100644 --- a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp +++ b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp @@ -18,14 +18,35 @@ // ROS 2 core #include +#include + namespace autoware::topic_relay_controller { +struct NodeParam +{ + std::string topic; + std::string remap_topic; + std::string topic_type; + size_t qos; + std::string frame_id; + std::string child_frame_id; + bool transient_local; + bool best_effort; + bool is_transform; +}; + class TopicRelayController : public rclcpp::Node { public: explicit TopicRelayController(const rclcpp::NodeOptions & options); private: + // Parameter + NodeParam node_param_; + + // Subscriber + rclcpp::GenericSubscription::SharedPtr sub_topic_; + rclcpp::Subscription::SharedPtr sub_transform_; }; } // namespace autoware::topic_relay_controller From 8cb9dd308f814987e36fe0e87571bea87c53718c Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Fri, 10 Jan 2025 16:02:53 +0900 Subject: [PATCH 13/25] modify: add include Signed-off-by: TetsuKawa --- .../src/topic_relay_controller_node.cpp | 3 +++ .../src/topic_relay_controller_node.hpp | 2 ++ 2 files changed, 5 insertions(+) diff --git a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp index 4dd51cf821e1d..6584a373e3881 100644 --- a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp +++ b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp @@ -13,6 +13,9 @@ #include "topic_relay_controller_node.hpp" +#include +#include + namespace autoware::topic_relay_controller { TopicRelayController::TopicRelayController(const rclcpp::NodeOptions & options) : Node("topic_relay_controller", options) diff --git a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp index fe4a789f6c5c4..699e9a2a66ace 100644 --- a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp +++ b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp @@ -20,6 +20,8 @@ #include +#include + namespace autoware::topic_relay_controller { struct NodeParam From af7abe848f828cbb1933c82b294704fb2f57fd6e Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Thu, 9 Jan 2025 16:34:06 +0900 Subject: [PATCH 14/25] feat: add publisher Signed-off-by: TetsuKawa --- .../src/topic_relay_controller_node.cpp | 14 ++++++++++---- .../src/topic_relay_controller_node.hpp | 4 ++++ 2 files changed, 14 insertions(+), 4 deletions(-) diff --git a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp index 6584a373e3881..e70bced3ebf96 100644 --- a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp +++ b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp @@ -45,23 +45,29 @@ TopicRelayController::TopicRelayController(const rclcpp::NodeOptions & options) } if (node_param_.is_transform) { + // Publisher + pub_transform_ = this->create_publisher( + node_param_.remap_topic, qos); + sub_transform_ = this->create_subscription( node_param_.topic, qos, [this](tf2_msgs::msg::TFMessage::ConstSharedPtr msg) { for (const auto & transform : msg->transforms) { if ( transform.header.frame_id == node_param_.frame_id && transform.child_frame_id == node_param_.child_frame_id) { - RCLCPP_INFO ( - this->get_logger(), "Received transform from %s to %s", - transform.header.frame_id.c_str(), transform.child_frame_id.c_str()); + pub_transform_->publish(*msg); } } }); } else { + // Publisher + pub_topic_ = this->create_generic_publisher( + node_param_.remap_topic, node_param_.topic_type, qos); + sub_topic_ = this->create_generic_subscription( node_param_.topic, node_param_.topic_type, qos, [this]([[maybe_unused]] std::shared_ptr msg) { - RCLCPP_INFO(this->get_logger(), "Received message"); + pub_topic_->publish(*msg); }); } } diff --git a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp index 699e9a2a66ace..7ee647d2893f0 100644 --- a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp +++ b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp @@ -49,6 +49,10 @@ class TopicRelayController : public rclcpp::Node // Subscriber rclcpp::GenericSubscription::SharedPtr sub_topic_; rclcpp::Subscription::SharedPtr sub_transform_; + + // Publisher + rclcpp::GenericPublisher::SharedPtr pub_topic_; + rclcpp::Publisher::SharedPtr pub_transform_; }; } // namespace autoware::topic_relay_controller From 3cc7e170652836df0e4147b4208905e90ec525bc Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Thu, 9 Jan 2025 17:25:13 +0900 Subject: [PATCH 15/25] feat: add service Signed-off-by: TetsuKawa --- .../launch/topic_relay_controller.launch.xml | 4 ++++ .../topic_relay_controller_tf.launch.xml | 4 ++++ .../package.xml | 1 + .../src/topic_relay_controller_node.cpp | 20 ++++++++++++++++--- .../src/topic_relay_controller_node.hpp | 9 +++++++++ 5 files changed, 35 insertions(+), 3 deletions(-) diff --git a/system/autoware_topic_relay_controller/launch/topic_relay_controller.launch.xml b/system/autoware_topic_relay_controller/launch/topic_relay_controller.launch.xml index 49672c0a044ab..7dbc806ee6424 100644 --- a/system/autoware_topic_relay_controller/launch/topic_relay_controller.launch.xml +++ b/system/autoware_topic_relay_controller/launch/topic_relay_controller.launch.xml @@ -6,6 +6,8 @@ + + @@ -14,5 +16,7 @@ + + diff --git a/system/autoware_topic_relay_controller/launch/topic_relay_controller_tf.launch.xml b/system/autoware_topic_relay_controller/launch/topic_relay_controller_tf.launch.xml index 6dfd1464715bc..801357c03f8e3 100644 --- a/system/autoware_topic_relay_controller/launch/topic_relay_controller_tf.launch.xml +++ b/system/autoware_topic_relay_controller/launch/topic_relay_controller_tf.launch.xml @@ -6,6 +6,8 @@ + + @@ -14,5 +16,7 @@ + + diff --git a/system/autoware_topic_relay_controller/package.xml b/system/autoware_topic_relay_controller/package.xml index 101c40b373048..537774e67b998 100644 --- a/system/autoware_topic_relay_controller/package.xml +++ b/system/autoware_topic_relay_controller/package.xml @@ -13,6 +13,7 @@ rclcpp rclcpp_components tf2_msgs + tier4_system_msgs ament_lint_auto autoware_lint_common diff --git a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp index e70bced3ebf96..78820e2129988 100644 --- a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp +++ b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp @@ -18,7 +18,7 @@ namespace autoware::topic_relay_controller { -TopicRelayController::TopicRelayController(const rclcpp::NodeOptions & options) : Node("topic_relay_controller", options) +TopicRelayController::TopicRelayController(const rclcpp::NodeOptions & options): Node("topic_relay_controller", options), is_relaying_(true) { // Parameter node_param_.topic = declare_parameter("topic"); @@ -27,6 +27,8 @@ TopicRelayController::TopicRelayController(const rclcpp::NodeOptions & options) node_param_.transient_local = declare_parameter("transient_local", false); node_param_.best_effort = declare_parameter("best_effort", false); node_param_.is_transform = (node_param_.topic == "/tf" || node_param_.topic == "/tf_static"); + node_param_.enable_relay_control = declare_parameter("enable_relay_control"); + node_param_.srv_name = declare_parameter("srv_name"); if (node_param_.is_transform) { node_param_.frame_id = declare_parameter("frame_id"); @@ -35,6 +37,17 @@ TopicRelayController::TopicRelayController(const rclcpp::NodeOptions & options) node_param_.topic_type = declare_parameter("topic_type"); } + // Service + if (node_param_.enable_relay_control) { + srv_change_relay_control_ = create_service( + node_param_.srv_name, + [this](const tier4_system_msgs::srv::ChangeTopicRelayControl::Request::SharedPtr request, + tier4_system_msgs::srv::ChangeTopicRelayControl::Response::SharedPtr response) { + is_relaying_ = request->relay_on; + response->status.success = true; + }); + } + // Subscriber rclcpp::QoS qos = rclcpp::QoS{node_param_.qos}; if (node_param_.transient_local) { @@ -54,7 +67,8 @@ TopicRelayController::TopicRelayController(const rclcpp::NodeOptions & options) for (const auto & transform : msg->transforms) { if ( transform.header.frame_id == node_param_.frame_id && - transform.child_frame_id == node_param_.child_frame_id) { + transform.child_frame_id == node_param_.child_frame_id && + is_relaying_) { pub_transform_->publish(*msg); } } @@ -67,7 +81,7 @@ TopicRelayController::TopicRelayController(const rclcpp::NodeOptions & options) sub_topic_ = this->create_generic_subscription( node_param_.topic, node_param_.topic_type, qos, [this]([[maybe_unused]] std::shared_ptr msg) { - pub_topic_->publish(*msg); + if (is_relaying_) pub_topic_->publish(*msg); }); } } diff --git a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp index 7ee647d2893f0..23d17ce3b7c21 100644 --- a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp +++ b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp @@ -19,6 +19,7 @@ #include #include +#include #include @@ -35,6 +36,8 @@ struct NodeParam bool transient_local; bool best_effort; bool is_transform; + bool enable_relay_control; + std::string srv_name; }; class TopicRelayController : public rclcpp::Node @@ -53,6 +56,12 @@ class TopicRelayController : public rclcpp::Node // Publisher rclcpp::GenericPublisher::SharedPtr pub_topic_; rclcpp::Publisher::SharedPtr pub_transform_; + + // Service + rclcpp::Service::SharedPtr srv_change_relay_control_; + + // State + bool is_relaying_; }; } // namespace autoware::topic_relay_controller From 6a1a945c6b091edbca439469f1f59248d22e81c0 Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Fri, 10 Jan 2025 15:40:40 +0900 Subject: [PATCH 16/25] modify: typo Signed-off-by: TetsuKawa --- .../launch/topic_relay_controller.launch.xml | 4 ++-- .../launch/topic_relay_controller_tf.launch.xml | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/system/autoware_topic_relay_controller/launch/topic_relay_controller.launch.xml b/system/autoware_topic_relay_controller/launch/topic_relay_controller.launch.xml index 7dbc806ee6424..4165cf7ea741b 100644 --- a/system/autoware_topic_relay_controller/launch/topic_relay_controller.launch.xml +++ b/system/autoware_topic_relay_controller/launch/topic_relay_controller.launch.xml @@ -6,7 +6,7 @@ - + @@ -16,7 +16,7 @@ - + diff --git a/system/autoware_topic_relay_controller/launch/topic_relay_controller_tf.launch.xml b/system/autoware_topic_relay_controller/launch/topic_relay_controller_tf.launch.xml index 801357c03f8e3..8a497982dd813 100644 --- a/system/autoware_topic_relay_controller/launch/topic_relay_controller_tf.launch.xml +++ b/system/autoware_topic_relay_controller/launch/topic_relay_controller_tf.launch.xml @@ -6,7 +6,7 @@ - + @@ -16,7 +16,7 @@ - + From 310cdd36c8937bbbc2507269613f582801af6280 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 10 Jan 2025 09:13:48 +0000 Subject: [PATCH 17/25] style(pre-commit): autofix Signed-off-by: TetsuKawa --- .../src/topic_relay_controller_node.cpp | 20 +++++++++---------- .../src/topic_relay_controller_node.hpp | 9 +++++---- 2 files changed, 15 insertions(+), 14 deletions(-) diff --git a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp index 78820e2129988..ace592f3c875e 100644 --- a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp +++ b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp @@ -18,12 +18,13 @@ namespace autoware::topic_relay_controller { -TopicRelayController::TopicRelayController(const rclcpp::NodeOptions & options): Node("topic_relay_controller", options), is_relaying_(true) +TopicRelayController::TopicRelayController(const rclcpp::NodeOptions & options) +: Node("topic_relay_controller", options), is_relaying_(true) { // Parameter node_param_.topic = declare_parameter("topic"); node_param_.remap_topic = declare_parameter("remap_topic"); - node_param_.qos = declare_parameter("qos", 1); + node_param_.qos = declare_parameter("qos", 1); node_param_.transient_local = declare_parameter("transient_local", false); node_param_.best_effort = declare_parameter("best_effort", false); node_param_.is_transform = (node_param_.topic == "/tf" || node_param_.topic == "/tf_static"); @@ -41,7 +42,8 @@ TopicRelayController::TopicRelayController(const rclcpp::NodeOptions & options): if (node_param_.enable_relay_control) { srv_change_relay_control_ = create_service( node_param_.srv_name, - [this](const tier4_system_msgs::srv::ChangeTopicRelayControl::Request::SharedPtr request, + [this]( + const tier4_system_msgs::srv::ChangeTopicRelayControl::Request::SharedPtr request, tier4_system_msgs::srv::ChangeTopicRelayControl::Response::SharedPtr response) { is_relaying_ = request->relay_on; response->status.success = true; @@ -59,24 +61,22 @@ TopicRelayController::TopicRelayController(const rclcpp::NodeOptions & options): if (node_param_.is_transform) { // Publisher - pub_transform_ = this->create_publisher( - node_param_.remap_topic, qos); + pub_transform_ = this->create_publisher(node_param_.remap_topic, qos); sub_transform_ = this->create_subscription( node_param_.topic, qos, [this](tf2_msgs::msg::TFMessage::ConstSharedPtr msg) { for (const auto & transform : msg->transforms) { if ( transform.header.frame_id == node_param_.frame_id && - transform.child_frame_id == node_param_.child_frame_id && - is_relaying_) { + transform.child_frame_id == node_param_.child_frame_id && is_relaying_) { pub_transform_->publish(*msg); } } }); } else { // Publisher - pub_topic_ = this->create_generic_publisher( - node_param_.remap_topic, node_param_.topic_type, qos); + pub_topic_ = + this->create_generic_publisher(node_param_.remap_topic, node_param_.topic_type, qos); sub_topic_ = this->create_generic_subscription( node_param_.topic, node_param_.topic_type, qos, @@ -85,7 +85,7 @@ TopicRelayController::TopicRelayController(const rclcpp::NodeOptions & options): }); } } -} // namespace autoware::topic_relay_controller +} // namespace autoware::topic_relay_controller #include RCLCPP_COMPONENTS_REGISTER_NODE(autoware::topic_relay_controller::TopicRelayController) diff --git a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp index 23d17ce3b7c21..14ac259bbee6e 100644 --- a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp +++ b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TOPIC_RELAY_CONTROLLER_HPP_ -#define TOPIC_RELAY_CONTROLLER_HPP_ +#ifndef TOPIC_RELAY_CONTROLLER_NODE_HPP_ +#define TOPIC_RELAY_CONTROLLER_NODE_HPP_ // ROS 2 core #include @@ -58,11 +58,12 @@ class TopicRelayController : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_transform_; // Service - rclcpp::Service::SharedPtr srv_change_relay_control_; + rclcpp::Service::SharedPtr + srv_change_relay_control_; // State bool is_relaying_; }; } // namespace autoware::topic_relay_controller -#endif // TOPIC_RELAY_CONTROLLER_HPP_ \ No newline at end of file +#endif // TOPIC_RELAY_CONTROLLER_NODE_HPP_ From 1429668378b78aaee88a32fcf849cbd260882398 Mon Sep 17 00:00:00 2001 From: TetsuKawa <70682030+TetsuKawa@users.noreply.github.com> Date: Tue, 14 Jan 2025 23:13:20 +0900 Subject: [PATCH 18/25] modify: add include memory Signed-off-by: TetsuKawa --- .../src/topic_relay_controller_node.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp index 14ac259bbee6e..7491ef4662021 100644 --- a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp +++ b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp @@ -22,6 +22,7 @@ #include #include +#include namespace autoware::topic_relay_controller { From 663ec502fff88b77c3a185bb2136ec502db486b2 Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Tue, 14 Jan 2025 22:21:49 +0900 Subject: [PATCH 19/25] modify: add qos setting Signed-off-by: TetsuKawa --- .../launch/topic_relay_controller_tf.launch.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/system/autoware_topic_relay_controller/launch/topic_relay_controller_tf.launch.xml b/system/autoware_topic_relay_controller/launch/topic_relay_controller_tf.launch.xml index 8a497982dd813..d23ef5f26d91e 100644 --- a/system/autoware_topic_relay_controller/launch/topic_relay_controller_tf.launch.xml +++ b/system/autoware_topic_relay_controller/launch/topic_relay_controller_tf.launch.xml @@ -2,6 +2,7 @@ + @@ -12,6 +13,7 @@ + From 3c5d5ae6e2b8369e8731317fae7bc652a554b2db Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 14 Jan 2025 14:15:30 +0000 Subject: [PATCH 20/25] style(pre-commit): autofix Signed-off-by: TetsuKawa --- .../src/topic_relay_controller_node.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp index 7491ef4662021..1bdcb435e01c4 100644 --- a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp +++ b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp @@ -21,8 +21,8 @@ #include #include -#include #include +#include namespace autoware::topic_relay_controller { From 677a5968a2d263aa7ff0cc00ba43ba20387562a9 Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Tue, 14 Jan 2025 22:20:41 +0900 Subject: [PATCH 21/25] feat: add enable_keep_publishing Signed-off-by: TetsuKawa --- .../launch/topic_relay_controller.launch.xml | 4 ++ .../topic_relay_controller_tf.launch.xml | 4 ++ .../src/topic_relay_controller_node.cpp | 44 ++++++++++++++++--- .../src/topic_relay_controller_node.hpp | 7 +++ 4 files changed, 53 insertions(+), 6 deletions(-) diff --git a/system/autoware_topic_relay_controller/launch/topic_relay_controller.launch.xml b/system/autoware_topic_relay_controller/launch/topic_relay_controller.launch.xml index 4165cf7ea741b..ae642dad39f93 100644 --- a/system/autoware_topic_relay_controller/launch/topic_relay_controller.launch.xml +++ b/system/autoware_topic_relay_controller/launch/topic_relay_controller.launch.xml @@ -8,6 +8,8 @@ + + @@ -18,5 +20,7 @@ + + diff --git a/system/autoware_topic_relay_controller/launch/topic_relay_controller_tf.launch.xml b/system/autoware_topic_relay_controller/launch/topic_relay_controller_tf.launch.xml index d23ef5f26d91e..648d53cf4e1b7 100644 --- a/system/autoware_topic_relay_controller/launch/topic_relay_controller_tf.launch.xml +++ b/system/autoware_topic_relay_controller/launch/topic_relay_controller_tf.launch.xml @@ -9,6 +9,8 @@ + + @@ -20,5 +22,7 @@ + + diff --git a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp index ace592f3c875e..afc1261e201a7 100644 --- a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp +++ b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp @@ -30,6 +30,8 @@ TopicRelayController::TopicRelayController(const rclcpp::NodeOptions & options) node_param_.is_transform = (node_param_.topic == "/tf" || node_param_.topic == "/tf_static"); node_param_.enable_relay_control = declare_parameter("enable_relay_control"); node_param_.srv_name = declare_parameter("srv_name"); + node_param_.enable_keep_publishing = declare_parameter("enable_keep_publishing"); + node_param_.update_rate = declare_parameter("update_rate"); if (node_param_.is_transform) { node_param_.frame_id = declare_parameter("frame_id"); @@ -46,6 +48,7 @@ TopicRelayController::TopicRelayController(const rclcpp::NodeOptions & options) const tier4_system_msgs::srv::ChangeTopicRelayControl::Request::SharedPtr request, tier4_system_msgs::srv::ChangeTopicRelayControl::Response::SharedPtr response) { is_relaying_ = request->relay_on; + RCLCPP_INFO(get_logger(), "relay control: %s", is_relaying_ ? "ON" : "OFF"); response->status.success = true; }); } @@ -64,15 +67,21 @@ TopicRelayController::TopicRelayController(const rclcpp::NodeOptions & options) pub_transform_ = this->create_publisher(node_param_.remap_topic, qos); sub_transform_ = this->create_subscription( - node_param_.topic, qos, [this](tf2_msgs::msg::TFMessage::ConstSharedPtr msg) { + node_param_.topic, qos, [this](tf2_msgs::msg::TFMessage::SharedPtr msg) { for (const auto & transform : msg->transforms) { if ( - transform.header.frame_id == node_param_.frame_id && - transform.child_frame_id == node_param_.child_frame_id && is_relaying_) { + transform.header.frame_id != node_param_.frame_id || + transform.child_frame_id != node_param_.child_frame_id || + !is_relaying_) return; + + if (node_param_.enable_keep_publishing) { + last_tf_topic_ = msg; + } else { pub_transform_->publish(*msg); } } - }); + } + ); } else { // Publisher pub_topic_ = @@ -81,8 +90,31 @@ TopicRelayController::TopicRelayController(const rclcpp::NodeOptions & options) sub_topic_ = this->create_generic_subscription( node_param_.topic, node_param_.topic_type, qos, [this]([[maybe_unused]] std::shared_ptr msg) { - if (is_relaying_) pub_topic_->publish(*msg); - }); + if (!is_relaying_) return; + + if (node_param_.enable_keep_publishing) { + last_topic_ = msg; + } else { + pub_topic_->publish(*msg); + } + } + ); + } + + // Timer + if (node_param_.enable_keep_publishing) { + const auto update_period_ns = rclcpp::Rate(node_param_.update_rate).period(); + timer_ = rclcpp::create_timer( + this, get_clock(), update_period_ns, [this]() { + if (!is_relaying_) return; + + if (node_param_.is_transform) { + if (last_tf_topic_) pub_transform_->publish(*last_tf_topic_); + } else { + if (last_topic_) pub_topic_->publish(*last_topic_); + } + } + ); } } } // namespace autoware::topic_relay_controller diff --git a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp index 1bdcb435e01c4..c5e2ac08203c7 100644 --- a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp +++ b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp @@ -39,6 +39,8 @@ struct NodeParam bool is_transform; bool enable_relay_control; std::string srv_name; + bool enable_keep_publishing; + int update_rate; }; class TopicRelayController : public rclcpp::Node @@ -62,8 +64,13 @@ class TopicRelayController : public rclcpp::Node rclcpp::Service::SharedPtr srv_change_relay_control_; + // Timer + rclcpp::TimerBase::SharedPtr timer_; + // State bool is_relaying_; + tf2_msgs::msg::TFMessage::SharedPtr last_tf_topic_; + std::shared_ptr last_topic_; }; } // namespace autoware::topic_relay_controller From 1cfa03ea22299a9157e9147494e29531fac140de Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 14 Jan 2025 13:26:28 +0000 Subject: [PATCH 22/25] style(pre-commit): autofix Signed-off-by: TetsuKawa --- .../src/topic_relay_controller_node.cpp | 30 ++++++++----------- 1 file changed, 13 insertions(+), 17 deletions(-) diff --git a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp index afc1261e201a7..b68d8262de24d 100644 --- a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp +++ b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp @@ -71,17 +71,16 @@ TopicRelayController::TopicRelayController(const rclcpp::NodeOptions & options) for (const auto & transform : msg->transforms) { if ( transform.header.frame_id != node_param_.frame_id || - transform.child_frame_id != node_param_.child_frame_id || - !is_relaying_) return; - + transform.child_frame_id != node_param_.child_frame_id || !is_relaying_) + return; + if (node_param_.enable_keep_publishing) { last_tf_topic_ = msg; } else { pub_transform_->publish(*msg); } } - } - ); + }); } else { // Publisher pub_topic_ = @@ -91,30 +90,27 @@ TopicRelayController::TopicRelayController(const rclcpp::NodeOptions & options) node_param_.topic, node_param_.topic_type, qos, [this]([[maybe_unused]] std::shared_ptr msg) { if (!is_relaying_) return; - + if (node_param_.enable_keep_publishing) { last_topic_ = msg; } else { pub_topic_->publish(*msg); } - } - ); + }); } // Timer if (node_param_.enable_keep_publishing) { const auto update_period_ns = rclcpp::Rate(node_param_.update_rate).period(); - timer_ = rclcpp::create_timer( - this, get_clock(), update_period_ns, [this]() { - if (!is_relaying_) return; + timer_ = rclcpp::create_timer(this, get_clock(), update_period_ns, [this]() { + if (!is_relaying_) return; - if (node_param_.is_transform) { - if (last_tf_topic_) pub_transform_->publish(*last_tf_topic_); - } else { - if (last_topic_) pub_topic_->publish(*last_topic_); - } + if (node_param_.is_transform) { + if (last_tf_topic_) pub_transform_->publish(*last_tf_topic_); + } else { + if (last_topic_) pub_topic_->publish(*last_topic_); } - ); + }); } } } // namespace autoware::topic_relay_controller From 12e9dacd2d53b31f07388d14ac793777c0355e85 Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Sun, 19 Jan 2025 17:31:26 +0900 Subject: [PATCH 23/25] feat: add maintainer Signed-off-by: TetsuKawa --- system/autoware_topic_relay_controller/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/system/autoware_topic_relay_controller/package.xml b/system/autoware_topic_relay_controller/package.xml index 537774e67b998..bd53a63067689 100644 --- a/system/autoware_topic_relay_controller/package.xml +++ b/system/autoware_topic_relay_controller/package.xml @@ -5,6 +5,7 @@ 0.1.0 The topic_relay_controller ROS 2 package Tetsuhiro Kawaguchi + Makoto Kurihara Apache License 2.0 ament_cmake_auto From 290e3dfd020975b173998974cc0000e2673d3ba1 Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Sun, 19 Jan 2025 17:31:37 +0900 Subject: [PATCH 24/25] feat: add read me Signed-off-by: TetsuKawa --- .../autoware_topic_relay_controller/README.md | 42 +++++++++++++++++++ 1 file changed, 42 insertions(+) diff --git a/system/autoware_topic_relay_controller/README.md b/system/autoware_topic_relay_controller/README.md index 1976a75dc6d48..7412be52789a9 100644 --- a/system/autoware_topic_relay_controller/README.md +++ b/system/autoware_topic_relay_controller/README.md @@ -13,3 +13,45 @@ ## Assumptions / Known limits TBD. + +# topic_relay_controller + +## Purpose +The `topic_relay_controller` node subscribes to a specified topic, remaps it, and republishes it. Additionally, it has the capability to continue publishing the last received value if the subscription stops. + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| ------------- | ------------------------------- | ------------------------------------------------------------------------- | +| `/` | `` | Topic to be subscribed, as defined by the `topic` parameter. | +| `/tf` | `tf2_msgs::msg::TFMessage` | (Optional) If the topic is `/tf`, used for transform message relay. | +| `/tf_static` | `tf2_msgs::msg::TFMessage` | (Optional) If the topic is `/tf_static`, used for static transforms. | + +### Output + +| Name | Type | Description | +| ------------- | ------------------------------- | ------------------------------------------------------------------------- | +| `/` | `` | Republished topic after remapping, as defined by the `remap_topic` parameter. | + + +## Parameters +- **`topic`** (string): The name of the input topic to subscribe to. +- **`remap_topic`** (string): The name of the output topic to publish to. +- **`topic_type`** (string): The type of messages being relayed. +- **`qos`** (integer, default: `1`): QoS profile to use for subscriptions and publications. +- **`transient_local`** (boolean, default: `false`): Enables transient local QoS for subscribers. +- **`best_effort`** (boolean, default: `false`): Enables best-effort QoS for subscribers. +- **`enable_relay_control`** (boolean, default: `true`): Allows dynamic relay control via a service. +- **`srv_name`** (string): The service name for relay control when `enable_relay_control` is `true`. +- **`enable_keep_publishing`** (boolean, default: `false`): Keeps publishing the last received topic value when not subscribed. +- **`update_rate`** (integer, default: `10`): The rate (Hz) for publishing the last topic value when `enable_keep_publishing` is `true`. +- **`frame_id`** (string, optional): Frame ID for transform messages when subscribing to `/tf` or `/tf_static`. +- **`child_frame_id`** (string, optional): Child frame ID for transform messages when subscribing to `/tf` or `/tf_static`. + +## Assumptions / Known limits +- The node assumes that the specified `topic` and `remap_topic` are valid and accessible within the ROS 2 environment. +- If `enable_keep_publishing` is `true`, the node continuously republishes the last received value even if no new messages are being received. +- For `/tf` and `/tf_static`, additional parameters like `frame_id` and `child_frame_id` are required for selective transformation relays. +- QoS settings must be carefully chosen to match the requirements of the subscribed and published topics. From 8695334845bf45adbbc0ca833446c5eb6b7421be Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 20 Jan 2025 03:09:58 +0000 Subject: [PATCH 25/25] style(pre-commit): autofix Signed-off-by: TetsuKawa --- .../autoware_topic_relay_controller/README.md | 20 ++++++++++--------- 1 file changed, 11 insertions(+), 9 deletions(-) diff --git a/system/autoware_topic_relay_controller/README.md b/system/autoware_topic_relay_controller/README.md index 7412be52789a9..b9844302b13f7 100644 --- a/system/autoware_topic_relay_controller/README.md +++ b/system/autoware_topic_relay_controller/README.md @@ -17,26 +17,27 @@ TBD. # topic_relay_controller ## Purpose + The `topic_relay_controller` node subscribes to a specified topic, remaps it, and republishes it. Additionally, it has the capability to continue publishing the last received value if the subscription stops. ## Inputs / Outputs ### Input -| Name | Type | Description | -| ------------- | ------------------------------- | ------------------------------------------------------------------------- | -| `/` | `` | Topic to be subscribed, as defined by the `topic` parameter. | -| `/tf` | `tf2_msgs::msg::TFMessage` | (Optional) If the topic is `/tf`, used for transform message relay. | -| `/tf_static` | `tf2_msgs::msg::TFMessage` | (Optional) If the topic is `/tf_static`, used for static transforms. | +| Name | Type | Description | +| ------------ | -------------------------- | -------------------------------------------------------------------- | +| `/` | `` | Topic to be subscribed, as defined by the `topic` parameter. | +| `/tf` | `tf2_msgs::msg::TFMessage` | (Optional) If the topic is `/tf`, used for transform message relay. | +| `/tf_static` | `tf2_msgs::msg::TFMessage` | (Optional) If the topic is `/tf_static`, used for static transforms. | ### Output -| Name | Type | Description | -| ------------- | ------------------------------- | ------------------------------------------------------------------------- | -| `/` | `` | Republished topic after remapping, as defined by the `remap_topic` parameter. | - +| Name | Type | Description | +| ---------------- | -------------------------- | ----------------------------------------------------------------------------- | +| `/` | `` | Republished topic after remapping, as defined by the `remap_topic` parameter. | ## Parameters + - **`topic`** (string): The name of the input topic to subscribe to. - **`remap_topic`** (string): The name of the output topic to publish to. - **`topic_type`** (string): The type of messages being relayed. @@ -51,6 +52,7 @@ The `topic_relay_controller` node subscribes to a specified topic, remaps it, an - **`child_frame_id`** (string, optional): Child frame ID for transform messages when subscribing to `/tf` or `/tf_static`. ## Assumptions / Known limits + - The node assumes that the specified `topic` and `remap_topic` are valid and accessible within the ROS 2 environment. - If `enable_keep_publishing` is `true`, the node continuously republishes the last received value even if no new messages are being received. - For `/tf` and `/tf_static`, additional parameters like `frame_id` and `child_frame_id` are required for selective transformation relays.