Skip to content

Commit

Permalink
po
Browse files Browse the repository at this point in the history
  • Loading branch information
@takayuki5168
takayuki5168 committed Oct 15, 2022
1 parent 2fe8167 commit 60676dc
Show file tree
Hide file tree
Showing 4 changed files with 137 additions and 93 deletions.
21 changes: 10 additions & 11 deletions planning/collision_free_path_planner/README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,14 +39,14 @@ start
:createPlannerData;
group generateOptimizedTrajectory
group getMaps
:getDrivableArea;
:getRoadClearanceMap;
:drawObstacleOnImage;
:getObstacleClearanceMap;
end group
group getMaps
:getDrivableArea;
:getRoadClearanceMap;
:drawObstacleOnImage;
:getObstacleClearanceMap;
end group
group generateOptimizedTrajectory
group optimizeTrajectory
:checkReplan;
if (replanning required?) then (yes)
Expand All @@ -61,16 +61,15 @@ group generateOptimizedTrajectory
endif
end group
:updateVelocity;
:applyPathVelocity;
:insertZeroVelocityOutsideDrivableArea;
:publishDebugMarkerInOptimization;
end group
:extendedOptimizedTrajectory;
:alignVelocity;
:extendTrajectory;
:convertToTrajectory;
:setZeroVelocityAfterStopPoint;
:publishDebugDataInMain;
Expand Down
7 changes: 4 additions & 3 deletions planning/collision_free_path_planner/include/collision_free_path_planner/node.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -222,20 +222,21 @@ class CollisionFreePathPlanner : public rclcpp::Node
// main functions
bool isDataReady();
PlannerData createPlannerData(const Path & path);
std::vector<TrajectoryPoint> generateOptimizedTrajectory(const PlannerData & planner_data);
std::vector<TrajectoryPoint> generateOptimizedTrajectory(
const PlannerData & planner_data, const CVMaps & cv_maps);
std::vector<TrajectoryPoint> extendTrajectory(
const std::vector<PathPoint> & path_points,
const std::vector<TrajectoryPoint> & optimized_points);
std::vector<TrajectoryPoint> generatePostProcessedTrajectory(
const PlannerData & planner_data, const std::vector<TrajectoryPoint> & merged_optimized_points);
void publishDebugDataInMain(const Path & path) const;

// functions for optimization
// functions in generateOptimizedTrajectory
std::vector<TrajectoryPoint> optimizeTrajectory(
const PlannerData & planner_data, const CVMaps & cv_maps);
std::vector<TrajectoryPoint> getPrevOptimizedTrajectory(
const std::vector<PathPoint> & path_points) const;
void updateVelocity(
void applyPathVelocity(
std::vector<TrajectoryPoint> & traj_points, const std::vector<PathPoint> & path_points) const;
void insertZeroVelocityOutsideDrivableArea(
const PlannerData & planner_data, std::vector<TrajectoryPoint> & traj_points,
Expand Down
37 changes: 37 additions & 0 deletions planning/collision_free_path_planner/include/collision_free_path_planner/utils/utils.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -171,6 +171,42 @@ size_t findForwardIndex(const T & points, const size_t begin_idx, const double f
return points.size() - 1;
}

template <typename T>
T cropPointsAfterOffsetPoint(
const T & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx,
const double offset)
{
if (points.empty()) {
return T{};
}

double sum_length =
-motion_utils::calcLongitudinalOffsetToSegment(points, target_seg_idx, target_pos);

// search forward
if (sum_length < offset) {
for (size_t i = target_seg_idx + 1; i < points.size(); ++i) {
sum_length += tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i - 1));
if (offset < sum_length) {
return T{points.begin() + i - 1, points.end()};
}
}

return T{};
}

// search backward
for (size_t i = target_seg_idx; 0 < i;
--i) { // NOTE: use size_t since i is always positive value
sum_length -= tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i - 1));
if (sum_length < offset) {
return T{points.begin() + i - 1, points.end()};
}
}

return points;
}

