nav2_waypoint_follower; Add an optional param to specify a sleep time…

… inbetween waypoints. Signed-off-by: jediofgever <fetulahatas1@gmail.com>
jediofgever · Sep 24, 2020 · 2a6d1b1 · 2a6d1b1
1 parent 182d3b3
commit 2a6d1b1
Show file tree

Hide file tree

Showing 4 changed files with 21 additions and 3 deletions.
diff --git a/doc/parameters/param_list.md b/doc/parameters/param_list.md
@@ -480,6 +480,7 @@ When `planner_plugins` parameter is not overridden, the following default plugin
 | ----------| --------| ------------|
 | stop_on_failure | true | Whether to fail action task if a single waypoint fails. If false, will continue to next waypoint. |
 | loop_rate | 20 | Rate to check for results from current navigation task |
+| sleep_time_inbetween_waypoints | 0 | Amount of time in milliseconds for robot to sleep/wait after each waypoint is reached. If zero, robot will directly continue to next waypoint. |
 
 # recoveries
 

diff --git a/nav2_bringup/bringup/params/nav2_params.yaml b/nav2_bringup/bringup/params/nav2_params.yaml
@@ -280,3 +280,9 @@ recoveries_server:
 robot_state_publisher:
   ros__parameters:
     use_sim_time: True
+
+waypoint_follower: 
+  ros__parameters:
+    loop_rate: 20
+    stop_on_failure: false   
+    sleep_time_inbetween_waypoints: 0 
diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp
@@ -119,6 +119,7 @@ class WaypointFollower : public nav2_util::LifecycleNode
   bool stop_on_failure_;
   ActionStatus current_goal_status_;
   int loop_rate_;
+  int sleep_time_inbetween_waypoints_;
   std::vector<int> failed_ids_;
 };
 

diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp
@@ -31,6 +31,7 @@ WaypointFollower::WaypointFollower()
 
   declare_parameter("stop_on_failure", true);
   declare_parameter("loop_rate", 20);
+  declare_parameter("sleep_time_inbetween_waypoints", 0);
 }
 
 WaypointFollower::~WaypointFollower()
@@ -44,6 +45,7 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & /*state*/)
 
   stop_on_failure_ = get_parameter("stop_on_failure").as_bool();
   loop_rate_ = get_parameter("loop_rate").as_int();
+  sleep_time_inbetween_waypoints_ = get_parameter("sleep_time_inbetween_waypoints").as_int();
 
   std::vector<std::string> new_args = rclcpp::NodeOptions().arguments();
   new_args.push_back("--ros-args");
@@ -192,9 +194,17 @@ WaypointFollower::followWaypoints()
           " moving to next.", goal_index);
       }
     } else if (current_goal_status_ == ActionStatus::SUCCEEDED) {
-      RCLCPP_INFO(
-        get_logger(), "Succeeded processing waypoint %i, "
-        "moving to next.", goal_index);
+      if (sleep_time_inbetween_waypoints_) {
+        RCLCPP_INFO(
+          get_logger(), "Succeeded processing waypoint %i, "
+          "sleeping for %i milliseconds before moving to next.", goal_index,
+          sleep_time_inbetween_waypoints_);
+        rclcpp::sleep_for(std::chrono::milliseconds(sleep_time_inbetween_waypoints_));
+      } else {
+        RCLCPP_INFO(
+          get_logger(), "Succeeded processing waypoint %i, "
+          "moving to next.", goal_index);
+      }
     }
 
     if (current_goal_status_ != ActionStatus::PROCESSING &&