From 447d7a19c1d14d0f7393568efcac6bae035683ad Mon Sep 17 00:00:00 2001 From: guiping meng Date: Thu, 15 Sep 2022 12:07:27 +0800 Subject: [PATCH] fix(freespace_planner): Can't stop before blocking obstacle in parking #980 Signed-off-by: guiping meng --- .../freespace_planner_node.cpp | 36 +++++++++++++++---- 1 file changed, 30 insertions(+), 6 deletions(-) diff --git a/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp b/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp index e63087bb7708b..8fb25a592fdf9 100644 --- a/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp +++ b/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp @@ -191,6 +191,22 @@ Trajectory createStopTrajectory(const PoseStamped & current_pose) return createTrajectory(current_pose, waypoints, 0.0); } +Trajectory createStopTrajectory(const Trajectory & trajectory) +{ + Trajectory stop_trajectory; + stop_trajectory.header = trajectory.header; + stop_trajectory.header.stamp = rclcpp::Clock().now(); + + stop_trajectory.points.reserve(trajectory.points.size()); + for (size_t i = 0; i < trajectory.points.size(); ++i) { + stop_trajectory.points.push_back(trajectory.points.at(i)); + } + for (size_t i = 0; i < trajectory.points.size(); ++i) { + stop_trajectory.points.at(i).longitudinal_velocity_mps = 0.0; + } + return stop_trajectory; +} + bool isStopped( const std::deque & odom_buffer, const double th_stopped_velocity_mps) { @@ -429,13 +445,21 @@ void FreespacePlannerNode::onTimer() initializePlanningAlgorithm(); if (isPlanRequired()) { - reset(); - // Stop before planning new trajectory - const auto stop_trajectory = createStopTrajectory(current_pose_); - trajectory_pub_->publish(stop_trajectory); - debug_pose_array_pub_->publish(trajectory2PoseArray(stop_trajectory)); - debug_partial_pose_array_pub_->publish(trajectory2PoseArray(stop_trajectory)); + if(partial_trajectory_.points.size()>0){ + const auto stop_trajectory = createStopTrajectory(partial_trajectory_); + trajectory_pub_->publish(stop_trajectory); + debug_pose_array_pub_->publish(trajectory2PoseArray(stop_trajectory)); + debug_partial_pose_array_pub_->publish(trajectory2PoseArray(stop_trajectory)); + } + else{ + const auto stop_trajectory = createStopTrajectory(current_pose_); + trajectory_pub_->publish(stop_trajectory); + debug_pose_array_pub_->publish(trajectory2PoseArray(stop_trajectory)); + debug_partial_pose_array_pub_->publish(trajectory2PoseArray(stop_trajectory)); + } + + reset(); // Plan new trajectory planTrajectory();