Skip to content

Move dwb goal/progress checker plugins to nav2_controller #1857

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 9 commits into from
Jul 20, 2020
Merged

Move dwb goal/progress checker plugins to nav2_controller #1857

Show file tree
Hide file tree
Changes from 5 commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
d7979ee
Move dwb goal/progress checker plugins to nav2_controller
siddhya Jun 8, 2020
2d466a8
Move goal/progress checker plugins to nav2_controller
siddhya Jul 8, 2020
73197ad
Move goal/progress checker plugins to nav2_controller
siddhya Jul 10, 2020
e271cd8
Update bringup.yaml for new plugins in nav2_controller
siddhya Jul 13, 2020
20ef7d5
Fix doc errors and update remaining yaml files
siddhya Jul 14, 2020
6558ec8
Remove mention of goal_checker from dwb docs
siddhya Jul 15, 2020
2dfc4b9
Add .plugin params to doc
siddhya Jul 15, 2020
9d93bae
Tests for progress_checker plugin
siddhya Jul 16, 2020
30740cf
Tweak plugin names/description in doc
siddhya Jul 20, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
46 changes: 28 additions & 18 deletions doc/parameters/param_list.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 |
Copy link
Contributor

@Marwan99 Marwan99 Jul 17, 2020

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Should we explain what is meant by progress?

Copy link
Member

@SteveMacenski SteveMacenski Jul 17, 2020

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Can you provide a suggested text?

Copy link
Contributor

@Marwan99 Marwan99 Jul 17, 2020

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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. What do you think?

Copy link
Contributor Author

@siddhya siddhya Jul 20, 2020

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Done.

| 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.

Expand All @@ -173,11 +173,36 @@ controller_server:
plugin: "dwb_core::DWBLocalPlanner"
```

When `controller_plugins` parameter is not overridden, the following default plugins are loaded:
When `controller_plugins`\`progress_checker_plugin`\`goal_checker_plugin` parameters are not overridden, the following default plugins are loaded:

| Namespace | Plugin |
| ----------| --------|
| "FollowPath" | "dwb_core::DWBLocalPlanner" |
| "progress_checker" | "nav2_controller::SimpleProgressChecker" |
| "goal_checker" | "nav2_controller::SimpleGoalChecker" |

## simple_progress_checker plugin

| Parameter | Default | Description |
| ----------| --------| ------------|
| `<nav2_controller plugin>`.required_movement_radius | 0.5 | Minimum distance to count as progress (m) |
| `<nav2_controller plugin>`.movement_time_allowance | 10.0 | Maximum time allowence for progress to happen (s) |


## simple_goal_checker plugin

| Parameter | Default | Description |
| ----------| --------| ------------|
| `<nav2_controller plugin>`.xy_goal_tolerance | 0.25 | Tolerance to meet goal completion criteria (m) |
| `<nav2_controller plugin>`.yaw_goal_tolerance | 0.25 | Tolerance to meet goal completion criteria (rad) |
| `<nav2_controller plugin>`.stateful | true | Whether to check for XY position tolerance after rotating to goal orientation in case of minor localization changes |

## stopped_goal_checker plugin

| Parameter | Default | Description |
| ----------| --------| ------------|
| `<nav2_controller plugin>`.rot_stopped_velocity | 0.25 | Velocity below is considered to be stopped at tolerance met (rad/s) |
| `<nav2_controller plugin>`.trans_stopped_velocity | 0.25 | Velocity below is considered to be stopped at tolerance met (m/s) |

# dwb_controller

Expand Down Expand Up @@ -350,14 +375,6 @@ When `controller_plugins` parameter is not overridden, the following default plu
| `<dwb plugin>`.lookahead_time | -1 | If > 0, amount of time to look forward for a collision for. |
| `<dwb plugin>`.`<name>`.scale | 1.0 | Weight scale |

## simple_goal_checker plugin

