implement velocity factor

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

Author: maxime-clem

common/motion_utils/include/motion_utils/factor/velocity_factor_interface.hpp

@@ -16,10 +16,11 @@
 #ifndef MOTION_UTILS__FACTOR__VELOCITY_FACTOR_INTERFACE_HPP_
 #define MOTION_UTILS__FACTOR__VELOCITY_FACTOR_INTERFACE_HPP_
 
+#include <rclcpp/time.hpp>
+
 #include <autoware_adapi_v1_msgs/msg/planning_behavior.hpp>
 #include <autoware_adapi_v1_msgs/msg/velocity_factor.hpp>
 #include <autoware_adapi_v1_msgs/msg/velocity_factor_array.hpp>
-#include <autoware_auto_planning_msgs/msg/path_point_with_lane_id.hpp>
 #include <geometry_msgs/msg/pose.hpp>
 
 #include <string>
@@ -37,13 +38,21 @@ class VelocityFactorInterface
 {
 public:
   [[nodiscard]] VelocityFactor get() const { return velocity_factor_; }
+  [[nodiscard]] autoware_adapi_v1_msgs::msg::VelocityFactorArray get_array(
+    const rclcpp::Time & now) const
+  {
+    autoware_adapi_v1_msgs::msg::VelocityFactorArray array;
+    array.header.stamp = now;
+    array.factors.push_back(velocity_factor_);
+    return array;
+  }
   void init(const VelocityFactorBehavior & behavior) { behavior_ = behavior; }
   void reset() { velocity_factor_.behavior = PlanningBehavior::UNKNOWN; }
 
+  template <class PointType>
   void set(
-    const std::vector<autoware_auto_planning_msgs::msg::PathPointWithLaneId> & points,
-    const Pose & curr_pose, const Pose & stop_pose, const VelocityFactorStatus status,
-    const std::string & detail = "");
+    const std::vector<PointType> & points, const Pose & curr_pose, const Pose & stop_pose,
+    const VelocityFactorStatus status, const std::string & detail = "");
 
 private:
   VelocityFactorBehavior behavior_{VelocityFactor::UNKNOWN};

common/motion_utils/src/factor/velocity_factor_interface.cpp

@@ -15,12 +15,16 @@
 #include <motion_utils/factor/velocity_factor_interface.hpp>
 #include <motion_utils/trajectory/trajectory.hpp>
 
+#include <autoware_auto_planning_msgs/msg/path_point.hpp>
+#include <autoware_auto_planning_msgs/msg/path_point_with_lane_id.hpp>
+#include <autoware_auto_planning_msgs/msg/trajectory_point.hpp>
+
 namespace motion_utils
 {
+template <class PointType>
 void VelocityFactorInterface::set(
-  const std::vector<autoware_auto_planning_msgs::msg::PathPointWithLaneId> & points,
-  const Pose & curr_pose, const Pose & stop_pose, const VelocityFactorStatus status,
-  const std::string & detail)
+  const std::vector<PointType> & points, const Pose & curr_pose, const Pose & stop_pose,
+  const VelocityFactorStatus status, const std::string & detail)
 {
   const auto & curr_point = curr_pose.position;
   const auto & stop_point = stop_pose.position;
@@ -32,4 +36,14 @@ void VelocityFactorInterface::set(
   velocity_factor_.detail = detail;
 }
 
+template void VelocityFactorInterface::set<autoware_auto_planning_msgs::msg::PathPointWithLaneId>(
+  const std::vector<autoware_auto_planning_msgs::msg::PathPointWithLaneId> &, const Pose &,
+  const Pose &, const VelocityFactorStatus, const std::string &);
+template void VelocityFactorInterface::set<autoware_auto_planning_msgs::msg::PathPoint>(
+  const std::vector<autoware_auto_planning_msgs::msg::PathPoint> &, const Pose &, const Pose &,
+  const VelocityFactorStatus, const std::string &);
+template void VelocityFactorInterface::set<autoware_auto_planning_msgs::msg::TrajectoryPoint>(
+  const std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> &, const Pose &,
+  const Pose &, const VelocityFactorStatus, const std::string &);
+
 }  // namespace motion_utils

planning/motion_velocity_planner/motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp

@@ -50,10 +50,15 @@ void OutOfLaneModule::init(rclcpp::Node & node, const std::string & module_name)
   logger_ = node.get_logger();
   clock_ = node.get_clock();
   init_parameters(node);
-  debug_publisher =
+  velocity_factor_interface_.init(motion_utils::PlanningBehavior::ROUTE_OBSTACLE);
+
+  debug_publisher_ =
     node.create_publisher<visualization_msgs::msg::MarkerArray>("~/" + ns_ + "/debug_markers", 1);
-  virtual_wall_publisher =
+  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)
 {
@@ -258,6 +263,15 @@ VelocityPlanningResult OutOfLaneModule::plan(
       result.slowdown_intervals.emplace_back(
         point_to_insert->point.pose.position, point_to_insert->point.pose.position,
         point_to_insert->slowdown.velocity);
+
+    const auto is_approaching = motion_utils::calcSignedArcLength(
+                                  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");
   } else if (!decisions.empty()) {
     RCLCPP_WARN(logger_, "Could not insert stop point (would violate max deceleration limits)");
   }
@@ -278,10 +292,11 @@ VelocityPlanningResult OutOfLaneModule::plan(
     total_time_us, calculate_lanelets_us, calculate_trajectory_footprints_us,
     calculate_overlapping_ranges_us, filter_predicted_objects_ms, calculate_decisions_us,
     calc_slowdown_points_us, insert_slowdown_points_us);
-  debug_publisher->publish(out_of_lane::debug::create_debug_marker_array(debug_data_));
+  debug_publisher_->publish(out_of_lane::debug::create_debug_marker_array(debug_data_));
   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()));
+  virtual_wall_publisher_->publish(virtual_wall_marker_creator.create_markers(clock_->now()));
+  velocity_factor_publisher_->publish(velocity_factor_interface_.get_array(clock_->now()));
   return result;
 }
 

planning/motion_velocity_planner/motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp

@@ -52,11 +52,7 @@ class OutOfLaneModule : public PluginModuleInterface
   std::string module_name_;
   std::optional<out_of_lane::SlowdownToInsert> prev_inserted_point_{};
   rclcpp::Clock::SharedPtr clock_{};
-  rclcpp::Logger logger_ = rclcpp::get_logger("");
   rclcpp::Time prev_inserted_point_time_{};
-  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr debug_publisher;
-  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr virtual_wall_publisher;
-  motion_utils::VirtualWallMarkerCreator virtual_wall_marker_creator{};
 
 protected:
   // Debug

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

@@ -18,6 +18,7 @@
 #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>
@@ -39,6 +40,13 @@ 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_;
+  rclcpp::Publisher<autoware_adapi_v1_msgs::msg::VelocityFactorArray>::SharedPtr
+    velocity_factor_publisher_;
+  motion_utils::VirtualWallMarkerCreator virtual_wall_marker_creator{};
 };
 
 }  // namespace motion_velocity_planner

planning/motion_velocity_planner/motion_velocity_planner_common/package.xml

@@ -29,7 +29,6 @@
   <depend>tier4_autoware_utils</depend>
   <depend>tier4_planning_msgs</depend>
   <depend>visualization_msgs</depend>
-  <!-- TODO(Maxime): remove; only needed for VelocityFactorInterface -->
 
   <test_depend>ament_cmake_ros</test_depend>
   <test_depend>ament_lint_auto</test_depend>