template <typename T>
T cropForwardPoints(
const T & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx,
Expand All @@ -182,6 +218,7 @@ T cropForwardPoints(

double sum_length =
-motion_utils::calcLongitudinalOffsetToSegment(points, target_seg_idx, target_pos);
std::cerr << "sum length " << sum_length << std::endl;
for (size_t i = target_seg_idx + 1; i < points.size(); ++i) {
sum_length += tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i - 1));
if (forward_length < sum_length) {
Expand Down
165 changes: 86 additions & 79 deletions planning/collision_free_path_planner/src/node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -58,6 +58,16 @@ StringStamped createStringStamped(const rclcpp::Time & now, const std::string &
msg.data = data;
return msg;
}

void setZeroVelocityAfterStopPoint(std::vector<TrajectoryPoint> & traj_points)
{
const auto opt_zero_vel_idx = motion_utils::searchZeroVelocityIndex(traj_points);
if (opt_zero_vel_idx) {
for (size_t i = opt_zero_vel_idx.get(); i < traj_points.size(); ++i) {
traj_points.at(i).longitudinal_velocity_mps = 0.0;
}
}
}
} // namespace

CollisionFreePathPlanner::CollisionFreePathPlanner(const rclcpp::NodeOptions & node_options)
Expand Down Expand Up @@ -217,18 +227,19 @@ void CollisionFreePathPlanner::onPath(const Path::SharedPtr path_ptr)
// 1. create planner data
const auto planner_data = createPlannerData(*path_ptr);

// 2. generate optimized trajectory
const auto optimized_traj_points = generateOptimizedTrajectory(planner_data);
// 2. create clearance maps
const CVMaps cv_maps = costmap_generator_ptr_->getMaps(planner_data, traj_param_);

// 3. generate optimized trajectory
const auto optimized_traj_points = generateOptimizedTrajectory(planner_data, cv_maps);

// 3. extend trajectory to connect the optimized trajectory and the following path smoothly
const auto extended_traj_points =
extendTrajectory(planner_data.path.points, optimized_traj_points);
// 4. extend trajectory to connect the optimized trajectory and the following path smoothly
auto extended_traj_points = extendTrajectory(planner_data.path.points, optimized_traj_points);

// 3. generate post processed trajectory
const auto post_processed_traj_points =
generatePostProcessedTrajectory(planner_data, extended_traj_points);
// 5. set zero velocity after stop point
setZeroVelocityAfterStopPoint(extended_traj_points);

// 4. publish debug data
// 6. publish debug data
publishDebugDataInMain(*path_ptr);

debug_data_ptr_->msg_stream << __func__ << ":= " << stop_watch_.toc(__func__) << " [ms]\n"
Expand All @@ -241,7 +252,7 @@ void CollisionFreePathPlanner::onPath(const Path::SharedPtr path_ptr)
debug_calculation_time_pub_->publish(calculation_time_msg);

const auto output_traj_msg =
points_utils::createTrajectory(path_ptr->header, post_processed_traj_points);
points_utils::createTrajectory(path_ptr->header, extended_traj_points);
traj_pub_->publish(output_traj_msg);
}

Expand Down Expand Up @@ -280,30 +291,27 @@ PlannerData CollisionFreePathPlanner::createPlannerData(const Path & path)
}

std::vector<TrajectoryPoint> CollisionFreePathPlanner::generateOptimizedTrajectory(
const PlannerData & planner_data)
const PlannerData & planner_data, const CVMaps & cv_maps)
{
stop_watch_.tic(__func__);

const auto & path = planner_data.path;

// 1. create clearance maps
const CVMaps cv_maps = costmap_generator_ptr_->getMaps(planner_data, traj_param_);

// 2. calculate trajectory with EB and MPT
// 1. calculate trajectory with EB and MPT
// NOTE: This function may return previously optimized trajectory points.
auto optimized_traj_points = optimizeTrajectory(planner_data, cv_maps);

// 3. update velocity
// 2. update velocity
// Even if optimization is skipped, velocity in trajectory points must be updated since velocity
// in input path may change
updateVelocity(optimized_traj_points, path.points);
applyPathVelocity(optimized_traj_points, path.points);

// 4. insert zero velocity when trajectory is over drivable area
// 3. insert zero velocity when trajectory is over drivable area
if (enable_outside_drivable_area_stop_) {
insertZeroVelocityOutsideDrivableArea(planner_data, optimized_traj_points, cv_maps);
}

// 5. publish debug marker
// 4. publish debug marker
publishDebugMarkerInOptimization(planner_data, optimized_traj_points);

debug_data_ptr_->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__)
Expand Down Expand Up @@ -390,7 +398,7 @@ std::vector<TrajectoryPoint> CollisionFreePathPlanner::getPrevOptimizedTrajector
return points_utils::convertToTrajectoryPoints(path_points);
}

