Skip to content

Commit

Permalink
feat(obstacle_cruise_planner): use motion utils (tier4#1381)
Browse files Browse the repository at this point in the history
  • Loading branch information
@purewater0901 @boyali
purewater0901 authored and boyali committed Oct 3, 2022
1 parent 3cb8e71 commit 244d3ba
Show file tree
Hide file tree
Showing 2 changed files with 24 additions and 91 deletions.
52 changes: 24 additions & 28 deletions planning/obstacle_cruise_planner/src/planner_interface.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -130,37 +130,33 @@ Trajectory PlannerInterface::generateStopTrajectory(
will_collide_with_obstacle = true;
}

const size_t collision_idx = motion_utils::findNearestIndex(
planner_data.traj.points, closest_stop_obstacle->collision_point);
const size_t zero_vel_idx = obstacle_cruise_utils::getIndexWithLongitudinalOffset(
planner_data.traj.points, -abs_ego_offset - feasible_margin_from_obstacle, collision_idx);
const size_t wall_idx = obstacle_cruise_utils::getIndexWithLongitudinalOffset(
planner_data.traj.points, -feasible_margin_from_obstacle, collision_idx);

// TODO(shimizu) insert stop point with interpolation
// Generate Output Trajectory
auto output_traj = planner_data.traj;
for (size_t o_idx = zero_vel_idx; o_idx < output_traj.points.size(); ++o_idx) {
output_traj.points.at(o_idx).longitudinal_velocity_mps = 0.0;
const double zero_vel_dist =
std::max(0.0, closest_obstacle_dist - abs_ego_offset - feasible_margin_from_obstacle);
const auto zero_vel_idx = motion_utils::insertStopPoint(0, zero_vel_dist, output_traj.points);
if (zero_vel_idx) {
const auto wall_idx = obstacle_cruise_utils::getIndexWithLongitudinalOffset(
output_traj.points, abs_ego_offset, *zero_vel_idx);

// virtual wall marker for stop obstacle
const auto marker_pose = output_traj.points.at(wall_idx).pose;
const auto markers = motion_utils::createStopVirtualWallMarker(
marker_pose, "obstacle stop", planner_data.current_time, 0);
tier4_autoware_utils::appendMarkerArray(markers, &debug_data.stop_wall_marker);
debug_data.obstacles_to_stop.push_back(*closest_stop_obstacle);

// Publish Stop Reason
const auto stop_pose = output_traj.points.at(*zero_vel_idx).pose;
const auto stop_reasons_msg =
makeStopReasonArray(planner_data.current_time, stop_pose, *closest_stop_obstacle);
stop_reasons_pub_->publish(stop_reasons_msg);

// Publish if ego vehicle collides with the obstacle with a limit acceleration
const auto stop_speed_exceeded_msg =
createStopSpeedExceededMsg(planner_data.current_time, will_collide_with_obstacle);
stop_speed_exceeded_pub_->publish(stop_speed_exceeded_msg);
}

// virtual wall marker for stop obstacle
const auto marker_pose = planner_data.traj.points.at(wall_idx).pose;
const auto markers = motion_utils::createStopVirtualWallMarker(
marker_pose, "obstacle stop", planner_data.current_time, 0);
tier4_autoware_utils::appendMarkerArray(markers, &debug_data.stop_wall_marker);
debug_data.obstacles_to_stop.push_back(*closest_stop_obstacle);

// Publish Stop Reason
const auto stop_pose = output_traj.points.at(zero_vel_idx).pose;
const auto stop_reasons_msg =
makeStopReasonArray(planner_data.current_time, stop_pose, *closest_stop_obstacle);
stop_reasons_pub_->publish(stop_reasons_msg);

// Publish if ego vehicle collides with the obstacle with a limit acceleration
const auto stop_speed_exceeded_msg =
createStopSpeedExceededMsg(planner_data.current_time, will_collide_with_obstacle);
stop_speed_exceeded_pub_->publish(stop_speed_exceeded_msg);

return output_traj;
}
63 changes: 0 additions & 63 deletions planning/obstacle_cruise_planner/src/utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -152,69 +152,6 @@ geometry_msgs::msg::Pose getCurrentObjectPose(
return interpolated_pose.get();
}

autoware_auto_planning_msgs::msg::Trajectory insertStopPoint(
const autoware_auto_planning_msgs::msg::Trajectory & trajectory,
const double distance_to_stop_point)
{
if (trajectory.points.size() < 2) {
return trajectory;
}

const double traj_length = motion_utils::calcArcLength(trajectory.points);
if (traj_length < distance_to_stop_point) {
return trajectory;
}

autoware_auto_planning_msgs::msg::Trajectory output;
output.header = trajectory.header;

double accumulated_length = 0;
size_t insert_idx = trajectory.points.size();
for (size_t i = 0; i < trajectory.points.size() - 1; ++i) {
const auto curr_traj_point = trajectory.points.at(i);
const auto next_traj_point = trajectory.points.at(i + 1);
const auto curr_pose = curr_traj_point.pose;
const auto next_pose = next_traj_point.pose;
const double segment_length = tier4_autoware_utils::calcDistance2d(curr_pose, next_pose);
accumulated_length += segment_length;

if (accumulated_length > distance_to_stop_point) {
const double ratio = 1 - (accumulated_length - distance_to_stop_point) / segment_length;

autoware_auto_planning_msgs::msg::TrajectoryPoint stop_point;
stop_point.pose = tier4_autoware_utils::calcInterpolatedPose(curr_pose, next_pose, ratio);
stop_point.lateral_velocity_mps = 0.0;
const double front_dist = tier4_autoware_utils::calcDistance2d(curr_pose, stop_point.pose);
const double back_dist = tier4_autoware_utils::calcDistance2d(stop_point.pose, next_pose);
if (front_dist < 1e-3) {
auto traj_point = trajectory.points.at(i);
traj_point.longitudinal_velocity_mps = 0.0;
output.points.push_back(traj_point);
} else if (back_dist < 1e-3) {
output.points.push_back(curr_traj_point);
} else {
output.points.push_back(curr_traj_point);
output.points.push_back(stop_point);
}
insert_idx = i + 1;
break;
}

output.points.push_back(curr_traj_point);
}

for (size_t i = insert_idx; i < trajectory.points.size() - 1; ++i) {
auto traj_point = trajectory.points.at(i);
traj_point.longitudinal_velocity_mps = 0.0;
output.points.push_back(traj_point);
}

// Terminal Velocity Should be zero
output.points.back().longitudinal_velocity_mps = 0.0;

return output;
}

boost::optional<TargetObstacle> getClosestStopObstacle(
const autoware_auto_planning_msgs::msg::Trajectory & traj,
const std::vector<TargetObstacle> & target_obstacles)
Expand Down

0 comments on commit 244d3ba

Please sign in to comment.