improve velocity factors

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

Author: maxime-clem

planning/motion_velocity_planner/motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp

@@ -56,9 +56,6 @@ void OutOfLaneModule::init(rclcpp::Node & node, const std::string & module_name)
     node.create_publisher<visualization_msgs::msg::MarkerArray>("~/" + ns_ + "/debug_markers", 1);
   virtual_wall_publisher_ =
     node.create_publisher<visualization_msgs::msg::MarkerArray>("~/" + ns_ + "/virtual_walls", 1);
-  velocity_factor_publisher_ =
-    node.create_publisher<autoware_adapi_v1_msgs::msg::VelocityFactorArray>(
-      std::string("/planning/velocity_factors/") + ns_, 1);
 }
 void OutOfLaneModule::init_parameters(rclcpp::Node & node)
 {
@@ -196,7 +193,7 @@ VelocityPlanningResult OutOfLaneModule::plan(
     if (overlapped_lanelet_it != other_lanelets.end()) {
       debug_data_.current_overlapped_lanelets.push_back(*overlapped_lanelet_it);
       RCLCPP_DEBUG(logger_, "Ego is already overlapping a lane, skipping the module\n");
-      return result;  // TODO(Maxime): properly populate the result
+      return result;
     }
   }
   // Calculate overlapping ranges
@@ -296,7 +293,7 @@ VelocityPlanningResult OutOfLaneModule::plan(
   virtual_wall_marker_creator.add_virtual_walls(
     out_of_lane::debug::create_virtual_walls(debug_data_, params_));
   virtual_wall_publisher_->publish(virtual_wall_marker_creator.create_markers(clock_->now()));
-  velocity_factor_publisher_->publish(velocity_factor_interface_.get_array(clock_->now()));
+  result.velocity_factor = velocity_factor_interface_.get();
   return result;
 }
 

planning/motion_velocity_planner/motion_velocity_planner_common/include/motion_velocity_planner_common/plugin_module_interface.hpp

@@ -44,8 +44,6 @@ class PluginModuleInterface
   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_;
-  rclcpp::Publisher<autoware_adapi_v1_msgs::msg::VelocityFactorArray>::SharedPtr
-    velocity_factor_publisher_;
   motion_utils::VirtualWallMarkerCreator virtual_wall_marker_creator{};
 };
 

planning/motion_velocity_planner/motion_velocity_planner_common/include/motion_velocity_planner_common/velocity_planning_result.hpp

@@ -17,6 +17,7 @@
 
 #include <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>
 
@@ -40,6 +41,7 @@ struct VelocityPlanningResult
 {
   std::vector<geometry_msgs::msg::Point> stop_points{};
   std::vector<SlowdownInterval> slowdown_intervals{};
+  autoware_adapi_v1_msgs::msg::VelocityFactor velocity_factor{};
 };
 }  // namespace motion_velocity_planner
 

planning/motion_velocity_planner/motion_velocity_planner_node/src/node.cpp

@@ -152,6 +152,9 @@ MotionVelocityPlannerNode::MotionVelocityPlannerNode(const rclcpp::NodeOptions &
   // Publishers
   trajectory_pub_ =
     this->create_publisher<autoware_auto_planning_msgs::msg::Trajectory>("~/output/trajectory", 1);
+  velocity_factor_publisher_ =
+    this->create_publisher<autoware_adapi_v1_msgs::msg::VelocityFactorArray>(
+      "~/output/velocity_factors", 1);
 
   // Parameters
   planner_data_.stop_line_extend_length = declare_parameter<double>("stop_line_extend_length");
@@ -402,6 +405,10 @@ autoware_auto_planning_msgs::msg::Trajectory MotionVelocityPlannerNode::generate
     smoothed_trajectory_points, std::make_shared<const PlannerData>(planner_data_));
 
   autoware_auto_planning_msgs::msg::Trajectory output_trajectory_msg = input_trajectory_msg;
+  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) {
       const auto seg_idx =
@@ -426,8 +433,10 @@ autoware_auto_planning_msgs::msg::Trajectory MotionVelocityPlannerNode::generate
         RCLCPP_WARN(get_logger(), "Failed to insert slowdown points");
       }
     }
+    velocity_factors.factors.push_back(planning_result.velocity_factor);
   }
 
+  velocity_factor_publisher_->publish(velocity_factors);
   return output_trajectory_msg;
 }
 

planning/motion_velocity_planner/motion_velocity_planner_node/src/node.hpp

@@ -90,6 +90,8 @@ class MotionVelocityPlannerNode : public rclcpp::Node
   // publishers
   rclcpp::Publisher<autoware_auto_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_;
 
   //  parameters
   rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr set_param_callback_;