void CollisionFreePathPlanner::updateVelocity(
void CollisionFreePathPlanner::applyPathVelocity(
std::vector<TrajectoryPoint> & traj_points, const std::vector<PathPoint> & path_points) const
{
stop_watch_.tic(__func__);
Expand All @@ -408,6 +416,8 @@ void CollisionFreePathPlanner::updateVelocity(
traj_points.at(i).longitudinal_velocity_mps = std::max(
path_points.at(nearest_seg_idx).longitudinal_velocity_mps,
path_points.at(max_idx).longitudinal_velocity_mps);

// TODO(murooka) insert stop point explicitly
}

debug_data_ptr_->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__)
Expand Down Expand Up @@ -470,88 +480,94 @@ void CollisionFreePathPlanner::publishDebugMarkerInOptimization(

const auto & p = planner_data;

if (enable_pub_debug_marker_) {
// debug marker
stop_watch_.tic("getDebugMarker");
const auto & debug_marker = getDebugMarker(*debug_data_ptr_, traj_points, vehicle_info_, false);
debug_data_ptr_->msg_stream << " getDebugMarker:= " << stop_watch_.toc("getDebugMarker")
if (!enable_pub_debug_marker_) {
debug_data_ptr_->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__)
<< " [ms]\n";
return;
}

stop_watch_.tic("publishDebugMarker");
debug_markers_pub_->publish(debug_marker);
debug_data_ptr_->msg_stream << " publishDebugMarker:= "
<< stop_watch_.toc("publishDebugMarker") << " [ms]\n";

// debug wall marker
stop_watch_.tic("getAndPublishDebugWallMarker");
const auto virtual_wall_marker_array = [&]() {
if (debug_data_ptr_->stop_pose_by_drivable_area) {
const auto & stop_pose = debug_data_ptr_->stop_pose_by_drivable_area.get();
return motion_utils::createStopVirtualWallMarker(
stop_pose, "drivable area", now(), 0, vehicle_info_.max_longitudinal_offset_m);
}
return motion_utils::createDeletedStopVirtualWallMarker(now(), 0);
}();
// debug marker
stop_watch_.tic("getDebugMarker");
const auto & debug_marker = getDebugMarker(*debug_data_ptr_, traj_points, vehicle_info_, false);
debug_data_ptr_->msg_stream << " getDebugMarker:= " << stop_watch_.toc("getDebugMarker")
<< " [ms]\n";

debug_wall_markers_pub_->publish(virtual_wall_marker_array);
debug_data_ptr_->msg_stream << " getAndPublishDebugWallMarker:= "
<< stop_watch_.toc("getAndPublishDebugWallMarker") << " [ms]\n";
}
stop_watch_.tic("publishDebugMarker");
debug_markers_pub_->publish(debug_marker);
debug_data_ptr_->msg_stream << " publishDebugMarker:= "
<< stop_watch_.toc("publishDebugMarker") << " [ms]\n";

// debug wall marker
stop_watch_.tic("getAndPublishDebugWallMarker");
const auto virtual_wall_marker_array = [&]() {
if (debug_data_ptr_->stop_pose_by_drivable_area) {
const auto & stop_pose = debug_data_ptr_->stop_pose_by_drivable_area.get();
return motion_utils::createStopVirtualWallMarker(
stop_pose, "drivable area", now(), 0, vehicle_info_.max_longitudinal_offset_m);
}
return motion_utils::createDeletedStopVirtualWallMarker(now(), 0);
}();

debug_wall_markers_pub_->publish(virtual_wall_marker_array);
debug_data_ptr_->msg_stream << " getAndPublishDebugWallMarker:= "
<< stop_watch_.toc("getAndPublishDebugWallMarker") << " [ms]\n";

debug_data_ptr_->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__)
<< " [ms]\n";
}

