feat(behavior_path_planner): cog centered path shape planning (autowa…

…refoundation#2835)

* feat(behavior_path_planner): cog centered planning

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* use ros parameter

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* fix

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* add test

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* Update common/motion_utils/include/motion_utils/trajectory/path_with_lane_id.hpp

Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* pre-commit

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

---------

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com>

common/motion_utils/CMakeLists.txt

@@ -12,6 +12,7 @@ ament_auto_add_library(motion_utils SHARED
   src/marker/marker_helper.cpp
   src/resample/resample.cpp
   src/trajectory/interpolation.cpp
+  src/trajectory/path_with_lane_id.cpp
   src/vehicle/vehicle_state_checker.cpp
 )
 

common/motion_utils/include/motion_utils/trajectory/path_with_lane_id.hpp

@@ -99,6 +99,15 @@ inline size_t findNearestSegmentIndexFromLaneId(
 
   return nearest_idx;
 }
+
+// @brief Calculates the path to be followed by the rear wheel center in order to make the vehicle
+// center follow the input path
+// @param [in] path with position to be followed by the center of the vehicle
+// @param [out] PathWithLaneId to be followed by the rear wheel center follow to make the vehicle
+// center follow the input path NOTE: rear_to_cog is supposed to be positive
+autoware_auto_planning_msgs::msg::PathWithLaneId convertToRearWheelCenter(
+  const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const double rear_to_cog,
+  const bool enable_last_point_compensation = true);
 }  // namespace motion_utils
 
 #endif  // MOTION_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_

common/motion_utils/include/motion_utils/trajectory/trajectory.hpp

@@ -15,6 +15,7 @@
 #ifndef MOTION_UTILS__TRAJECTORY__TRAJECTORY_HPP_
 #define MOTION_UTILS__TRAJECTORY__TRAJECTORY_HPP_
 
+#include "interpolation/spline_interpolation_points_2d.hpp"
 #include "tier4_autoware_utils/geometry/geometry.hpp"
 #include "tier4_autoware_utils/geometry/pose_deviation.hpp"
 #include "tier4_autoware_utils/math/constants.hpp"

common/motion_utils/src/trajectory/path_with_lane_id.cpp

@@ -0,0 +1,57 @@
+// Copyright 2023 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "motion_utils/trajectory/path_with_lane_id.hpp"
+
+namespace motion_utils
+{
+// NOTE: rear_to_cog is supposed to be positive
+autoware_auto_planning_msgs::msg::PathWithLaneId convertToRearWheelCenter(
+  const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const double rear_to_cog,
+  const bool enable_last_point_compensation)
+{
+  auto cog_path = path;
+
+  // calculate curvature and yaw from spline interpolation
+  const auto spline = SplineInterpolationPoints2d(path.points);
+  const auto curvature_vec = spline.getSplineInterpolatedCurvatures();
+  const auto yaw_vec = spline.getSplineInterpolatedYaws();
+
+  for (size_t i = 0; i < path.points.size(); ++i) {
+    // calculate beta, which is CoG's velocity direction
+    const double beta = std::atan(rear_to_cog * curvature_vec.at(i));
+
+    // apply beta to CoG pose
+    geometry_msgs::msg::Pose cog_pose_with_beta;
+    cog_pose_with_beta.position = tier4_autoware_utils::getPoint(path.points.at(i));
+    cog_pose_with_beta.orientation =
+      tier4_autoware_utils::createQuaternionFromYaw(yaw_vec.at(i) - beta);
+
+    const auto rear_pose =
+      tier4_autoware_utils::calcOffsetPose(cog_pose_with_beta, -rear_to_cog, 0.0, 0.0);
+
+    // update pose
+    tier4_autoware_utils::setPose(rear_pose, cog_path.points.at(i));
+  }
+
+  // compensate for the last pose
+  if (enable_last_point_compensation) {
+    cog_path.points.back() = path.points.back();
+  }
+
+  insertOrientation(cog_path.points, true);
+
+  return cog_path;
+}
+}  // namespace motion_utils

