diff --git a/doc/parameters/param_list.md b/doc/parameters/param_list.md index 531657aece3..fd525952d76 100644 --- a/doc/parameters/param_list.md +++ b/doc/parameters/param_list.md @@ -154,12 +154,15 @@ 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" | 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 | +| `.plugin` | "nav2_controller::SimpleGoalChecker" | Default plugin | | controller_plugins | ["FollowPath"] | List of mapped names for controller plugins for processing requests and parameters | +| `.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) | -| 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. @@ -173,11 +176,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 @@ -193,7 +221,6 @@ When `controller_plugins` parameter is not overridden, the following default plu | ``.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 | @@ -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_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" 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_controller/CMakeLists.txt b/nav2_controller/CMakeLists.txt index b4cb1b50d03..5ca5a22605c 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 @@ -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() diff --git a/nav2_controller/include/nav2_controller/nav2_controller.hpp b/nav2_controller/include/nav2_controller/nav2_controller.hpp index b0b28b462a1..3d4e7be277e 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" @@ -201,6 +203,22 @@ class ControllerServer : public nav2_util::LifecycleNode std::unique_ptr odom_sub_; rclcpp_lifecycle::LifecyclePublisher::SharedPtr vel_publisher_; + // 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_; @@ -210,8 +228,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_dwb_controller/dwb_plugins/include/dwb_plugins/simple_goal_checker.hpp b/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp similarity index 92% rename from nav2_dwb_controller/dwb_plugins/include/dwb_plugins/simple_goal_checker.hpp rename to nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp index 697a0dc10e4..b7e483c1611 100644 --- a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/simple_goal_checker.hpp +++ b/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp @@ -32,8 +32,8 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DWB_PLUGINS__SIMPLE_GOAL_CHECKER_HPP_ -#define DWB_PLUGINS__SIMPLE_GOAL_CHECKER_HPP_ +#ifndef NAV2_CONTROLLER__PLUGINS__SIMPLE_GOAL_CHECKER_HPP_ +#define NAV2_CONTROLLER__PLUGINS__SIMPLE_GOAL_CHECKER_HPP_ #include #include @@ -42,7 +42,7 @@ #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "nav2_core/goal_checker.hpp" -namespace dwb_plugins +namespace nav2_controller { /** @@ -72,6 +72,6 @@ class SimpleGoalChecker : public nav2_core::GoalChecker double xy_goal_tolerance_sq_; }; -} // namespace dwb_plugins +} // namespace nav2_controller -#endif // DWB_PLUGINS__SIMPLE_GOAL_CHECKER_HPP_ +#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..92a5374a5e0 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; + 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_ 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..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 @@ -24,5 +25,6 @@ ament_cmake + diff --git a/nav2_controller/plugins.xml b/nav2_controller/plugins.xml new file mode 100644 index 00000000000..ad27127c03d --- /dev/null +++ b/nav2_controller/plugins.xml @@ -0,0 +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_dwb_controller/dwb_plugins/src/simple_goal_checker.cpp b/nav2_controller/plugins/simple_goal_checker.cpp similarity index 94% rename from nav2_dwb_controller/dwb_plugins/src/simple_goal_checker.cpp rename to nav2_controller/plugins/simple_goal_checker.cpp index b108b551989..f1ec7dba6ca 100644 --- a/nav2_dwb_controller/dwb_plugins/src/simple_goal_checker.cpp +++ b/nav2_controller/plugins/simple_goal_checker.cpp @@ -34,7 +34,7 @@ #include #include -#include "dwb_plugins/simple_goal_checker.hpp" +#include "nav2_controller/plugins/simple_goal_checker.hpp" #include "pluginlib/class_list_macros.hpp" #include "angles/angles.h" #include "nav2_util/node_utils.hpp" @@ -43,7 +43,7 @@ #include "tf2/utils.h" #pragma GCC diagnostic pop -namespace dwb_plugins +namespace nav2_controller { SimpleGoalChecker::SimpleGoalChecker() @@ -103,6 +103,6 @@ bool SimpleGoalChecker::isGoalReached( return fabs(dyaw) < yaw_goal_tolerance_; } -} // namespace dwb_plugins +} // namespace nav2_controller -PLUGINLIB_EXPORT_CLASS(dwb_plugins::SimpleGoalChecker, nav2_core::GoalChecker) +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 65% rename from nav2_controller/src/progress_checker.cpp rename to nav2_controller/plugins/simple_progress_checker.cpp index 68fe96de50c..498d13256cb 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) +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. @@ -47,21 +51,27 @@ void ProgressChecker::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() +{ + baseline_pose_set_ = false; } -void ProgressChecker::reset_baseline_pose(const geometry_msgs::msg::Pose2D & pose) +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 +91,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..d4f34d3918f --- /dev/null +++ b/nav2_controller/plugins/test/CMakeLists.txt @@ -0,0 +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_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/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 fc7c36e2737..a01df102fad 100644 --- a/nav2_controller/src/nav2_controller.cpp +++ b/nav2_controller/src/nav2_controller.cpp @@ -22,7 +22,6 @@ #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" using namespace std::chrono_literals; @@ -32,6 +31,12 @@ 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"} @@ -40,6 +45,8 @@ 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("min_x_velocity_threshold", rclcpp::ParameterValue(0.0001)); declare_parameter("min_y_velocity_threshold", rclcpp::ParameterValue(0.0001)); @@ -61,12 +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_) { + 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_) { + 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()); @@ -79,9 +103,28 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) costmap_ros_->on_configure(state); - auto node = shared_from_this(); - - progress_checker_ = std::make_unique(node); + 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 { @@ -172,6 +215,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 +339,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)", @@ -310,7 +355,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()); @@ -375,7 +422,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..69aa10f25b0 100644 --- a/nav2_core/include/nav2_core/controller.hpp +++ b/nav2_core/include/nav2_core/controller.hpp @@ -112,20 +112,6 @@ class Controller virtual geometry_msgs::msg::TwistStamped computeVelocityCommands( 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..041ae88fbdb --- /dev/null +++ b/nav2_core/include/nav2_core/progress_checker.hpp @@ -0,0 +1,62 @@ +// 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 +#include + +#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; + + virtual ~ProgressChecker() {} + + /** + * @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 + * @return True if progress is made + */ + virtual bool 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..8c7041bcb72 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,6 @@ DWBLocalPlanner::setPlan(const nav_msgs::msg::Path & path) } traj_generator_->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)