std::vector<TrajectoryPoint> CollisionFreePathPlanner::extendTrajectory(
const std::vector<PathPoint> & path_points, const std::vector<TrajectoryPoint> & optimized_points)
const std::vector<PathPoint> & path_points,
const std::vector<TrajectoryPoint> & optimized_traj_points)
{
stop_watch_.tic(__func__);

// guard
if (path_points.empty()) {
return optimized_points;
return optimized_traj_points;
}
if (optimized_points.empty()) {
if (optimized_traj_points.empty()) {
return points_utils::convertToTrajectoryPoints(path_points);
}

/*
const double accum_arc_length = motion_utils::calcArcLength(optimized_points);
if (accum_arc_length > traj_param_.output_traj_length) {
RCLCPP_INFO_THROTTLE(
get_logger(), *this->get_clock(), std::chrono::milliseconds(10000).count(),
"[Avoidance] Not extend trajectory");
return std::vector<TrajectoryPoint>{};
}
*/
const auto & joint_start_pose = optimized_traj_points.back().pose;

// calculate end idx of optimized points on path points
const auto opt_end_path_seg_idx = motion_utils::findNearestSegmentIndex(
path_points, optimized_points.back().pose, std::numeric_limits<double>::max(),
path_points, joint_start_pose, traj_param_.delta_dist_threshold_for_closest_point,
traj_param_.delta_yaw_threshold_for_closest_point);
if (!opt_end_path_seg_idx) {
RCLCPP_INFO_THROTTLE(
get_logger(), *get_clock(), std::chrono::seconds(5).count(),
"[Avoidance] Not extend trajectory since could not find nearest idx from last opt point");
RCLCPP_INFO_EXPRESSION(
rclcpp::get_logger("mpt_optimizer"), enable_debug_info_,
"Not extend trajectory since could not find nearest idx from last opt point");
return std::vector<TrajectoryPoint>{};
}
const size_t end_path_seg_idx = opt_end_path_seg_idx.get();

auto extended_traj_points = optimized_points;
const size_t extended_start_point_idx = end_path_seg_idx + 1;
for (size_t i = extended_start_point_idx; i < path_points.size(); ++i) {
const auto extended_traj_point = points_utils::convertToTrajectoryPoint(path_points.at(i));
extended_traj_points.push_back(extended_traj_point);
}
// crop forward trajectory
const auto forward_traj_points = [&]() -> std::vector<TrajectoryPoint> {
constexpr double joint_traj_length_for_resampling = 10.0;
const auto forward_path_points = points_utils::cropPointsAfterOffsetPoint(
path_points, joint_start_pose.position, end_path_seg_idx, joint_traj_length_for_resampling);

if (forward_path_points.empty()) { // compensate goal pose
// TODO(murooka) optimization last point may be path last point
std::vector<TrajectoryPoint>{points_utils::convertToTrajectoryPoint(path_points.back())};
}

return points_utils::convertToTrajectoryPoints(forward_path_points);
}();

// TODO(murooka) resample
const auto extended_traj_points = concatVectors(optimized_traj_points, forward_traj_points);

// TODO(murooka) update vleocity on joint
const auto resampled_traj_points = points_utils::resampleTrajectoryPoints(
extended_traj_points, traj_param_.output_delta_arc_length);

// TODO(murooka) compensate goal pose
// TODO(murooka) update velocity on joint

debug_data_ptr_->msg_stream << " " << __func__ << ":= " << stop_watch_.toc(__func__)
<< " [ms]\n";
return extended_traj_points;
return resampled_traj_points;
}

std::vector<TrajectoryPoint> CollisionFreePathPlanner::generatePostProcessedTrajectory(
Expand All @@ -570,15 +586,6 @@ std::vector<TrajectoryPoint> CollisionFreePathPlanner::generatePostProcessedTraj

auto traj_points = optimized_traj_points;

// insert zero velocity after stop point
// NOTE: This implementation is required for stopping due to going outside the drivable area.
const auto opt_zero_vel_idx = motion_utils::searchZeroVelocityIndex(traj_points);
if (opt_zero_vel_idx) {
for (size_t i = opt_zero_vel_idx.get(); i < traj_points.size(); ++i) {
traj_points.at(i).longitudinal_velocity_mps = 0.0;
}
}

/*
// concat trajectories
const auto traj_points = concatVectors(traj_points, extended_traj_points);
Expand Down

0 comments on commit 60676dc

Please sign in to comment.