Move dwb goal/progress checker plugins to nav2_controller

doc/parameters/param_list.md

@@ -154,6 +154,8 @@ When `plugins` parameter is not overridden, the following default plugins are lo
 | Parameter | Default | Description |
 | ----------| --------| ------------|
 | controller_frequency | 20.0 | Frequency to run controller |
+| progress_checker_plugin | ["progress_checker"] | Check the progress of the robot |
+| goal_checker_plugin | ["goal_checker"] | Check if the goal has been reached |
 | controller_plugins | ["FollowPath"] | List of mapped names for controller plugins for processing requests and parameters |
 | min_x_velocity_threshold | 0.0001 | Minimum X velocity to use (m/s) |
 | min_y_velocity_threshold | 0.0001 | Minimum Y velocity to use (m/s) |
@@ -173,11 +175,36 @@ controller_server:
       plugin: "dwb_core::DWBLocalPlanner"
 ```
 
-When `controller_plugins` parameter is not overridden, the following default plugins are loaded:
+When `controller_plugins`\`progress_checker_plugin`\`goal_checker_plugin` parameters are not overridden, the following default plugins are loaded:
 
 | Namespace | Plugin |
 | ----------| --------|
 | "FollowPath" | "dwb_core::DWBLocalPlanner" |
+| "progress_checker" | "nav2_controller::SimpleProgressChecker" |
+| "goal_checker" | "nav2_controller::SimpleGoalChecker" |
+
+## simple_progress_checker plugin
+
+| Parameter | Default | Description |
+| ----------| --------| ------------|
+| `<nav2_controller plugin>`.required_movement_radius | 0.5 | Minimum distance to count as progress (m) |
+| `<nav2_controller plugin>`.movement_time_allowance | 10.0 | Maximum time allowence for progress to happen (s) |
+
+
+## simple_goal_checker plugin
+
+| Parameter | Default | Description |
+| ----------| --------| ------------|
+| `<nav2_controller plugin>`.xy_goal_tolerance | 0.25 | Tolerance to meet goal completion criteria (m) |
+| `<nav2_controller plugin>`.yaw_goal_tolerance | 0.25 | Tolerance to meet goal completion criteria (rad) |
+| `<nav2_controller plugin>`.stateful | true | Whether to check for XY position tolerance after rotating to goal orientation in case of minor localization changes |
+
+## stopped_goal_checker plugin
+
+| Parameter | Default | Description |
+| ----------| --------| ------------|
+| `<nav2_controller plugin>`.rot_stopped_velocity | 0.25 | Velocity below is considered to be stopped at tolerance met (rad/s) |
+| `<nav2_controller plugin>`.trans_stopped_velocity | 0.25 | Velocity below is considered to be stopped at tolerance met (m/s) |
 
 # dwb_controller
 
@@ -350,14 +377,6 @@ When `controller_plugins` parameter is not overridden, the following default plu
 | `<dwb plugin>`.lookahead_time | -1 | If > 0, amount of time to look forward for a collision for. |
 | `<dwb plugin>`.`<name>`.scale | 1.0 | Weight scale |
 
-## simple_goal_checker plugin
-
-| Parameter | Default | Description |
-| ----------| --------| ------------|
-| `<dwb plugin>`.xy_goal_tolerance | 0.25 | Tolerance to meet goal completion criteria (m) |
-| `<dwb plugin>`.yaw_goal_tolerance | 0.25 | Tolerance to meet goal completion criteria (rad) |
-| `<dwb plugin>`.stateful | true | Whether to check for XY position tolerance after rotating to goal orientation in case of minor localization changes |
-
 ## standard_traj_generator plugin
 
 | Parameter | Default | Description |
@@ -375,13 +394,6 @@ When `controller_plugins` parameter is not overridden, the following default plu
 | ----------| --------| ------------|
 | `<dwb plugin>`.sim_time | N/A | Time to simulate ahead by (s) |
 
-## stopped_goal_checker plugin
-
-| Parameter | Default | Description |
-| ----------| --------| ------------|
-| `<dwb plugin>`.rot_stopped_velocity | 0.25 | Velocity below is considered to be stopped at tolerance met (rad/s) |
-| `<dwb plugin>`.trans_stopped_velocity | 0.25 | Velocity below is considered to be stopped at tolerance met (m/s) |
-
 # lifecycle_manager
 
 | Parameter | Default | Description |

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
@@ -79,6 +106,10 @@ if(BUILD_TESTING)
 endif()
 
 ament_export_include_directories(include)
-ament_export_libraries(${library_name})
+ament_export_libraries(simple_progress_checker
+  simple_goal_checker
+  stopped_goal_checker
+  ${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"
@@ -201,6 +203,22 @@ class ControllerServer : public nav2_util::LifecycleNode
   std::unique_ptr<nav_2d_utils::OdomSubscriber> odom_sub_;
   rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::Twist>::SharedPtr vel_publisher_;
 
+  // Progress Checker Plugin
+  pluginlib::ClassLoader<nav2_core::ProgressChecker> progress_checker_loader_;
+  nav2_core::ProgressChecker::Ptr progress_checker_;
+  std::string default_progress_checker_id_;
+  std::string default_progress_checker_type_;
+  std::string progress_checker_id_;
+  std::string progress_checker_type_;
+
+  // Goal Checker Plugin
+  pluginlib::ClassLoader<nav2_core::GoalChecker> goal_checker_loader_;
+  nav2_core::GoalChecker::Ptr goal_checker_;
+  std::string default_goal_checker_id_;
+  std::string default_goal_checker_type_;
+  std::string goal_checker_id_;
+  std::string goal_checker_type_;
+
   // Controller Plugins
   pluginlib::ClassLoader<nav2_core::Controller> lp_loader_;
   ControllerMap controllers_;
@@ -210,8 +228,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;
+  bool 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

@@ -9,6 +9,7 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
   <build_depend>nav2_common</build_depend>
+  <depend>angles</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_action</depend>
   <depend>std_msgs</depend>
@@ -24,5 +25,6 @@
 
   <export>
     <build_type>ament_cmake</build_type>
+    <nav2_core plugin="${prefix}/plugins.xml" />
   </export>
 </package>