cleanup velocity factors, launch param, and remove .pages

Signed-off-by: Maxime CLEMENT <[email protected]>
tier4 · Jun 18, 2024 · b469d3f · b469d3f
1 parent d31cad4
commit b469d3f
Show file tree

Hide file tree

Showing 7 changed files with 9 additions and 199 deletions.
diff --git a/common/motion_utils/include/motion_utils/factor/velocity_factor_interface.hpp b/common/motion_utils/include/motion_utils/factor/velocity_factor_interface.hpp
diff --git a/common/motion_utils/src/factor/velocity_factor_interface.cpp b/common/motion_utils/src/factor/velocity_factor_interface.cpp
diff --git a/...ng_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py b/...ng_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py
@@ -154,7 +154,7 @@ def launch_setup(context, *args, **kwargs):
     ) as f:
         motion_velocity_smoother_param = yaml.safe_load(f)["/**"]["ros__parameters"]
     with open(
-        LaunchConfiguration("behavior_velocity_smoother_type_param_path").perform(context), "r"
+        LaunchConfiguration("motion_velocity_smoother_type_param_path").perform(context), "r"
     ) as f:
         behavior_velocity_smoother_type_param = yaml.safe_load(f)["/**"]["ros__parameters"]
     with open(

diff --git a/planning/.pages b/planning/.pages
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
@@ -50,7 +50,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::VelocityFactor::ROUTE_OBSTACLE);
 
   debug_publisher_ =
     node.create_publisher<visualization_msgs::msg::MarkerArray>("~/" + ns_ + "/debug_markers", 1);
@@ -265,11 +264,13 @@ VelocityPlanningResult OutOfLaneModule::plan(
                                   ego_trajectory_points, ego_data.pose.position,
                                   point_to_insert->point.pose.position) > 0.1 &&
                                 ego_data.velocity > 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, point_to_insert->point.pose, status, "out_of_lane");
-    // result.velocity_factor = velocity_factor_interface_.get();
+    const auto status =
+      is_approaching ? result.velocity_factor->APPROACHING : result.velocity_factor->STOPPED;
+    result.velocity_factor.emplace();
+    result.velocity_factor->pose = point_to_insert->point.pose;
+    result.velocity_factor->status = status;
+    result.velocity_factor->type = result.velocity_factor->ROUTE_OBSTACLE;
+    result.velocity_factor->detail = "out_of_lane";
   } else if (!decisions.empty()) {
     RCLCPP_WARN(logger_, "Could not insert stop point (would violate max deceleration limits)");
   }

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
@@ -18,7 +18,6 @@
 #include "planner_data.hpp"
 #include "velocity_planning_result.hpp"
 
-#include <motion_utils/factor/velocity_factor_interface.hpp>
 #include <rclcpp/rclcpp.hpp>
 
 #include <autoware_auto_planning_msgs/msg/trajectory_point.hpp>
@@ -40,7 +39,6 @@ class PluginModuleInterface
     const std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> & ego_trajectory_points,
     const std::shared_ptr<const PlannerData> planner_data) = 0;
   virtual std::string get_module_name() const = 0;
-  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/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
@@ -23,6 +23,7 @@
 #include <rclcpp/rclcpp.hpp>
 #include <tier4_autoware_utils/ros/logger_level_configure.hpp>
 
+#include <autoware_adapi_v1_msgs/msg/velocity_factor_array.hpp>
 #include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
 #include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
 #include <autoware_auto_planning_msgs/msg/trajectory.hpp>