Add param for finish (#40)

amslabtech · Oct 30, 2023 · 5d29e9e · 5d29e9e
1 parent ca003cd
commit 5d29e9e
Show file tree

Hide file tree

Showing 3 changed files with 11 additions and 4 deletions.
diff --git 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
@@ -5,6 +5,7 @@
     <!-- param -->
     <arg name="hz" default="20"/>
     <arg name="subscribe_count_th" default="3"/>
+    <arg name="sleep_time_after_finish" default="0.5"/>
     <arg name="planner_param" default="$(find point_follow_planner)/config/planner_param.yaml"/>
     <arg name="robot_param" default="$(find point_follow_planner)/config/robot_param.yaml"/>
     <!-- topic name -->
@@ -22,6 +23,7 @@
         <rosparam command="load" file="$(arg robot_param)"/>
         <param name="hz" value="$(arg hz)"/>
         <param name="subscribe_count_th" value="$(arg subscribe_count_th)"/>
+        <param name="sleep_time_after_finish" value="$(arg sleep_time_after_finish)"/>
         <!-- topic name -->
         <remap from="/cmd_vel" to="$(arg cmd_vel)"/>
         <remap from="/footprint" to="$(arg footprint)"/>

diff --git a/src/point_follow_planner.cpp b/src/point_follow_planner.cpp
@@ -25,6 +25,7 @@ PointFollowPlanner::PointFollowPlanner(void)
     private_nh_.param<double>("angle_resolution", angle_resolution_, {0.2});
     private_nh_.param<double>("predict_time", predict_time_, {3.0});
     private_nh_.param<double>("dt", dt_, {0.1});
+    private_nh_.param<double>("sleep_time_after_finish", sleep_time_after_finish_, {0.5});
     private_nh_.param<double>("angle_to_goal_th", angle_to_goal_th_, {0.26});
     private_nh_.param<double>("dist_to_goal_th", dist_to_goal_th_, {0.3});
     private_nh_.param<double>("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();