Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(obstacle_cruise_planner): use motion utils #1381

Merged
merged 10 commits into from
Jul 21, 2022
Merged

feat(obstacle_cruise_planner): use motion utils #1381

Show file tree
Hide file tree
Changes from 3 commits
Commits
Show all changes
10 commits
Select commit Hold shift + click to select a range
adf47ad
feat(obstacle_cruise_planner): use motion utils
purewater0901 Jul 20, 2022
95e931d
remove unnecessary code
purewater0901 Jul 20, 2022
38ae6a7
Merge branch 'main' into feat/use-motion-utils
purewater0901 Jul 21, 2022
775ae02
Merge remote-tracking branch 'upstream/main' into feat/use-motion-utils
purewater0901 Jul 21, 2022
7973f0d
fix offset and points
purewater0901 Jul 21, 2022
71f9f69
add maximum value
purewater0901 Jul 21, 2022
165553d
fix orientation
purewater0901 Jul 21, 2022
800b9af
Merge branch 'feat/fix-insert-stop-point' into feat/use-motion-utils
purewater0901 Jul 21, 2022
ccd7439
empty commit
purewater0901 Jul 21, 2022
2b027db
fix conflict
purewater0901 Jul 21, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
52 changes: 24 additions & 28 deletions planning/obstacle_cruise_planner/src/planner_interface.cpp
View file
Open in desktop
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 =
closest_obstacle_dist - vehicle_info_.max_longitudinal_offset_m - feasible_margin_from_obstacle;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

use abs_ego_offset

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

done

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(
planner_data.traj.points, vehicle_info_.max_longitudinal_offset_m, *zero_vel_idx);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

use abs_ego_offset

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

done


// 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);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

use output_traj.points for wall_idx handling.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

done

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
Open in desktop
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