diff --git a/planning/collision_free_path_planner/README.md b/planning/collision_free_path_planner/README.md index c4912eecb1ea3..9da6f002e121e 100644 --- a/planning/collision_free_path_planner/README.md +++ b/planning/collision_free_path_planner/README.md @@ -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) @@ -61,16 +61,15 @@ group generateOptimizedTrajectory endif end group - :updateVelocity; + :applyPathVelocity; :insertZeroVelocityOutsideDrivableArea; :publishDebugMarkerInOptimization; end group -:extendedOptimizedTrajectory; -:alignVelocity; +:extendTrajectory; -:convertToTrajectory; +:setZeroVelocityAfterStopPoint; :publishDebugDataInMain; diff --git a/planning/collision_free_path_planner/include/collision_free_path_planner/node.hpp b/planning/collision_free_path_planner/include/collision_free_path_planner/node.hpp index c73ee41bfd35d..197bd82e20a07 100644 --- a/planning/collision_free_path_planner/include/collision_free_path_planner/node.hpp +++ b/planning/collision_free_path_planner/include/collision_free_path_planner/node.hpp @@ -222,7 +222,8 @@ class CollisionFreePathPlanner : public rclcpp::Node // main functions bool isDataReady(); PlannerData createPlannerData(const Path & path); - std::vector generateOptimizedTrajectory(const PlannerData & planner_data); + std::vector generateOptimizedTrajectory( + const PlannerData & planner_data, const CVMaps & cv_maps); std::vector extendTrajectory( const std::vector & path_points, const std::vector & optimized_points); @@ -230,12 +231,12 @@ class CollisionFreePathPlanner : public rclcpp::Node const PlannerData & planner_data, const std::vector & merged_optimized_points); void publishDebugDataInMain(const Path & path) const; - // functions for optimization + // functions in generateOptimizedTrajectory std::vector optimizeTrajectory( const PlannerData & planner_data, const CVMaps & cv_maps); std::vector getPrevOptimizedTrajectory( const std::vector & path_points) const; - void updateVelocity( + void applyPathVelocity( std::vector & traj_points, const std::vector & path_points) const; void insertZeroVelocityOutsideDrivableArea( const PlannerData & planner_data, std::vector & traj_points, diff --git a/planning/collision_free_path_planner/include/collision_free_path_planner/utils/utils.hpp b/planning/collision_free_path_planner/include/collision_free_path_planner/utils/utils.hpp index 2fe3f8a6a7c37..58a19484d35ea 100644 --- a/planning/collision_free_path_planner/include/collision_free_path_planner/utils/utils.hpp +++ b/planning/collision_free_path_planner/include/collision_free_path_planner/utils/utils.hpp @@ -171,6 +171,42 @@ size_t findForwardIndex(const T & points, const size_t begin_idx, const double f return points.size() - 1; } +template +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 T cropForwardPoints( const T & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, @@ -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) { diff --git a/planning/collision_free_path_planner/src/node.cpp b/planning/collision_free_path_planner/src/node.cpp index 7573ba60b8caa..be41390850755 100644 --- a/planning/collision_free_path_planner/src/node.cpp +++ b/planning/collision_free_path_planner/src/node.cpp @@ -58,6 +58,16 @@ StringStamped createStringStamped(const rclcpp::Time & now, const std::string & msg.data = data; return msg; } + +void setZeroVelocityAfterStopPoint(std::vector & 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) @@ -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" @@ -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); } @@ -280,30 +291,27 @@ PlannerData CollisionFreePathPlanner::createPlannerData(const Path & path) } std::vector 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__) @@ -390,7 +398,7 @@ std::vector CollisionFreePathPlanner::getPrevOptimizedTrajector return points_utils::convertToTrajectoryPoints(path_points); } -void CollisionFreePathPlanner::updateVelocity( +void CollisionFreePathPlanner::applyPathVelocity( std::vector & traj_points, const std::vector & path_points) const { stop_watch_.tic(__func__); @@ -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__) @@ -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 CollisionFreePathPlanner::extendTrajectory( - const std::vector & path_points, const std::vector & optimized_points) + const std::vector & path_points, + const std::vector & 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{}; - } - */ + 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::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{}; } 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 { + 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{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 CollisionFreePathPlanner::generatePostProcessedTrajectory( @@ -570,15 +586,6 @@ std::vector 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);