Feature:start stop (#18)

* Add topic for stop/resume exploration + docs

* Clang format

README.md

@@ -49,7 +49,7 @@ Then just run the nav2 stack with slam:
 ```
 export TURTLEBOT3_MODEL=waffle
 export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:/opt/ros/${ROS_DISTRO}/share/turtlebot3_gazebo/models
-ros2 launch nav2_bringup tb3_simulation_launch.py slam:=true
+ros2 launch nav2_bringup tb3_simulation_launch.py slam:=True
 ```
 
 And run this package with
@@ -57,7 +57,12 @@ And run this package with
 ros2 launch explore_lite explore.launch.py
 ```
 
-You can open a rviz2 and add the exploration frontiers marker to see the algorithm working and choose a frontier to explore.
+You can open rviz2 and add the exploration frontiers marker (topic is `explore/frontiers`) to see the algorithm working and the frontier chosen to explore.
+
+### Additional features
+#### Stop/Resume exploration
+By default the exploration node will start right away the frontier-based exploration algorithm. Alternatively, you can stop the exploration by publishing to a `False` to `explore/resume` topic. This will stop the exploration and the robot will stop moving. You can resume the exploration by publishing to `True` to `explore/resume`.
+
 
 #### TB3 troubleshooting (with foxy)
 If you have trouble with TB3 in simulation, as we did, add these extra steps for configuring it.

explore/config/params.yaml

@@ -4,7 +4,7 @@
     costmap_topic: map
     costmap_updates_topic: map_updates
     visualize: true
-    planner_frequency: 0.25
+    planner_frequency: 0.15
     progress_timeout: 30.0
     potential_scale: 3.0
     orientation_scale: 0.0

explore/config/params_costmap.yaml

@@ -4,7 +4,7 @@ explore_node:
     costmap_topic: /global_costmap/costmap
     costmap_updates_topic: /global_costmap/costmap_updates
     visualize: true
-    planner_frequency: 0.25
+    planner_frequency: 0.15
     progress_timeout: 30.0
     potential_scale: 3.0
     orientation_scale: 0.0

explore/include/explore/explore.h

@@ -49,6 +49,7 @@
 #include <memory>
 #include <mutex>
 #include <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/bool.hpp>
 #include <std_msgs/msg/color_rgba.hpp>
 #include <string>
 #include <vector>
@@ -80,6 +81,7 @@ public:
 
   void start();
   void stop();
+  void resume();
 
   using NavigationGoalHandle =
       rclcpp_action::ClientGoalHandle<nav2_msgs::action::NavigateToPose>;
@@ -118,6 +120,10 @@ private:
   rclcpp::TimerBase::SharedPtr exploring_timer_;
   // rclcpp::TimerBase::SharedPtr oneshot_;
 
+  rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr resume_subscription_;
+
+  void resumeCallback(const std_msgs::msg::Bool::SharedPtr msg);
+
   std::vector<geometry_msgs::msg::Point> frontier_blacklist_;
   geometry_msgs::msg::Point prev_goal_;
   double prev_distance_;

explore/src/costmap_client.cpp

@@ -36,11 +36,11 @@
  *********************************************************************/
 
 #include <explore/costmap_client.h>
+#include <unistd.h>
 
 #include <functional>
 #include <mutex>
 #include <string>
-#include <unistd.h>
 
 namespace explore
 {

explore/src/explore.cpp

@@ -69,7 +69,8 @@ Explore::Explore()
   this->get_parameter("min_frontier_size", min_frontier_size);
   progress_timeout_ = timeout;
   move_base_client_ =
-      rclcpp_action::create_client<nav2_msgs::action::NavigateToPose>(this,ACTION_NAME);
+      rclcpp_action::create_client<nav2_msgs::action::NavigateToPose>(
+          this, ACTION_NAME);
 
   search_ = frontier_exploration::FrontierSearch(costmap_client_.getCostmap(),
                                                  potential_scale_, gain_scale_,
@@ -77,11 +78,17 @@ Explore::Explore()
 
   if (visualize_) {
     marker_array_publisher_ =
-        this->create_publisher<visualization_msgs::msg::MarkerArray>("frontier"
+        this->create_publisher<visualization_msgs::msg::MarkerArray>("explore/"
+                                                                     "frontier"
                                                                      "s",
                                                                      10);
   }
 
+  // Subscription to resume or stop exploration
+  resume_subscription_ = this->create_subscription<std_msgs::msg::Bool>(
+      "explore/resume", 10,
+      std::bind(&Explore::resumeCallback, this, std::placeholders::_1));
+
   RCLCPP_INFO(logger_, "Waiting to connect to move_base nav2 server");
   move_base_client_->wait_for_action_server();
   RCLCPP_INFO(logger_, "Connected to move_base nav2 server");
@@ -89,6 +96,8 @@ Explore::Explore()
   exploring_timer_ = this->create_wall_timer(
       std::chrono::milliseconds((uint16_t)(1000.0 / planner_frequency_)),
       [this]() { makePlan(); });
+  // Start exploration right away
+  exploring_timer_->execute_callback();
 }
 
 Explore::~Explore()
@@ -96,6 +105,15 @@ Explore::~Explore()
   stop();
 }
 
+void Explore::resumeCallback(const std_msgs::msg::Bool::SharedPtr msg)
+{
+  if (msg->data) {
+    resume();
+  } else {
+    stop();
+  }
+}
+
 void Explore::visualizeFrontiers(
     const std::vector<frontier_exploration::Frontier>& frontiers)
 {
@@ -132,13 +150,14 @@ void Explore::visualizeFrontiers(
   m.color.a = 255;
   // lives forever
 #ifdef ELOQUENT
-  m.lifetime = rclcpp::Duration(0); // deprecated in galactic warning
+  m.lifetime = rclcpp::Duration(0);  // deprecated in galactic warning
 #elif DASHING
-  m.lifetime = rclcpp::Duration(0); // deprecated in galactic warning
+  m.lifetime = rclcpp::Duration(0);  // deprecated in galactic warning
 #else
-  m.lifetime = rclcpp::Duration::from_seconds(0); // foxy onwards
+  m.lifetime = rclcpp::Duration::from_seconds(0);  // foxy onwards
 #endif
-  // m.lifetime = rclcpp::Duration::from_nanoseconds(0); // suggested in galactic
+  // m.lifetime = rclcpp::Duration::from_nanoseconds(0); // suggested in
+  // galactic
   m.frame_locked = true;
 
   // weighted frontiers are always sorted
@@ -199,6 +218,7 @@ void Explore::makePlan()
   }
 
   if (frontiers.empty()) {
+    RCLCPP_WARN(logger_, "No frontiers found, stopping.");
     stop();
     return;
   }
@@ -215,6 +235,7 @@ void Explore::makePlan()
                          return goalOnBlacklist(f.centroid);
                        });
   if (frontier == frontiers.end()) {
+    RCLCPP_WARN(logger_, "All frontiers traversed/tried out, stopping.");
     stop();
     return;
   }
@@ -295,13 +316,15 @@ void Explore::reachedGoal(const NavigationGoalHandle::WrappedResult& result,
       RCLCPP_DEBUG(logger_, "Goal was aborted");
       frontier_blacklist_.push_back(frontier_goal);
       RCLCPP_DEBUG(logger_, "Adding current goal to black list");
+      // If it was aborted probably because we've found another frontier goal,
+      // so just return and don't make plan again
       return;
     case rclcpp_action::ResultCode::CANCELED:
       RCLCPP_DEBUG(logger_, "Goal was canceled");
-      return;
+      break;
     default:
       RCLCPP_WARN(logger_, "Unknown result code from move base nav2");
-      return;
+      break;
   }
   // find new goal immediately regardless of planning frequency.
   // execute via timer to prevent dead lock in move_base_client (this is
@@ -311,9 +334,9 @@ void Explore::reachedGoal(const NavigationGoalHandle::WrappedResult& result,
   //     ros::Duration(0, 0), [this](const ros::TimerEvent&) { makePlan(); },
   //     true);
 
-  // TODO: Implement this with ros2 timers?
-  // Because of the async nature of ros2 calls I think this is not needed.
-  // makePlan();
+  // Because of the 1-thread-executor nature of ros2 I think timer is not
+  // needed.
+  makePlan();
 }
 
 void Explore::start()
@@ -323,9 +346,18 @@ void Explore::start()
 
 void Explore::stop()
 {
+  RCLCPP_INFO(logger_, "Exploration stopped.");
   move_base_client_->async_cancel_all_goals();
   exploring_timer_->cancel();
-  RCLCPP_INFO(logger_, "Exploration stopped.");
+}
+
+void Explore::resume()
+{
+  RCLCPP_INFO(logger_, "Exploration resuming.");
+  // Reactivate the timer
+  exploring_timer_->reset();
+  // Resume immediately
+  exploring_timer_->execute_callback();
 }
 
 }  // namespace explore