feat(behavior_velocity_stop_line): replace autoware_motion_utils to a…

…utoware_trajectory (autowarefoundation#9299)

* feat(behavior_velocity_stop_line): replace autoware_motion_utils to autoware_trajectory

Signed-off-by: Y.Hisaki <[email protected]>

* update

Signed-off-by: Y.Hisaki <[email protected]>

---------

Signed-off-by: Y.Hisaki <[email protected]>

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

@@ -44,6 +44,9 @@ class VelocityFactorInterface
     const std::vector<PointType> & points, const Pose & curr_pose, const Pose & stop_pose,
     const VelocityFactorStatus status, const std::string & detail = "");
 
+  void set(
+    const double & distance, const VelocityFactorStatus & status, const std::string & detail = "");
+
 private:
   VelocityFactorBehavior behavior_{VelocityFactor::UNKNOWN};
   VelocityFactor velocity_factor_{};

common/autoware_motion_utils/src/factor/velocity_factor_interface.cpp

@@ -36,6 +36,15 @@ void VelocityFactorInterface::set(
   velocity_factor_.detail = detail;
 }
 
+void VelocityFactorInterface::set(
+  const double & distance, const VelocityFactorStatus & status, const std::string & detail)
+{
+  velocity_factor_.behavior = behavior_;
+  velocity_factor_.distance = static_cast<float>(distance);
+  velocity_factor_.status = status;
+  velocity_factor_.detail = detail;
+}
+
 template void VelocityFactorInterface::set<tier4_planning_msgs::msg::PathPointWithLaneId>(
   const std::vector<tier4_planning_msgs::msg::PathPointWithLaneId> &, const Pose &, const Pose &,
   const VelocityFactorStatus, const std::string &);

common/autoware_trajectory/include/autoware/trajectory/detail/utils.hpp

@@ -36,7 +36,7 @@ namespace autoware::trajectory::detail
  */
 geometry_msgs::msg::Point to_point(const geometry_msgs::msg::Point & p);
 geometry_msgs::msg::Point to_point(const geometry_msgs::msg::Pose & p);
-geometry_msgs::msg::Point to_point(const Eigen::Ref<const Eigen::Vector2d> & p);
+geometry_msgs::msg::Point to_point(const Eigen::Vector2d & p);
 geometry_msgs::msg::Point to_point(const autoware_planning_msgs::msg::PathPoint & p);
 geometry_msgs::msg::Point to_point(const tier4_planning_msgs::msg::PathPointWithLaneId & p);
 geometry_msgs::msg::Point to_point(const lanelet::BasicPoint2d & p);

common/autoware_trajectory/src/detail/util.cpp

@@ -30,7 +30,7 @@ geometry_msgs::msg::Point to_point(const geometry_msgs::msg::Pose & p)
   return p.position;
 }
 
-geometry_msgs::msg::Point to_point(const Eigen::Ref<const Eigen::Vector2d> & p)
+geometry_msgs::msg::Point to_point(const Eigen::Vector2d & p)
 {
   geometry_msgs::msg::Point point;
   point.x = p(0);

planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/package.xml

@@ -23,6 +23,7 @@
   <depend>autoware_motion_utils</depend>
   <depend>autoware_planning_msgs</depend>
   <depend>autoware_route_handler</depend>
+  <depend>autoware_trajectory</depend>
   <depend>eigen</depend>
   <depend>geometry_msgs</depend>
   <depend>pluginlib</depend>

planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/debug.cpp

@@ -12,12 +12,12 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include "autoware/behavior_velocity_planner_common/utilization/util.hpp"
+#include "autoware/motion_utils/marker/virtual_wall_marker_creator.hpp"
+#include "autoware/universe_utils/geometry/geometry.hpp"
+#include "autoware/universe_utils/ros/marker_helper.hpp"
 #include "scene.hpp"
 
-#include <autoware/behavior_velocity_planner_common/utilization/util.hpp>
-#include <autoware/motion_utils/marker/virtual_wall_marker_creator.hpp>
-#include <autoware/universe_utils/geometry/geometry.hpp>
-#include <autoware/universe_utils/ros/marker_helper.hpp>
 #ifdef ROS_DISTRO_GALACTIC
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 #else

planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.cpp

@@ -15,6 +15,7 @@
 #include "manager.hpp"
 
 #include "autoware/universe_utils/ros/parameter.hpp"
+#include "autoware_lanelet2_extension/utility/query.hpp"
 
 #include <memory>
 #include <set>
@@ -57,7 +58,7 @@ std::vector<StopLineWithLaneId> StopLineModuleManager::getStopLinesWithLaneIdOnP
     }
 
     for (const auto & stop_line : traffic_sign_reg_elem->refLines()) {
-      stop_lines_with_lane_id.push_back(std::make_pair(stop_line, lane_id));
+      stop_lines_with_lane_id.emplace_back(stop_line, lane_id);
     }
   }
 

planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.hpp

@@ -15,11 +15,11 @@
 #ifndef MANAGER_HPP_
 #define MANAGER_HPP_
 
+#include "autoware/behavior_velocity_planner_common/plugin_interface.hpp"
+#include "autoware/behavior_velocity_planner_common/plugin_wrapper.hpp"
+#include "autoware/behavior_velocity_planner_common/scene_module_interface.hpp"
 #include "scene.hpp"
 
-#include <autoware/behavior_velocity_planner_common/plugin_interface.hpp>
-#include <autoware/behavior_velocity_planner_common/plugin_wrapper.hpp>
-#include <autoware/behavior_velocity_planner_common/scene_module_interface.hpp>
 #include <rclcpp/rclcpp.hpp>
 
 #include <tier4_planning_msgs/msg/path_with_lane_id.hpp>

planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp

@@ -14,96 +14,93 @@
 
 #include "scene.hpp"
 
-#include <autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp>
-#include <autoware/behavior_velocity_planner_common/utilization/util.hpp>
-#include <autoware/motion_utils/trajectory/trajectory.hpp>
-#include <autoware/universe_utils/system/time_keeper.hpp>
+#include "autoware/behavior_velocity_planner_common/utilization/util.hpp"
+#include "autoware/trajectory/path_point_with_lane_id.hpp"
 
