Move dwb goal/progress checker plugins to nav2_controller

nav2_controller/CMakeLists.txt

@@ -4,6 +4,7 @@ project(nav2_controller)
 find_package(ament_cmake REQUIRED)
 find_package(nav2_core REQUIRED)
 find_package(nav2_common REQUIRED)
+find_package(angles REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_action REQUIRED)
 find_package(std_msgs REQUIRED)
@@ -29,12 +30,12 @@ set(library_name ${executable_name}_core)
 
 add_library(${library_name}
   src/nav2_controller.cpp
-  src/progress_checker.cpp
 )
 
 target_compile_definitions(${library_name} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
 
 set(dependencies
+  angles
   rclcpp
   rclcpp_action
   std_msgs
@@ -46,20 +47,46 @@ set(dependencies
   pluginlib
 )
 
+add_library(simple_progress_checker SHARED plugins/simple_progress_checker.cpp)
+ament_target_dependencies(simple_progress_checker ${dependencies})
+# prevent pluginlib from using boost
+target_compile_definitions(simple_progress_checker PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
+
+add_library(simple_goal_checker SHARED plugins/simple_goal_checker.cpp)
+ament_target_dependencies(simple_goal_checker ${dependencies})
+# prevent pluginlib from using boost
+target_compile_definitions(simple_goal_checker PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
+
+add_library(stopped_goal_checker SHARED plugins/stopped_goal_checker.cpp)
+target_link_libraries(stopped_goal_checker simple_goal_checker)
+ament_target_dependencies(stopped_goal_checker ${dependencies})
+# prevent pluginlib from using boost
+target_compile_definitions(stopped_goal_checker PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
+
 ament_target_dependencies(${library_name}
   ${dependencies}
 )
 
 # prevent pluginlib from using boost
 target_compile_definitions(${library_name} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
 
+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)
+  ament_lint_auto_find_test_dependencies()
+
+  find_package(ament_cmake_gtest REQUIRED)
+  add_subdirectory(plugins/test)
+endif()
+
 ament_target_dependencies(${executable_name}
   ${dependencies}
 )
 
 target_link_libraries(${executable_name} ${library_name})
 
-install(TARGETS ${library_name}
+install(TARGETS simple_progress_checker simple_goal_checker stopped_goal_checker ${library_name}
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin
@@ -72,13 +99,20 @@ install(TARGETS ${executable_name}
 install(DIRECTORY include/
   DESTINATION include/
 )
+install(FILES plugins.xml
+        DESTINATION share/${PROJECT_NAME}
+)
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()
 endif()
 
 ament_export_include_directories(include)
+ament_export_libraries(simple_progress_checker)
+ament_export_libraries(simple_goal_checker)
+ament_export_libraries(stopped_goal_checker)
 ament_export_libraries(${library_name})
+pluginlib_export_plugin_description_file(nav2_core plugins.xml)
 
 ament_package()

nav2_controller/include/nav2_controller/nav2_controller.hpp

@@ -22,6 +22,8 @@
 #include <vector>
 
 #include "nav2_core/controller.hpp"
+#include "nav2_core/progress_checker.hpp"
+#include "nav2_core/goal_checker.hpp"
 #include "nav2_costmap_2d/costmap_2d_ros.hpp"
 #include "tf2_ros/transform_listener.h"
 #include "nav2_msgs/action/follow_path.hpp"
@@ -202,6 +204,11 @@ class ControllerServer : public nav2_util::LifecycleNode
   rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::Twist>::SharedPtr vel_publisher_;
 
   // Controller Plugins
+  pluginlib::ClassLoader<nav2_core::ProgressChecker> progress_checker_loader_;
+  nav2_core::ProgressChecker::Ptr progress_checker_;
+  pluginlib::ClassLoader<nav2_core::GoalChecker> goal_checker_loader_;
+  nav2_core::GoalChecker::Ptr goal_checker_;
+
   pluginlib::ClassLoader<nav2_core::Controller> lp_loader_;
   ControllerMap controllers_;
   std::vector<std::string> default_ids_;
@@ -210,8 +217,6 @@ class ControllerServer : public nav2_util::LifecycleNode
   std::vector<std::string> controller_types_;
   std::string controller_ids_concat_, current_controller_;
 
-  std::unique_ptr<ProgressChecker> progress_checker_;
-
   double controller_frequency_;
   double min_x_velocity_threshold_;
   double min_y_velocity_threshold_;

nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp

@@ -0,0 +1,77 @@
+/*
+ * 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.
+ */
+
+#ifndef NAV2_CONTROLLER__PLUGINS__SIMPLE_GOAL_CHECKER_HPP_
+#define NAV2_CONTROLLER__PLUGINS__SIMPLE_GOAL_CHECKER_HPP_
+
+#include <memory>
+#include <string>
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
+#include "nav2_core/goal_checker.hpp"
+
+namespace nav2_controller
+{
+
+/**
+ * @class SimpleGoalChecker
+ * @brief Goal Checker plugin that only checks the position difference
+ *
+ * This class can be stateful if the stateful parameter is set to true (which it is by default).
+ * This means that the goal checker will not check if the xy position matches again once it is found to be true.
+ */
+class SimpleGoalChecker : public nav2_core::GoalChecker
+{
+public:
+  SimpleGoalChecker();
+  // Standard GoalChecker Interface
+  void initialize(
+    const rclcpp_lifecycle::LifecycleNode::SharedPtr & nh,
+    const std::string & plugin_name) override;
+  void reset() override;
+  bool isGoalReached(
+    const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose,
+    const geometry_msgs::msg::Twist & velocity) override;
+
+protected:
+  double xy_goal_tolerance_, yaw_goal_tolerance_;
+  bool stateful_, check_xy_;
+  // Cached squared xy_goal_tolerance_
+  double xy_goal_tolerance_sq_;
+};
+
+}  // namespace nav2_controller
+
+#endif  // NAV2_CONTROLLER__PLUGINS__SIMPLE_GOAL_CHECKER_HPP_

nav2_controller/include/nav2_controller/progress_checker.hpp → nav2_controller/include/nav2_controller/plugins/simple_progress_checker.hpp

@@ -12,40 +12,32 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef NAV2_CONTROLLER__PROGRESS_CHECKER_HPP_
-#define NAV2_CONTROLLER__PROGRESS_CHECKER_HPP_
+#ifndef NAV2_CONTROLLER__PLUGINS__SIMPLE_PROGRESS_CHECKER_HPP_
+#define NAV2_CONTROLLER__PLUGINS__SIMPLE_PROGRESS_CHECKER_HPP_
 
+#include <string>
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp_lifecycle/lifecycle_node.hpp"
+#include "nav2_core/progress_checker.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "geometry_msgs/msg/pose2_d.hpp"
 
 namespace nav2_controller
 {
 /**
- * @class nav2_controller::ProgressChecker
- * @brief This class is used to check the position of the robot to make sure
- * that it is actually progressing towards a goal.
- */
-class ProgressChecker
+* @class SimpleProgressChecker
+* @brief This plugin is used to check the position of the robot to make sure
+* that it is actually progressing towards a goal.
+*/
+
+class SimpleProgressChecker : public nav2_core::ProgressChecker
 {
 public:
-  /**
-   * @brief Constructor of ProgressChecker
-   * @param node Node pointer
-   */
-  explicit ProgressChecker(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node);
-  /**
-   * @brief Checks if the robot has moved compare to previous
-   * pose
-   * @param current_pose Current pose of the robot
-   * @throw nav2_core::PlannerException when failed to make progress
-   */
-  void check(geometry_msgs::msg::PoseStamped & current_pose);
-  /**
-   * @brief Reset class state upon calling
-   */
-  void reset() {baseline_pose_set_ = false;}
+  void initialize(
+    const rclcpp_lifecycle::LifecycleNode::SharedPtr & node,
+    const std::string & plugin_name) override;
+  void check(geometry_msgs::msg::PoseStamped & current_pose) override;
+  void reset() override;
 
 protected:
   /**
@@ -72,4 +64,4 @@ class ProgressChecker
 };
 }  // namespace nav2_controller
 
-#endif  // NAV2_CONTROLLER__PROGRESS_CHECKER_HPP_
+#endif  // NAV2_CONTROLLER__PLUGINS__SIMPLE_PROGRESS_CHECKER_HPP_

nav2_dwb_controller/dwb_plugins/include/dwb_plugins/stopped_goal_checker.hpp → nav2_controller/include/nav2_controller/plugins/stopped_goal_checker.hpp

@@ -32,17 +32,17 @@
  *  POSSIBILITY OF SUCH DAMAGE.
  */
 
-#ifndef DWB_PLUGINS__STOPPED_GOAL_CHECKER_HPP_
-#define DWB_PLUGINS__STOPPED_GOAL_CHECKER_HPP_
+#ifndef NAV2_CONTROLLER__PLUGINS__STOPPED_GOAL_CHECKER_HPP_
+#define NAV2_CONTROLLER__PLUGINS__STOPPED_GOAL_CHECKER_HPP_
 
 #include <memory>
 #include <string>
 
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp_lifecycle/lifecycle_node.hpp"
-#include "dwb_plugins/simple_goal_checker.hpp"
+#include "nav2_controller/plugins/simple_goal_checker.hpp"
 
-namespace dwb_plugins
+namespace nav2_controller
 {
 
 /**
@@ -65,6 +65,6 @@ class StoppedGoalChecker : public SimpleGoalChecker
   double rot_stopped_velocity_, trans_stopped_velocity_;
 };
 
-}  // namespace dwb_plugins
+}  // namespace nav2_controller
 
-#endif  // DWB_PLUGINS__STOPPED_GOAL_CHECKER_HPP_
+#endif  // NAV2_CONTROLLER__PLUGINS__STOPPED_GOAL_CHECKER_HPP_

nav2_controller/package.xml

@@ -24,5 +24,6 @@
 
   <export>
     <build_type>ament_cmake</build_type>
+    <nav2_core plugin="${prefix}/plugins.xml" />
   </export>
 </package>

nav2_controller/plugins.xml

@@ -0,0 +1,17 @@
+<class_libraries>
+  <library path="simple_progress_checker">
+    <class type="nav2_controller::SimpleProgressChecker" base_class_type="nav2_core::ProgressChecker">
+      <description></description>
+    </class>
+  </library>
+  <library path="simple_goal_checker">
+    <class type="nav2_controller::SimpleGoalChecker" base_class_type="nav2_core::GoalChecker">
+      <description></description>
+    </class>
+  </library>
+  <library path="stopped_goal_checker">
+    <class type="nav2_controller::StoppedGoalChecker" base_class_type="nav2_core::GoalChecker">
+      <description></description>
+    </class>
+  </library>
+</class_libraries>