fix(motion_velocity_planner)!: remove velocity factor (#1738)

* fix(dynamic_obstacle_stop): remove velocity factor Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * fix(obstacle_velocity_limiter): remove velocity factor Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * fix(out_of_lane): remove velocity factor Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * fix(motion_velocity_planner): remove velocity factor Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> --------- Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
tier4 · Dec 26, 2024 · 3ae5f63 · 3ae5f63
1 parent 55ffc57
commit 3ae5f63
Show file tree

Hide file tree

Showing 8 changed files with 4 additions and 41 deletions.
diff --git a/...utoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp b/...utoware_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<autoware::motion_utils::PlanningFactorInterface>(
     &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/...motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp b/...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<visualization_msgs::msg::MarkerArray>("~/" + ns_ + "/debug_markers", 1);

diff --git a/...n_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp b/...n_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<autoware::motion_utils::PlanningFactorInterface>(&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/...lanner_common/include/autoware/motion_velocity_planner_common/plugin_module_interface.hpp b/...lanner_common/include/autoware/motion_velocity_planner_common/plugin_module_interface.hpp
@@ -19,7 +19,6 @@
 #include "velocity_planning_result.hpp"
 
 #include <autoware/motion_utils/factor/planning_factor_interface.hpp>
-#include <autoware/motion_utils/factor/velocity_factor_interface.hpp>
 #include <autoware/universe_utils/ros/processing_time_publisher.hpp>
 #include <rclcpp/rclcpp.hpp>
 
@@ -47,7 +46,6 @@ class PluginModuleInterface
     const std::shared_ptr<const PlannerData> 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<visualization_msgs::msg::MarkerArray>::SharedPtr debug_publisher_;
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr virtual_wall_publisher_;

diff --git a/...anner_common/include/autoware/motion_velocity_planner_common/velocity_planning_result.hpp b/...anner_common/include/autoware/motion_velocity_planner_common/velocity_planning_result.hpp
@@ -17,7 +17,6 @@
 
 #include <autoware/motion_utils/marker/virtual_wall_marker_creator.hpp>
 
-#include <autoware_adapi_v1_msgs/msg/velocity_factor.hpp>
 #include <geometry_msgs/msg/point.hpp>
 #include <visualization_msgs/msg/marker_array.hpp>
 
@@ -43,7 +42,6 @@ struct VelocityPlanningResult
 {
   std::vector<geometry_msgs::msg::Point> stop_points{};
   std::vector<SlowdownInterval> slowdown_intervals{};
-  std::optional<autoware_adapi_v1_msgs::msg::VelocityFactor> 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
@@ -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/<MODULE_NAME>` | 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
@@ -82,9 +82,6 @@ MotionVelocityPlannerNode::MotionVelocityPlannerNode(const rclcpp::NodeOptions &
   // Publishers
   trajectory_pub_ =
     this->create_publisher<autoware_planning_msgs::msg::Trajectory>("~/output/trajectory", 1);
-  velocity_factor_publisher_ =
-    this->create_publisher<autoware_adapi_v1_msgs::msg::VelocityFactorArray>(
-      "~/output/velocity_factors", 1);
   processing_time_publisher_ =
     this->create_publisher<tier4_debug_msgs::msg::Float64Stamped>("~/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<const PlannerData>(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
@@ -93,8 +93,6 @@ class MotionVelocityPlannerNode : public rclcpp::Node
   // publishers
   rclcpp::Publisher<autoware_planning_msgs::msg::Trajectory>::SharedPtr trajectory_pub_;
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr debug_viz_pub_;
-  rclcpp::Publisher<autoware_adapi_v1_msgs::msg::VelocityFactorArray>::SharedPtr
-    velocity_factor_publisher_;
   autoware::universe_utils::ProcessingTimePublisher processing_diag_publisher_{
     this, "~/debug/processing_time_ms_diag"};
   rclcpp::Publisher<tier4_debug_msgs::msg::Float64Stamped>::SharedPtr processing_time_publisher_;