-#include <algorithm>
-#include <vector>
+#include <tier4_planning_msgs/msg/detail/path_with_lane_id__struct.hpp>
 
 namespace autoware::behavior_velocity_planner
 {
-namespace bg = boost::geometry;
 
 StopLineModule::StopLineModule(
-  const int64_t module_id, const size_t lane_id, const lanelet::ConstLineString3d & stop_line,
-  const PlannerParam & planner_param, const rclcpp::Logger logger,
+  const int64_t module_id, const size_t lane_id, lanelet::ConstLineString3d stop_line,
+  const PlannerParam & planner_param, const rclcpp::Logger & logger,
   const rclcpp::Clock::SharedPtr clock)
 : SceneModuleInterface(module_id, logger, clock),
   lane_id_(lane_id),
-  stop_line_(stop_line),
+  stop_line_(std::move(stop_line)),
   state_(State::APPROACH),
+  planner_param_(planner_param),
   debug_data_()
 {
   velocity_factor_.init(PlanningBehavior::STOP_SIGN);
-  planner_param_ = planner_param;
 }
 
 bool StopLineModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason)
 {
-  universe_utils::ScopedTimeTrack st(
-    std::string(__func__) + " (lane_id:=" + std::to_string(module_id_) + ")", *getTimeKeeper());
+  auto trajectory =
+    trajectory::Trajectory<tier4_planning_msgs::msg::PathPointWithLaneId>::Builder{}.build(
+      path->points);
+
+  if (!trajectory) {
+    return true;
+  }
+
   debug_data_ = DebugData();
-  if (path->points.empty()) return true;
-  const auto base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m;
+  const double base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m;
   debug_data_.base_link2front = base_link2front;
-  first_stop_path_point_distance_ = autoware::motion_utils::calcArcLength(path->points);
+  first_stop_path_point_distance_ = trajectory->length();
   *stop_reason = planning_utils::initializeStopReason(StopReason::STOP_LINE);
 
   const LineString2d stop_line = planning_utils::extendLine(
     stop_line_[0], stop_line_[1], planner_data_->stop_line_extend_length);
 
-  time_keeper_->start_track("createTargetPoint");
-  // Calculate stop pose and insert index
-  const auto stop_point = arc_lane_utils::createTargetPoint(
-    *path, stop_line, planner_param_.stop_margin,
-    planner_data_->vehicle_info_.max_longitudinal_offset_m);
-  time_keeper_->end_track("createTargetPoint");
+  // Calculate intersection with stop line
+  const auto trajectory_stop_line_intersection =
+    trajectory->crossed(stop_line.front(), stop_line.back());
+
   // If no collision found, do nothing
-  if (!stop_point) {
+  if (!trajectory_stop_line_intersection) {
     RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 5000 /* ms */, "is no collision");
     return true;
   }
 
-  const auto stop_point_idx = stop_point->first;
-  auto stop_pose = stop_point->second;
+  const double stop_point_s =
+    *trajectory_stop_line_intersection -
+    (base_link2front + planner_param_.stop_margin);  // consider vehicle length and stop margin
+
+  if (stop_point_s < 0.0) {
+    return true;
+  }
+
+  const auto stop_pose = trajectory->compute(stop_point_s);
 
   /**
    * @brief : calculate signed arc length consider stop margin from stop line
    *
    * |----------------------------|
    * s---ego----------x--|--------g
    */
-  time_keeper_->start_track(
-    "calcSegmentIndexFromPointIndex & findEgoSegmentIndex & calcSignedArcLength");
-  const size_t stop_line_seg_idx = planning_utils::calcSegmentIndexFromPointIndex(
-    path->points, stop_pose.position, stop_point_idx);
-  const size_t current_seg_idx = findEgoSegmentIndex(path->points);
-  const double signed_arc_dist_to_stop_point = autoware::motion_utils::calcSignedArcLength(
-    path->points, planner_data_->current_odometry->pose.position, current_seg_idx,
-    stop_pose.position, stop_line_seg_idx);
-  time_keeper_->end_track(
-    "calcSegmentIndexFromPointIndex & findEgoSegmentIndex & calcSignedArcLength");
+  const double ego_on_trajectory_s =
+    trajectory->closest(planner_data_->current_odometry->pose.position);
+  const double signed_arc_dist_to_stop_point = stop_point_s - ego_on_trajectory_s;
+
   switch (state_) {
     case State::APPROACH: {
       // Insert stop pose
-      planning_utils::insertStopPoint(stop_pose.position, stop_line_seg_idx, *path);
+      trajectory->longitudinal_velocity_mps.range(stop_point_s, trajectory->length()).set(0.0);
 
-      // Update first stop index
-      first_stop_path_point_distance_ = autoware::motion_utils::calcSignedArcLength(
-        path->points, 0, stop_pose.position, stop_line_seg_idx);
-      debug_data_.stop_pose = stop_pose;
+      // Update first stop path point distance
+      first_stop_path_point_distance_ = stop_point_s;
+      debug_data_.stop_pose = stop_pose.point.pose;
 
       // Get stop point and stop factor
       {
         tier4_planning_msgs::msg::StopFactor stop_factor;
-        stop_factor.stop_pose = stop_pose;
+        stop_factor.stop_pose = stop_pose.point.pose;
         stop_factor.stop_factor_points.push_back(getCenterOfStopLine(stop_line_));
         planning_utils::appendStopReason(stop_factor, stop_reason);
-        velocity_factor_.set(
-          path->points, planner_data_->current_odometry->pose, stop_pose,
-          VelocityFactor::APPROACHING);
+        velocity_factor_.set(signed_arc_dist_to_stop_point, VelocityFactor::APPROACHING);
       }
 
       // Move to stopped state if stopped
@@ -125,34 +122,22 @@ bool StopLineModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop
     }
 
     case State::STOPPED: {
-      // Change state after vehicle departure
-      const auto stopped_pose = autoware::motion_utils::calcLongitudinalOffsetPose(
-        path->points, planner_data_->current_odometry->pose.position, 0.0);
-
-      if (!stopped_pose) {
-        break;
-      }
-
-      SegmentIndexWithPose ego_pos_on_path;
-      ego_pos_on_path.pose = stopped_pose.value();
-      ego_pos_on_path.index = findEgoSegmentIndex(path->points);
-
       // Insert stop pose
-      planning_utils::insertStopPoint(ego_pos_on_path.pose.position, ego_pos_on_path.index, *path);
-
-      debug_data_.stop_pose = stop_pose;
+      trajectory->longitudinal_velocity_mps.range(ego_on_trajectory_s, trajectory->length())
+        .set(0.0);
+      const auto ego_pos_on_path = trajectory->compute(ego_on_trajectory_s).point.pose;
+      debug_data_.stop_pose = ego_pos_on_path;
 
       // Get stop point and stop factor
       {
         tier4_planning_msgs::msg::StopFactor stop_factor;
-        stop_factor.stop_pose = ego_pos_on_path.pose;
+        stop_factor.stop_pose = ego_pos_on_path;
         stop_factor.stop_factor_points.push_back(getCenterOfStopLine(stop_line_));
         planning_utils::appendStopReason(stop_factor, stop_reason);
-        velocity_factor_.set(
-          path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::STOPPED);
+        velocity_factor_.set(signed_arc_dist_to_stop_point, VelocityFactor::STOPPED);
       }
 
-      const auto elapsed_time = (clock_->now() - *stopped_time_).seconds();
+      const double elapsed_time = (clock_->now() - *stopped_time_).seconds();
 
       if (planner_param_.stop_duration_sec < elapsed_time) {
         RCLCPP_INFO(logger_, "STOPPED -> START");
@@ -175,6 +160,8 @@ bool StopLineModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop
     }
   }
 
+  path->points = trajectory->restore();
+
   return true;
 }
 

planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp

@@ -15,23 +15,20 @@
 #ifndef SCENE_HPP_
 #define SCENE_HPP_
 
+#include "autoware/behavior_velocity_planner_common/scene_module_interface.hpp"
+#include "autoware/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp"
+#include "autoware/behavior_velocity_planner_common/utilization/util.hpp"
+
 #include <memory>
-#include <string>
-#include <unordered_map>
 #include <utility>
 #include <vector>
 
 #define EIGEN_MPL2_ONLY
 #include <Eigen/Core>
 #include <Eigen/Geometry>
-#include <autoware/behavior_velocity_planner_common/scene_module_interface.hpp>
-#include <autoware/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp>
-#include <autoware/behavior_velocity_planner_common/utilization/util.hpp>
-#include <autoware_lanelet2_extension/utility/query.hpp>
 #include <rclcpp/rclcpp.hpp>
 
 #include <lanelet2_core/LaneletMap.h>
-#include <lanelet2_routing/RoutingGraph.h>
 
 namespace autoware::behavior_velocity_planner
 {
@@ -42,24 +39,6 @@ class StopLineModule : public SceneModuleInterface
 public:
   enum class State { APPROACH, STOPPED, START };
 
-  struct SegmentIndexWithPose
-  {
-    size_t index;
-    geometry_msgs::msg::Pose pose;
-  };
-
-  struct SegmentIndexWithPoint2d
-  {
-    size_t index;
-    Point2d point;
-  };
-
-  struct SegmentIndexWithOffset
-  {
-    size_t index;
-    double offset;
-  };
-
   struct DebugData
   {
     double base_link2front;
@@ -77,10 +56,9 @@ class StopLineModule : public SceneModuleInterface
     bool show_stop_line_collision_check;
   };
 
-public:
   StopLineModule(
-    const int64_t module_id, const size_t lane_id, const lanelet::ConstLineString3d & stop_line,
-    const PlannerParam & planner_param, const rclcpp::Logger logger,
+    const int64_t module_id, const size_t lane_id, lanelet::ConstLineString3d stop_line,
+    const PlannerParam & planner_param, const rclcpp::Logger & logger,
     const rclcpp::Clock::SharedPtr clock);
 
   bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override;