From d34000ee92c6dec236e39961f34189d24f6c119d Mon Sep 17 00:00:00 2001 From: Yuki TAKAGI <141538661+yuki-takagi-66@users.noreply.github.com> Date: Sat, 14 Oct 2023 01:03:29 +0900 Subject: [PATCH] feat(obstacle_cruise_planner)!: add ego pose consideration (#5036) (#942) * feat: add the feature to consider the current ego position * feat: add ego pose consideration. Both the lateral error and the yaw error are assumed to decrease to zero by a specified constant time (e.g 1.5 second). * feat: (continue) implement parameter settings for "cosider-current-ego-pose" * cleaned up the code * change description of the parameters * align format * Update planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp * Update planning/obstacle_cruise_planner/src/node.cpp --------- Signed-off-by: Yuki Takagi Co-authored-by: Kosuke Takeuchi --- .../config/obstacle_cruise_planner.param.yaml | 7 ++ .../include/obstacle_cruise_planner/node.hpp | 7 ++ .../obstacle_cruise_planner/polygon_utils.hpp | 8 +- planning/obstacle_cruise_planner/src/node.cpp | 73 ++++++++++++++-- .../src/polygon_utils.cpp | 83 +++++++------------ 5 files changed, 118 insertions(+), 60 deletions(-) diff --git a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml index 37d3ab4984501..a883ab16f1880 100644 --- a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml +++ b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml @@ -99,6 +99,13 @@ successive_num_to_entry_slow_down_condition: 5 successive_num_to_exit_slow_down_condition: 5 + # consider the current ego pose (it is not the nearest pose on the reference trajectory) + # Both the lateral error and the yaw error are assumed to decrease to zero by the time duration "time_to_convergence" + # The both errors decrease with constant rates against the time. + consider_current_pose: + enable_to_consider_current_pose: false + time_to_convergence: 1.5 #[s] + cruise: pid_based_planner: use_velocity_limit_based_planner: true diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp index 1927b266b0186..ac6b964b0df9f 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp @@ -48,6 +48,10 @@ class ObstacleCruisePlannerNode : public rclcpp::Node void onSmoothedTrajectory(const Trajectory::ConstSharedPtr msg); // main functions + std::vector createOneStepPolygons( + const std::vector & traj_points, + const vehicle_info_util::VehicleInfo & vehicle_info, + const geometry_msgs::msg::Pose & current_ego_pose, const double lat_margin = 0.0) const; std::vector convertToObstacles(const std::vector & traj_points) const; std::tuple, std::vector, std::vector> determineEgoBehaviorAgainstObstacles( @@ -189,6 +193,9 @@ class ObstacleCruisePlannerNode : public rclcpp::Node double lat_hysteresis_margin_for_slow_down; int successive_num_to_entry_slow_down_condition; int successive_num_to_exit_slow_down_condition; + // consideration for the current ego pose + bool enable_to_consider_current_pose{false}; + double time_to_convergence{1.5}; }; BehaviorDeterminationParam behavior_determination_param_; diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp index fdbf5b413d978..c86d6ba73ae7a 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp @@ -33,6 +33,11 @@ namespace bg = boost::geometry; using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; +Polygon2d createOneStepPolygon( + const std::vector & last_poses, + const std::vector & current_poses, + const vehicle_info_util::VehicleInfo & vehicle_info, const double lat_margin); + std::optional getCollisionPoint( const std::vector & traj_points, const std::vector & traj_polygons, const Obstacle & obstacle, const bool is_driving_forward); @@ -45,9 +50,6 @@ std::vector getCollisionPoints( const double max_lat_dist = std::numeric_limits::max(), const double max_prediction_time_for_collision_check = std::numeric_limits::max()); -std::vector createOneStepPolygons( - const std::vector & traj_points, - const vehicle_info_util::VehicleInfo & vehicle_info, const double lat_margin = 0.0); } // namespace polygon_utils #endif // OBSTACLE_CRUISE_PLANNER__POLYGON_UTILS_HPP_ diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index f6fe3896088cf..95ef6330dbdcc 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -272,6 +272,10 @@ ObstacleCruisePlannerNode::BehaviorDeterminationParam::BehaviorDeterminationPara "behavior_determination.slow_down.successive_num_to_entry_slow_down_condition"); successive_num_to_exit_slow_down_condition = node.declare_parameter( "behavior_determination.slow_down.successive_num_to_exit_slow_down_condition"); + enable_to_consider_current_pose = node.declare_parameter( + "behavior_determination.consider_current_pose.enable_to_consider_current_pose"); + time_to_convergence = node.declare_parameter( + "behavior_determination.consider_current_pose.time_to_convergence"); } void ObstacleCruisePlannerNode::BehaviorDeterminationParam::onParam( @@ -330,6 +334,12 @@ void ObstacleCruisePlannerNode::BehaviorDeterminationParam::onParam( tier4_autoware_utils::updateParam( parameters, "behavior_determination.slow_down.successive_num_to_exit_slow_down_condition", successive_num_to_exit_slow_down_condition); + tier4_autoware_utils::updateParam( + parameters, "behavior_determination.consider_current_pose.enable_to_consider_current_pose", + enable_to_consider_current_pose); + tier4_autoware_utils::updateParam( + parameters, "behavior_determination.consider_current_pose.time_to_convergence", + time_to_convergence); } ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & node_options) @@ -510,6 +520,57 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms get_logger(), enable_calculation_time_info_, "%s := %f [ms]", __func__, calculation_time); } +std::vector ObstacleCruisePlannerNode::createOneStepPolygons( + const std::vector & traj_points, + const vehicle_info_util::VehicleInfo & vehicle_info, + const geometry_msgs::msg::Pose & current_ego_pose, const double lat_margin) const +{ + const auto & p = behavior_determination_param_; + const bool is_enable_current_pose_consideration = p.enable_to_consider_current_pose; + const double step_length = p.decimate_trajectory_step_length; + const double time_to_convergence = p.time_to_convergence; + + std::vector polygons; + const double current_ego_lat_error = + motion_utils::calcLateralOffset(traj_points, current_ego_pose.position); + const double current_ego_yaw_error = + motion_utils::calcYawDeviation(traj_points, current_ego_pose); + double time_elapsed = 0.0; + + std::vector last_poses = {traj_points.at(0).pose}; + if (is_enable_current_pose_consideration) { + last_poses.push_back(current_ego_pose); + } + + for (size_t i = 0; i < traj_points.size(); ++i) { + std::vector current_poses = {traj_points.at(i).pose}; + + // estimate the future ego pose with assuming that the pose error against the reference path + // will decrease to zero by the time_to_convergence + if (is_enable_current_pose_consideration && time_elapsed < time_to_convergence) { + const double rem_ratio = (time_to_convergence - time_elapsed) / time_to_convergence; + geometry_msgs::msg::Pose indexed_pose_err; + indexed_pose_err.set__orientation( + tier4_autoware_utils::createQuaternionFromYaw(current_ego_yaw_error * rem_ratio)); + indexed_pose_err.set__position( + tier4_autoware_utils::createPoint(0.0, current_ego_lat_error * rem_ratio, 0.0)); + + current_poses.push_back( + tier4_autoware_utils::transformPose(indexed_pose_err, traj_points.at(i).pose)); + + if (traj_points.at(i).longitudinal_velocity_mps != 0.0) { + time_elapsed += step_length / traj_points.at(i).longitudinal_velocity_mps; + } else { + time_elapsed = std::numeric_limits::max(); + } + } + polygons.push_back( + polygon_utils::createOneStepPolygon(last_poses, current_poses, vehicle_info, lat_margin)); + last_poses = current_poses; + } + return polygons; +} + std::vector ObstacleCruisePlannerNode::convertToObstacles( const std::vector & traj_points) const { @@ -629,7 +690,7 @@ ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles( // calculated decimated trajectory points and trajectory polygon const auto decimated_traj_points = decimateTrajectoryPoints(traj_points); const auto decimated_traj_polys = - polygon_utils::createOneStepPolygons(decimated_traj_points, vehicle_info_); + createOneStepPolygons(decimated_traj_points, vehicle_info_, ego_odom_ptr_->pose.pose); debug_data_ptr_->detection_polygons = decimated_traj_polys; // determine ego's behavior from stop, cruise and slow down @@ -959,8 +1020,8 @@ ObstacleCruisePlannerNode::createCollisionPointForStopObstacle( } // calculate collision points with trajectory with lateral stop margin - const auto traj_polys_with_lat_margin = - polygon_utils::createOneStepPolygons(traj_points, vehicle_info_, p.max_lat_margin_for_stop); + const auto traj_polys_with_lat_margin = createOneStepPolygons( + traj_points, vehicle_info_, ego_odom_ptr_->pose.pose, p.max_lat_margin_for_stop); const auto collision_point = polygon_utils::getCollisionPoint( traj_points, traj_polys_with_lat_margin, obstacle, is_driving_forward_); return collision_point; @@ -1020,9 +1081,9 @@ std::optional ObstacleCruisePlannerNode::createSlowDownObstacl const auto obstacle_poly = obstacle.toPolygon(); // calculate collision points with trajectory with lateral stop margin - // NOTE: For additional margin, hysteresis is not devided by two. - const auto traj_polys_with_lat_margin = polygon_utils::createOneStepPolygons( - traj_points, vehicle_info_, + // NOTE: For additional margin, hysteresis is not divided by two. + const auto traj_polys_with_lat_margin = createOneStepPolygons( + traj_points, vehicle_info_, ego_odom_ptr_->pose.pose, p.max_lat_margin_for_slow_down + p.lat_hysteresis_margin_for_slow_down); std::vector front_collision_polygons; diff --git a/planning/obstacle_cruise_planner/src/polygon_utils.cpp b/planning/obstacle_cruise_planner/src/polygon_utils.cpp index 077b783f0e65e..e6b4ed89ca460 100644 --- a/planning/obstacle_cruise_planner/src/polygon_utils.cpp +++ b/planning/obstacle_cruise_planner/src/polygon_utils.cpp @@ -31,36 +31,6 @@ geometry_msgs::msg::Point calcOffsetPosition( return tier4_autoware_utils::calcOffsetPose(pose, offset_x, offset_y, 0.0).position; } -Polygon2d createOneStepPolygon( - const geometry_msgs::msg::Pose & base_step_pose, const geometry_msgs::msg::Pose & next_step_pose, - const vehicle_info_util::VehicleInfo & vehicle_info, const double lat_margin) -{ - Polygon2d polygon; - - const double base_to_front = vehicle_info.max_longitudinal_offset_m; - const double width = vehicle_info.vehicle_width_m / 2.0 + lat_margin; - const double base_to_rear = vehicle_info.rear_overhang_m; - - // base step - appendPointToPolygon(polygon, calcOffsetPosition(base_step_pose, base_to_front, width)); - appendPointToPolygon(polygon, calcOffsetPosition(base_step_pose, base_to_front, -width)); - appendPointToPolygon(polygon, calcOffsetPosition(base_step_pose, -base_to_rear, -width)); - appendPointToPolygon(polygon, calcOffsetPosition(base_step_pose, -base_to_rear, width)); - - // next step - appendPointToPolygon(polygon, calcOffsetPosition(next_step_pose, base_to_front, width)); - appendPointToPolygon(polygon, calcOffsetPosition(next_step_pose, base_to_front, -width)); - appendPointToPolygon(polygon, calcOffsetPosition(next_step_pose, -base_to_rear, -width)); - appendPointToPolygon(polygon, calcOffsetPosition(next_step_pose, -base_to_rear, width)); - - bg::correct(polygon); - - Polygon2d hull_polygon; - bg::convex_hull(polygon, hull_polygon); - - return hull_polygon; -} - PointWithStamp calcNearestCollisionPoint( const size_t first_within_idx, const std::vector & collision_points, const std::vector & decimated_traj_points, const bool is_driving_forward) @@ -132,6 +102,38 @@ std::optional>> getCollisionIndex( namespace polygon_utils { +Polygon2d createOneStepPolygon( + const std::vector & last_poses, + const std::vector & current_poses, + const vehicle_info_util::VehicleInfo & vehicle_info, const double lat_margin) +{ + Polygon2d polygon; + + const double base_to_front = vehicle_info.max_longitudinal_offset_m; + const double width = vehicle_info.vehicle_width_m / 2.0 + lat_margin; + const double base_to_rear = vehicle_info.rear_overhang_m; + + for (auto & pose : last_poses) { + appendPointToPolygon(polygon, calcOffsetPosition(pose, base_to_front, width)); + appendPointToPolygon(polygon, calcOffsetPosition(pose, base_to_front, -width)); + appendPointToPolygon(polygon, calcOffsetPosition(pose, -base_to_rear, -width)); + appendPointToPolygon(polygon, calcOffsetPosition(pose, -base_to_rear, width)); + } + for (auto & pose : current_poses) { + appendPointToPolygon(polygon, calcOffsetPosition(pose, base_to_front, width)); + appendPointToPolygon(polygon, calcOffsetPosition(pose, base_to_front, -width)); + appendPointToPolygon(polygon, calcOffsetPosition(pose, -base_to_rear, -width)); + appendPointToPolygon(polygon, calcOffsetPosition(pose, -base_to_rear, width)); + } + + bg::correct(polygon); + + Polygon2d hull_polygon; + bg::convex_hull(polygon, hull_polygon); + + return hull_polygon; +} + std::optional getCollisionPoint( const std::vector & traj_points, const std::vector & traj_polygons, const Obstacle & obstacle, const bool is_driving_forward) @@ -184,25 +186,4 @@ std::vector getCollisionPoints( return collision_points; } -std::vector createOneStepPolygons( - const std::vector & traj_points, - const vehicle_info_util::VehicleInfo & vehicle_info, const double lat_margin) -{ - std::vector polygons; - - for (size_t i = 0; i < traj_points.size(); ++i) { - const auto polygon = [&]() { - if (i == 0) { - return createOneStepPolygon( - traj_points.at(i).pose, traj_points.at(i).pose, vehicle_info, lat_margin); - } - return createOneStepPolygon( - traj_points.at(i - 1).pose, traj_points.at(i).pose, vehicle_info, lat_margin); - }(); - - polygons.push_back(polygon); - } - return polygons; -} - } // namespace polygon_utils