common/motion_utils/test/src/trajectory/test_path_with_lane_id.cpp

@@ -161,3 +161,31 @@ TEST(path_with_lane_id, findNearestIndexFromLaneId)
     findNearestSegmentIndexFromLaneId(PathWithLaneId{}, createPoint(2.4, 1.3, 0.0), 100),
     std::invalid_argument);
 }
+
+// NOTE: This test is temporary for the current implementation.
+TEST(path_with_lane_id, convertToRearWheelCenter)
+{
+  using motion_utils::convertToRearWheelCenter;
+
+  PathWithLaneId path;
+
+  PathPointWithLaneId p1;
+  p1.point.pose = createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
+  path.points.push_back(p1);
+  PathPointWithLaneId p2;
+  p2.point.pose = createPose(1.0, 2.0, 0.0, 0.0, 0.0, 0.0);
+  path.points.push_back(p2);
+  PathPointWithLaneId p3;
+  p3.point.pose = createPose(2.0, 4.0, 0.0, 0.0, 0.0, 0.0);
+  path.points.push_back(p3);
+
+  const auto cog_path = convertToRearWheelCenter(path, 1.0);
+
+  constexpr double epsilon = 1e-6;
+  EXPECT_NEAR(cog_path.points.at(0).point.pose.position.x, -0.4472136, epsilon);
+  EXPECT_NEAR(cog_path.points.at(0).point.pose.position.y, -0.8944272, epsilon);
+  EXPECT_NEAR(cog_path.points.at(1).point.pose.position.x, 0.5527864, epsilon);
+  EXPECT_NEAR(cog_path.points.at(1).point.pose.position.y, 1.1055728, epsilon);
+  EXPECT_NEAR(cog_path.points.at(2).point.pose.position.x, 2.0, epsilon);
+  EXPECT_NEAR(cog_path.points.at(2).point.pose.position.y, 4.0, epsilon);
+}

planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp

@@ -62,6 +62,7 @@ struct BehaviorPathPlannerParameters
   bool turn_signal_on_swerving;
 
   double enable_akima_spline_first;
+  double enable_cog_on_centerline;
   double input_path_interval;
   double output_path_interval;
 

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -482,6 +482,7 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam()
   p.turn_signal_on_swerving = declare_parameter<bool>("turn_signal_on_swerving");
 
   p.enable_akima_spline_first = declare_parameter<bool>("enable_akima_spline_first");
+  p.enable_cog_on_centerline = declare_parameter<bool>("enable_cog_on_centerline");
   p.input_path_interval = declare_parameter<double>("input_path_interval");
   p.output_path_interval = declare_parameter<double>("output_path_interval");
   p.visualize_maximum_drivable_area = declare_parameter<bool>("visualize_maximum_drivable_area");

planning/behavior_path_planner/src/utilities.cpp

@@ -15,6 +15,7 @@
 #include "behavior_path_planner/utilities.hpp"
 
 #include "behavior_path_planner/util/drivable_area_expansion/drivable_area_expansion.hpp"
+#include "motion_utils/trajectory/path_with_lane_id.hpp"
 
 #include <lanelet2_extension/utility/message_conversion.hpp>
 #include <lanelet2_extension/utility/query.hpp>
@@ -1706,6 +1707,13 @@ PathWithLaneId getCenterLinePath(
     route_handler.getCenterLinePath(lanelet_sequence, s_backward, s_forward, true);
   const auto resampled_path_with_lane_id = motion_utils::resamplePath(
     raw_path_with_lane_id, parameter.input_path_interval, parameter.enable_akima_spline_first);
+
+  // convert centerline, which we consider as CoG center,  to rear wheel center
+  if (parameter.enable_cog_on_centerline) {
+    const double rear_to_cog = parameter.vehicle_length / 2 - parameter.rear_overhang;
+    return motion_utils::convertToRearWheelCenter(resampled_path_with_lane_id, rear_to_cog);
+  }
+
   return resampled_path_with_lane_id;
 }