Fix bug when resuming/stopping the exploration (#23)

* Fix bug when resuming/stopping the exploration and add toy test

* Add to actions explore_lite test

.github/workflows/build_test.yaml

@@ -15,8 +15,18 @@ jobs:
           package-name: multirobot_map_merge
           target-ros2-distro: galactic
           skip-tests: true
-      - name: Run gtests manually
+      - name: Run gtests manually multirobot_map_merge
         run: |
           source /opt/ros/galactic/setup.sh && source ros_ws/install/setup.sh
           cd ros_ws/build/multirobot_map_merge
           ./test_merging_pipeline
+      - uses: ros-tooling/action-ros-ci@v0.2
+        with:
+          package-name: explore_lite
+          target-ros2-distro: galactic
+          skip-tests: true
+      - name: Run gtests manually explore_lite
+        run: |
+          source /opt/ros/galactic/setup.sh && source ros_ws/install/setup.sh
+          cd ros_ws/build/explore_lite
+          ./test_explore

explore/CMakeLists.txt

@@ -88,7 +88,7 @@ target_include_directories(explore PUBLIC
   $<INSTALL_INTERFACE:include>)
 
 
-target_link_libraries(explore)
+target_link_libraries(explore ${rclcpp_LIBRARIES})
 
 ament_target_dependencies(explore ${DEPENDENCIES})
 
@@ -97,6 +97,25 @@ install(TARGETS explore
 
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
 
+#############
+## Testing ##
+#############
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  set(ament_cmake_copyright_FOUND TRUE)
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+
+  find_package(ament_cmake_gtest REQUIRED)
+
+  ament_add_gtest(test_explore test/test_explore.cpp)
+  target_link_libraries(test_explore ${catkin_LIBRARIES})
+  ament_target_dependencies(test_explore ${DEPENDENCIES})
+
+  
+endif()
+
 
 ament_export_include_directories(include)
 ament_package()

explore/include/explore/explore.h

@@ -139,6 +139,7 @@ private:
   bool visualize_;
   bool return_to_init_;
   std::string robot_base_frame_;
+  bool resuming_ = false;
 };
 }  // namespace explore
 

explore/src/explore.cpp

@@ -40,6 +40,15 @@
 
 #include <thread>
 
+inline static bool same_point(const geometry_msgs::msg::Point& one,
+                              const geometry_msgs::msg::Point& two)
+{
+  double dx = one.x - two.x;
+  double dy = one.y - two.y;
+  double dist = sqrt(dx * dx + dy * dy);
+  return dist < 0.01;
+}
+
 namespace explore
 {
 Explore::Explore()
@@ -189,7 +198,7 @@ void Explore::visualizeFrontiers(
   for (auto& frontier : frontiers) {
     m.type = visualization_msgs::msg::Marker::POINTS;
     m.id = int(id);
-    m.pose.position = {};
+    // m.pose.position = {}; // compile warning
     m.scale.x = 0.1;
     m.scale.y = 0.1;
     m.scale.z = 0.1;
@@ -263,7 +272,7 @@ void Explore::makePlan()
   geometry_msgs::msg::Point target_position = frontier->centroid;
 
   // time out if we are not making any progress
-  bool same_goal = prev_goal_ == target_position;
+  bool same_goal = same_point(prev_goal_, target_position);
 
   prev_goal_ = target_position;
   if (!same_goal || prev_distance_ > frontier->min_distance) {
@@ -272,15 +281,19 @@ void Explore::makePlan()
     prev_distance_ = frontier->min_distance;
   }
   // black list if we've made no progress for a long time
-  if (this->now() - last_progress_ >
-      tf2::durationFromSec(progress_timeout_)) {  // TODO: is progress_timeout_
-                                                  // in seconds?
+  if ((this->now() - last_progress_ >
+      tf2::durationFromSec(progress_timeout_)) && !resuming_) {
     frontier_blacklist_.push_back(target_position);
     RCLCPP_DEBUG(logger_, "Adding current goal to black list");
     makePlan();
     return;
   }
 
+  // ensure only first call of makePlan was set resuming to true
+  if (resuming_) {
+    resuming_ = false;
+  }
+
   // we don't need to do anything if we still pursuing the same goal
   if (same_goal) {
     return;
@@ -356,7 +369,8 @@ void Explore::reachedGoal(const NavigationGoalHandle::WrappedResult& result,
       return;
     case rclcpp_action::ResultCode::CANCELED:
       RCLCPP_DEBUG(logger_, "Goal was canceled");
-      break;
+      // If goal canceled might be because exploration stopped from topic. Don't make new plan.
+      return;
     default:
       RCLCPP_WARN(logger_, "Unknown result code from move base nav2");
       break;
@@ -392,6 +406,7 @@ void Explore::stop(bool finished_exploring)
 
 void Explore::resume()
 {
+  resuming_ = true;
   RCLCPP_INFO(logger_, "Exploration resuming.");
   // Reactivate the timer
   exploring_timer_->reset();

explore/test/test_explore.cpp

@@ -0,0 +1,77 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2022, Carlos Alvarez.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Carlos Alvarez nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ *********************************************************************/
+
+#include <gtest/gtest.h>
+#include <geometry_msgs/msg/point.hpp>
+#include <cmath>
+
+#define private public
+
+inline static bool same_point(const geometry_msgs::msg::Point& one,
+                              const geometry_msgs::msg::Point& two)
+{
+  double dx = one.x - two.x;
+  double dy = one.y - two.y;
+  double dist = sqrt(dx * dx + dy * dy);
+  return dist < 0.01;
+}
+
+TEST(Explore, testSameGoal)
+{
+    geometry_msgs::msg::Point goal1;
+    geometry_msgs::msg::Point goal2;
+    // Populate the goal with known values
+    goal1.x = 1.0;
+    goal1.y = 2.0;
+    goal1.z = 3.0;
+
+    goal2.x = 0.0;
+    goal2.y = 0.0;
+    goal2.z = 0.0;
+    auto same_goal = same_point(goal1, goal2);
+    EXPECT_FALSE(same_goal);
+    goal2.x = goal1.x;
+    goal2.y = goal1.y;
+    goal2.z = goal1.z;
+    same_goal = same_point(goal1, goal2);
+    EXPECT_TRUE(same_goal);
+}
+
+int main(int argc, char** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}