diff --git a/README.md b/README.md
index ff101bf..5c2c1af 100644
--- a/README.md
+++ b/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.
diff --git a/explore/config/params.yaml b/explore/config/params.yaml
index f51c5b6..04dcc64 100644
--- a/explore/config/params.yaml
+++ b/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
diff --git a/explore/include/explore/explore.h b/explore/include/explore/explore.h
index 30f96a3..f677b62 100644
--- a/explore/include/explore/explore.h
+++ b/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
 
diff --git a/explore/src/explore.cpp b/explore/src/explore.cpp
index 9398949..836377e 100644
--- a/explore/src/explore.cpp
+++ b/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()