diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index ab2092a3120ca..f6fe3896088cf 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -129,11 +129,11 @@ double calcObstacleMaxLength(const Shape & shape) } TrajectoryPoint getExtendTrajectoryPoint( - const double extend_distance, const TrajectoryPoint & goal_point) + const double extend_distance, const TrajectoryPoint & goal_point, const bool is_driving_forward) { TrajectoryPoint extend_trajectory_point; - extend_trajectory_point.pose = - tier4_autoware_utils::calcOffsetPose(goal_point.pose, extend_distance, 0.0, 0.0); + extend_trajectory_point.pose = tier4_autoware_utils::calcOffsetPose( + goal_point.pose, extend_distance * (is_driving_forward ? 1.0 : -1.0), 0.0, 0.0); extend_trajectory_point.longitudinal_velocity_mps = goal_point.longitudinal_velocity_mps; extend_trajectory_point.lateral_velocity_mps = goal_point.lateral_velocity_mps; extend_trajectory_point.acceleration_mps2 = goal_point.acceleration_mps2; @@ -145,6 +145,8 @@ std::vector extendTrajectoryPoints( const double step_length) { auto output_points = input_points; + const auto is_driving_forward_opt = motion_utils::isDrivingForwardWithTwist(input_points); + const bool is_driving_forward = is_driving_forward_opt ? *is_driving_forward_opt : true; if (extend_distance < std::numeric_limits::epsilon()) { return output_points; @@ -154,11 +156,13 @@ std::vector extendTrajectoryPoints( double extend_sum = 0.0; while (extend_sum <= (extend_distance - step_length)) { - const auto extend_trajectory_point = getExtendTrajectoryPoint(extend_sum, goal_point); + const auto extend_trajectory_point = + getExtendTrajectoryPoint(extend_sum, goal_point, is_driving_forward); output_points.push_back(extend_trajectory_point); extend_sum += step_length; } - const auto extend_trajectory_point = getExtendTrajectoryPoint(extend_distance, goal_point); + const auto extend_trajectory_point = + getExtendTrajectoryPoint(extend_distance, goal_point, is_driving_forward); output_points.push_back(extend_trajectory_point); return output_points;