From d7979eeba2db0a18ff536ce67ad39c01261564a0 Mon Sep 17 00:00:00 2001 From: Siddarth Gore Date: Mon, 8 Jun 2020 21:24:47 +0530 Subject: [PATCH 1/9] Move dwb goal/progress checker plugins to nav2_controller Signed-off-by: Siddarth Gore --- nav2_controller/CMakeLists.txt | 38 +++++- .../nav2_controller/nav2_controller.hpp | 9 +- .../plugins/simple_goal_checker.hpp | 77 +++++++++++++ .../simple_progress_checker.hpp} | 40 +++---- .../plugins}/stopped_goal_checker.hpp | 12 +- nav2_controller/package.xml | 1 + nav2_controller/plugins.xml | 17 +++ .../plugins/simple_goal_checker.cpp | 108 ++++++++++++++++++ .../simple_progress_checker.cpp} | 31 +++-- .../plugins}/stopped_goal_checker.cpp | 8 +- nav2_controller/plugins/test/CMakeLists.txt | 2 + .../plugins}/test/goal_checker.cpp | 12 +- nav2_controller/src/nav2_controller.cpp | 24 +++- nav2_core/include/nav2_core/controller.hpp | 13 --- .../include/nav2_core/progress_checker.hpp | 57 +++++++++ .../include/dwb_core/dwb_local_planner.hpp | 17 --- .../dwb_core/src/dwb_local_planner.cpp | 47 +------- .../dwb_plugins/CMakeLists.txt | 17 +-- .../dwb_plugins/test/CMakeLists.txt | 3 - 19 files changed, 381 insertions(+), 152 deletions(-) create mode 100644 nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp rename nav2_controller/include/nav2_controller/{progress_checker.hpp => plugins/simple_progress_checker.hpp} (63%) rename {nav2_dwb_controller/dwb_plugins/include/dwb_plugins => nav2_controller/include/nav2_controller/plugins}/stopped_goal_checker.hpp (88%) create mode 100644 nav2_controller/plugins.xml create mode 100644 nav2_controller/plugins/simple_goal_checker.cpp rename nav2_controller/{src/progress_checker.cpp => plugins/simple_progress_checker.cpp} (67%) rename {nav2_dwb_controller/dwb_plugins/src => nav2_controller/plugins}/stopped_goal_checker.cpp (93%) create mode 100644 nav2_controller/plugins/test/CMakeLists.txt rename {nav2_dwb_controller/dwb_plugins => nav2_controller/plugins}/test/goal_checker.cpp (94%) create mode 100644 nav2_core/include/nav2_core/progress_checker.hpp diff --git a/nav2_controller/CMakeLists.txt b/nav2_controller/CMakeLists.txt index b4cb1b50d03..464fd935498 100644 --- a/nav2_controller/CMakeLists.txt +++ b/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,6 +47,22 @@ 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} ) @@ -53,13 +70,23 @@ ament_target_dependencies(${library_name} # 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,6 +99,9 @@ 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) @@ -79,6 +109,10 @@ if(BUILD_TESTING) 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() diff --git a/nav2_controller/include/nav2_controller/nav2_controller.hpp b/nav2_controller/include/nav2_controller/nav2_controller.hpp index b0b28b462a1..890a628ec5f 100644 --- a/nav2_controller/include/nav2_controller/nav2_controller.hpp +++ b/nav2_controller/include/nav2_controller/nav2_controller.hpp @@ -22,6 +22,8 @@ #include #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::SharedPtr vel_publisher_; // Controller Plugins + pluginlib::ClassLoader progress_checker_loader_; + nav2_core::ProgressChecker::Ptr progress_checker_; + pluginlib::ClassLoader goal_checker_loader_; + nav2_core::GoalChecker::Ptr goal_checker_; + pluginlib::ClassLoader lp_loader_; ControllerMap controllers_; std::vector default_ids_; @@ -210,8 +217,6 @@ class ControllerServer : public nav2_util::LifecycleNode std::vector controller_types_; std::string controller_ids_concat_, current_controller_; - std::unique_ptr progress_checker_; - double controller_frequency_; double min_x_velocity_threshold_; double min_y_velocity_threshold_; diff --git a/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp b/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp new file mode 100644 index 00000000000..b7e483c1611 --- /dev/null +++ b/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 +#include + +#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_ diff --git a/nav2_controller/include/nav2_controller/progress_checker.hpp b/nav2_controller/include/nav2_controller/plugins/simple_progress_checker.hpp similarity index 63% rename from nav2_controller/include/nav2_controller/progress_checker.hpp rename to nav2_controller/include/nav2_controller/plugins/simple_progress_checker.hpp index 3d754ad7aa2..f94a33557a1 100644 --- a/nav2_controller/include/nav2_controller/progress_checker.hpp +++ b/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 #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_ diff --git a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/stopped_goal_checker.hpp b/nav2_controller/include/nav2_controller/plugins/stopped_goal_checker.hpp similarity index 88% rename from nav2_dwb_controller/dwb_plugins/include/dwb_plugins/stopped_goal_checker.hpp rename to nav2_controller/include/nav2_controller/plugins/stopped_goal_checker.hpp index 23f727d9456..869edb6330b 100644 --- a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/stopped_goal_checker.hpp +++ b/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 #include #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_ diff --git a/nav2_controller/package.xml b/nav2_controller/package.xml index c678de23c75..5c0e45246e0 100644 --- a/nav2_controller/package.xml +++ b/nav2_controller/package.xml @@ -24,5 +24,6 @@ ament_cmake + diff --git a/nav2_controller/plugins.xml b/nav2_controller/plugins.xml new file mode 100644 index 00000000000..a400e926e17 --- /dev/null +++ b/nav2_controller/plugins.xml @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/nav2_controller/plugins/simple_goal_checker.cpp b/nav2_controller/plugins/simple_goal_checker.cpp new file mode 100644 index 00000000000..f1ec7dba6ca --- /dev/null +++ b/nav2_controller/plugins/simple_goal_checker.cpp @@ -0,0 +1,108 @@ +/* + * 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 +#include +#include "nav2_controller/plugins/simple_goal_checker.hpp" +#include "pluginlib/class_list_macros.hpp" +#include "angles/angles.h" +#include "nav2_util/node_utils.hpp" +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wpedantic" +#include "tf2/utils.h" +#pragma GCC diagnostic pop + +namespace nav2_controller +{ + +SimpleGoalChecker::SimpleGoalChecker() +: xy_goal_tolerance_(0.25), + yaw_goal_tolerance_(0.25), + stateful_(true), + check_xy_(true), + xy_goal_tolerance_sq_(0.0625) +{ +} + +void SimpleGoalChecker::initialize( + const rclcpp_lifecycle::LifecycleNode::SharedPtr & nh, + const std::string & plugin_name) +{ + nav2_util::declare_parameter_if_not_declared( + nh, + plugin_name + ".xy_goal_tolerance", rclcpp::ParameterValue(0.25)); + nav2_util::declare_parameter_if_not_declared( + nh, + plugin_name + ".yaw_goal_tolerance", rclcpp::ParameterValue(0.25)); + nav2_util::declare_parameter_if_not_declared( + nh, + plugin_name + ".stateful", rclcpp::ParameterValue(true)); + + nh->get_parameter(plugin_name + ".xy_goal_tolerance", xy_goal_tolerance_); + nh->get_parameter(plugin_name + ".yaw_goal_tolerance", yaw_goal_tolerance_); + nh->get_parameter(plugin_name + ".stateful", stateful_); + + xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_; +} + +void SimpleGoalChecker::reset() +{ + check_xy_ = true; +} + +bool SimpleGoalChecker::isGoalReached( + const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose, + const geometry_msgs::msg::Twist &) +{ + if (check_xy_) { + double dx = query_pose.position.x - goal_pose.position.x, + dy = query_pose.position.y - goal_pose.position.y; + if (dx * dx + dy * dy > xy_goal_tolerance_sq_) { + return false; + } + // We are within the window + // If we are stateful, change the state. + if (stateful_) { + check_xy_ = false; + } + } + double dyaw = angles::shortest_angular_distance( + tf2::getYaw(query_pose.orientation), + tf2::getYaw(goal_pose.orientation)); + return fabs(dyaw) < yaw_goal_tolerance_; +} + +} // namespace nav2_controller + +PLUGINLIB_EXPORT_CLASS(nav2_controller::SimpleGoalChecker, nav2_core::GoalChecker) diff --git a/nav2_controller/src/progress_checker.cpp b/nav2_controller/plugins/simple_progress_checker.cpp similarity index 67% rename from nav2_controller/src/progress_checker.cpp rename to nav2_controller/plugins/simple_progress_checker.cpp index 68fe96de50c..97442cba17b 100644 --- a/nav2_controller/src/progress_checker.cpp +++ b/nav2_controller/plugins/simple_progress_checker.cpp @@ -12,33 +12,37 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "nav2_controller/progress_checker.hpp" +#include "nav2_controller/plugins/simple_progress_checker.hpp" #include +#include #include "nav2_core/exceptions.hpp" #include "nav_2d_utils/conversions.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/pose2_d.hpp" #include "nav2_util/node_utils.hpp" +#include "pluginlib/class_list_macros.hpp" namespace nav2_controller { static double pose_distance(const geometry_msgs::msg::Pose2D &, const geometry_msgs::msg::Pose2D &); -ProgressChecker::ProgressChecker(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node) -: nh_(node) +void SimpleProgressChecker::initialize( + const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, + const std::string & plugin_name) { + nh_ = node; nav2_util::declare_parameter_if_not_declared( - nh_, "required_movement_radius", rclcpp::ParameterValue(0.5)); + nh_, plugin_name + ".required_movement_radius", rclcpp::ParameterValue(0.5)); nav2_util::declare_parameter_if_not_declared( - nh_, "movement_time_allowance", rclcpp::ParameterValue(10.0)); + nh_, plugin_name + ".movement_time_allowance", rclcpp::ParameterValue(10.0)); // Scale is set to 0 by default, so if it was not set otherwise, set to 0 - nh_->get_parameter_or("required_movement_radius", radius_, 0.5); + nh_->get_parameter_or(plugin_name + ".required_movement_radius", radius_, 0.5); double time_allowance_param = 0.0; - nh_->get_parameter_or("movement_time_allowance", time_allowance_param, 10.0); + nh_->get_parameter_or(plugin_name + ".movement_time_allowance", time_allowance_param, 10.0); time_allowance_ = rclcpp::Duration::from_seconds(time_allowance_param); } -void ProgressChecker::check(geometry_msgs::msg::PoseStamped & current_pose) +void SimpleProgressChecker::check(geometry_msgs::msg::PoseStamped & current_pose) { // relies on short circuit evaluation to not call is_robot_moved_enough if // baseline_pose is not set. @@ -54,14 +58,19 @@ void ProgressChecker::check(geometry_msgs::msg::PoseStamped & current_pose) } } -void ProgressChecker::reset_baseline_pose(const geometry_msgs::msg::Pose2D & pose) +void SimpleProgressChecker::reset() +{ + baseline_pose_set_ = false; +} + +void SimpleProgressChecker::reset_baseline_pose(const geometry_msgs::msg::Pose2D & pose) { baseline_pose_ = pose; baseline_time_ = nh_->now(); baseline_pose_set_ = true; } -bool ProgressChecker::is_robot_moved_enough(const geometry_msgs::msg::Pose2D & pose) +bool SimpleProgressChecker::is_robot_moved_enough(const geometry_msgs::msg::Pose2D & pose) { if (pose_distance(pose, baseline_pose_) > radius_) { return true; @@ -81,3 +90,5 @@ static double pose_distance( } } // namespace nav2_controller + +PLUGINLIB_EXPORT_CLASS(nav2_controller::SimpleProgressChecker, nav2_core::ProgressChecker) diff --git a/nav2_dwb_controller/dwb_plugins/src/stopped_goal_checker.cpp b/nav2_controller/plugins/stopped_goal_checker.cpp similarity index 93% rename from nav2_dwb_controller/dwb_plugins/src/stopped_goal_checker.cpp rename to nav2_controller/plugins/stopped_goal_checker.cpp index c94948e8967..2e24ad181aa 100644 --- a/nav2_dwb_controller/dwb_plugins/src/stopped_goal_checker.cpp +++ b/nav2_controller/plugins/stopped_goal_checker.cpp @@ -35,14 +35,14 @@ #include #include #include -#include "dwb_plugins/stopped_goal_checker.hpp" +#include "nav2_controller/plugins/stopped_goal_checker.hpp" #include "pluginlib/class_list_macros.hpp" #include "nav2_util/node_utils.hpp" using std::hypot; using std::fabs; -namespace dwb_plugins +namespace nav2_controller { StoppedGoalChecker::StoppedGoalChecker() @@ -80,6 +80,6 @@ bool StoppedGoalChecker::isGoalReached( hypot(velocity.linear.x, velocity.linear.y) <= trans_stopped_velocity_; } -} // namespace dwb_plugins +} // namespace nav2_controller -PLUGINLIB_EXPORT_CLASS(dwb_plugins::StoppedGoalChecker, nav2_core::GoalChecker) +PLUGINLIB_EXPORT_CLASS(nav2_controller::StoppedGoalChecker, nav2_core::GoalChecker) diff --git a/nav2_controller/plugins/test/CMakeLists.txt b/nav2_controller/plugins/test/CMakeLists.txt new file mode 100644 index 00000000000..9bf0ba41f03 --- /dev/null +++ b/nav2_controller/plugins/test/CMakeLists.txt @@ -0,0 +1,2 @@ +ament_add_gtest(gctest goal_checker.cpp) +target_link_libraries(gctest simple_goal_checker stopped_goal_checker) diff --git a/nav2_dwb_controller/dwb_plugins/test/goal_checker.cpp b/nav2_controller/plugins/test/goal_checker.cpp similarity index 94% rename from nav2_dwb_controller/dwb_plugins/test/goal_checker.cpp rename to nav2_controller/plugins/test/goal_checker.cpp index 3e5b5e0357b..fe175db4450 100644 --- a/nav2_dwb_controller/dwb_plugins/test/goal_checker.cpp +++ b/nav2_controller/plugins/test/goal_checker.cpp @@ -36,13 +36,13 @@ #include #include "gtest/gtest.h" -#include "dwb_plugins/simple_goal_checker.hpp" -#include "dwb_plugins/stopped_goal_checker.hpp" +#include "nav2_controller/plugins/simple_goal_checker.hpp" +#include "nav2_controller/plugins/stopped_goal_checker.hpp" #include "nav_2d_utils/conversions.hpp" #include "nav2_util/lifecycle_node.hpp" -using dwb_plugins::SimpleGoalChecker; -using dwb_plugins::StoppedGoalChecker; +using nav2_controller::SimpleGoalChecker; +using nav2_controller::StoppedGoalChecker; void checkMacro( nav2_core::GoalChecker & gc, @@ -151,8 +151,8 @@ TEST(VelocityIterator, two_checks) SimpleGoalChecker gc; StoppedGoalChecker sgc; - gc.initialize(x, "dwb"); - sgc.initialize(x, "dwb"); + gc.initialize(x, "nav2_controller"); + sgc.initialize(x, "nav2_controller"); sameResult(gc, sgc, 0, 0, 0, 0, 0, 0, 0, 0, 0, true); sameResult(gc, sgc, 0, 0, 0, 1, 0, 0, 0, 0, 0, false); sameResult(gc, sgc, 0, 0, 0, 0, 1, 0, 0, 0, 0, false); diff --git a/nav2_controller/src/nav2_controller.cpp b/nav2_controller/src/nav2_controller.cpp index fc7c36e2737..b678888ae35 100644 --- a/nav2_controller/src/nav2_controller.cpp +++ b/nav2_controller/src/nav2_controller.cpp @@ -22,8 +22,9 @@ #include "nav_2d_utils/conversions.hpp" #include "nav2_util/node_utils.hpp" #include "nav2_util/geometry_utils.hpp" -#include "nav2_controller/progress_checker.hpp" #include "nav2_controller/nav2_controller.hpp" +#include "nav2_controller/plugins/simple_progress_checker.hpp" +#include "nav2_controller/plugins/simple_goal_checker.hpp" using namespace std::chrono_literals; @@ -32,6 +33,8 @@ namespace nav2_controller ControllerServer::ControllerServer() : LifecycleNode("controller_server", "", true), + progress_checker_loader_("nav2_core", "nav2_core::ProgressChecker"), + goal_checker_loader_("nav2_core", "nav2_core::GoalChecker"), lp_loader_("nav2_core", "nav2_core::Controller"), default_ids_{"FollowPath"}, default_types_{"dwb_core::DWBLocalPlanner"} @@ -41,6 +44,12 @@ ControllerServer::ControllerServer() declare_parameter("controller_frequency", 20.0); declare_parameter("controller_plugins", default_ids_); + declare_parameter( + "progress_checker_name", + rclcpp::ParameterValue(std::string("nav2_controller::SimpleProgressChecker"))); + declare_parameter( + "goal_checker_name", + rclcpp::ParameterValue(std::string("nav2_controller::SimpleGoalChecker"))); declare_parameter("min_x_velocity_threshold", rclcpp::ParameterValue(0.0001)); declare_parameter("min_y_velocity_threshold", rclcpp::ParameterValue(0.0001)); declare_parameter("min_theta_velocity_threshold", rclcpp::ParameterValue(0.0001)); @@ -62,6 +71,8 @@ nav2_util::CallbackReturn ControllerServer::on_configure(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "Configuring controller interface"); + std::string progress_checker_name; + std::string goal_checker_name; get_parameter("controller_plugins", controller_ids_); if (controller_ids_ == default_ids_) { @@ -72,6 +83,8 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) controller_types_.resize(controller_ids_.size()); get_parameter("controller_frequency", controller_frequency_); + get_parameter("progress_checker_name", progress_checker_name); + get_parameter("goal_checker_name", goal_checker_name); get_parameter("min_x_velocity_threshold", min_x_velocity_threshold_); get_parameter("min_y_velocity_threshold", min_y_velocity_threshold_); get_parameter("min_theta_velocity_threshold", min_theta_velocity_threshold_); @@ -81,7 +94,10 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) auto node = shared_from_this(); - progress_checker_ = std::make_unique(node); + progress_checker_ = progress_checker_loader_.createUniqueInstance(progress_checker_name); + progress_checker_->initialize(node, progress_checker_name); + goal_checker_ = goal_checker_loader_.createUniqueInstance(goal_checker_name); + goal_checker_->initialize(node, goal_checker_name); for (size_t i = 0; i != controller_ids_.size(); i++) { try { @@ -172,6 +188,7 @@ ControllerServer::on_cleanup(const rclcpp_lifecycle::State & state) vel_publisher_.reset(); action_server_.reset(); + goal_checker_->reset(); return nav2_util::CallbackReturn::SUCCESS; } @@ -295,6 +312,7 @@ void ControllerServer::setPlannerPath(const nav_msgs::msg::Path & path) controllers_[current_controller_]->setPlan(path); auto end_pose = *(path.poses.end() - 1); + goal_checker_->reset(); RCLCPP_DEBUG( get_logger(), "Path end point is (%.2f, %.2f)", @@ -375,7 +393,7 @@ bool ControllerServer::isGoalReached() nav_2d_msgs::msg::Twist2D twist = getThresholdedTwist(odom_sub_->getTwist()); geometry_msgs::msg::Twist velocity = nav_2d_utils::twist2Dto3D(twist); - return controllers_[current_controller_]->isGoalReached(pose, velocity); + return goal_checker_->isGoalReached(pose.pose, end_pose_, velocity); } bool ControllerServer::getRobotPose(geometry_msgs::msg::PoseStamped & pose) diff --git a/nav2_core/include/nav2_core/controller.hpp b/nav2_core/include/nav2_core/controller.hpp index 1e6e98052c3..d262f977424 100644 --- a/nav2_core/include/nav2_core/controller.hpp +++ b/nav2_core/include/nav2_core/controller.hpp @@ -113,19 +113,6 @@ class Controller const geometry_msgs::msg::PoseStamped & pose, const geometry_msgs::msg::Twist & velocity) = 0; - /** - * @brief Controller isGoalReached - Check whether the robot has reached its goal, given the current pose & velocity. - * - * The pose that it checks against is the last pose in the current global plan. - * The calculation is delegated to the goal_checker plugin. - * - * @param pose Current pose - * @param velocity Current velocity - * @return True if the robot should be considered as having reached the goal. - */ - virtual bool isGoalReached( - const geometry_msgs::msg::PoseStamped & pose, - const geometry_msgs::msg::Twist & velocity) = 0; }; } // namespace nav2_core diff --git a/nav2_core/include/nav2_core/progress_checker.hpp b/nav2_core/include/nav2_core/progress_checker.hpp new file mode 100644 index 00000000000..1b6d89599bc --- /dev/null +++ b/nav2_core/include/nav2_core/progress_checker.hpp @@ -0,0 +1,57 @@ +// Copyright (c) 2019 Intel Corporation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_CORE__PROGRESS_CHECKER_HPP_ +#define NAV2_CORE__PROGRESS_CHECKER_HPP_ + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/pose2_d.hpp" + +namespace nav2_core +{ +/** + * @class nav2_core::ProgressChecker + * @brief This class defines the plugin interface used to check the + * position of the robot to make sure that it is actually progressing + * towards a goal. + */ +class ProgressChecker +{ +public: + typedef std::shared_ptr Ptr; + /** + * @brief Initialize parameters for ProgressChecker + * @param node Node pointer + */ + virtual void initialize( + const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, + const std::string & plugin_name) = 0; + /** + * @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 + */ + virtual void check(geometry_msgs::msg::PoseStamped & current_pose) = 0; + /** + * @brief Reset class state upon calling + */ + virtual void reset() {} + +}; +} // namespace nav2_core + +#endif // NAV2_CORE__PROGRESS_CHECKER_HPP_ diff --git a/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp b/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp index 4ded71cb7eb..2da4b44a52c 100644 --- a/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp +++ b/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp @@ -99,20 +99,6 @@ class DWBLocalPlanner : public nav2_core::Controller const geometry_msgs::msg::PoseStamped & pose, const geometry_msgs::msg::Twist & velocity) override; - /** - * @brief nav2_core isGoalReached - Check whether the robot has reached its goal, given the current pose & velocity. - * - * The pose that it checks against is the last pose in the current global plan. - * The calculation is delegated to the goal_checker plugin. - * - * @param pose Current pose - * @param velocity Current velocity - * @return True if the robot should be considered as having reached the goal. - */ - bool isGoalReached( - const geometry_msgs::msg::PoseStamped & pose, - const geometry_msgs::msg::Twist & velocity) override; - /** * @brief Score a given command. Can be used for testing. * @@ -210,9 +196,6 @@ class DWBLocalPlanner : public nav2_core::Controller pluginlib::ClassLoader traj_gen_loader_; TrajectoryGenerator::Ptr traj_generator_; - pluginlib::ClassLoader goal_checker_loader_; - nav2_core::GoalChecker::Ptr goal_checker_; - pluginlib::ClassLoader critic_loader_; std::vector critics_; diff --git a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp index 011ea530422..5fc475db6ac 100644 --- a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp +++ b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp @@ -59,7 +59,6 @@ namespace dwb_core DWBLocalPlanner::DWBLocalPlanner() : traj_gen_loader_("dwb_core", "dwb_core::TrajectoryGenerator"), - goal_checker_loader_("dwb_core", "nav2_core::GoalChecker"), critic_loader_("dwb_core", "dwb_core::TrajectoryCritic") { } @@ -87,9 +86,6 @@ void DWBLocalPlanner::configure( declare_parameter_if_not_declared( node_, dwb_plugin_name_ + ".trajectory_generator_name", rclcpp::ParameterValue(std::string("dwb_plugins::StandardTrajectoryGenerator"))); - declare_parameter_if_not_declared( - node_, dwb_plugin_name_ + ".goal_checker_name", - rclcpp::ParameterValue(std::string("dwb_plugins::SimpleGoalChecker"))); declare_parameter_if_not_declared( node_, dwb_plugin_name_ + ".transform_tolerance", rclcpp::ParameterValue(0.1)); @@ -98,7 +94,6 @@ void DWBLocalPlanner::configure( rclcpp::ParameterValue(true)); std::string traj_generator_name; - std::string goal_checker_name; double transform_tolerance; node_->get_parameter(dwb_plugin_name_ + ".transform_tolerance", transform_tolerance); @@ -109,7 +104,6 @@ void DWBLocalPlanner::configure( node_->get_parameter(dwb_plugin_name_ + ".prune_distance", prune_distance_); node_->get_parameter(dwb_plugin_name_ + ".debug_trajectory_details", debug_trajectory_details_); node_->get_parameter(dwb_plugin_name_ + ".trajectory_generator_name", traj_generator_name); - node_->get_parameter(dwb_plugin_name_ + ".goal_checker_name", goal_checker_name); node_->get_parameter( dwb_plugin_name_ + ".short_circuit_trajectory_evaluation", short_circuit_trajectory_evaluation_); @@ -118,10 +112,8 @@ void DWBLocalPlanner::configure( pub_->on_configure(); traj_generator_ = traj_gen_loader_.createUniqueInstance(traj_generator_name); - goal_checker_ = goal_checker_loader_.createUniqueInstance(goal_checker_name); traj_generator_->initialize(node_, dwb_plugin_name_); - goal_checker_->initialize(node_, dwb_plugin_name_); try { loadCritics(); @@ -149,7 +141,6 @@ DWBLocalPlanner::cleanup() pub_->on_cleanup(); traj_generator_.reset(); - goal_checker_.reset(); } std::string @@ -263,42 +254,6 @@ DWBLocalPlanner::loadBackwardsCompatibleParameters() /* *INDENT-ON* */ } -bool -DWBLocalPlanner::isGoalReached( - const geometry_msgs::msg::PoseStamped & pose, - const geometry_msgs::msg::Twist & velocity) -{ - if (global_plan_.poses.size() == 0) { - RCLCPP_WARN( - rclcpp::get_logger( - "DWBLocalPlanner"), "Cannot check if the goal is reached without the goal being set!"); - return false; - } - nav_2d_msgs::msg::Pose2DStamped local_start_pose2d, goal_pose2d, local_goal_pose2d; - - nav_2d_utils::transformPose( - tf_, costmap_ros_->getGlobalFrameID(), - nav_2d_utils::poseStampedToPose2D(pose), - local_start_pose2d, transform_tolerance_); - - goal_pose2d.header.frame_id = global_plan_.header.frame_id; - goal_pose2d.pose = global_plan_.poses.back(); - - nav_2d_utils::transformPose( - tf_, costmap_ros_->getGlobalFrameID(), goal_pose2d, - local_goal_pose2d, transform_tolerance_); - - geometry_msgs::msg::PoseStamped local_start_pose, local_goal_pose; - local_start_pose = nav_2d_utils::pose2DToPoseStamped(local_start_pose2d); - local_goal_pose = nav_2d_utils::pose2DToPoseStamped(local_goal_pose2d); - - bool ret = goal_checker_->isGoalReached(local_start_pose.pose, local_goal_pose.pose, velocity); - if (ret) { - RCLCPP_INFO(rclcpp::get_logger("DWBLocalPlanner"), "Goal reached!"); - } - return ret; -} - void DWBLocalPlanner::setPlan(const nav_msgs::msg::Path & path) { @@ -308,7 +263,7 @@ DWBLocalPlanner::setPlan(const nav_msgs::msg::Path & path) } traj_generator_->reset(); - goal_checker_->reset(); + //goal_checker_->reset(); pub_->publishGlobalPlan(path2d); global_plan_ = path2d; diff --git a/nav2_dwb_controller/dwb_plugins/CMakeLists.txt b/nav2_dwb_controller/dwb_plugins/CMakeLists.txt index ea4d7868eaa..6e54ad061e3 100644 --- a/nav2_dwb_controller/dwb_plugins/CMakeLists.txt +++ b/nav2_dwb_controller/dwb_plugins/CMakeLists.txt @@ -27,19 +27,6 @@ include_directories( include ) -add_library(simple_goal_checker SHARED src/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 src/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") - - add_library(standard_traj_generator SHARED src/standard_traj_generator.cpp src/limited_accel_generator.cpp @@ -60,7 +47,7 @@ if(BUILD_TESTING) add_subdirectory(test) endif() -install(TARGETS simple_goal_checker stopped_goal_checker standard_traj_generator +install(TARGETS standard_traj_generator ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin @@ -73,8 +60,6 @@ install(FILES plugins.xml ) ament_export_include_directories(include) -ament_export_libraries(simple_goal_checker) -ament_export_libraries(stopped_goal_checker) ament_export_libraries(standard_traj_generator) pluginlib_export_plugin_description_file(dwb_core plugins.xml) diff --git a/nav2_dwb_controller/dwb_plugins/test/CMakeLists.txt b/nav2_dwb_controller/dwb_plugins/test/CMakeLists.txt index 272f38cf4ca..29845a84998 100644 --- a/nav2_dwb_controller/dwb_plugins/test/CMakeLists.txt +++ b/nav2_dwb_controller/dwb_plugins/test/CMakeLists.txt @@ -1,7 +1,4 @@ ament_add_gtest(vtest velocity_iterator_test.cpp) -ament_add_gtest(goal_checker goal_checker.cpp) -target_link_libraries(goal_checker simple_goal_checker stopped_goal_checker) - ament_add_gtest(twist_gen_test twist_gen.cpp) target_link_libraries(twist_gen_test standard_traj_generator) From 2d466a8ec745d5f57a9150cf536538c0358994bb Mon Sep 17 00:00:00 2001 From: Siddarth Gore Date: Wed, 8 Jul 2020 14:38:42 +0530 Subject: [PATCH 2/9] Move goal/progress checker plugins to nav2_controller Address review comments Signed-off-by: Siddarth Gore --- nav2_controller/CMakeLists.txt | 11 ++++------- .../plugins/simple_progress_checker.hpp | 2 +- nav2_controller/package.xml | 1 + nav2_controller/plugins.xml | 6 +++--- nav2_controller/plugins/simple_progress_checker.cpp | 7 ++++--- nav2_controller/src/nav2_controller.cpp | 4 +++- nav2_core/include/nav2_core/controller.hpp | 1 - nav2_core/include/nav2_core/progress_checker.hpp | 12 +++++++----- .../dwb_core/src/dwb_local_planner.cpp | 1 - 9 files changed, 23 insertions(+), 22 deletions(-) diff --git a/nav2_controller/CMakeLists.txt b/nav2_controller/CMakeLists.txt index 464fd935498..5ca5a22605c 100644 --- a/nav2_controller/CMakeLists.txt +++ b/nav2_controller/CMakeLists.txt @@ -99,9 +99,6 @@ 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) @@ -109,10 +106,10 @@ if(BUILD_TESTING) 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}) +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() diff --git a/nav2_controller/include/nav2_controller/plugins/simple_progress_checker.hpp b/nav2_controller/include/nav2_controller/plugins/simple_progress_checker.hpp index f94a33557a1..92a5374a5e0 100644 --- a/nav2_controller/include/nav2_controller/plugins/simple_progress_checker.hpp +++ b/nav2_controller/include/nav2_controller/plugins/simple_progress_checker.hpp @@ -36,7 +36,7 @@ class SimpleProgressChecker : public nav2_core::ProgressChecker void initialize( const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, const std::string & plugin_name) override; - void check(geometry_msgs::msg::PoseStamped & current_pose) override; + bool check(geometry_msgs::msg::PoseStamped & current_pose) override; void reset() override; protected: diff --git a/nav2_controller/package.xml b/nav2_controller/package.xml index 5c0e45246e0..4e4dc40acde 100644 --- a/nav2_controller/package.xml +++ b/nav2_controller/package.xml @@ -9,6 +9,7 @@ ament_cmake nav2_common + angles rclcpp rclcpp_action std_msgs diff --git a/nav2_controller/plugins.xml b/nav2_controller/plugins.xml index a400e926e17..ad27127c03d 100644 --- a/nav2_controller/plugins.xml +++ b/nav2_controller/plugins.xml @@ -1,17 +1,17 @@ - + Checks if distance between current and previous pose is above a threshold - + Checks if current pose is within goal window for x,y and yaw - + Checks linear and angular velocity after stopping diff --git a/nav2_controller/plugins/simple_progress_checker.cpp b/nav2_controller/plugins/simple_progress_checker.cpp index 97442cba17b..498d13256cb 100644 --- a/nav2_controller/plugins/simple_progress_checker.cpp +++ b/nav2_controller/plugins/simple_progress_checker.cpp @@ -42,7 +42,7 @@ void SimpleProgressChecker::initialize( time_allowance_ = rclcpp::Duration::from_seconds(time_allowance_param); } -void SimpleProgressChecker::check(geometry_msgs::msg::PoseStamped & current_pose) +bool SimpleProgressChecker::check(geometry_msgs::msg::PoseStamped & current_pose) { // relies on short circuit evaluation to not call is_robot_moved_enough if // baseline_pose is not set. @@ -51,11 +51,12 @@ void SimpleProgressChecker::check(geometry_msgs::msg::PoseStamped & current_pose if ((!baseline_pose_set_) || (is_robot_moved_enough(current_pose2d))) { reset_baseline_pose(current_pose2d); - return; + return true; } if ((nh_->now() - baseline_time_) > time_allowance_) { - throw nav2_core::PlannerException("Failed to make progress"); + return false; } + return true; } void SimpleProgressChecker::reset() diff --git a/nav2_controller/src/nav2_controller.cpp b/nav2_controller/src/nav2_controller.cpp index b678888ae35..fe8f3f48e02 100644 --- a/nav2_controller/src/nav2_controller.cpp +++ b/nav2_controller/src/nav2_controller.cpp @@ -328,7 +328,9 @@ void ControllerServer::computeAndPublishVelocity() throw nav2_core::PlannerException("Failed to obtain robot pose"); } - progress_checker_->check(pose); + if (!progress_checker_->check(pose)) { + throw nav2_core::PlannerException("Failed to make progress"); + } nav_2d_msgs::msg::Twist2D twist = getThresholdedTwist(odom_sub_->getTwist()); diff --git a/nav2_core/include/nav2_core/controller.hpp b/nav2_core/include/nav2_core/controller.hpp index d262f977424..69aa10f25b0 100644 --- a/nav2_core/include/nav2_core/controller.hpp +++ b/nav2_core/include/nav2_core/controller.hpp @@ -112,7 +112,6 @@ class Controller virtual geometry_msgs::msg::TwistStamped computeVelocityCommands( const geometry_msgs::msg::PoseStamped & pose, const geometry_msgs::msg::Twist & velocity) = 0; - }; } // namespace nav2_core diff --git a/nav2_core/include/nav2_core/progress_checker.hpp b/nav2_core/include/nav2_core/progress_checker.hpp index 1b6d89599bc..b15bb2eb0e0 100644 --- a/nav2_core/include/nav2_core/progress_checker.hpp +++ b/nav2_core/include/nav2_core/progress_checker.hpp @@ -15,6 +15,9 @@ #ifndef NAV2_CORE__PROGRESS_CHECKER_HPP_ #define NAV2_CORE__PROGRESS_CHECKER_HPP_ +#include +#include + #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" @@ -37,20 +40,19 @@ class ProgressChecker * @param node Node pointer */ virtual void initialize( - const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, - const std::string & plugin_name) = 0; + const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, + const std::string & plugin_name) = 0; /** * @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 + * @return True if progress is made */ - virtual void check(geometry_msgs::msg::PoseStamped & current_pose) = 0; + virtual bool check(geometry_msgs::msg::PoseStamped & current_pose) = 0; /** * @brief Reset class state upon calling */ virtual void reset() {} - }; } // namespace nav2_core diff --git a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp index 5fc475db6ac..8c7041bcb72 100644 --- a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp +++ b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp @@ -263,7 +263,6 @@ DWBLocalPlanner::setPlan(const nav_msgs::msg::Path & path) } traj_generator_->reset(); - //goal_checker_->reset(); pub_->publishGlobalPlan(path2d); global_plan_ = path2d; From 73197adcac0ae123de161010179dfe53b13d9868 Mon Sep 17 00:00:00 2001 From: Siddarth Gore Date: Fri, 10 Jul 2020 22:12:57 +0530 Subject: [PATCH 3/9] Move goal/progress checker plugins to nav2_controller Use new plugin declaration format and doc update Signed-off-by: Siddarth Gore --- doc/parameters/param_list.md | 44 +++++++++------ .../nav2_controller/nav2_controller.hpp | 13 ++++- nav2_controller/src/nav2_controller.cpp | 53 +++++++++++++------ 3 files changed, 77 insertions(+), 33 deletions(-) diff --git a/doc/parameters/param_list.md b/doc/parameters/param_list.md index 531657aece3..ebf832c8c88 100644 --- a/doc/parameters/param_list.md +++ b/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 | +| ----------| --------| ------------| +| ``.required_movement_radius | 0.5 | Minimum distance to count as progress (m) | +| ``.movement_time_allowance | 10.0 | Maximum time allowence for progress to happen (s) | + + +## simple_goal_checker plugin + +| Parameter | Default | Description | +| ----------| --------| ------------| +| ``.xy_goal_tolerance | 0.25 | Tolerance to meet goal completion criteria (m) | +| ``.yaw_goal_tolerance | 0.25 | Tolerance to meet goal completion criteria (rad) | +| ``.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 | +| ----------| --------| ------------| +| ``.rot_stopped_velocity | 0.25 | Velocity below is considered to be stopped at tolerance met (rad/s) | +| ``.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 | ``.lookahead_time | -1 | If > 0, amount of time to look forward for a collision for. | | ``.``.scale | 1.0 | Weight scale | -## simple_goal_checker plugin - -| Parameter | Default | Description | -| ----------| --------| ------------| -| ``.xy_goal_tolerance | 0.25 | Tolerance to meet goal completion criteria (m) | -| ``.yaw_goal_tolerance | 0.25 | Tolerance to meet goal completion criteria (rad) | -| ``.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 | ----------| --------| ------------| | ``.sim_time | N/A | Time to simulate ahead by (s) | -## stopped_goal_checker plugin - -| Parameter | Default | Description | -| ----------| --------| ------------| -| ``.rot_stopped_velocity | 0.25 | Velocity below is considered to be stopped at tolerance met (rad/s) | -| ``.trans_stopped_velocity | 0.25 | Velocity below is considered to be stopped at tolerance met (m/s) | - # lifecycle_manager | Parameter | Default | Description | diff --git a/nav2_controller/include/nav2_controller/nav2_controller.hpp b/nav2_controller/include/nav2_controller/nav2_controller.hpp index 890a628ec5f..3d4e7be277e 100644 --- a/nav2_controller/include/nav2_controller/nav2_controller.hpp +++ b/nav2_controller/include/nav2_controller/nav2_controller.hpp @@ -203,12 +203,23 @@ class ControllerServer : public nav2_util::LifecycleNode std::unique_ptr odom_sub_; rclcpp_lifecycle::LifecyclePublisher::SharedPtr vel_publisher_; - // Controller Plugins + // Progress Checker Plugin pluginlib::ClassLoader 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 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 lp_loader_; ControllerMap controllers_; std::vector default_ids_; diff --git a/nav2_controller/src/nav2_controller.cpp b/nav2_controller/src/nav2_controller.cpp index fe8f3f48e02..5f34e2281ce 100644 --- a/nav2_controller/src/nav2_controller.cpp +++ b/nav2_controller/src/nav2_controller.cpp @@ -23,8 +23,6 @@ #include "nav2_util/node_utils.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_controller/nav2_controller.hpp" -#include "nav2_controller/plugins/simple_progress_checker.hpp" -#include "nav2_controller/plugins/simple_goal_checker.hpp" using namespace std::chrono_literals; @@ -34,7 +32,11 @@ namespace nav2_controller ControllerServer::ControllerServer() : LifecycleNode("controller_server", "", true), progress_checker_loader_("nav2_core", "nav2_core::ProgressChecker"), + default_progress_checker_id_{"progress_checker"}, + default_progress_checker_type_{"nav2_controller::SimpleProgressChecker"}, goal_checker_loader_("nav2_core", "nav2_core::GoalChecker"), + default_goal_checker_id_{"goal_checker"}, + default_goal_checker_type_{"nav2_controller::SimpleGoalChecker"}, lp_loader_("nav2_core", "nav2_core::Controller"), default_ids_{"FollowPath"}, default_types_{"dwb_core::DWBLocalPlanner"} @@ -43,13 +45,9 @@ ControllerServer::ControllerServer() declare_parameter("controller_frequency", 20.0); + declare_parameter("progress_checker_plugin", default_progress_checker_id_); + declare_parameter("goal_checker_plugin", default_goal_checker_id_); declare_parameter("controller_plugins", default_ids_); - declare_parameter( - "progress_checker_name", - rclcpp::ParameterValue(std::string("nav2_controller::SimpleProgressChecker"))); - declare_parameter( - "goal_checker_name", - rclcpp::ParameterValue(std::string("nav2_controller::SimpleGoalChecker"))); declare_parameter("min_x_velocity_threshold", rclcpp::ParameterValue(0.0001)); declare_parameter("min_y_velocity_threshold", rclcpp::ParameterValue(0.0001)); declare_parameter("min_theta_velocity_threshold", rclcpp::ParameterValue(0.0001)); @@ -71,8 +69,15 @@ nav2_util::CallbackReturn ControllerServer::on_configure(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "Configuring controller interface"); - std::string progress_checker_name; - std::string goal_checker_name; + + 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_); + } + 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_); + } get_parameter("controller_plugins", controller_ids_); if (controller_ids_ == default_ids_) { @@ -83,8 +88,6 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) controller_types_.resize(controller_ids_.size()); get_parameter("controller_frequency", controller_frequency_); - get_parameter("progress_checker_name", progress_checker_name); - get_parameter("goal_checker_name", goal_checker_name); get_parameter("min_x_velocity_threshold", min_x_velocity_threshold_); get_parameter("min_y_velocity_threshold", min_y_velocity_threshold_); get_parameter("min_theta_velocity_threshold", min_theta_velocity_threshold_); @@ -94,10 +97,28 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) auto node = shared_from_this(); - progress_checker_ = progress_checker_loader_.createUniqueInstance(progress_checker_name); - progress_checker_->initialize(node, progress_checker_name); - goal_checker_ = goal_checker_loader_.createUniqueInstance(goal_checker_name); - goal_checker_->initialize(node, goal_checker_name); + try { + progress_checker_type_ = nav2_util::get_plugin_type_param(node, progress_checker_id_); + progress_checker_ = progress_checker_loader_.createUniqueInstance(progress_checker_type_); + RCLCPP_INFO( + get_logger(), "Created progress_checker : %s of type %s", + progress_checker_id_.c_str(), progress_checker_type_.c_str()); + progress_checker_->initialize(node, progress_checker_id_); + } catch (const pluginlib::PluginlibException & ex) { + RCLCPP_FATAL(get_logger(), "Failed to create controller. Exception: %s", ex.what()); + exit(-1); + } + try { + goal_checker_type_ = nav2_util::get_plugin_type_param(node, goal_checker_id_); + goal_checker_ = goal_checker_loader_.createUniqueInstance(goal_checker_type_); + RCLCPP_INFO( + get_logger(), "Created goal_checker : %s of type %s", + goal_checker_id_.c_str(), goal_checker_type_.c_str()); + goal_checker_->initialize(node, goal_checker_id_); + } catch (const pluginlib::PluginlibException & ex) { + RCLCPP_FATAL(get_logger(), "Failed to create controller. Exception: %s", ex.what()); + exit(-1); + } for (size_t i = 0; i != controller_ids_.size(); i++) { try { From e271cd8585b636077c128b92dec47188f639d3be Mon Sep 17 00:00:00 2001 From: Siddarth Gore Date: Mon, 13 Jul 2020 14:38:35 +0530 Subject: [PATCH 4/9] Update bringup.yaml for new plugins in nav2_controller Also remove redundent file from dwb_plugins Signed-off-by: Siddarth Gore --- nav2_bringup/bringup/params/nav2_params.yaml | 13 +++ .../dwb_plugins/simple_goal_checker.hpp | 77 ------------- .../dwb_plugins/src/simple_goal_checker.cpp | 108 ------------------ 3 files changed, 13 insertions(+), 185 deletions(-) delete mode 100644 nav2_dwb_controller/dwb_plugins/include/dwb_plugins/simple_goal_checker.hpp delete mode 100644 nav2_dwb_controller/dwb_plugins/src/simple_goal_checker.cpp diff --git a/nav2_bringup/bringup/params/nav2_params.yaml b/nav2_bringup/bringup/params/nav2_params.yaml index 137cec171c7..1fc0b571ce4 100644 --- a/nav2_bringup/bringup/params/nav2_params.yaml +++ b/nav2_bringup/bringup/params/nav2_params.yaml @@ -86,8 +86,21 @@ controller_server: min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 + progress_checker_plugin: "progress_checker" + goal_checker_plugin: "goal_checker" controller_plugins: ["FollowPath"] + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + stateful: True # DWB parameters FollowPath: plugin: "dwb_core::DWBLocalPlanner" diff --git a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/simple_goal_checker.hpp b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/simple_goal_checker.hpp deleted file mode 100644 index 697a0dc10e4..00000000000 --- a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/simple_goal_checker.hpp +++ /dev/null @@ -1,77 +0,0 @@ -/* - * 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 DWB_PLUGINS__SIMPLE_GOAL_CHECKER_HPP_ -#define DWB_PLUGINS__SIMPLE_GOAL_CHECKER_HPP_ - -#include -#include - -#include "rclcpp/rclcpp.hpp" -#include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "nav2_core/goal_checker.hpp" - -namespace dwb_plugins -{ - -/** - * @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 dwb_plugins - -#endif // DWB_PLUGINS__SIMPLE_GOAL_CHECKER_HPP_ diff --git a/nav2_dwb_controller/dwb_plugins/src/simple_goal_checker.cpp b/nav2_dwb_controller/dwb_plugins/src/simple_goal_checker.cpp deleted file mode 100644 index b108b551989..00000000000 --- a/nav2_dwb_controller/dwb_plugins/src/simple_goal_checker.cpp +++ /dev/null @@ -1,108 +0,0 @@ -/* - * 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 -#include -#include "dwb_plugins/simple_goal_checker.hpp" -#include "pluginlib/class_list_macros.hpp" -#include "angles/angles.h" -#include "nav2_util/node_utils.hpp" -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wpedantic" -#include "tf2/utils.h" -#pragma GCC diagnostic pop - -namespace dwb_plugins -{ - -SimpleGoalChecker::SimpleGoalChecker() -: xy_goal_tolerance_(0.25), - yaw_goal_tolerance_(0.25), - stateful_(true), - check_xy_(true), - xy_goal_tolerance_sq_(0.0625) -{ -} - -void SimpleGoalChecker::initialize( - const rclcpp_lifecycle::LifecycleNode::SharedPtr & nh, - const std::string & plugin_name) -{ - nav2_util::declare_parameter_if_not_declared( - nh, - plugin_name + ".xy_goal_tolerance", rclcpp::ParameterValue(0.25)); - nav2_util::declare_parameter_if_not_declared( - nh, - plugin_name + ".yaw_goal_tolerance", rclcpp::ParameterValue(0.25)); - nav2_util::declare_parameter_if_not_declared( - nh, - plugin_name + ".stateful", rclcpp::ParameterValue(true)); - - nh->get_parameter(plugin_name + ".xy_goal_tolerance", xy_goal_tolerance_); - nh->get_parameter(plugin_name + ".yaw_goal_tolerance", yaw_goal_tolerance_); - nh->get_parameter(plugin_name + ".stateful", stateful_); - - xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_; -} - -void SimpleGoalChecker::reset() -{ - check_xy_ = true; -} - -bool SimpleGoalChecker::isGoalReached( - const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose, - const geometry_msgs::msg::Twist &) -{ - if (check_xy_) { - double dx = query_pose.position.x - goal_pose.position.x, - dy = query_pose.position.y - goal_pose.position.y; - if (dx * dx + dy * dy > xy_goal_tolerance_sq_) { - return false; - } - // We are within the window - // If we are stateful, change the state. - if (stateful_) { - check_xy_ = false; - } - } - double dyaw = angles::shortest_angular_distance( - tf2::getYaw(query_pose.orientation), - tf2::getYaw(goal_pose.orientation)); - return fabs(dyaw) < yaw_goal_tolerance_; -} - -} // namespace dwb_plugins - -PLUGINLIB_EXPORT_CLASS(dwb_plugins::SimpleGoalChecker, nav2_core::GoalChecker) From 20ef7d5264a5d332762683def274ceb5567fa593 Mon Sep 17 00:00:00 2001 From: Siddarth Gore Date: Tue, 14 Jul 2020 12:46:15 +0530 Subject: [PATCH 5/9] Fix doc errors and update remaining yaml files Signed-off-by: Siddarth Gore --- doc/parameters/param_list.md | 6 ++---- .../bringup/params/nav2_multirobot_params_1.yaml | 13 +++++++++++++ .../bringup/params/nav2_multirobot_params_2.yaml | 13 +++++++++++++ 3 files changed, 28 insertions(+), 4 deletions(-) diff --git a/doc/parameters/param_list.md b/doc/parameters/param_list.md index ebf832c8c88..e1ff45a98fc 100644 --- a/doc/parameters/param_list.md +++ b/doc/parameters/param_list.md @@ -154,14 +154,12 @@ 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 | +| 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) | | min_theta_velocity_threshold | 0.0001 | Minimum angular velocity to use (rad/s) | -| required_movement_radius | 0.5 | Minimum amount a robot must move to be progressing to goal (m) | -| movement_time_allowance | 10.0 | Maximum amount of time a robot has to move the minimum radius (s) | **NOTE:** When `controller_plugins` parameter is overridden, each plugin namespace defined in the list needs to have a `plugin` parameter defining the type of plugin to be loaded in the namespace. diff --git a/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml index 0c9c0ffa7af..842bc963338 100644 --- a/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml +++ b/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml @@ -85,8 +85,21 @@ controller_server: min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 + progress_checker_plugin: "progress_checker" + goal_checker_plugin: "goal_checker" controller_plugins: ["FollowPath"] + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + stateful: True # DWB parameters FollowPath: plugin: "dwb_core::DWBLocalPlanner" diff --git a/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml b/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml index 6f3cb4e31ca..1e793eba1de 100644 --- a/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml +++ b/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml @@ -85,8 +85,21 @@ controller_server: min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 + progress_checker_plugin: "progress_checker" + goal_checker_plugin: "goal_checker" controller_plugins: ["FollowPath"] + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + stateful: True # DWB parameters FollowPath: plugin: "dwb_core::DWBLocalPlanner" From 6558ec8132481c98aa84765c7fd6b955f0fe3d56 Mon Sep 17 00:00:00 2001 From: Siddarth Gore Date: Wed, 15 Jul 2020 14:43:29 +0530 Subject: [PATCH 6/9] Remove mention of goal_checker from dwb docs Signed-off-by: Siddarth Gore --- doc/parameters/param_list.md | 1 - 1 file changed, 1 deletion(-) diff --git a/doc/parameters/param_list.md b/doc/parameters/param_list.md index e1ff45a98fc..c9fc8cd5878 100644 --- a/doc/parameters/param_list.md +++ b/doc/parameters/param_list.md @@ -218,7 +218,6 @@ When `controller_plugins`\`progress_checker_plugin`\`goal_checker_plugin` parame | ``.prune_distance | 1.0 | Distance (m) to prune backward until | | ``.debug_trajectory_details | false | Publish debug information | | ``.trajectory_generator_name | "dwb_plugins::StandardTrajectoryGenerator" | Trajectory generator plugin name | -| ``.goal_checker_name | "dwb_plugins::SimpleGoalChecker" | Goal checker plugin name | | ``.transform_tolerance | 0.1 | TF transform tolerance | | ``.short_circuit_trajectory_evaluation | true | Stop evaluating scores after best score is found | | ``.path_distance_bias | N/A | Old version of `PathAlign.scale`, use that instead | From 2dfc4b93fa4b1eb5656bb06bc1300c2aa9aa729f Mon Sep 17 00:00:00 2001 From: Siddarth Gore Date: Wed, 15 Jul 2020 22:36:53 +0530 Subject: [PATCH 7/9] Add .plugin params to doc Signed-off-by: Siddarth Gore --- doc/parameters/param_list.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/doc/parameters/param_list.md b/doc/parameters/param_list.md index c9fc8cd5878..4302c624201 100644 --- a/doc/parameters/param_list.md +++ b/doc/parameters/param_list.md @@ -155,8 +155,11 @@ When `plugins` parameter is not overridden, the following default plugins are lo | ----------| --------| ------------| | controller_frequency | 20.0 | Frequency to run controller | | progress_checker_plugin | "progress_checker" | Check the progress of the robot | +| progress_checker.plugin | "nav2_controller::SimpleProgressChecker" | Default plugin | | goal_checker_plugin | "goal_checker" | Check if the goal has been reached | +| goal_checker.plugin | "nav2_controller::SimpleGoalChecker" | Default plugin | | controller_plugins | ["FollowPath"] | List of mapped names for controller plugins for processing requests and parameters | +| FollowPath.plugin | "dwb_core::DWBLocalPlanner" | Default plugin | | 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) | | min_theta_velocity_threshold | 0.0001 | Minimum angular velocity to use (rad/s) | From 9d93bae7f27ce6a80c0737744a44be2b30937ebb Mon Sep 17 00:00:00 2001 From: Siddarth Gore Date: Thu, 16 Jul 2020 17:15:03 +0530 Subject: [PATCH 8/9] Tests for progress_checker plugin declare .plugin only if not declared before Signed-off-by: Siddarth Gore --- nav2_controller/plugins/test/CMakeLists.txt | 2 + .../plugins/test/progress_checker.cpp | 135 ++++++++++++++++++ nav2_controller/src/nav2_controller.cpp | 16 ++- .../include/nav2_core/progress_checker.hpp | 3 + 4 files changed, 151 insertions(+), 5 deletions(-) create mode 100644 nav2_controller/plugins/test/progress_checker.cpp diff --git a/nav2_controller/plugins/test/CMakeLists.txt b/nav2_controller/plugins/test/CMakeLists.txt index 9bf0ba41f03..d4f34d3918f 100644 --- a/nav2_controller/plugins/test/CMakeLists.txt +++ b/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) diff --git a/nav2_controller/plugins/test/progress_checker.cpp b/nav2_controller/plugins/test/progress_checker.cpp new file mode 100644 index 00000000000..418ff1032ae --- /dev/null +++ b/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 +#include + +#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("progress_checker"); + + nav2_core::ProgressChecker * pc = new SimpleProgressChecker; + pc->reset(); + delete pc; + EXPECT_TRUE(true); +} + +TEST(SimpleProgressChecker, unit_tests) +{ + auto x = std::make_shared("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(); +} diff --git a/nav2_controller/src/nav2_controller.cpp b/nav2_controller/src/nav2_controller.cpp index 5f34e2281ce..a01df102fad 100644 --- a/nav2_controller/src/nav2_controller.cpp +++ b/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_); diff --git a/nav2_core/include/nav2_core/progress_checker.hpp b/nav2_core/include/nav2_core/progress_checker.hpp index b15bb2eb0e0..041ae88fbdb 100644 --- a/nav2_core/include/nav2_core/progress_checker.hpp +++ b/nav2_core/include/nav2_core/progress_checker.hpp @@ -35,6 +35,9 @@ class ProgressChecker { public: typedef std::shared_ptr Ptr; + + virtual ~ProgressChecker() {} + /** * @brief Initialize parameters for ProgressChecker * @param node Node pointer From 30740cfdfb282058b97ed2b0eb766f607e6a983e Mon Sep 17 00:00:00 2001 From: Siddarth Gore Date: Mon, 20 Jul 2020 14:32:23 +0530 Subject: [PATCH 9/9] Tweak plugin names/description in doc Signed-off-by: Siddarth Gore --- doc/parameters/param_list.md | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/doc/parameters/param_list.md b/doc/parameters/param_list.md index 4302c624201..fd525952d76 100644 --- a/doc/parameters/param_list.md +++ b/doc/parameters/param_list.md @@ -154,12 +154,12 @@ 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 | -| progress_checker.plugin | "nav2_controller::SimpleProgressChecker" | Default plugin | +| progress_checker_plugin | "progress_checker" | Plugin used by the controller to check whether the robot has at least covered a set distance/displacement in a set amount of time, thus checking the progress of the robot. | +| `.plugin` | "nav2_controller::SimpleProgressChecker" | Default plugin | | goal_checker_plugin | "goal_checker" | Check if the goal has been reached | -| goal_checker.plugin | "nav2_controller::SimpleGoalChecker" | Default plugin | +| `.plugin` | "nav2_controller::SimpleGoalChecker" | Default plugin | | controller_plugins | ["FollowPath"] | List of mapped names for controller plugins for processing requests and parameters | -| FollowPath.plugin | "dwb_core::DWBLocalPlanner" | Default plugin | +| `.plugin` | "dwb_core::DWBLocalPlanner" | Default plugin | | 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) | | min_theta_velocity_threshold | 0.0001 | Minimum angular velocity to use (rad/s) |