diff --git a/include/point_follow_planner/point_follow_planner.h b/include/point_follow_planner/point_follow_planner.h index e9744a4..df067c3 100644 --- a/include/point_follow_planner/point_follow_planner.h +++ b/include/point_follow_planner/point_follow_planner.h @@ -112,6 +112,7 @@ class PointFollowPlanner double angle_resolution_; double predict_time_; double dt_; + double sleep_time_after_finish_; double angle_to_goal_th_; double dist_to_goal_th_; double turn_direction_th_; @@ -151,7 +152,7 @@ class PointFollowPlanner geometry_msgs::Twist current_velocity_; geometry_msgs::Twist previous_velocity_; - std_msgs::Bool has_finished; + std_msgs::Bool has_finished_; tf::TransformListener listener_; }; diff --git a/launch/local_planner.launch b/launch/local_planner.launch index a2deca5..1391325 100644 --- a/launch/local_planner.launch +++ b/launch/local_planner.launch @@ -5,6 +5,7 @@ + @@ -22,6 +23,7 @@ + diff --git a/src/point_follow_planner.cpp b/src/point_follow_planner.cpp index d43936f..5aad299 100644 --- a/src/point_follow_planner.cpp +++ b/src/point_follow_planner.cpp @@ -25,6 +25,7 @@ PointFollowPlanner::PointFollowPlanner(void) private_nh_.param("angle_resolution", angle_resolution_, {0.2}); private_nh_.param("predict_time", predict_time_, {3.0}); private_nh_.param("dt", dt_, {0.1}); + private_nh_.param("sleep_time_after_finish", sleep_time_after_finish_, {0.5}); private_nh_.param("angle_to_goal_th", angle_to_goal_th_, {0.26}); private_nh_.param("dist_to_goal_th", dist_to_goal_th_, {0.3}); private_nh_.param("turn_direction_th", turn_direction_th_, {0.1}); @@ -48,6 +49,7 @@ PointFollowPlanner::PointFollowPlanner(void) ROS_INFO_STREAM("angle_resolution: " << angle_resolution_); ROS_INFO_STREAM("predict_time: " << predict_time_); ROS_INFO_STREAM("dt: " << dt_); + ROS_INFO_STREAM("sleep_time_after_finish: " << sleep_time_after_finish_); ROS_INFO_STREAM("angle_to_goal_th: " << angle_to_goal_th_); ROS_INFO_STREAM("dist_to_goal_th: " << dist_to_goal_th_); ROS_INFO_STREAM("turn_direction_th: " << turn_direction_th_); @@ -440,7 +442,7 @@ geometry_msgs::Twist PointFollowPlanner::calc_cmd_vel() } else { - has_finished.data = true; + has_finished_.data = true; has_reached_ = false; } generate_trajectory(best_traj, cmd_vel.linear.x, cmd_vel.angular.z); @@ -505,12 +507,14 @@ void PointFollowPlanner::process() if (can_move()) cmd_vel = calc_cmd_vel(); cmd_vel_pub_.publish(cmd_vel); - finish_flag_pub_.publish(has_finished); + finish_flag_pub_.publish(has_finished_); + if (has_finished_.data) + ros::Duration(sleep_time_after_finish_).sleep(); odom_updated_ = false; local_map_updated_ = false; is_behind_obj_ = false; - has_finished.data = false; + has_finished_.data = false; ros::spinOnce(); loop_rate.sleep();