From 3ae5f63dd0b6a12610ae5f84c8214dc6738f1d76 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 26 Dec 2024 09:52:34 +0900 Subject: [PATCH] fix(motion_velocity_planner)!: remove velocity factor (#1738) * fix(dynamic_obstacle_stop): remove velocity factor Signed-off-by: satoshi-ota * fix(obstacle_velocity_limiter): remove velocity factor Signed-off-by: satoshi-ota * fix(out_of_lane): remove velocity factor Signed-off-by: satoshi-ota * fix(motion_velocity_planner): remove velocity factor Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../src/dynamic_obstacle_stop_module.cpp | 10 ---------- .../src/obstacle_velocity_limiter_module.cpp | 1 - .../src/out_of_lane_module.cpp | 10 ---------- .../plugin_module_interface.hpp | 2 -- .../velocity_planning_result.hpp | 2 -- .../autoware_motion_velocity_planner_node/README.md | 8 ++++---- .../autoware_motion_velocity_planner_node/src/node.cpp | 10 ---------- .../autoware_motion_velocity_planner_node/src/node.hpp | 2 -- 8 files changed, 4 insertions(+), 41 deletions(-) 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 bb2107e2a3117..724de4f0faf27 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,6 @@ 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"); @@ -162,18 +161,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; planning_factor_interface_->add( ego_trajectory_points, ego_data.pose, *stop_pose, PlanningFactor::STOP, SafetyFactorArray{}); - 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(); create_virtual_walls(); } } 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 3fa3ec7cbf782..045ddb7447e9a 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 76ade4f81aada..c4d3b3ac99adf 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,6 @@ 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"); @@ -312,18 +311,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; planning_factor_interface_->add( ego_trajectory_points, ego_data.pose, *slowdown_pose, PlanningFactor::SLOW_DOWN, SafetyFactorArray{}); - velocity_factor_interface_.set( - ego_trajectory_points, ego_data.pose, *slowdown_pose, status, "out_of_lane"); - result.velocity_factor = velocity_factor_interface_.get(); 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_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 4c923afe03f65..c2cfd105e2833 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/plugin_module_interface.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/plugin_module_interface.hpp @@ -19,7 +19,6 @@ #include "velocity_planning_result.hpp" #include -#include #include #include @@ -47,7 +46,6 @@ class PluginModuleInterface const std::shared_ptr planner_data) = 0; virtual std::string get_module_name() const = 0; virtual void publish_planning_factor() {} - autoware::motion_utils::VelocityFactorInterface velocity_factor_interface_; rclcpp::Logger logger_ = rclcpp::get_logger(""); rclcpp::Publisher::SharedPtr debug_publisher_; rclcpp::Publisher::SharedPtr virtual_wall_publisher_; 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 586d27f2c9614..c2222846cc264 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); debug_viz_pub_ = @@ -425,20 +422,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 6e33b89e026c3..80b995318f720 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 processing_time_publisher_;