Skip to content

Commit

Permalink
feat(obstacle_cruise_planner)!: add ego pose consideration (autowaref…
Browse files Browse the repository at this point in the history 
…oundation#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 <[email protected]>
Co-authored-by: Kosuke Takeuchi <[email protected]>
  • Loading branch information
@yuki-takagi-66 @kosuke55
yuki-takagi-66 and kosuke55 authored Oct 13, 2023
1 parent 11cb5a4 commit d34000e
Show file tree
Hide file tree
Showing 5 changed files with 118 additions and 60 deletions.
7 changes: 7 additions & 0 deletions planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
7 changes: 7 additions & 0 deletions planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,6 +48,10 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
void onSmoothedTrajectory(const Trajectory::ConstSharedPtr msg);

// main functions
std::vector<Polygon2d> createOneStepPolygons(
const std::vector<TrajectoryPoint> & 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<Obstacle> convertToObstacles(const std::vector<TrajectoryPoint> & traj_points) const;
std::tuple<std::vector<StopObstacle>, std::vector<CruiseObstacle>, std::vector<SlowDownObstacle>>
determineEgoBehaviorAgainstObstacles(
Expand Down Expand Up @@ -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_;

Expand Down
8 changes: 5 additions & 3 deletions planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,6 +33,11 @@ namespace bg = boost::geometry;
using tier4_autoware_utils::Point2d;
using tier4_autoware_utils::Polygon2d;

Polygon2d createOneStepPolygon(
const std::vector<geometry_msgs::msg::Pose> & last_poses,
const std::vector<geometry_msgs::msg::Pose> & current_poses,
const vehicle_info_util::VehicleInfo & vehicle_info, const double lat_margin);

std::optional<geometry_msgs::msg::Point> getCollisionPoint(
const std::vector<TrajectoryPoint> & traj_points, const std::vector<Polygon2d> & traj_polygons,
const Obstacle & obstacle, const bool is_driving_forward);
Expand All @@ -45,9 +50,6 @@ std::vector<PointWithStamp> getCollisionPoints(
const double max_lat_dist = std::numeric_limits<double>::max(),
const double max_prediction_time_for_collision_check = std::numeric_limits<double>::max());

std::vector<Polygon2d> createOneStepPolygons(
const std::vector<TrajectoryPoint> & 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_
73 changes: 67 additions & 6 deletions planning/obstacle_cruise_planner/src/node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<int>(
"behavior_determination.slow_down.successive_num_to_exit_slow_down_condition");
enable_to_consider_current_pose = node.declare_parameter<bool>(
"behavior_determination.consider_current_pose.enable_to_consider_current_pose");
time_to_convergence = node.declare_parameter<double>(
"behavior_determination.consider_current_pose.time_to_convergence");
}

void ObstacleCruisePlannerNode::BehaviorDeterminationParam::onParam(
Expand Down Expand Up @@ -330,6 +334,12 @@ void ObstacleCruisePlannerNode::BehaviorDeterminationParam::onParam(
tier4_autoware_utils::updateParam<int>(
parameters, "behavior_determination.slow_down.successive_num_to_exit_slow_down_condition",
successive_num_to_exit_slow_down_condition);
tier4_autoware_utils::updateParam<bool>(
parameters, "behavior_determination.consider_current_pose.enable_to_consider_current_pose",
enable_to_consider_current_pose);
tier4_autoware_utils::updateParam<double>(
parameters, "behavior_determination.consider_current_pose.time_to_convergence",
time_to_convergence);
}

ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & node_options)
Expand Down Expand Up @@ -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<Polygon2d> ObstacleCruisePlannerNode::createOneStepPolygons(
const std::vector<TrajectoryPoint> & 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<Polygon2d> 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<geometry_msgs::msg::Pose> 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<geometry_msgs::msg::Pose> 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<double>::max();
}
}
polygons.push_back(
polygon_utils::createOneStepPolygon(last_poses, current_poses, vehicle_info, lat_margin));
last_poses = current_poses;
}
return polygons;
}

std::vector<Obstacle> ObstacleCruisePlannerNode::convertToObstacles(
const std::vector<TrajectoryPoint> & traj_points) const
{
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -1020,9 +1081,9 @@ std::optional<SlowDownObstacle> 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<Polygon2d> front_collision_polygons;
Expand Down
83 changes: 32 additions & 51 deletions planning/obstacle_cruise_planner/src/polygon_utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<PointWithStamp> & collision_points,
const std::vector<TrajectoryPoint> & decimated_traj_points, const bool is_driving_forward)
Expand Down Expand Up @@ -132,6 +102,38 @@ std::optional<std::pair<size_t, std::vector<PointWithStamp>>> getCollisionIndex(

namespace polygon_utils
{
Polygon2d createOneStepPolygon(
const std::vector<geometry_msgs::msg::Pose> & last_poses,
const std::vector<geometry_msgs::msg::Pose> & 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<geometry_msgs::msg::Point> getCollisionPoint(
const std::vector<TrajectoryPoint> & traj_points, const std::vector<Polygon2d> & traj_polygons,
const Obstacle & obstacle, const bool is_driving_forward)
Expand Down Expand Up @@ -184,25 +186,4 @@ std::vector<PointWithStamp> getCollisionPoints(
return collision_points;
}

std::vector<Polygon2d> createOneStepPolygons(
const std::vector<TrajectoryPoint> & traj_points,
const vehicle_info_util::VehicleInfo & vehicle_info, const double lat_margin)
{
std::vector<Polygon2d> 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

0 comments on commit d34000e

Please sign in to comment.