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();