Move dwb goal/progress checker plugins to nav2_controller

nav2_controller/plugins/test/CMakeLists.txt

@@ -1,2 +1,4 @@
+ament_add_gtest(pctest progress_checker.cpp)
+target_link_libraries(pctest simple_progress_checker)
 ament_add_gtest(gctest goal_checker.cpp)
 target_link_libraries(gctest simple_goal_checker stopped_goal_checker)

nav2_controller/plugins/test/progress_checker.cpp

@@ -0,0 +1,135 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2017, Locus Robotics
+ *  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 copyright holder 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 HOLDER 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 <memory>
+#include <string>
+
+#include "gtest/gtest.h"
+#include "nav2_controller/plugins/simple_progress_checker.hpp"
+#include "nav_2d_utils/conversions.hpp"
+#include "nav2_util/lifecycle_node.hpp"
+
+using nav2_controller::SimpleProgressChecker;
+
+class TestLifecycleNode : public nav2_util::LifecycleNode
+{
+public:
+  explicit TestLifecycleNode(const std::string & name)
+  : nav2_util::LifecycleNode(name)
+  {
+  }
+
+  nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State &)
+  {
+    return nav2_util::CallbackReturn::SUCCESS;
+  }
+
+  nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State &)
+  {
+    return nav2_util::CallbackReturn::SUCCESS;
+  }
+
+  nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &)
+  {
+    return nav2_util::CallbackReturn::SUCCESS;
+  }
+
+  nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State &)
+  {
+    return nav2_util::CallbackReturn::SUCCESS;
+  }
+
+  nav2_util::CallbackReturn onShutdown(const rclcpp_lifecycle::State &)
+  {
+    return nav2_util::CallbackReturn::SUCCESS;
+  }
+
+  nav2_util::CallbackReturn onError(const rclcpp_lifecycle::State &)
+  {
+    return nav2_util::CallbackReturn::SUCCESS;
+  }
+};
+
+void checkMacro(
+  nav2_core::ProgressChecker & pc,
+  double x0, double y0,
+  double x1, double y1,
+  int delay,
+  bool expected_result)
+{
+  pc.reset();
+  geometry_msgs::msg::PoseStamped pose0, pose1;
+  pose0.pose.position.x = x0;
+  pose0.pose.position.y = y0;
+  pose1.pose.position.x = x1;
+  pose1.pose.position.y = y1;
+  EXPECT_TRUE(pc.check(pose0));
+  rclcpp::sleep_for(std::chrono::seconds(delay));
+  if (expected_result) {
+    EXPECT_TRUE(pc.check(pose1));
+  } else {
+    EXPECT_FALSE(pc.check(pose1));
+  }
+}
+
+TEST(SimpleProgressChecker, progress_checker_reset)
+{
+  auto x = std::make_shared<TestLifecycleNode>("progress_checker");
+
+  nav2_core::ProgressChecker * pc = new SimpleProgressChecker;
+  pc->reset();
+  delete pc;
+  EXPECT_TRUE(true);
+}
+
+TEST(SimpleProgressChecker, unit_tests)
+{
+  auto x = std::make_shared<TestLifecycleNode>("progress_checker");
+
+  SimpleProgressChecker pc;
+  pc.initialize(x, "nav2_controller");
+  checkMacro(pc, 0, 0, 0, 0, 1, true);
+  checkMacro(pc, 0, 0, 1, 0, 1, true);
+  checkMacro(pc, 0, 0, 0, 1, 1, true);
+  checkMacro(pc, 0, 0, 1, 0, 11, true);
+  checkMacro(pc, 0, 0, 0, 1, 11, true);
+  checkMacro(pc, 0, 0, 0, 0, 11, false);
+}
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

nav2_controller/src/nav2_controller.cpp

@@ -68,21 +68,29 @@ ControllerServer::~ControllerServer()
 nav2_util::CallbackReturn
 ControllerServer::on_configure(const rclcpp_lifecycle::State & state)
 {
+  auto node = shared_from_this();
+
   RCLCPP_INFO(get_logger(), "Configuring controller interface");
 
   get_parameter("progress_checker_plugin", progress_checker_id_);
   if (progress_checker_id_ == default_progress_checker_id_) {
-    declare_parameter(default_progress_checker_id_ + ".plugin", default_progress_checker_type_);
+    nav2_util::declare_parameter_if_not_declared(
+      node, default_progress_checker_id_ + ".plugin",
+      rclcpp::ParameterValue(default_progress_checker_type_));
   }
   get_parameter("goal_checker_plugin", goal_checker_id_);
   if (goal_checker_id_ == default_goal_checker_id_) {
-    declare_parameter(default_goal_checker_id_ + ".plugin", default_goal_checker_type_);
+    nav2_util::declare_parameter_if_not_declared(
+      node, default_goal_checker_id_ + ".plugin",
+      rclcpp::ParameterValue(default_goal_checker_type_));
   }
 
   get_parameter("controller_plugins", controller_ids_);
   if (controller_ids_ == default_ids_) {
     for (size_t i = 0; i < default_ids_.size(); ++i) {
-      declare_parameter(default_ids_[i] + ".plugin", default_types_[i]);
+      nav2_util::declare_parameter_if_not_declared(
+        node, default_ids_[i] + ".plugin",
+        rclcpp::ParameterValue(default_types_[i]));
     }
   }
   controller_types_.resize(controller_ids_.size());
@@ -95,8 +103,6 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state)
 
   costmap_ros_->on_configure(state);
 
-  auto node = shared_from_this();
-
   try {
     progress_checker_type_ = nav2_util::get_plugin_type_param(node, progress_checker_id_);
     progress_checker_ = progress_checker_loader_.createUniqueInstance(progress_checker_type_);

nav2_core/include/nav2_core/progress_checker.hpp

@@ -35,6 +35,9 @@ class ProgressChecker
 {
 public:
   typedef std::shared_ptr<nav2_core::ProgressChecker> Ptr;
+
+  virtual ~ProgressChecker() {}
+
   /**
    * @brief Initialize parameters for ProgressChecker
    * @param node Node pointer