Skip to content

Commit

Permalink
feat(obstacle_cruise): ignore right beside objects (autowarefoundatio…
Browse files Browse the repository at this point in the history 
…n#6754) (#1267)

* ignore near beside objects
* change a magic num

Signed-off-by: Yuki Takagi <[email protected]>
  • Loading branch information
@yuki-takagi-66
yuki-takagi-66 authored Apr 21, 2024
1 parent 1ad6c07 commit f7d91e3
Show file tree
Hide file tree
Showing 3 changed files with 48 additions and 64 deletions.
2 changes: 1 addition & 1 deletion planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -87,7 +87,7 @@
obstacle_traj_angle_threshold : 1.22 # [rad] = 70 [deg], yaw threshold of crossing obstacle against the nearest trajectory point for cruise or stop

stop:
max_lat_margin: 0.2 # lateral margin between obstacle and trajectory band with ego's width
max_lat_margin: 0.3 # lateral margin between obstacle and trajectory band with ego's width
crossing_obstacle:
collision_time_margin : 4.0 # time threshold of collision between obstacle adn ego for cruise or stop [s]

Expand Down
66 changes: 47 additions & 19 deletions planning/obstacle_cruise_planner/src/node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -556,9 +556,10 @@ std::vector<Polygon2d> ObstacleCruisePlannerNode::createOneStepPolygons(
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;

const double front_length = vehicle_info.max_longitudinal_offset_m;
const double rear_length = vehicle_info.rear_overhang_m;
const double vehicle_width = vehicle_info.vehicle_width_m;

const size_t nearest_idx =
motion_utils::findNearestSegmentIndex(traj_points, current_ego_pose.position);
Expand All @@ -567,41 +568,68 @@ std::vector<Polygon2d> ObstacleCruisePlannerNode::createOneStepPolygons(
tier4_autoware_utils::inverseTransformPose(current_ego_pose, nearest_pose);
const double current_ego_lat_error = current_ego_pose_error.position.y;
const double current_ego_yaw_error = tf2::getYaw(current_ego_pose_error.orientation);
double time_elapsed = 0.0;

std::vector<Polygon2d> polygons;
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);
}
double time_elapsed{0.0};

std::vector<Polygon2d> output_polygons;
Polygon2d tmp_polys{};
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;
if (p.enable_to_consider_current_pose && time_elapsed < p.time_to_convergence) {
const double rem_ratio = (p.time_to_convergence - time_elapsed) / p.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 / std::abs(traj_points.at(i).longitudinal_velocity_mps);
time_elapsed +=
p.decimate_trajectory_step_length / std::abs(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;

Polygon2d idx_poly{};
for (const auto & pose : current_poses) {
if (i == 0 && traj_points.at(i).longitudinal_velocity_mps > 1e-3) {
boost::geometry::append(
idx_poly,
tier4_autoware_utils::toFootprint(pose, front_length, rear_length, vehicle_width)
.outer());
boost::geometry::append(
idx_poly,
tier4_autoware_utils::fromMsg(tier4_autoware_utils::calcOffsetPose(
pose, front_length, vehicle_width * 0.5 + lat_margin, 0.0)
.position)
.to_2d());
boost::geometry::append(
idx_poly, tier4_autoware_utils::fromMsg(
tier4_autoware_utils::calcOffsetPose(
pose, front_length, -vehicle_width * 0.5 - lat_margin, 0.0)
.position)
.to_2d());
} else {
boost::geometry::append(
idx_poly, tier4_autoware_utils::toFootprint(
pose, front_length, rear_length, vehicle_width + lat_margin * 2.0)
.outer());
}
}

boost::geometry::append(tmp_polys, idx_poly.outer());
Polygon2d hull_polygon;
boost::geometry::convex_hull(tmp_polys, hull_polygon);
boost::geometry::correct(hull_polygon);

output_polygons.push_back(hull_polygon);
tmp_polys = std::move(idx_poly);
}
return polygons;
return output_polygons;
}

std::vector<Obstacle> ObstacleCruisePlannerNode::convertToObstacles(
Expand Down
44 changes: 0 additions & 44 deletions planning/obstacle_cruise_planner/src/polygon_utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,18 +20,6 @@

namespace
{
void appendPointToPolygon(Polygon2d & polygon, const geometry_msgs::msg::Point & geom_point)
{
Point2d point(geom_point.x, geom_point.y);
bg::append(polygon.outer(), point);
}

geometry_msgs::msg::Point calcOffsetPosition(
const geometry_msgs::msg::Pose & pose, const double offset_x, const double offset_y)
{
return tier4_autoware_utils::calcOffsetPose(pose, offset_x, offset_y, 0.0).position;
}

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 @@ -104,38 +92,6 @@ 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<std::pair<geometry_msgs::msg::Point, double>> getCollisionPoint(
const std::vector<TrajectoryPoint> & traj_points, const std::vector<Polygon2d> & traj_polygons,
const Obstacle & obstacle, const bool is_driving_forward,
Expand Down

0 comments on commit f7d91e3

Please sign in to comment.