From 5582e7dd810e66c720f318f9c0fc0c89a6737d7c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Andr=C3=A9s=20=C3=81lvarez=20Restrepo?= Date: Mon, 4 Jul 2022 19:07:52 -0500 Subject: [PATCH] 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 | 12 ++++- explore/CMakeLists.txt | 21 ++++++++- explore/include/explore/explore.h | 1 + explore/src/explore.cpp | 27 ++++++++--- explore/test/test_explore.cpp | 77 +++++++++++++++++++++++++++++++ 5 files changed, 130 insertions(+), 8 deletions(-) create mode 100644 explore/test/test_explore.cpp diff --git a/.github/workflows/build_test.yaml b/.github/workflows/build_test.yaml index 05487f4..2238f9c 100644 --- a/.github/workflows/build_test.yaml +++ b/.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 diff --git a/explore/CMakeLists.txt b/explore/CMakeLists.txt index ccdd448..1ebcfc6 100644 --- a/explore/CMakeLists.txt +++ b/explore/CMakeLists.txt @@ -88,7 +88,7 @@ target_include_directories(explore PUBLIC $) -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() \ No newline at end of file diff --git a/explore/include/explore/explore.h b/explore/include/explore/explore.h index f677b62..6c6adbc 100644 --- a/explore/include/explore/explore.h +++ b/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 diff --git a/explore/src/explore.cpp b/explore/src/explore.cpp index 836377e..acdbc91 100644 --- a/explore/src/explore.cpp +++ b/explore/src/explore.cpp @@ -40,6 +40,15 @@ #include +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(); diff --git a/explore/test/test_explore.cpp b/explore/test/test_explore.cpp new file mode 100644 index 0000000..2fe0913 --- /dev/null +++ b/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 +#include +#include + +#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(); +}