| Parameter | Default | Description |
| ----------| --------| ------------|
| `<dwb plugin>`.xy_goal_tolerance | 0.25 | Tolerance to meet goal completion criteria (m) |
| `<dwb plugin>`.yaw_goal_tolerance | 0.25 | Tolerance to meet goal completion criteria (rad) |
| `<dwb plugin>`.stateful | true | Whether to check for XY position tolerance after rotating to goal orientation in case of minor localization changes |

## standard_traj_generator plugin

| Parameter | Default | Description |
Expand All @@ -375,13 +392,6 @@ When `controller_plugins` parameter is not overridden, the following default plu
| ----------| --------| ------------|
| `<dwb plugin>`.sim_time | N/A | Time to simulate ahead by (s) |

## stopped_goal_checker plugin

| Parameter | Default | Description |
| ----------| --------| ------------|
| `<dwb plugin>`.rot_stopped_velocity | 0.25 | Velocity below is considered to be stopped at tolerance met (rad/s) |
| `<dwb plugin>`.trans_stopped_velocity | 0.25 | Velocity below is considered to be stopped at tolerance met (m/s) |

# lifecycle_manager

| Parameter | Default | Description |
Expand Down
13 changes: 13 additions & 0 deletions nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down
13 changes: 13 additions & 0 deletions nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down
13 changes: 13 additions & 0 deletions nav2_bringup/bringup/params/nav2_params.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down
37 changes: 34 additions & 3 deletions nav2_controller/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand All @@ -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
Expand All @@ -46,20 +47,46 @@ set(dependencies
pluginlib
)

