Feature/return initial pose (#20)

* Add return to initial pose param and logic

* Format clang

* Add readme feature

* Fix log

* Fix return statement

* Deelete commented block

README.md

@@ -63,6 +63,8 @@ You can open rviz2 and add the exploration frontiers marker (topic is `explore/f
 #### 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`.
 
+#### Returning to initial pose
+The robot will return to its initial pose after exploration if you want by defining the parameter `return_to_init` to `True` when launching the node.
 
 #### 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

@@ -1,6 +1,7 @@
 /**:
   ros__parameters:
     robot_base_frame: base_link
+    return_to_init: true
     costmap_topic: map
     costmap_updates_topic: map_updates
     visualize: true

explore/include/explore/explore.h

@@ -80,7 +80,7 @@ public:
   ~Explore();
 
   void start();
-  void stop();
+  void stop(bool finished_exploring = false);
   void resume();
 
   using NavigationGoalHandle =
@@ -121,7 +121,6 @@ private:
   // 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_;
@@ -130,11 +129,16 @@ private:
   rclcpp::Time last_progress_;
   size_t last_markers_count_;
 
+  geometry_msgs::msg::Pose initial_pose_;
+  void returnToInitialPose(void);
+
   // parameters
   double planner_frequency_;
   double potential_scale_, orientation_scale_, gain_scale_;
   double progress_timeout_;
   bool visualize_;
+  bool return_to_init_;
+  std::string robot_base_frame_;
 };
 }  // namespace explore
 

explore/src/explore.cpp

@@ -59,6 +59,7 @@ Explore::Explore()
   this->declare_parameter<float>("orientation_scale", 0.0);
   this->declare_parameter<float>("gain_scale", 1.0);
   this->declare_parameter<float>("min_frontier_size", 0.5);
+  this->declare_parameter<bool>("return_to_init", false);
 
   this->get_parameter("planner_frequency", planner_frequency_);
   this->get_parameter("progress_timeout", timeout);
@@ -67,6 +68,9 @@ Explore::Explore()
   this->get_parameter("orientation_scale", orientation_scale_);
   this->get_parameter("gain_scale", gain_scale_);
   this->get_parameter("min_frontier_size", min_frontier_size);
+  this->get_parameter("return_to_init", return_to_init_);
+  this->get_parameter("robot_base_frame", robot_base_frame_);
+
   progress_timeout_ = timeout;
   move_base_client_ =
       rclcpp_action::create_client<nav2_msgs::action::NavigateToPose>(
@@ -93,6 +97,23 @@ Explore::Explore()
   move_base_client_->wait_for_action_server();
   RCLCPP_INFO(logger_, "Connected to move_base nav2 server");
 
+  if (return_to_init_) {
+    RCLCPP_INFO(logger_, "Getting initial pose of the robot");
+    geometry_msgs::msg::TransformStamped transformStamped;
+    std::string map_frame = costmap_client_.getGlobalFrameID();
+    try {
+      transformStamped = tf_buffer_.lookupTransform(
+          map_frame, robot_base_frame_, tf2::TimePointZero);
+      initial_pose_.position.x = transformStamped.transform.translation.x;
+      initial_pose_.position.y = transformStamped.transform.translation.y;
+      initial_pose_.orientation = transformStamped.transform.rotation;
+    } catch (tf2::TransformException& ex) {
+      RCLCPP_ERROR(logger_, "Couldn't find transform from %s to %s: %s",
+                   map_frame.c_str(), robot_base_frame_.c_str(), ex.what());
+      return_to_init_ = false;
+    }
+  }
+
   exploring_timer_ = this->create_wall_timer(
       std::chrono::milliseconds((uint16_t)(1000.0 / planner_frequency_)),
       [this]() { makePlan(); });
@@ -219,7 +240,7 @@ void Explore::makePlan()
 
   if (frontiers.empty()) {
     RCLCPP_WARN(logger_, "No frontiers found, stopping.");
-    stop();
+    stop(true);
     return;
   }
 
@@ -236,7 +257,7 @@ void Explore::makePlan()
                        });
   if (frontier == frontiers.end()) {
     RCLCPP_WARN(logger_, "All frontiers traversed/tried out, stopping.");
-    stop();
+    stop(true);
     return;
   }
   geometry_msgs::msg::Point target_position = frontier->centroid;
@@ -288,6 +309,20 @@ void Explore::makePlan()
   move_base_client_->async_send_goal(goal, send_goal_options);
 }
 
+void Explore::returnToInitialPose()
+{
+  RCLCPP_INFO(logger_, "Returning to initial pose.");
+  auto goal = nav2_msgs::action::NavigateToPose::Goal();
+  goal.pose.pose.position = initial_pose_.position;
+  goal.pose.pose.orientation = initial_pose_.orientation;
+  goal.pose.header.frame_id = costmap_client_.getGlobalFrameID();
+  goal.pose.header.stamp = this->now();
+
+  auto send_goal_options =
+      rclcpp_action::Client<nav2_msgs::action::NavigateToPose>::SendGoalOptions();
+  move_base_client_->async_send_goal(goal, send_goal_options);
+}
+
 bool Explore::goalOnBlacklist(const geometry_msgs::msg::Point& goal)
 {
   constexpr static size_t tolerace = 5;
@@ -344,11 +379,15 @@ void Explore::start()
   RCLCPP_INFO(logger_, "Exploration started.");
 }
 
-void Explore::stop()
+void Explore::stop(bool finished_exploring)
 {
   RCLCPP_INFO(logger_, "Exploration stopped.");
   move_base_client_->async_cancel_all_goals();
   exploring_timer_->cancel();
+
+  if (return_to_init_ && finished_exploring) {
+    returnToInitialPose();
+  }
 }
 
 void Explore::resume()