From 53724037838c73575eaa7a7c794d729bf7d93f8c Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Tue, 26 Nov 2024 14:30:27 +0900 Subject: [PATCH] fix(obstacle_cruise_planner)!: remove stop reason (#9464) fix(obstacle_cruise_planner): remove stop reason Signed-off-by: satoshi-ota --- .../README.md | 11 ++-- .../planner_interface.hpp | 2 - .../obstacle_cruise_planner/type_alias.hpp | 5 -- .../launch/obstacle_cruise_planner.launch.xml | 1 - .../src/planner_interface.cpp | 52 ------------------- 5 files changed, 5 insertions(+), 66 deletions(-) diff --git a/planning/autoware_obstacle_cruise_planner/README.md b/planning/autoware_obstacle_cruise_planner/README.md index 1cb8128110e7c..42a569c07ddb2 100644 --- a/planning/autoware_obstacle_cruise_planner/README.md +++ b/planning/autoware_obstacle_cruise_planner/README.md @@ -23,12 +23,11 @@ The `autoware_obstacle_cruise_planner` package has following modules. ### Output topics -| Name | Type | Description | -| ------------------------------- | ---------------------------------------------- | ------------------------------------- | -| `~/output/trajectory` | autoware_planning_msgs::Trajectory | output trajectory | -| `~/output/velocity_limit` | tier4_planning_msgs::VelocityLimit | velocity limit for cruising | -| `~/output/clear_velocity_limit` | tier4_planning_msgs::VelocityLimitClearCommand | clear command for velocity limit | -| `~/output/stop_reasons` | tier4_planning_msgs::StopReasonArray | reasons that make the vehicle to stop | +| Name | Type | Description | +| ------------------------------- | ---------------------------------------------- | -------------------------------- | +| `~/output/trajectory` | autoware_planning_msgs::Trajectory | output trajectory | +| `~/output/velocity_limit` | tier4_planning_msgs::VelocityLimit | velocity limit for cruising | +| `~/output/clear_velocity_limit` | tier4_planning_msgs::VelocityLimitClearCommand | clear command for velocity limit | ## Design 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 e4fb1c6cfe36d..ef6ae1662dcee 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 @@ -54,7 +54,6 @@ class PlannerInterface slow_down_param_(SlowDownParam(node)), stop_param_(StopParam(node, longitudinal_info)) { - stop_reasons_pub_ = node.create_publisher("~/output/stop_reasons", 1); velocity_factors_pub_ = node.create_publisher("/planning/velocity_factors/obstacle_cruise", 1); stop_speed_exceeded_pub_ = @@ -146,7 +145,6 @@ class PlannerInterface stop_watch_; // Publishers - rclcpp::Publisher::SharedPtr stop_reasons_pub_; 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 541a8281b2f1d..04badd2a956ef 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 @@ -30,8 +30,6 @@ #include "sensor_msgs/msg/point_cloud2.hpp" #include "tier4_debug_msgs/msg/float32_stamped.hpp" #include "tier4_debug_msgs/msg/float64_stamped.hpp" -#include "tier4_planning_msgs/msg/stop_factor.hpp" -#include "tier4_planning_msgs/msg/stop_reason_array.hpp" #include "tier4_planning_msgs/msg/stop_speed_exceeded.hpp" #include "tier4_planning_msgs/msg/velocity_limit.hpp" #include "tier4_planning_msgs/msg/velocity_limit_clear_command.hpp" @@ -58,9 +56,6 @@ using nav_msgs::msg::Odometry; using sensor_msgs::msg::PointCloud2; using tier4_debug_msgs::msg::Float32Stamped; using tier4_debug_msgs::msg::Float64Stamped; -using tier4_planning_msgs::msg::StopFactor; -using tier4_planning_msgs::msg::StopReason; -using tier4_planning_msgs::msg::StopReasonArray; using tier4_planning_msgs::msg::StopSpeedExceeded; using tier4_planning_msgs::msg::VelocityLimit; using tier4_planning_msgs::msg::VelocityLimitClearCommand; diff --git a/planning/autoware_obstacle_cruise_planner/launch/obstacle_cruise_planner.launch.xml b/planning/autoware_obstacle_cruise_planner/launch/obstacle_cruise_planner.launch.xml index aaf6db3e36613..189cd44379276 100644 --- a/planning/autoware_obstacle_cruise_planner/launch/obstacle_cruise_planner.launch.xml +++ b/planning/autoware_obstacle_cruise_planner/launch/obstacle_cruise_planner.launch.xml @@ -31,6 +31,5 @@ - diff --git a/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp b/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp index 61946a8e4b37f..bfff561e3ffe2 100644 --- a/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp @@ -39,54 +39,6 @@ StopSpeedExceeded createStopSpeedExceededMsg( return msg; } -tier4_planning_msgs::msg::StopReasonArray makeStopReasonArray( - const PlannerData & planner_data, const geometry_msgs::msg::Pose & stop_pose, - const StopObstacle & stop_obstacle) -{ - // create header - std_msgs::msg::Header header; - header.frame_id = "map"; - header.stamp = planner_data.current_time; - - // create stop factor - StopFactor stop_factor; - stop_factor.stop_pose = stop_pose; - geometry_msgs::msg::Point stop_factor_point = stop_obstacle.collision_point; - stop_factor_point.z = stop_pose.position.z; - stop_factor.dist_to_stop_pose = autoware::motion_utils::calcSignedArcLength( - planner_data.traj_points, planner_data.ego_pose.position, stop_pose.position); - stop_factor.stop_factor_points.emplace_back(stop_factor_point); - - // create stop reason stamped - StopReason stop_reason_msg; - stop_reason_msg.reason = StopReason::OBSTACLE_STOP; - stop_reason_msg.stop_factors.emplace_back(stop_factor); - - // create stop reason array - StopReasonArray stop_reason_array; - stop_reason_array.header = header; - stop_reason_array.stop_reasons.emplace_back(stop_reason_msg); - return stop_reason_array; -} - -StopReasonArray makeEmptyStopReasonArray(const rclcpp::Time & current_time) -{ - // create header - std_msgs::msg::Header header; - header.frame_id = "map"; - header.stamp = current_time; - - // create stop reason stamped - StopReason stop_reason_msg; - stop_reason_msg.reason = StopReason::OBSTACLE_STOP; - - // create stop reason array - StopReasonArray stop_reason_array; - stop_reason_array.header = header; - stop_reason_array.stop_reasons.emplace_back(stop_reason_msg); - return stop_reason_array; -} - double calcMinimumDistanceToStop( const double initial_vel, const double max_acc, const double min_acc) { @@ -226,7 +178,6 @@ std::vector PlannerInterface::generateStopTrajectory( : std::abs(vehicle_info_.min_longitudinal_offset_m); if (stop_obstacles.empty()) { - stop_reasons_pub_->publish(makeEmptyStopReasonArray(planner_data.current_time)); velocity_factors_pub_->publish( obstacle_cruise_utils::makeVelocityFactorArray(planner_data.current_time)); // delete marker @@ -381,9 +332,6 @@ std::vector PlannerInterface::generateStopTrajectory( // Publish Stop Reason const auto stop_pose = output_traj_points.at(*zero_vel_idx).pose; - const auto stop_reasons_msg = - makeStopReasonArray(planner_data, stop_pose, *determined_stop_obstacle); - stop_reasons_pub_->publish(stop_reasons_msg); velocity_factors_pub_->publish(obstacle_cruise_utils::makeVelocityFactorArray( planner_data.current_time, PlanningBehavior::ROUTE_OBSTACLE, stop_pose)); // Store stop reason debug data