add_library(simple_progress_checker SHARED plugins/simple_progress_checker.cpp)
ament_target_dependencies(simple_progress_checker ${dependencies})
# prevent pluginlib from using boost
target_compile_definitions(simple_progress_checker PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")

add_library(simple_goal_checker SHARED plugins/simple_goal_checker.cpp)
ament_target_dependencies(simple_goal_checker ${dependencies})
# prevent pluginlib from using boost
target_compile_definitions(simple_goal_checker PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")

add_library(stopped_goal_checker SHARED plugins/stopped_goal_checker.cpp)
target_link_libraries(stopped_goal_checker simple_goal_checker)
ament_target_dependencies(stopped_goal_checker ${dependencies})
# prevent pluginlib from using boost
target_compile_definitions(stopped_goal_checker PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")

ament_target_dependencies(${library_name}
${dependencies}
)

# prevent pluginlib from using boost
target_compile_definitions(${library_name} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")

if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights
set(ament_cmake_copyright_FOUND TRUE)
ament_lint_auto_find_test_dependencies()

find_package(ament_cmake_gtest REQUIRED)
add_subdirectory(plugins/test)
endif()

ament_target_dependencies(${executable_name}
${dependencies}
)

target_link_libraries(${executable_name} ${library_name})

install(TARGETS ${library_name}
install(TARGETS simple_progress_checker simple_goal_checker stopped_goal_checker ${library_name}
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin
Expand All @@ -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()
20 changes: 18 additions & 2 deletions nav2_controller/include/nav2_controller/nav2_controller.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,8 @@
#include <vector>

#include "nav2_core/controller.hpp"
#include "nav2_core/progress_checker.hpp"
#include "nav2_core/goal_checker.hpp"
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
#include "tf2_ros/transform_listener.h"
#include "nav2_msgs/action/follow_path.hpp"
Expand Down Expand Up @@ -201,6 +203,22 @@ class ControllerServer : public nav2_util::LifecycleNode
std::unique_ptr<nav_2d_utils::OdomSubscriber> odom_sub_;
rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::Twist>::SharedPtr vel_publisher_;

// Progress Checker Plugin
pluginlib::ClassLoader<nav2_core::ProgressChecker> progress_checker_loader_;
nav2_core::ProgressChecker::Ptr progress_checker_;
std::string default_progress_checker_id_;
std::string default_progress_checker_type_;
std::string progress_checker_id_;
std::string progress_checker_type_;

// Goal Checker Plugin
pluginlib::ClassLoader<nav2_core::GoalChecker> goal_checker_loader_;
nav2_core::GoalChecker::Ptr goal_checker_;
std::string default_goal_checker_id_;
std::string default_goal_checker_type_;
std::string goal_checker_id_;
std::string goal_checker_type_;

// Controller Plugins
pluginlib::ClassLoader<nav2_core::Controller> lp_loader_;
ControllerMap controllers_;
Expand All @@ -210,8 +228,6 @@ class ControllerServer : public nav2_util::LifecycleNode
std::vector<std::string> controller_types_;
std::string controller_ids_concat_, current_controller_;

std::unique_ptr<ProgressChecker> progress_checker_;

double controller_frequency_;
double min_x_velocity_threshold_;
double min_y_velocity_threshold_;
Expand Down
10 changes: 5 additions & 5 deletions ...clude/dwb_plugins/simple_goal_checker.hpp → ...ontroller/plugins/simple_goal_checker.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 <memory>
#include <string>
Expand All @@ -42,7 +42,7 @@
#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "nav2_core/goal_checker.hpp"

namespace dwb_plugins
namespace nav2_controller
{

/**
Expand Down Expand Up @@ -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_
40 changes: 16 additions & 24 deletions ...lude/nav2_controller/progress_checker.hpp → ...oller/plugins/simple_progress_checker.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,40 +12,32 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef NAV2_CONTROLLER__PROGRESS_CHECKER_HPP_
#define NAV2_CONTROLLER__PROGRESS_CHECKER_HPP_
#ifndef NAV2_CONTROLLER__PLUGINS__SIMPLE_PROGRESS_CHECKER_HPP_
#define NAV2_CONTROLLER__PLUGINS__SIMPLE_PROGRESS_CHECKER_HPP_

#include <string>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "nav2_core/progress_checker.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/pose2_d.hpp"

namespace nav2_controller
{
/**
* @class nav2_controller::ProgressChecker
* @brief This class is used to check the position of the robot to make sure
* that it is actually progressing towards a goal.
*/
class ProgressChecker
* @class SimpleProgressChecker
* @brief This plugin is used to check the position of the robot to make sure
* that it is actually progressing towards a goal.
*/

class SimpleProgressChecker : public nav2_core::ProgressChecker
{
public:
/**
* @brief Constructor of ProgressChecker
* @param node Node pointer
*/
explicit ProgressChecker(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node);
/**
* @brief Checks if the robot has moved compare to previous
* pose
* @param current_pose Current pose of the robot
* @throw nav2_core::PlannerException when failed to make progress
*/
void check(geometry_msgs::msg::PoseStamped & current_pose);
/**
* @brief Reset class state upon calling
*/
void reset() {baseline_pose_set_ = false;}
void initialize(
const rclcpp_lifecycle::LifecycleNode::SharedPtr & node,
const std::string & plugin_name) override;
bool check(geometry_msgs::msg::PoseStamped & current_pose) override;
void reset() override;

protected:
/**
Expand All @@ -72,4 +64,4 @@ class ProgressChecker
};
} // namespace nav2_controller

#endif // NAV2_CONTROLLER__PROGRESS_CHECKER_HPP_
#endif // NAV2_CONTROLLER__PLUGINS__SIMPLE_PROGRESS_CHECKER_HPP_
12 changes: 6 additions & 6 deletions ...lude/dwb_plugins/stopped_goal_checker.hpp → ...ntroller/plugins/stopped_goal_checker.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -32,17 +32,17 @@
* POSSIBILITY OF SUCH DAMAGE.
*/

#ifndef DWB_PLUGINS__STOPPED_GOAL_CHECKER_HPP_
#define DWB_PLUGINS__STOPPED_GOAL_CHECKER_HPP_
#ifndef NAV2_CONTROLLER__PLUGINS__STOPPED_GOAL_CHECKER_HPP_
#define NAV2_CONTROLLER__PLUGINS__STOPPED_GOAL_CHECKER_HPP_

#include <memory>
#include <string>

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "dwb_plugins/simple_goal_checker.hpp"
#include "nav2_controller/plugins/simple_goal_checker.hpp"

namespace dwb_plugins
namespace nav2_controller
{

/**
Expand All @@ -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_
Loading