diff --git a/full_ros_build.Dockerfile b/full_ros_build.Dockerfile
index 85667a13902c69dc574ba992f33b3c219636a93d..a090b305b97acf8d316751ab04239dd8062b8216 100644
--- a/full_ros_build.Dockerfile
+++ b/full_ros_build.Dockerfile
@@ -63,6 +63,8 @@ RUN wget https://raw.githubusercontent.com/ros2/ros2/$ROS2_BRANCH/ros2.repos \
# get skip keys
COPY ./tools/skip_keys.txt ./
+RUN rosdep update
+
# copy underlay manifests
COPY --from=cache /tmp/underlay_ws src/underlay
RUN cd src/underlay && colcon list --names-only | \
diff --git a/nav2_amcl/include/nav2_amcl/amcl_node.hpp b/nav2_amcl/include/nav2_amcl/amcl_node.hpp
index 8801d98d6bf00b470eb7d8a69022d68f4d5f8545..763a601ed88dc2758ed94353f60f7e553d3975fa 100644
--- a/nav2_amcl/include/nav2_amcl/amcl_node.hpp
+++ b/nav2_amcl/include/nav2_amcl/amcl_node.hpp
@@ -34,7 +34,6 @@
#include "nav2_util/lifecycle_node.hpp"
#include "nav2_amcl/motion_model/motion_model.hpp"
#include "nav2_amcl/sensors/laser/laser.hpp"
-#include "nav2_util/parameter_events_subscriber.hpp"
#include "nav_msgs/srv/set_map.hpp"
#include "sensor_msgs/msg/laser_scan.hpp"
#include "std_srvs/srv/empty.hpp"
@@ -105,10 +104,6 @@ protected:
tf2::Transform latest_tf_;
void waitForTransforms();
- // Parameter Event Subscriber
- std::shared_ptr<nav2_util::ParameterEventsSubscriber> param_subscriber_;
- void parameterEventCallback(const rcl_interfaces::msg::ParameterEvent::SharedPtr & event);
-
// Message filters
void initMessageFilters();
std::unique_ptr<message_filters::Subscriber<sensor_msgs::msg::LaserScan>> laser_scan_sub_;
diff --git a/nav2_amcl/package.xml b/nav2_amcl/package.xml
index 2f07ac4bc3af0834361203901022118d1db6263e..2d25f6e383399c11d688b9e24449432d51c91f1c 100644
--- a/nav2_amcl/package.xml
+++ b/nav2_amcl/package.xml
@@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>nav2_amcl</name>
- <version>0.2.4</version>
+ <version>0.3.0</version>
<description>
<p>
amcl is a probabilistic localization system for a robot moving in
diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp
index 3c24cc5b1159314184d57f89b437bab237a24c2e..e857cf648de26c1071b440911c463b29e223c954 100644
--- a/nav2_amcl/src/amcl_node.cpp
+++ b/nav2_amcl/src/amcl_node.cpp
@@ -194,21 +194,14 @@ AmclNode::on_configure(const rclcpp_lifecycle::State & /*state*/)
RCLCPP_INFO(get_logger(), "Configuring");
initParameters();
- if (always_reset_initial_pose_) {
- initial_pose_is_known_ = false;
- }
initTransforms();
- initPubSub();
initMessageFilters();
+ initPubSub();
initServices();
initOdometry();
initParticleFilter();
initLaserScan();
- param_subscriber_ = std::make_shared<nav2_util::ParameterEventsSubscriber>(shared_from_this());
- param_subscriber_->set_event_callback(
- std::bind(&AmclNode::parameterEventCallback, this, std::placeholders::_1));
-
return nav2_util::CallbackReturn::SUCCESS;
}
@@ -350,15 +343,6 @@ AmclNode::on_shutdown(const rclcpp_lifecycle::State & /*state*/)
return nav2_util::CallbackReturn::SUCCESS;
}
-void
-AmclNode::parameterEventCallback(const rcl_interfaces::msg::ParameterEvent::SharedPtr & /*event*/)
-{
- initParameters();
- initMessageFilters();
- initOdometry();
- initParticleFilter();
-}
-
void
AmclNode::checkLaserReceived()
{
@@ -1056,6 +1040,10 @@ AmclNode::initParameters()
" this isn't allowed so max_particles will be set to min_particles.");
max_particles_ = min_particles_;
}
+
+ if (always_reset_initial_pose_) {
+ initial_pose_is_known_ = false;
+ }
}
void
@@ -1175,6 +1163,9 @@ AmclNode::initTransforms()
void
AmclNode::initMessageFilters()
{
+ laser_scan_sub_ = std::make_unique<message_filters::Subscriber<sensor_msgs::msg::LaserScan>>(
+ rclcpp_node_.get(), scan_topic_, rmw_qos_profile_sensor_data);
+
laser_scan_filter_ = std::make_unique<tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan>>(
*laser_scan_sub_, *tf_buffer_, odom_frame_id_, 10, rclcpp_node_);
@@ -1202,9 +1193,6 @@ AmclNode::initPubSub()
std::bind(&AmclNode::mapReceived, this, std::placeholders::_1));
RCLCPP_INFO(get_logger(), "Subscribed to map topic.");
-
- laser_scan_sub_ = std::make_unique<message_filters::Subscriber<sensor_msgs::msg::LaserScan>>(
- rclcpp_node_.get(), scan_topic_, rmw_qos_profile_sensor_data);
}
void
@@ -1237,9 +1225,7 @@ AmclNode::initOdometry()
init_cov_[1] = last_published_pose_.pose.covariance[7];
init_cov_[2] = last_published_pose_.pose.covariance[35];
}
- if (motion_model_) {
- motion_model_.reset();
- }
+
motion_model_ = std::unique_ptr<nav2_amcl::MotionModel>(nav2_amcl::MotionModel::createMotionModel(
robot_model_type_, alpha1_, alpha2_, alpha3_, alpha4_, alpha5_));
@@ -1249,11 +1235,6 @@ AmclNode::initOdometry()
void
AmclNode::initParticleFilter()
{
- if (pf_ != nullptr) {
- pf_free(pf_);
- pf_ = nullptr;
- }
-
// Create the particle filter
pf_ = pf_alloc(min_particles_, max_particles_, alpha_slow_, alpha_fast_,
(pf_init_model_fn_t)AmclNode::uniformPoseGenerator,
diff --git a/nav2_behavior_tree/README.md b/nav2_behavior_tree/README.md
index 2d7e3971aefd017e0f26c5aecf56378bdcd379c2..21daa18882f4def7fa1a9174d9992ae4d98ba8ba 100644
--- a/nav2_behavior_tree/README.md
+++ b/nav2_behavior_tree/README.md
@@ -9,7 +9,7 @@ This module is used by the nav2_bt_navigator to implement a ROS2 node that execu
## The bt_action_node Template and the Behavior Tree Engine
-The [bt_action_node template](include/nav2_behavior_tree/bt_action_node.hpp) allows one to easily integrate a ROS2 action into a BehaviorTree. To do so, one derives from the BTActionNode template, providing the action message type. For example,
+The [bt_action_node template](include/nav2_behavior_tree/bt_action_node.hpp) allows one to easily integrate a ROS2 action into a BehaviorTree. To do so, one derives from the BtActionNode template, providing the action message type. For example,
```C++
#include "nav2_msgs/action/follow_path.hpp"
@@ -66,8 +66,8 @@ The nav2_behavior_tree package provides several navigation-specific nodes that a
| BT Node | Type | Description |
|----------|:-------------|------|
| Backup | Action | Invokes the BackUp ROS2 action server, which causes the robot to back up to a specific pose. This is used in nav2 Behavior Trees as a recovery behavior. The nav2_recoveries module implements the BackUp action server. |
-| ComputePathToPose | Action | Invokes the ComputePathToPose ROS2 action server, which is implemented by the nav2_planner module. |
-| FollowPath | Action |Invokes the FollowPath ROS2 action server, which is implemented by the controller plugin modules loaded. |
+| ComputePathToPose | Action | Invokes the ComputePathToPose ROS2 action server, which is implemented by the nav2_planner module. The server address can be remapped using the `server_name` input port. |
+| FollowPath | Action |Invokes the FollowPath ROS2 action server, which is implemented by the controller plugin modules loaded. The server address can be remapped using the `server_name` input port. |
| GoalReached | Condition | Checks the distance to the goal, if the distance to goal is less than the pre-defined threshold, the tree returns SUCCESS, otherwise it returns FAILURE. |
| IsStuck | Condition | Determines if the robot is not progressing towards the goal. If the robot is stuck and not progressing, the condition returns SUCCESS, otherwise it returns FAILURE. |
| NavigateToPose | Action | Invokes the NavigateToPose ROS2 action server, which is implemented by the bt_navigator module. |
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp
index 89818b807f4ee6ab29f896dbfae219de6a8c0a9b..de0ecfbc2659ec8557ae398fc6a11287e312f061 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp
@@ -46,12 +46,7 @@ public:
config().blackboard->get<std::chrono::milliseconds>("server_timeout");
getInput<std::chrono::milliseconds>("server_timeout", server_timeout_);
- // Now that we have the ROS node to use, create the action client for this BT action
- action_client_ = rclcpp_action::create_client<ActionT>(node_, action_name_);
-
- // Make sure the server is actually there before continuing
- RCLCPP_INFO(node_->get_logger(), "Waiting for \"%s\" action server", action_name_.c_str());
- action_client_->wait_for_action_server();
+ createActionClient(action_name_);
// Give the derive class a chance to do any initialization
RCLCPP_INFO(node_->get_logger(), "\"%s\" BtActionNode initialized", xml_tag_name.c_str());
@@ -63,6 +58,17 @@ public:
{
}
+ // Create instance of an action server
+ void createActionClient(const std::string & action_name)
+ {
+ // Now that we have the ROS node to use, create the action client for this BT action
+ action_client_ = rclcpp_action::create_client<ActionT>(node_, action_name);
+
+ // Make sure the server is actually there before continuing
+ RCLCPP_INFO(node_->get_logger(), "Waiting for \"%s\" action server", action_name.c_str());
+ action_client_->wait_for_action_server();
+ }
+
// Any subclass of BtActionNode that accepts parameters must provide a providedPorts method
// and call providedBasicPorts in it.
static BT::PortsList providedBasicPorts(BT::PortsList addition)
diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml
index 719d0aa5c2459b8583b538ecad40ea8955aaf149..3665d197803b46a3cb3b6d1c8e442979533fd03d 100644
--- a/nav2_behavior_tree/package.xml
+++ b/nav2_behavior_tree/package.xml
@@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>nav2_behavior_tree</name>
- <version>0.2.4</version>
+ <version>0.3.0</version>
<description>TODO</description>
<maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
<maintainer email="carlos.a.orduno@intel.com">Carlos Orduno</maintainer>
diff --git a/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp b/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp
index 77f4109d684472a685b2f4e8a40008690e14b793..cd4a816614dff2767652c79362ba3a92e79c4ad0 100644
--- a/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp
+++ b/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp
@@ -34,6 +34,11 @@ public:
const BT::NodeConfiguration & conf)
: BtActionNode<nav2_msgs::action::ComputePathToPose>(xml_tag_name, action_name, conf)
{
+ std::string remapped_action_name;
+ if (getInput("server_name", remapped_action_name)) {
+ action_client_.reset();
+ createActionClient(remapped_action_name);
+ }
}
void on_tick() override
@@ -58,7 +63,8 @@ public:
return providedBasicPorts({
BT::OutputPort<nav_msgs::msg::Path>("path", "Path created by ComputePathToPose node"),
BT::InputPort<geometry_msgs::msg::PoseStamped>("goal", "Destination to plan to"),
- BT::InputPort<std::string>("planner_id", "")
+ BT::InputPort<std::string>("planner_id", ""),
+ BT::InputPort<std::string>("server_name", "")
});
}
diff --git a/nav2_behavior_tree/plugins/action/follow_path_action.cpp b/nav2_behavior_tree/plugins/action/follow_path_action.cpp
index 3809aab64e135ec96aa0e29edf1de88066ecdb89..e22f94fcee72faa307fb1c174c03c7ab57deb936 100644
--- a/nav2_behavior_tree/plugins/action/follow_path_action.cpp
+++ b/nav2_behavior_tree/plugins/action/follow_path_action.cpp
@@ -33,6 +33,11 @@ public:
const BT::NodeConfiguration & conf)
: BtActionNode<nav2_msgs::action::FollowPath>(xml_tag_name, action_name, conf)
{
+ std::string remapped_action_name;
+ if (getInput("server_name", remapped_action_name)) {
+ action_client_.reset();
+ createActionClient(remapped_action_name);
+ }
config().blackboard->set("path_updated", false);
}
@@ -61,6 +66,7 @@ public:
return providedBasicPorts({
BT::InputPort<nav_msgs::msg::Path>("path", "Path to follow"),
BT::InputPort<std::string>("controller_id", ""),
+ BT::InputPort<std::string>("server_name", "")
});
}
};
diff --git a/nav2_bringup/bringup/package.xml b/nav2_bringup/bringup/package.xml
index f09f5fab3c52b1769356254959e083d40c351723..f4646138b2cab172abe8d6f5c6bb5917c5659775 100644
--- a/nav2_bringup/bringup/package.xml
+++ b/nav2_bringup/bringup/package.xml
@@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>nav2_bringup</name>
- <version>0.2.4</version>
+ <version>0.3.0</version>
<description>Bringup scripts and configurations for the navigation2 stack</description>
<maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
<maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>
diff --git a/nav2_bringup/bringup/rviz/nav2_default_view.rviz b/nav2_bringup/bringup/rviz/nav2_default_view.rviz
index d871e38417efa4e8dc55ab19f297458750fd2190..e2981ba20d3dc3453ebaa528b6fed000985d03c2 100644
--- a/nav2_bringup/bringup/rviz/nav2_default_view.rviz
+++ b/nav2_bringup/bringup/rviz/nav2_default_view.rviz
@@ -425,6 +425,7 @@ Visualization Manager:
Value: true
Enabled: true
Name: Controller
+<<<<<<< HEAD
- Class: rviz_common/Group
Displays:
- Class: rviz_default_plugins/Image
@@ -476,6 +477,18 @@ Visualization Manager:
Value: true
Enabled: false
Name: Realsense
+ - Class: rviz_default_plugins/MarkerArray
+ Enabled: true
+ Name: MarkerArray
+ Namespaces:
+ {}
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /waypoints
+ Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
diff --git a/nav2_bringup/nav2_gazebo_spawner/package.xml b/nav2_bringup/nav2_gazebo_spawner/package.xml
index 2e37bba034280ae80af89771a63f39d2b35b0aac..01cfa8852d83d409b268729b000b57f7b5239f65 100644
--- a/nav2_bringup/nav2_gazebo_spawner/package.xml
+++ b/nav2_bringup/nav2_gazebo_spawner/package.xml
@@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>nav2_gazebo_spawner</name>
- <version>0.2.4</version>
+ <version>0.3.0</version>
<description>Package for spawning a robot model into Gazebo for navigation2</description>
<maintainer email="carlos.orduno@intel.com">lkumarbe</maintainer>
<maintainer email="lalit.kumar.begani@intel.com">lkumarbe</maintainer>
diff --git a/nav2_bringup/nav2_gazebo_spawner/setup.py b/nav2_bringup/nav2_gazebo_spawner/setup.py
index 640a2f1edf7de81a1f264e727ed59a8498b732f3..c71c075e7420a20a50433fa7a139a2295664ec1c 100644
--- a/nav2_bringup/nav2_gazebo_spawner/setup.py
+++ b/nav2_bringup/nav2_gazebo_spawner/setup.py
@@ -4,7 +4,7 @@ PACKAGE_NAME = 'nav2_gazebo_spawner'
setup(
name=PACKAGE_NAME,
- version='0.2.4',
+ version='0.3.0',
packages=[PACKAGE_NAME],
data_files=[
('share/ament_index/resource_index/packages',
diff --git a/nav2_bt_navigator/package.xml b/nav2_bt_navigator/package.xml
index 64d508e67a0f2d2a912afac55bc24c1b10b6c9c5..9f6a35158e993a2b04a60aa652eee0c12fa1bf6b 100644
--- a/nav2_bt_navigator/package.xml
+++ b/nav2_bt_navigator/package.xml
@@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>nav2_bt_navigator</name>
- <version>0.2.4</version>
+ <version>0.3.0</version>
<description>TODO</description>
<maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
<license>Apache-2.0</license>
diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp
index 7a5ad444f47ca5ad0aeecc1176a6bd00f5d7a02b..b9609243fe50a0914110dbbec764e33eb5c5d550 100644
--- a/nav2_bt_navigator/src/bt_navigator.cpp
+++ b/nav2_bt_navigator/src/bt_navigator.cpp
@@ -226,12 +226,12 @@ BtNavigator::navigateToPose()
case nav2_behavior_tree::BtStatus::FAILED:
RCLCPP_ERROR(get_logger(), "Navigation failed");
- action_server_->terminate_goals();
+ action_server_->terminate_current();
break;
case nav2_behavior_tree::BtStatus::CANCELED:
RCLCPP_INFO(get_logger(), "Navigation canceled");
- action_server_->terminate_goals();
+ action_server_->terminate_all();
// Reset the BT so that it can be run again in the future
bt_->resetTree(tree_->root_node);
break;
diff --git a/nav2_common/package.xml b/nav2_common/package.xml
index eddcb9e4144558f2af37f80397c906747357f081..031c0fecead41350a7bd777177aaabc4215328e8 100644
--- a/nav2_common/package.xml
+++ b/nav2_common/package.xml
@@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>nav2_common</name>
- <version>0.2.4</version>
+ <version>0.3.0</version>
<description>Common support functionality used throughout the navigation 2 stack</description>
<maintainer email="carl.r.delsey@intel.com">Carl Delsey</maintainer>
<license>Apache-2.0</license>
diff --git a/nav2_controller/package.xml b/nav2_controller/package.xml
index d81a2afc4c0d50a625f2a7890e8753de2f5f1512..443826209ae523888e6e6f2d302ea0e4df65b88c 100644
--- a/nav2_controller/package.xml
+++ b/nav2_controller/package.xml
@@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>nav2_controller</name>
- <version>0.2.4</version>
+ <version>0.3.0</version>
<description>Controller action interface</description>
<maintainer email="carl.r.delsey@intel.com">Carl Delsey</maintainer>
<license>Apache-2.0</license>
diff --git a/nav2_controller/src/nav2_controller.cpp b/nav2_controller/src/nav2_controller.cpp
index a2ce6decb23453f7b76943dda5e3128bb0f5a3b0..2b911467691347c67f02011716fc6e41c9b9dde6 100644
--- a/nav2_controller/src/nav2_controller.cpp
+++ b/nav2_controller/src/nav2_controller.cpp
@@ -38,9 +38,8 @@ ControllerServer::ControllerServer()
std::vector<std::string> default_id, default_type;
default_type.push_back("dwb_core::DWBLocalPlanner");
default_id.push_back("FollowPath");
- controller_ids_ = declare_parameter("controller_plugin_ids",
- default_id);
- controller_types_ = declare_parameter("controller_plugin_types", default_type);
+ declare_parameter("controller_plugin_ids", default_id);
+ declare_parameter("controller_plugin_types", default_type);
// The costmap node is used in the implementation of the controller
costmap_ros_ = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
@@ -60,6 +59,11 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state)
{
RCLCPP_INFO(get_logger(), "Configuring controller interface");
+ get_parameter("controller_plugin_ids", controller_ids_);
+ get_parameter("controller_plugin_types", controller_types_);
+ get_parameter("controller_frequency", controller_frequency_);
+ RCLCPP_INFO(get_logger(), "Controller frequency set to %.4fHz", controller_frequency_);
+
costmap_ros_->on_configure(state);
auto node = shared_from_this();
@@ -92,9 +96,6 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state)
controller_ids_concat_ += controller_ids_[i] + std::string(" ");
}
- get_parameter("controller_frequency", controller_frequency_);
- RCLCPP_INFO(get_logger(), "Controller frequency set to %.4fHz", controller_frequency_);
-
odom_sub_ = std::make_unique<nav_2d_utils::OdomSubscriber>(node);
vel_publisher_ = create_publisher<geometry_msgs::msg::Twist>("cmd_vel", 1);
@@ -149,6 +150,7 @@ ControllerServer::on_cleanup(const rclcpp_lifecycle::State & state)
for (it = controllers_.begin(); it != controllers_.end(); ++it) {
it->second->cleanup();
}
+ controllers_.clear();
costmap_ros_->on_cleanup(state);
// Release any allocated resources
@@ -198,7 +200,7 @@ void ControllerServer::computeControl()
RCLCPP_ERROR(get_logger(), "FollowPath called with controller name %s, "
"which does not exist. Available controllers are %s.",
c_name.c_str(), controller_ids_concat_.c_str());
- action_server_->terminate_goals();
+ action_server_->terminate_current();
return;
}
} else {
@@ -222,7 +224,7 @@ void ControllerServer::computeControl()
if (action_server_->is_cancel_requested()) {
RCLCPP_INFO(get_logger(), "Goal was canceled. Stopping the robot.");
- action_server_->terminate_goals();
+ action_server_->terminate_all();
publishZeroVelocity();
return;
}
@@ -244,7 +246,7 @@ void ControllerServer::computeControl()
} catch (nav2_core::PlannerException & e) {
RCLCPP_ERROR(this->get_logger(), e.what());
publishZeroVelocity();
- action_server_->terminate_goals();
+ action_server_->terminate_current();
return;
}
diff --git a/nav2_core/package.xml b/nav2_core/package.xml
index 8fc23b679627f1dcd2dce21592f754a79da6b8e3..bff00b7a64148dc044b0aea78f9ed6561a1180cc 100644
--- a/nav2_core/package.xml
+++ b/nav2_core/package.xml
@@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>nav2_core</name>
- <version>0.2.4</version>
+ <version>0.3.0</version>
<description>A set of headers for plugins core to the navigation2 stack</description>
<maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>
<maintainer email="carl.r.delsey@intel.com">Carl Delsey</maintainer>
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp
index 943871a3191af343f02e01bfa0f6c3bf7908ed05..c139f28b65efbed6c6efe6062c5abeb6a7949239 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp
@@ -50,7 +50,6 @@
#include "nav2_costmap_2d/layered_costmap.hpp"
#include "nav2_costmap_2d/layer.hpp"
#include "nav2_util/lifecycle_node.hpp"
-#include "nav2_util/parameter_events_subscriber.hpp"
#include "pluginlib/class_loader.hpp"
#include "tf2/convert.h"
#include "tf2/LinearMath/Transform.h"
@@ -280,8 +279,6 @@ protected:
// Parameters
void getParameters();
- void paramEventCallback(const rcl_interfaces::msg::ParameterEvent::SharedPtr & event);
-
bool always_send_full_costmap_{false};
std::string footprint_;
float footprint_padding_{0};
@@ -307,8 +304,6 @@ protected:
std::vector<geometry_msgs::msg::Point> padded_footprint_;
std::unique_ptr<ClearCostmapService> clear_costmap_service_;
-
- std::shared_ptr<nav2_util::ParameterEventsSubscriber> param_subscriber_;
};
} // namespace nav2_costmap_2d
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp
index 49f21ebe44cc35a608c8579b2af9279bc07807b2..edc4803061263149f268ada1fc5556f8a1808e05 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp
@@ -99,8 +99,7 @@ public:
virtual void reset()
{
- undeclareAllParameters();
- onInitialize();
+ matchSize();
}
/** @brief Given a distance, compute a cost.
@@ -169,8 +168,6 @@ private:
unsigned int index, unsigned int mx, unsigned int my,
unsigned int src_x, unsigned int src_y);
- void setParamCallbacks();
-
double inflation_radius_, inscribed_radius_, cost_scaling_factor_;
bool inflate_unknown_;
unsigned int cell_inflation_radius_;
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp
index 6cd9f3ac3d79f90bc9bb80fe647e7e75b5953fd4..b0eca7251a160316406b27d54323a1d4726667b2 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp
@@ -46,7 +46,6 @@
#include "nav2_costmap_2d/costmap_2d.hpp"
#include "nav2_costmap_2d/layered_costmap.hpp"
#include "nav2_util/lifecycle_node.hpp"
-#include "nav2_util/parameter_events_subscriber.hpp"
namespace nav2_costmap_2d
{
@@ -65,11 +64,10 @@ public:
tf2_ros::Buffer * tf,
nav2_util::LifecycleNode::SharedPtr node,
rclcpp::Node::SharedPtr client_node,
- rclcpp::Node::SharedPtr rclcpp_node,
- std::shared_ptr<nav2_util::ParameterEventsSubscriber> param_subscriber = nullptr);
+ rclcpp::Node::SharedPtr rclcpp_node);
virtual void deactivate() {} /** @brief Stop publishers. */
virtual void activate() {} /** @brief Restart publishers if they've been stopped. */
- virtual void reset() {}
+ virtual void reset() = 0;
/**
* @brief This is called by the LayeredCostmap to poll this plugin as to how
@@ -137,8 +135,6 @@ protected:
nav2_util::LifecycleNode::SharedPtr node_;
rclcpp::Node::SharedPtr client_node_;
rclcpp::Node::SharedPtr rclcpp_node_;
- std::shared_ptr<nav2_util::ParameterEventsSubscriber> param_subscriber_;
- std::vector<nav2_util::ParameterEventsCallbackHandle::SharedPtr> callback_handles_;
/** @brief This is called at the end of initialize(). Override to
* implement subclass-specific initialization.
@@ -155,8 +151,6 @@ protected:
// Names of the parameters declared on the ROS node
std::unordered_set<std::string> local_params_;
- std::recursive_mutex mutex_;
-
private:
std::vector<geometry_msgs::msg::Point> footprint_spec_;
};
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp
index 69816f5e921837bccbba78b453c5ae86515d291a..8e56cb3836a5db38c98fc6d8ac516106dcaf70bf 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp
@@ -83,6 +83,10 @@ public:
virtual void activate();
virtual void deactivate();
virtual void reset();
+ /**
+ * @brief triggers the update of observations buffer
+ */
+ void resetBuffersLastUpdated();
/**
* @brief A callback to handle buffering LaserScan messages
@@ -160,8 +164,6 @@ protected:
double * max_x,
double * max_y);
- void setParamCallbacks();
-
std::string global_frame_; ///< @brief The global frame for the costmap
double max_obstacle_height_; ///< @brief Max Obstacle Height
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp
index 41ade34c811a1f13d0a61b04ace02c4cddcee276..764496cc68956b12c0243540f573e3cc0c444050 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp
@@ -85,7 +85,7 @@ protected:
virtual void resetMaps();
private:
- void setParamCallbacks();
+ void reconfigureCB();
void clearNonLethal(double wx, double wy, double w_size_x, double w_size_y, bool clear_no_info);
virtual void raytraceFreespace(
const nav2_costmap_2d::Observation & clearing_observation,
diff --git a/nav2_costmap_2d/package.xml b/nav2_costmap_2d/package.xml
index 19dc49a6a8e1ce162abcdfa8eb19109721277e3f..da024b34f422a25cb8ece7c8338db8fd173c80f0 100644
--- a/nav2_costmap_2d/package.xml
+++ b/nav2_costmap_2d/package.xml
@@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format ="3">
<name>nav2_costmap_2d</name>
- <version>0.2.4</version>
+ <version>0.3.0</version>
<description>
This package provides an implementation of a 2D costmap that takes in sensor
data from the world, builds a 2D or 3D occupancy grid of the data (depending
diff --git a/nav2_costmap_2d/plugins/inflation_layer.cpp b/nav2_costmap_2d/plugins/inflation_layer.cpp
index 10988cda84580c10185aa9dbabf711adc776f2b9..7206334c91282bea54080f42e7ed18f4dd0ff987 100644
--- a/nav2_costmap_2d/plugins/inflation_layer.cpp
+++ b/nav2_costmap_2d/plugins/inflation_layer.cpp
@@ -75,8 +75,6 @@ InflationLayer::InflationLayer()
void
InflationLayer::onInitialize()
{
- RCLCPP_INFO(node_->get_logger(), "Initializing inflation layer!");
-
declareParameter("enabled", rclcpp::ParameterValue(true));
declareParameter("inflation_radius", rclcpp::ParameterValue(0.55));
declareParameter("cost_scaling_factor", rclcpp::ParameterValue(10.0));
@@ -92,44 +90,6 @@ InflationLayer::onInitialize()
need_reinflation_ = false;
cell_inflation_radius_ = cellDistance(inflation_radius_);
matchSize();
- setParamCallbacks();
-}
-
-void
-InflationLayer::setParamCallbacks()
-{
- if (param_subscriber_) {
- callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ + ".enabled",
- [&](const rclcpp::Parameter & p) {
- std::lock_guard<std::recursive_mutex> lock(mutex_);
- enabled_ = p.get_value<bool>();
- need_reinflation_ = true;
- }));
- callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ +
- ".inflate_unknown",
- [&](const rclcpp::Parameter & p) {
- std::lock_guard<std::recursive_mutex> lock(mutex_);
- inflate_unknown_ = p.get_value<bool>();
- need_reinflation_ = true;
- }));
- callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ +
- ".cost_scaling_factor",
- [&](const rclcpp::Parameter & p) {
- std::lock_guard<std::recursive_mutex> lock(mutex_);
- cost_scaling_factor_ = p.get_value<double>();
- need_reinflation_ = true;
- computeCaches();
- }));
- callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ +
- ".inflation_radius",
- [&](const rclcpp::Parameter & p) {
- std::lock_guard<std::recursive_mutex> lock(mutex_);
- inflation_radius_ = p.get_value<double>();
- cell_inflation_radius_ = cellDistance(inflation_radius_);
- need_reinflation_ = true;
- computeCaches();
- }));
- }
}
void
diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp
index 3ab2e7da16d551e51388fa84a01dc78cfec4589a..983343e3fedfade209779773c09ebe9b69b76a2c 100644
--- a/nav2_costmap_2d/plugins/obstacle_layer.cpp
+++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp
@@ -243,37 +243,6 @@ void ObstacleLayer::onInitialize()
observation_notifiers_.back()->setTargetFrames(target_frames);
}
}
- setParamCallbacks();
-}
-
-void
-ObstacleLayer::setParamCallbacks()
-{
- if (param_subscriber_) {
- callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ + ".enabled",
- [&](const rclcpp::Parameter & p) {
- std::lock_guard<std::recursive_mutex> lock(mutex_);
- enabled_ = p.get_value<bool>();
- }));
- callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ +
- ".footprint_clearing_enabled",
- [&](const rclcpp::Parameter & p) {
- std::lock_guard<std::recursive_mutex> lock(mutex_);
- footprint_clearing_enabled_ = p.get_value<bool>();
- }));
- callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ +
- ".max_obstacle_height",
- [&](const rclcpp::Parameter & p) {
- std::lock_guard<std::recursive_mutex> lock(mutex_);
- max_obstacle_height_ = p.get_value<double>();
- }));
- callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ +
- ".combination_method",
- [&](const rclcpp::Parameter & p) {
- std::lock_guard<std::recursive_mutex> lock(mutex_);
- combination_method_ = p.get_value<int>();
- }));
- }
}
void
@@ -619,13 +588,9 @@ ObstacleLayer::activate()
observation_subscribers_[i]->subscribe();
}
}
-
- for (unsigned int i = 0; i < observation_buffers_.size(); ++i) {
- if (observation_buffers_[i]) {
- observation_buffers_[i]->resetLastUpdated();
- }
- }
+ resetBuffersLastUpdated();
}
+
void
ObstacleLayer::deactivate()
{
@@ -651,11 +616,19 @@ ObstacleLayer::updateRaytraceBounds(
void
ObstacleLayer::reset()
{
- deactivate();
resetMaps();
+ resetBuffersLastUpdated();
current_ = true;
- activate();
- undeclareAllParameters();
+}
+
+void
+ObstacleLayer::resetBuffersLastUpdated()
+{
+ for (unsigned int i = 0; i < observation_buffers_.size(); ++i) {
+ if (observation_buffers_[i]) {
+ observation_buffers_[i]->resetLastUpdated();
+ }
+ }
}
} // namespace nav2_costmap_2d
diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp
index aa775208c3b51d02838c33f84d9125b77f4a4fd5..c97c050e70f47ad1fe83f6d8d6f3735cd8cbbfa6 100644
--- a/nav2_costmap_2d/plugins/static_layer.cpp
+++ b/nav2_costmap_2d/plugins/static_layer.cpp
@@ -94,19 +94,6 @@ StaticLayer::onInitialize()
rclcpp::SystemDefaultsQoS(),
std::bind(&StaticLayer::incomingUpdate, this, std::placeholders::_1));
}
-
- auto cb = [&](const rclcpp::Parameter & p) {
- if (enabled_ != p.get_value<bool>()) {
- enabled_ = p.get_value<bool>();
- has_updated_data_ = true;
- x_ = y_ = 0;
- width_ = size_x_;
- height_ = size_y_;
- }
- };
- if (param_subscriber_) {
- callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ + ".enabled", cb));
- }
}
void
@@ -122,11 +109,7 @@ StaticLayer::deactivate()
void
StaticLayer::reset()
{
- map_sub_.reset();
- map_update_sub_.reset();
-
- undeclareAllParameters();
- onInitialize();
+ has_updated_data_ = true;
}
void
diff --git a/nav2_costmap_2d/plugins/voxel_layer.cpp b/nav2_costmap_2d/plugins/voxel_layer.cpp
index 0b3afabf5068ea119f0c473c361d12ca45981fa1..8a68b960d67b42afcefb842dc03e43f52e504b8f 100644
--- a/nav2_costmap_2d/plugins/voxel_layer.cpp
+++ b/nav2_costmap_2d/plugins/voxel_layer.cpp
@@ -95,63 +95,6 @@ void VoxelLayer::onInitialize()
unknown_threshold_ += (VOXEL_BITS - size_z_);
matchSize();
- setParamCallbacks();
-}
-
-void
-VoxelLayer::setParamCallbacks()
-{
- if (param_subscriber_) {
- callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ + ".enabled",
- [&](const rclcpp::Parameter & p) {
- std::lock_guard<std::recursive_mutex> lock(mutex_);
- enabled_ = p.get_value<bool>();
- }));
- callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ +
- ".footprint_clearing_enabled",
- [&](const rclcpp::Parameter & p) {
- std::lock_guard<std::recursive_mutex> lock(mutex_);
- footprint_clearing_enabled_ = p.get_value<bool>();
- }));
- callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ +
- ".max_obstacle_height",
- [&](const rclcpp::Parameter & p) {
- std::lock_guard<std::recursive_mutex> lock(mutex_);
- max_obstacle_height_ = p.get_value<double>();
- }));
- callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ +
- ".combination_method",
- [&](const rclcpp::Parameter & p) {
- std::lock_guard<std::recursive_mutex> lock(mutex_);
- combination_method_ = p.get_value<int>();
- }));
- callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ + ".z_voxels",
- [&](const rclcpp::Parameter & p) {
- std::lock_guard<std::recursive_mutex> lock(mutex_);
- size_z_ = p.get_value<int>();
- }));
- callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ + ".origin_z",
- [&](const rclcpp::Parameter & p) {
- std::lock_guard<std::recursive_mutex> lock(mutex_);
- origin_z_ = p.get_value<double>();
- }));
- callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ + ".z_resolution",
- [&](const rclcpp::Parameter & p) {
- std::lock_guard<std::recursive_mutex> lock(mutex_);
- z_resolution_ = p.get_value<double>();
- }));
- callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ +
- ".unknown_threshold",
- [&](const rclcpp::Parameter & p) {
- std::lock_guard<std::recursive_mutex> lock(mutex_);
- unknown_threshold_ = p.get_value<int>() + (VOXEL_BITS - size_z_);
- }));
- callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ + ".mark_threshold",
- [&](const rclcpp::Parameter & p) {
- std::lock_guard<std::recursive_mutex> lock(mutex_);
- mark_threshold_ = p.get_value<int>();
- }));
- }
}
VoxelLayer::~VoxelLayer()
@@ -167,17 +110,18 @@ void VoxelLayer::matchSize()
void VoxelLayer::reset()
{
- voxel_pub_->on_deactivate();
- deactivate();
+ // Call the base class method before adding our own functionality
+ ObstacleLayer::reset();
resetMaps();
- activate();
- undeclareAllParameters();
- voxel_pub_->on_activate();
}
void VoxelLayer::resetMaps()
{
- Costmap2D::resetMaps();
+ // Call the base class method before adding our own functionality
+ // Note: at the time this was written, ObstacleLayer doesn't implement
+ // resetMaps so this goes to the next layer down Costmap2DLayer which also
+ // doesn't implement this, so it actually goes all the way to Costmap2D
+ ObstacleLayer::resetMaps();
voxel_grid_.reset();
}
diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp
index f1afd9d3af8aa9ce2399861df78242b1097ab1a7..4b232743d26c58ac130be19a03695a6b22589054 100644
--- a/nav2_costmap_2d/src/costmap_2d_ros.cpp
+++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp
@@ -137,8 +137,6 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/)
tf_buffer_->setCreateTimerInterface(timer_interface);
tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
- param_subscriber_ = std::make_shared<nav2_util::ParameterEventsSubscriber>(shared_from_this());
-
// Then load and add the plug-ins to the costmap
for (unsigned int i = 0; i < plugin_names_.size(); ++i) {
RCLCPP_INFO(get_logger(), "Using plugin \"%s\"", plugin_names_[i].c_str());
@@ -148,7 +146,7 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/)
// TODO(mjeronimo): instead of get(), use a shared ptr
plugin->initialize(layered_costmap_, plugin_names_[i], tf_buffer_.get(),
- shared_from_this(), client_node_, rclcpp_node_, param_subscriber_);
+ shared_from_this(), client_node_, rclcpp_node_);
RCLCPP_INFO(get_logger(), "Initialized plugin \"%s\"", plugin_names_[i].c_str());
}
@@ -177,8 +175,6 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/)
// Add cleaning service
clear_costmap_service_ = std::make_unique<ClearCostmapService>(shared_from_this(), *this);
- param_subscriber_->set_event_callback(
- std::bind(&Costmap2DROS::paramEventCallback, this, std::placeholders::_1));
return nav2_util::CallbackReturn::SUCCESS;
}
@@ -249,7 +245,6 @@ Costmap2DROS::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
{
RCLCPP_INFO(get_logger(), "Cleaning up");
- resetLayers();
delete layered_costmap_;
layered_costmap_ = nullptr;
@@ -283,72 +278,6 @@ Costmap2DROS::on_shutdown(const rclcpp_lifecycle::State &)
return nav2_util::CallbackReturn::SUCCESS;
}
-void
-Costmap2DROS::paramEventCallback(const rcl_interfaces::msg::ParameterEvent::SharedPtr & /*event*/)
-{
- if (map_update_thread_ != NULL) {
- map_update_thread_shutdown_ = true;
- map_update_thread_->join();
- delete map_update_thread_;
- }
- map_update_thread_shutdown_ = false;
-
- get_parameter("transform_tolerance", transform_tolerance_);
- get_parameter("map_update_frequency", map_update_frequency_);
- get_parameter("map_publish_frequency", map_publish_frequency_);
- if (map_publish_frequency_ > 0) {
- publish_cycle_ = rclcpp::Duration::from_seconds(1 / map_publish_frequency_);
- } else {
- publish_cycle_ = rclcpp::Duration(-1);
- }
-
- get_parameter("width", map_width_meters_);
- get_parameter("height", map_height_meters_);
- get_parameter("resolution", resolution_);
- get_parameter("origin_x", origin_x_);
- get_parameter("origin_y", origin_y_);
- if (!layered_costmap_->isSizeLocked()) {
- layered_costmap_->resizeMap((unsigned int)(map_width_meters_ / resolution_),
- (unsigned int)(map_height_meters_ / resolution_), resolution_, origin_x_, origin_y_);
- }
-
- double footprint_padding;
- get_parameter("footprint_padding", footprint_padding);
- if (footprint_padding_ != footprint_padding) {
- footprint_padding_ = footprint_padding;
- setRobotFootprint(unpadded_footprint_);
- }
-
- std::string footprint;
- double robot_radius;
- get_parameter("footprint", footprint);
- get_parameter("robot_radius", robot_radius);
- if (footprint_ != footprint || robot_radius_ != robot_radius) {
- footprint_ = footprint;
- robot_radius_ = robot_radius;
- use_radius_ = true;
- if (footprint_ != "" && footprint_ != "[]") {
- std::vector<geometry_msgs::msg::Point> new_footprint;
- if (makeFootprintFromString(footprint_, new_footprint)) {
- use_radius_ = false;
- } else {
- RCLCPP_ERROR(
- get_logger(), "The footprint parameter is invalid: \"%s\", using radius (%lf) instead",
- footprint_.c_str(), robot_radius_);
- }
- }
- if (use_radius_) {
- setRobotFootprint(makeFootprintFromRadius(robot_radius_));
- } else {
- std::vector<geometry_msgs::msg::Point> new_footprint;
- makeFootprintFromString(footprint_, new_footprint);
- setRobotFootprint(new_footprint);
- }
- }
- map_update_thread_ = new std::thread(std::bind(
- &Costmap2DROS::mapUpdateLoop, this, map_update_frequency_));
-}
-
void
Costmap2DROS::getParameters()
{
diff --git a/nav2_costmap_2d/src/layer.cpp b/nav2_costmap_2d/src/layer.cpp
index c51e43f8db1578d11ddac86b2bdf18afc9500566..228c060f70d10ac93b04e73c824cc22269567004 100644
--- a/nav2_costmap_2d/src/layer.cpp
+++ b/nav2_costmap_2d/src/layer.cpp
@@ -49,8 +49,7 @@ Layer::initialize(
LayeredCostmap * parent, std::string name, tf2_ros::Buffer * tf,
nav2_util::LifecycleNode::SharedPtr node,
rclcpp::Node::SharedPtr client_node,
- rclcpp::Node::SharedPtr rclcpp_node,
- std::shared_ptr<nav2_util::ParameterEventsSubscriber> param_subscriber)
+ rclcpp::Node::SharedPtr rclcpp_node)
{
layered_costmap_ = parent;
name_ = name;
@@ -58,7 +57,6 @@ Layer::initialize(
node_ = node;
client_node_ = client_node;
rclcpp_node_ = rclcpp_node;
- param_subscriber_ = param_subscriber;
onInitialize();
}
diff --git a/nav2_costmap_2d/test/integration/obstacle_tests.cpp b/nav2_costmap_2d/test/integration/obstacle_tests.cpp
index 0f1ddd3928226202530ca1dbb231ebc59caa97de..e82718dbef2350806b554a554c5b78a48791aac9 100644
--- a/nav2_costmap_2d/test/integration/obstacle_tests.cpp
+++ b/nav2_costmap_2d/test/integration/obstacle_tests.cpp
@@ -357,20 +357,9 @@ TEST_F(TestNode, testRepeatedResets) {
return plugin->hasParameter(layer_param);
}));
- // Reset all layers. This will un-declare all params and might re-declare internal ones
- // Should run without throwing exceptions
+ // Reset all layers. Parameters should be declared if not declared, otherwise skipped.
ASSERT_NO_THROW(
for_each(begin(*plugins), end(*plugins), [](const auto & plugin) {
plugin->reset();
}));
-
- // Check for node-level param
- ASSERT_TRUE(node_->has_parameter(node_dummy.first));
-
- // Layer-level parameters shouldn't be found
- ASSERT_TRUE(
- none_of(begin(*plugins), end(*plugins), [&layer_dummy](const auto & plugin) {
- string layer_param = layer_dummy.first + "_" + plugin->getName();
- return plugin->hasParameter(layer_param);
- }));
}
diff --git a/nav2_dwb_controller/README.md b/nav2_dwb_controller/README.md
index 8d9193a1c0023ff02b6aa40f59f07aea80c98fe7..976cdc646f0368e1829ebcd016a4c8b8d06d2260 100644
--- a/nav2_dwb_controller/README.md
+++ b/nav2_dwb_controller/README.md
@@ -32,3 +32,57 @@ For the most part, the DWB codebase is unchanged, other than what was required f
## New local planner interface
See `nav2_core` `Controller` interface, which defines an interface for the controller plugins. These plugins are loaded into the `nav2_controller_server` to compute control commands from requests to the ROS2 action server.
+
+## DWB Plugins
+
+DWB is highly configurable through the use of plugins. There are three types of
+plugins used. For each of them, a few implementations are available but you can
+create custom ones if you need to.
+
+### Trajectory Generator Plugins
+
+These plugins generate the set of possible trajectories that should be evaluated
+by the critics. The trajectory with the best score determines the output command
+velocity.
+
+There are two trajectory generators provided with Navigation 2. Only one can be
+loaded at a time.
+
+* **StandardTrajectoryGenerator** - This is similar to the trajectory rollout
+ algorithm used in base_local_planner in ROS 1.
+* **LimitedAccelGenerator** - This is similar to DWA used in ROS 1.
+
+### Goal Checker Plugins
+
+These plugins check whether we have reached the goal or not. Again, only one can
+be loaded at a time.
+
+* **SimpleGoalChecker** - This checks whether the robot has reached the goal pose
+* **StoppedGoalChecker** - This checks whether the robot has reached the goal pose
+ and come to a stop.
+
+### Critic Plugins
+
+These plugins score the trajectories generated by the trajectory generator.
+Multiple plugins can be loaded and the sum of their scores determines the chosen
+command velocity.
+
+* **BaseObstacle** - Scores a trajectory based on where the path passes over the
+ costmap. To use this properly, you must use the inflation layer in costmap to
+ expand obstacles by the robot's radius.
+* **ObstacleFootprint** - Scores a trajectory based on verifying all points along
+ the robot's footprint don't touch an obstacle marked in the costmap.
+* **GoalAlign** - Scores a trajectory based on how well aligned the trajectory is
+ with the goal pose.
+* **GoalDist** - Scores a trajectory based on how close the trajectory gets the robot
+ to the goal pose.
+* **PathAlign** - Scores a trajectory based on how well it is aligned to the path
+ provided by the global planner.
+* **PathDist** - Scores a trajectory based on how far it ends up from the path
+ provided by the global planner.
+* **PreferForward** - Scores trajectories that move the robot forwards more highly
+* **RotateToGoal** - Only allows the robot to rotate to the goal orientation when it
+ is sufficiently close to the goal location
+* **Oscillation** - Prevents the robot from just moving backwards and forwards.
+* **Twirling** - Prevents holonomic robots from spinning as they make their way to
+ the goal.
diff --git a/nav2_dwb_controller/costmap_queue/package.xml b/nav2_dwb_controller/costmap_queue/package.xml
index 47ca2cba935e9b4a42ab7f67f19b02ec1a3e9e86..f371cbebd314aa528e705139bfb7fe5a15ff5c84 100644
--- a/nav2_dwb_controller/costmap_queue/package.xml
+++ b/nav2_dwb_controller/costmap_queue/package.xml
@@ -1,7 +1,7 @@
<?xml version="1.0"?>
<package format="2">
<name>costmap_queue</name>
- <version>0.2.4</version>
+ <version>0.3.0</version>
<description>The costmap_queue package</description>
<maintainer email="davidvlu@gmail.com">David V. Lu!!</maintainer>
<license>BSD-3-Clause</license>
diff --git a/nav2_dwb_controller/dwb_core/package.xml b/nav2_dwb_controller/dwb_core/package.xml
index 5701c13a9120d605ae0d1df9c71d35db3c00f8a1..4c97237b715af15492eacde04d6ad9667fce9fa1 100644
--- a/nav2_dwb_controller/dwb_core/package.xml
+++ b/nav2_dwb_controller/dwb_core/package.xml
@@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>dwb_core</name>
- <version>0.2.4</version>
+ <version>0.3.0</version>
<description>TODO</description>
<maintainer email="carl.r.delsey@intel.com">Carl Delsey</maintainer>
<license>BSD-3-Clause</license>
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 ba381910cdf703852407c89ff6003ff3860ac8b9..933a39454db79510b0d5bff83251997304ddc264 100644
--- a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp
+++ b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp
@@ -81,7 +81,6 @@ void DWBLocalPlanner::configure(
rclcpp::ParameterValue(std::string("dwb_plugins::StandardTrajectoryGenerator")));
declare_parameter_if_not_declared(node_, "goal_checker_name",
rclcpp::ParameterValue(std::string("dwb_plugins::SimpleGoalChecker")));
- declare_parameter_if_not_declared(node_, "use_dwa", rclcpp::ParameterValue(false));
declare_parameter_if_not_declared(node_, "transform_tolerance", rclcpp::ParameterValue(0.1));
std::string traj_generator_name;
diff --git a/nav2_dwb_controller/dwb_critics/package.xml b/nav2_dwb_controller/dwb_critics/package.xml
index 0e21f612f207026f85b2f0139123c03c4134be00..cdf47d352231a5411e182dc25d697e2e50b40acf 100644
--- a/nav2_dwb_controller/dwb_critics/package.xml
+++ b/nav2_dwb_controller/dwb_critics/package.xml
@@ -1,7 +1,7 @@
<?xml version="1.0"?>
<package format="2">
<name>dwb_critics</name>
- <version>0.2.4</version>
+ <version>0.3.0</version>
<description>The dwb_critics package</description>
<maintainer email="davidvlu@gmail.com">David V. Lu!!</maintainer>
<license>BSD-3-Clause</license>
diff --git a/nav2_dwb_controller/dwb_msgs/package.xml b/nav2_dwb_controller/dwb_msgs/package.xml
index 51404c49b2e205839a1bc5c7380ddb1f54c3491e..eb02a2d7af6e1361e2eb830df10a4306c3b78ab5 100644
--- a/nav2_dwb_controller/dwb_msgs/package.xml
+++ b/nav2_dwb_controller/dwb_msgs/package.xml
@@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>dwb_msgs</name>
- <version>0.2.4</version>
+ <version>0.3.0</version>
<description>Message/Service definitions specifically for the dwb_core</description>
<maintainer email="davidvlu@gmail.com">David V. Lu!!</maintainer>
<license>BSD-3-Clause</license>
diff --git a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/limited_accel_generator.hpp b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/limited_accel_generator.hpp
index b37cdf59880f9c41fb6115edceb1afbf46c017eb..edbf001d4b0523df4d62d3aea1fdc655bce140b8 100644
--- a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/limited_accel_generator.hpp
+++ b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/limited_accel_generator.hpp
@@ -50,7 +50,6 @@ class LimitedAccelGenerator : public StandardTrajectoryGenerator
{
public:
void initialize(const nav2_util::LifecycleNode::SharedPtr & nh) override;
- void checkUseDwaParam(const nav2_util::LifecycleNode::SharedPtr & nh) override;
void startNewIteration(const nav_2d_msgs::msg::Twist2D & current_velocity) override;
dwb_msgs::msg::Trajectory2D generateTrajectory(
const geometry_msgs::msg::Pose2D & start_pose,
diff --git a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/standard_traj_generator.hpp b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/standard_traj_generator.hpp
index 62ab2d8ef317ed668f45beec5063f5db2acddbd3..5c4f965f4ff2ae89335cdd5f11bf0a90c9edb122 100644
--- a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/standard_traj_generator.hpp
+++ b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/standard_traj_generator.hpp
@@ -71,15 +71,6 @@ protected:
*/
virtual void initializeIterator(const nav2_util::LifecycleNode::SharedPtr & nh);
- /**
- * @brief Check if the deprecated use_dwa parameter is set to the functionality that matches this class
- *
- * The functionality guarded by the use_dwa parameter has been split between this class and the derived
- * LimitedAccelGenerator. If use_dwa was false, this class should be used. If it was true, then LimitedAccelGenerator.
- * If this is NOT the case, this function will throw an exception.
- */
- virtual void checkUseDwaParam(const nav2_util::LifecycleNode::SharedPtr & nh);
-
/**
* @brief Calculate the velocity after a set period of time, given the desired velocity and acceleration limits
*
diff --git a/nav2_dwb_controller/dwb_plugins/package.xml b/nav2_dwb_controller/dwb_plugins/package.xml
index 2c111df37e8835e7829e4074274689dea340edc9..00ae398ee58aa7074738bc9934db16523643a643 100644
--- a/nav2_dwb_controller/dwb_plugins/package.xml
+++ b/nav2_dwb_controller/dwb_plugins/package.xml
@@ -1,7 +1,7 @@
<?xml version="1.0"?>
<package format="2">
<name>dwb_plugins</name>
- <version>0.2.4</version>
+ <version>0.3.0</version>
<description>
Standard implementations of the GoalChecker
and TrajectoryGenerators for dwb_core
diff --git a/nav2_dwb_controller/dwb_plugins/src/limited_accel_generator.cpp b/nav2_dwb_controller/dwb_plugins/src/limited_accel_generator.cpp
index 90b935b9b15bb5088b9277910c875607f432ce27..05adae5b4c33e1f65e2cf4e8a2dafb1a72151f38 100644
--- a/nav2_dwb_controller/dwb_plugins/src/limited_accel_generator.cpp
+++ b/nav2_dwb_controller/dwb_plugins/src/limited_accel_generator.cpp
@@ -64,17 +64,6 @@ void LimitedAccelGenerator::initialize(const nav2_util::LifecycleNode::SharedPtr
}
}
-void LimitedAccelGenerator::checkUseDwaParam(const nav2_util::LifecycleNode::SharedPtr & nh)
-{
- bool use_dwa = false;
- nh->get_parameter("use_dwa", use_dwa);
- if (!use_dwa) {
- throw nav2_core::
- PlannerException("Deprecated parameter use_dwa set to false. "
- "Please use StandardTrajectoryGenerator for that functionality.");
- }
-}
-
void LimitedAccelGenerator::startNewIteration(const nav_2d_msgs::msg::Twist2D & current_velocity)
{
// Limit our search space to just those within the limited acceleration_time
diff --git a/nav2_dwb_controller/dwb_plugins/src/standard_traj_generator.cpp b/nav2_dwb_controller/dwb_plugins/src/standard_traj_generator.cpp
index 0260d1bd63b1d72e81066d831a63649176f52760..6d98b44780eb337378cdd606372158b37519bdde 100644
--- a/nav2_dwb_controller/dwb_plugins/src/standard_traj_generator.cpp
+++ b/nav2_dwb_controller/dwb_plugins/src/standard_traj_generator.cpp
@@ -59,7 +59,6 @@ void StandardTrajectoryGenerator::initialize(const nav2_util::LifecycleNode::Sha
"discretize_by_time", rclcpp::ParameterValue(false));
nh->get_parameter("sim_time", sim_time_);
- checkUseDwaParam(nh);
/*
* If discretize_by_time, then sim_granularity represents the amount of time that should be between
@@ -88,18 +87,6 @@ void StandardTrajectoryGenerator::initializeIterator(
velocity_iterator_->initialize(nh, kinematics_);
}
-void StandardTrajectoryGenerator::checkUseDwaParam(
- const nav2_util::LifecycleNode::SharedPtr & nh)
-{
- bool use_dwa = true;
- nh->get_parameter("use_dwa", use_dwa);
- if (use_dwa) {
- throw nav2_core::
- PlannerException("Deprecated parameter use_dwa set to true. "
- "Please use LimitedAccelGenerator for that functionality.");
- }
-}
-
void StandardTrajectoryGenerator::startNewIteration(
const nav_2d_msgs::msg::Twist2D & current_velocity)
{
diff --git a/nav2_dwb_controller/dwb_plugins/test/twist_gen.cpp b/nav2_dwb_controller/dwb_plugins/test/twist_gen.cpp
index 1bee7fdda2c052ceaa0abc397bd69103efddd620..26a5e052f0e0aa0a9ff75d1892402ed6c79d9c39 100644
--- a/nav2_dwb_controller/dwb_plugins/test/twist_gen.cpp
+++ b/nav2_dwb_controller/dwb_plugins/test/twist_gen.cpp
@@ -71,8 +71,6 @@ std::vector<rclcpp::Parameter> getDefaultKinematicParameters()
parameters.push_back(rclcpp::Parameter("max_speed_xy", 0.55));
parameters.push_back(rclcpp::Parameter("min_speed_theta", 0.4));
- parameters.push_back(rclcpp::Parameter("use_dwa", false));
-
return parameters;
}
@@ -210,26 +208,9 @@ TEST(VelocityIterator, no_limits_samples)
checkLimits(twists, 0.0, 0.55, -0.1, 0.1, -1.0, 1.0, hypot(0.55, 0.1), 0.0, 0.0);
}
-TEST(VelocityIterator, dwa_gen_exception)
-{
- auto nh = makeTestNode("dwa_gen_exception");
- nh->set_parameters({rclcpp::Parameter("use_dwa", true)});
- StandardTrajectoryGenerator gen;
- EXPECT_THROW(gen.initialize(nh), nav2_core::PlannerException);
-}
-
-TEST(VelocityIterator, no_dwa_gen_exception)
-{
- auto nh = makeTestNode("no_dwa_gen_exception");
- nh->set_parameters({rclcpp::Parameter("use_dwa", false)});
- dwb_plugins::LimitedAccelGenerator gen;
- EXPECT_THROW(gen.initialize(nh), nav2_core::PlannerException);
-}
-
TEST(VelocityIterator, dwa_gen)
{
auto nh = makeTestNode("dwa_gen");
- nh->set_parameters({rclcpp::Parameter("use_dwa", true)});
nh->set_parameters({rclcpp::Parameter("min_speed_theta", -1.0)});
dwb_plugins::LimitedAccelGenerator gen;
gen.initialize(nh);
@@ -242,7 +223,6 @@ TEST(VelocityIterator, dwa_gen)
TEST(VelocityIterator, nonzero)
{
auto nh = makeTestNode("nonzero");
- nh->set_parameters({rclcpp::Parameter("use_dwa", true)});
nh->set_parameters({rclcpp::Parameter("min_speed_theta", -1.0)});
dwb_plugins::LimitedAccelGenerator gen;
gen.initialize(nh);
@@ -402,7 +382,6 @@ TEST(TrajectoryGenerator, accel)
TEST(TrajectoryGenerator, dwa)
{
auto nh = makeTestNode("dwa");
- nh->set_parameters({rclcpp::Parameter("use_dwa", true)});
nh->set_parameters({rclcpp::Parameter("sim_period", 1.0)});
nh->set_parameters({rclcpp::Parameter("sim_time", 5.0)});
nh->set_parameters({rclcpp::Parameter("discretize_by_time", true)});
diff --git a/nav2_dwb_controller/nav2_dwb_controller/package.xml b/nav2_dwb_controller/nav2_dwb_controller/package.xml
index 93ba1950dd5541d290006dbce217fee10ac26b6d..1ddf5a31fac994301d29e8455e27a70a9e00ebe0 100644
--- a/nav2_dwb_controller/nav2_dwb_controller/package.xml
+++ b/nav2_dwb_controller/nav2_dwb_controller/package.xml
@@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>nav2_dwb_controller</name>
- <version>0.2.4</version>
+ <version>0.3.0</version>
<description>
ROS2 controller (DWB) metapackage
</description>
diff --git a/nav2_dwb_controller/nav_2d_msgs/package.xml b/nav2_dwb_controller/nav_2d_msgs/package.xml
index f49208b558816e4c97296c1b9ff09b3dd0ead624..bf40619e219ea234558b6c44f3df5998c09e71b3 100644
--- a/nav2_dwb_controller/nav_2d_msgs/package.xml
+++ b/nav2_dwb_controller/nav_2d_msgs/package.xml
@@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>nav_2d_msgs</name>
- <version>0.2.4</version>
+ <version>0.3.0</version>
<description>Basic message types for two dimensional navigation, extending from geometry_msgs::Pose2D.</description>
<maintainer email="davidvlu@gmail.com">David V. Lu!!</maintainer>
<license>BSD-3-Clause</license>
diff --git a/nav2_dwb_controller/nav_2d_utils/package.xml b/nav2_dwb_controller/nav_2d_utils/package.xml
index dde5b5ba0bd1a3ee07c1653e49d489f496ee8e15..7c6107a684d2c8380366745a8d5d7cdaac4cebd1 100644
--- a/nav2_dwb_controller/nav_2d_utils/package.xml
+++ b/nav2_dwb_controller/nav_2d_utils/package.xml
@@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>nav_2d_utils</name>
- <version>0.2.4</version>
+ <version>0.3.0</version>
<description>A handful of useful utility functions for nav_2d packages.</description>
<maintainer email="davidvlu@gmail.com">David V. Lu!!</maintainer>
<license>BSD-3-Clause</license>
diff --git a/nav2_experimental/nav2_rl/nav2_turtlebot3_rl/package.xml b/nav2_experimental/nav2_rl/nav2_turtlebot3_rl/package.xml
index cf13baa876259913dc52c4906b28a32d51dda183..4e32391f2fc5dcc7e8ff513d21f462b5902b4a3e 100644
--- a/nav2_experimental/nav2_rl/nav2_turtlebot3_rl/package.xml
+++ b/nav2_experimental/nav2_rl/nav2_turtlebot3_rl/package.xml
@@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="2">
<name>nav2_turtlebot3_rl</name>
- <version>0.2.4</version>
+ <version>0.3.0</version>
<description>This package enables Reinfocement Learning with Gazebo and Turtlebot3</description>
<maintainer email="mohammad.haghighipanah@intel.com">Mohammad Haghighipanah</maintainer>
diff --git a/nav2_lifecycle_manager/README.md b/nav2_lifecycle_manager/README.md
new file mode 100644
index 0000000000000000000000000000000000000000..d2112266b52cb103fc71f2bed454ff82e41bb120
--- /dev/null
+++ b/nav2_lifecycle_manager/README.md
@@ -0,0 +1,17 @@
+### Background on lifecycle enabled nodes
+Using ROS2’s managed/lifecycle nodes feature allows the system startup to ensure that all required nodes have been instantiated correctly before they begin their execution. Using lifecycle nodes also allows nodes to be restarted or replaced on-line. More details about managed nodes can be found on [ROS2 Design website](https://design.ros2.org/articles/node_lifecycle.html). Several nodes in the navigation2 stack, such as map_server, planner_server, and controller_server, are lifecycle enabled. These nodes provide the required overrides of the lifecycle functions: ```on_configure()```, ```on_activate()```, ```on_deactivate()```, ```on_cleanup()```, ```on_shutdown()```, and ```on_error()```.
+
+
+### nav2_lifecycle_manager
+Navigation2’s lifecycle manager is used to change the states of the lifecycle nodes in order to achieve a controlled _startup_, _shutdown_, _reset_, _pause_, or _resume_ of the navigation stack. The lifecycle manager presents a ```lifecycle_manager/manage_nodes``` service, from which clients can invoke the startup, shutdown, reset, pause, or resume functions. Based on this service request, the lifecycle manager calls the necessary lifecycle services in the lifecycle managed nodes. Currently, the RVIZ panel uses this ```lifecycle_manager/manage_nodes``` service when user presses the buttons on the RVIZ panel (e.g.,startup, reset, shutdown, etc.).
+
+In order to start the navigation stack and be able to navigate, the necessary nodes must be configured and activated. Thus, for example when _startup_ is requested from the lifecycle manager's manage_nodes service, the lifecycle managers calls _configure()_ and _activate()_ on the lifecycle enabled nodes in the node list.
+
+The lifecycle manager has a default nodes list for all the nodes that it manages. This list can be changed using the lifecycle manager’s _“node_namesâ€_ parameter.
+
+The diagram below shows an _example_ of a list of managed nodes, and how it interfaces with the lifecycle manager.
+<img src="./doc/diagram_lifecycle_manager.JPG" title="" width="100%" align="middle">
+
+The UML diagram below shows the sequence of service calls once the _startup_ is requested from the lifecycle manager.
+
+<img src="./doc/uml_lifecycle_manager.JPG" title="Lifecycle manager UML diagram" width="100%" align="middle">
\ No newline at end of file
diff --git a/nav2_lifecycle_manager/doc/diagram_lifecycle_manager.JPG b/nav2_lifecycle_manager/doc/diagram_lifecycle_manager.JPG
new file mode 100644
index 0000000000000000000000000000000000000000..c7afb2742d6250dff1710a197d5d2548931d0cbc
Binary files /dev/null and b/nav2_lifecycle_manager/doc/diagram_lifecycle_manager.JPG differ
diff --git a/nav2_lifecycle_manager/doc/uml_lifecycle_manager.JPG b/nav2_lifecycle_manager/doc/uml_lifecycle_manager.JPG
new file mode 100644
index 0000000000000000000000000000000000000000..eb5ee7ec359a7e354c4313544052874aca2bcc24
Binary files /dev/null and b/nav2_lifecycle_manager/doc/uml_lifecycle_manager.JPG differ
diff --git a/nav2_lifecycle_manager/package.xml b/nav2_lifecycle_manager/package.xml
index 8b4e1bbdae7f3e06c9bba99212f2fdb8c996055b..116d1d0a01b3fd0fe17c25580436f9abfd6ad083 100644
--- a/nav2_lifecycle_manager/package.xml
+++ b/nav2_lifecycle_manager/package.xml
@@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>nav2_lifecycle_manager</name>
- <version>0.2.4</version>
+ <version>0.3.0</version>
<description>A controller/manager for the lifecycle nodes of the Navigation 2 system</description>
<maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
<license>Apache-2.0</license>
diff --git a/nav2_map_server/CMakeLists.txt b/nav2_map_server/CMakeLists.txt
index f15d7366c1848bab29c4f16fdea867848a605474..f3debba6f9b8cddff82e30e833b26200436da8b2 100644
--- a/nav2_map_server/CMakeLists.txt
+++ b/nav2_map_server/CMakeLists.txt
@@ -8,6 +8,7 @@ find_package(nav2_common REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_lifecycle REQUIRED)
find_package(nav_msgs REQUIRED)
+find_package(nav2_msgs REQUIRED)
find_package(yaml_cpp_vendor REQUIRED)
find_package(std_msgs REQUIRED)
find_package(tf2 REQUIRED)
@@ -40,6 +41,7 @@ set(map_server_dependencies
rclcpp
rclcpp_lifecycle
nav_msgs
+ nav2_msgs
yaml_cpp_vendor
std_msgs
tf2
diff --git a/nav2_map_server/README.md b/nav2_map_server/README.md
index 272a2ff1874977f16116b96c3041288d61569ee2..d0a11bb9918b1ae9d83d706672be02083236a767 100644
--- a/nav2_map_server/README.md
+++ b/nav2_map_server/README.md
@@ -1,89 +1,97 @@
-# Map Server
-
-The `Map Server` provides maps to the rest of the Navigation2 system using both topic and
-service interfaces.
-
-## Changes from ROS1 Navigation Map Server
-
-While the nav2 map server provides the same general function as the nav1 map server, the new
-code has some changes to accomodate ROS2 as well as some architectural improvements.
-
-### Architecture
-
-In contrast to the ROS1 navigation map server, the nav2 map server will support a variety
-of map types, and thus some aspects of the original code have been refactored to support
-this new extensible framework. In particular, there is now a `MapLoader` abstract base class
-and type-specific map loaders which derive from this class. There is currently one such
-derived class, the `OccGridLoader`, which converts an input image to an OccupancyGrid and
-makes this available via topic and service interfaces. The `MapServer` class is a ROS2 node
-that uses the appropriate loader, based on an input parameter.
-
-### Command-line arguments, ROS2 Node Parameters, and YAML files
-
-The Map Server is a composable ROS2 node. By default, there is a map_server executable that
-instances one of these nodes, but it is possible to compose multiple map server nodes into
-a single process, if desired.
-
-The command line for the map server executable is slightly different that it was with ROS1.
-With ROS1, one invoked the map server and passing the map YAML filename, like this:
-
-```
-$ map_server map.yaml
-```
-
-Where the YAML file specified contained the various map metadata, such as:
-
-```
-image: testmap.png
-resolution: 0.1
-origin: [2.0, 3.0, 1.0]
-negate: 0
-occupied_thresh: 0.65
-free_thresh: 0.196
-```
-
-The Navigation2 software retains the map YAML file format from Nav1, but uses the ROS2 parameter
-mechanism to get the name of the YAML file to use. This effectively introduces a
-level of indirection to get the map yaml filename. For example, for a node named 'map_server',
-the parameter file would look like this:
-
-```
-# map_server_params.yaml
-map_server:
- ros__parameters:
- yaml_filename: "map.yaml"
-```
-
-One can invoke the map service executable directly, passing the params file on the command line,
-like this:
-
-```
-$ map_server __params:=map_server_params.yaml
-```
-
-There is also possibility of having multiple map server nodes in a single process, where the parameters file would separate the parameters by node name, like this:
-
-```
-# combined_params.yaml
-map_server1:
- ros__parameters:
- yaml_filename: "some_map.yaml"
-
-map_server2:
- ros__parameters:
- yaml_filename: "another_map.yaml"
-```
-
-Then, one would invoke this process with the params file that contains the parameters for both nodes:
-
-```
-$ process_with_multiple_map_servers __params:=combined_params.yaml
-```
-
-## Currently Supported Map Types
-- Occupancy grid (nav_msgs/msg/OccupancyGrid), via the OccGridLoader
-
-## Future Plans
-- Allow for dynamic configuration of conversion parameters
-- Support additional map types, e.g. GridMap (https://github.com/ros-planning/navigation2/issues/191)
-- Port and refactor Map Saver (https://github.com/ros-planning/navigation2/issues/188)
+# Map Server
+
+The `Map Server` provides maps to the rest of the Navigation2 system using both topic and
+service interfaces.
+
+## Changes from ROS1 Navigation Map Server
+
+While the nav2 map server provides the same general function as the nav1 map server, the new
+code has some changes to accomodate ROS2 as well as some architectural improvements.
+
+In addition, there is now a new "load_map" service which can be used to dynamically load a map.
+
+### Architecture
+
+In contrast to the ROS1 navigation map server, the nav2 map server will support a variety
+of map types, and thus some aspects of the original code have been refactored to support
+this new extensible framework. In particular, there is now a `MapLoader` abstract base class
+and type-specific map loaders which derive from this class. There is currently one such
+derived class, the `OccGridLoader`, which converts an input image to an OccupancyGrid and
+makes this available via topic and service interfaces. The `MapServer` class is a ROS2 node
+that uses the appropriate loader, based on an input parameter.
+
+### Command-line arguments, ROS2 Node Parameters, and YAML files
+
+The Map Server is a composable ROS2 node. By default, there is a map_server executable that
+instances one of these nodes, but it is possible to compose multiple map server nodes into
+a single process, if desired.
+
+The command line for the map server executable is slightly different that it was with ROS1.
+With ROS1, one invoked the map server and passing the map YAML filename, like this:
+
+```
+$ map_server map.yaml
+```
+
+Where the YAML file specified contained the various map metadata, such as:
+
+```
+image: testmap.png
+resolution: 0.1
+origin: [2.0, 3.0, 1.0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
+```
+
+The Navigation2 software retains the map YAML file format from Nav1, but uses the ROS2 parameter
+mechanism to get the name of the YAML file to use. This effectively introduces a
+level of indirection to get the map yaml filename. For example, for a node named 'map_server',
+the parameter file would look like this:
+
+```
+# map_server_params.yaml
+map_server:
+ ros__parameters:
+ yaml_filename: "map.yaml"
+```
+
+One can invoke the map service executable directly, passing the params file on the command line,
+like this:
+
+```
+$ map_server __params:=map_server_params.yaml
+```
+
+There is also possibility of having multiple map server nodes in a single process, where the parameters file would separate the parameters by node name, like this:
+
+```
+# combined_params.yaml
+map_server1:
+ ros__parameters:
+ yaml_filename: "some_map.yaml"
+
+map_server2:
+ ros__parameters:
+ yaml_filename: "another_map.yaml"
+```
+
+Then, one would invoke this process with the params file that contains the parameters for both nodes:
+
+```
+$ process_with_multiple_map_servers __params:=combined_params.yaml
+```
+
+## Currently Supported Map Types
+- Occupancy grid (nav_msgs/msg/OccupancyGrid), via the OccGridLoader
+
+## Services
+As in ROS navigation, the map_server node provides a "map" service to get the map. See the nav_msgs/srv/GetMap.srv file for details.
+
+NEW in ROS2 Eloquent, map_server also now provides a "load_map" service. See nav2_msgs/srv/LoadMap.srv for details.
+
+Example:
+```
+$ ros2 service call /load_map nav2_msgs/srv/LoadMap "{type: 0, map_id: /ros/maps/map.yaml}
+```
+
diff --git a/nav2_map_server/include/nav2_map_server/occ_grid_loader.hpp b/nav2_map_server/include/nav2_map_server/occ_grid_loader.hpp
index 65b929bbf3050b816d8bf566be4eab4ba03116ea..305d1251a415f7c8c192ff507a0bab8a181f1e51 100644
--- a/nav2_map_server/include/nav2_map_server/occ_grid_loader.hpp
+++ b/nav2_map_server/include/nav2_map_server/occ_grid_loader.hpp
@@ -24,6 +24,7 @@
#include "nav2_util/lifecycle_node.hpp"
#include "nav_msgs/msg/occupancy_grid.hpp"
#include "nav_msgs/srv/get_map.hpp"
+#include "nav2_msgs/srv/load_map.hpp"
#include "rclcpp/rclcpp.hpp"
namespace nav2_map_server
@@ -103,9 +104,17 @@ protected:
// Load the image and generate an OccupancyGrid
void loadMapFromFile(const LoadParameters & loadParameters);
+ // Load the map yaml and image from yaml file name
+ bool loadMapFromYaml(
+ std::string yaml_file,
+ std::shared_ptr<nav2_msgs::srv::LoadMap::Response> response = nullptr);
+
// A service to provide the occupancy grid (GetMap) and the message to return
rclcpp::Service<nav_msgs::srv::GetMap>::SharedPtr occ_service_;
+ // A service to load the occupancy grid from file at run time (LoadMap)
+ rclcpp::Service<nav2_msgs::srv::LoadMap>::SharedPtr load_map_service_;
+
// A topic on which the occupancy grid will be published
rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::OccupancyGrid>::SharedPtr occ_pub_;
@@ -121,6 +130,9 @@ protected:
// The name of the service for getting a map
static constexpr const char * service_name_{"map"};
+ // The name of the service for loading a map
+ static constexpr const char * load_map_service_name_{"load_map"};
+
// Timer for republishing map
rclcpp::TimerBase::SharedPtr timer_;
};
diff --git a/nav2_map_server/package.xml b/nav2_map_server/package.xml
index de12b9ef38773858ccd8b1062606604220b618c7..8bbc316b312ce92081359aa793a6fa46f9ebc4b3 100644
--- a/nav2_map_server/package.xml
+++ b/nav2_map_server/package.xml
@@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>nav2_map_server</name>
- <version>0.2.4</version>
+ <version>0.3.0</version>
<description>
Refactored map server for ROS2 Navigation
</description>
diff --git a/nav2_map_server/src/occ_grid_loader.cpp b/nav2_map_server/src/occ_grid_loader.cpp
index 270732db6bba4da5c4b3d4e68b17672115de8579..6cd4129bd2de4236a9846562a95710180f763ba9 100644
--- a/nav2_map_server/src/occ_grid_loader.cpp
+++ b/nav2_map_server/src/occ_grid_loader.cpp
@@ -41,6 +41,7 @@
#include "tf2/LinearMath/Quaternion.h"
#include "yaml-cpp/yaml.h"
#include "nav2_util/geometry_utils.hpp"
+#include "lifecycle_msgs/msg/state.hpp"
using namespace std::chrono_literals;
@@ -126,24 +127,31 @@ OccGridLoader::LoadParameters OccGridLoader::load_map_yaml(const std::string & y
return loadParameters;
}
-nav2_util::CallbackReturn OccGridLoader::on_configure(const rclcpp_lifecycle::State & /*state*/)
+bool OccGridLoader::loadMapFromYaml(
+ std::string yaml_file,
+ std::shared_ptr<nav2_msgs::srv::LoadMap::Response> response)
{
- RCLCPP_INFO(node_->get_logger(), "OccGridLoader: Configuring");
-
- msg_ = std::make_unique<nav_msgs::msg::OccupancyGrid>();
+ if (yaml_file.empty()) {
+ RCLCPP_ERROR(node_->get_logger(), "YAML file name is empty, can't load!");
+ response->result = nav2_msgs::srv::LoadMap::Response::RESULT_MAP_DOES_NOT_EXIST;
+ return false;
+ }
+ RCLCPP_INFO(node_->get_logger(), "Loading yaml file: %s", yaml_file.c_str());
LoadParameters loadParameters;
try {
- loadParameters = load_map_yaml(yaml_filename_);
+ loadParameters = load_map_yaml(yaml_file);
} catch (YAML::Exception & e) {
RCLCPP_ERROR(
node_->get_logger(), "Failed processing YAML file %s at position (%d:%d) for reason: %s",
- yaml_filename_.c_str(), e.mark.line, e.mark.column, e.what());
- throw std::runtime_error("Failed to load map yaml file.");
+ yaml_file.c_str(), e.mark.line, e.mark.column, e.what());
+ response->result = nav2_msgs::srv::LoadMap::Response::RESULT_INVALID_MAP_METADATA;
+ return false;
} catch (std::exception & e) {
RCLCPP_ERROR(
node_->get_logger(), "Failed to parse map YAML loaded from file %s for reason: %s",
- yaml_filename_.c_str(), e.what());
- throw std::runtime_error("Failed to load map yaml file.");
+ yaml_file.c_str(), e.what());
+ response->result = nav2_msgs::srv::LoadMap::Response::RESULT_INVALID_MAP_METADATA;
+ return false;
}
try {
@@ -152,25 +160,67 @@ nav2_util::CallbackReturn OccGridLoader::on_configure(const rclcpp_lifecycle::St
RCLCPP_ERROR(
node_->get_logger(), "Failed to load image file %s for reason: %s",
loadParameters.image_file_name.c_str(), e.what());
+ response->result = nav2_msgs::srv::LoadMap::Response::RESULT_INVALID_MAP_DATA;
+ return false;
+ }
+ return true;
+}
+
+nav2_util::CallbackReturn OccGridLoader::on_configure(const rclcpp_lifecycle::State & /*state*/)
+{
+ RCLCPP_INFO(node_->get_logger(), "OccGridLoader: Configuring");
- throw std::runtime_error("Failed to load map image file.");
+ // initialize Occupancy Grid msg - needed by loadMapFromYaml
+ msg_ = std::make_unique<nav_msgs::msg::OccupancyGrid>();
+
+ if (!loadMapFromYaml(yaml_filename_)) {
+ throw std::runtime_error("Failed to load map yaml file: " + yaml_filename_);
}
- // Create a service callback handle
+ // Create GetMap service callback handle
auto handle_occ_callback = [this](
const std::shared_ptr<rmw_request_id_t>/*request_header*/,
const std::shared_ptr<nav_msgs::srv::GetMap::Request>/*request*/,
std::shared_ptr<nav_msgs::srv::GetMap::Response> response) -> void {
- RCLCPP_INFO(node_->get_logger(), "OccGridLoader: Handling map request");
+ if (node_->get_current_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) {
+ RCLCPP_WARN(node_->get_logger(),
+ "Received GetMap request but not in ACTIVE state, ignoring!");
+ return;
+ }
+ RCLCPP_INFO(node_->get_logger(), "OccGridLoader: Handling GetMap request");
response->map = *msg_;
};
// Create a service that provides the occupancy grid
occ_service_ = node_->create_service<nav_msgs::srv::GetMap>(service_name_, handle_occ_callback);
+ // Create the load_map service callback handle
+ auto load_map_callback = [this](
+ const std::shared_ptr<rmw_request_id_t>/*request_header*/,
+ const std::shared_ptr<nav2_msgs::srv::LoadMap::Request> request,
+ std::shared_ptr<nav2_msgs::srv::LoadMap::Response> response) -> void {
+ // if not in ACTIVE state, ignore request
+ if (node_->get_current_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) {
+ RCLCPP_WARN(node_->get_logger(),
+ "Received LoadMap request but not in ACTIVE state, ignoring!");
+ return;
+ }
+ RCLCPP_INFO(node_->get_logger(), "OccGridLoader: Handling LoadMap request");
+ // Load from file
+ if (loadMapFromYaml(request->map_url, response)) {
+ response->map = *msg_;
+ response->result = nav2_msgs::srv::LoadMap::Response::RESULT_SUCCESS;
+ occ_pub_->publish(*msg_); // publish new map
+ }
+ };
+
// Create a publisher using the QoS settings to emulate a ROS1 latched topic
- occ_pub_ = node_->create_publisher<nav_msgs::msg::OccupancyGrid>(
- topic_name_, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable());
+ occ_pub_ = node_->create_publisher<nav_msgs::msg::OccupancyGrid>(topic_name_,
+ rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable());
+
+ // Create a service that loads the occupancy grid from a file
+ load_map_service_ = node_->create_service<nav2_msgs::srv::LoadMap>(load_map_service_name_,
+ load_map_callback);
return nav2_util::CallbackReturn::SUCCESS;
}
@@ -202,6 +252,7 @@ nav2_util::CallbackReturn OccGridLoader::on_cleanup(const rclcpp_lifecycle::Stat
occ_pub_.reset();
occ_service_.reset();
+ load_map_service_.reset();
msg_.reset();
return nav2_util::CallbackReturn::SUCCESS;
@@ -212,11 +263,14 @@ void OccGridLoader::loadMapFromFile(const LoadParameters & loadParameters)
Magick::InitializeMagick(nullptr);
nav_msgs::msg::OccupancyGrid msg;
+ RCLCPP_INFO(node_->get_logger(), "Loading image_file: %s",
+ loadParameters.image_file_name.c_str());
Magick::Image img(loadParameters.image_file_name);
// Copy the image data into the map structure
msg.info.width = img.size().width();
msg.info.height = img.size().height();
+
msg.info.resolution = loadParameters.resolution;
msg.info.origin.position.x = loadParameters.origin[0];
msg.info.origin.position.y = loadParameters.origin[1];
diff --git a/nav2_map_server/test/component/CMakeLists.txt b/nav2_map_server/test/component/CMakeLists.txt
index cad00898cd451ea88a7e7c80ecd917292111fea5..85c1bba1ae6f2e61f160cc1c02dc8996898010bf 100644
--- a/nav2_map_server/test/component/CMakeLists.txt
+++ b/nav2_map_server/test/component/CMakeLists.txt
@@ -7,15 +7,16 @@ ament_add_gtest_executable(test_occ_grid_node
ament_target_dependencies(test_occ_grid_node rclcpp nav_msgs)
target_link_libraries(test_occ_grid_node
${library_name}
+ stdc++fs
)
-# Flaky test
-#ament_add_test(test_occ_grid_cpp
-# GENERATE_RESULT_FOR_RETURN_CODE_ZERO
-# COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_occ_grid_launch.py"
-# WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
-# ENV
-# TEST_DIR=${TEST_DIR}
-# TEST_LAUNCH_DIR=${TEST_LAUNCH_DIR}
-# TEST_EXECUTABLE=$<TARGET_FILE:test_occ_grid_node>
-#)
+# occ_grid component test
+ament_add_test(test_occ_grid_node
+ GENERATE_RESULT_FOR_RETURN_CODE_ZERO
+ COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_occ_grid_launch.py"
+ WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
+ ENV
+ TEST_DIR=${TEST_DIR}
+ TEST_LAUNCH_DIR=${TEST_LAUNCH_DIR}
+ TEST_EXECUTABLE=$<TARGET_FILE:test_occ_grid_node>
+)
diff --git a/nav2_map_server/test/component/test_occ_grid_node.cpp b/nav2_map_server/test/component/test_occ_grid_node.cpp
index 37918323aa4ed464c22c55402f6214d7116e82d5..4185a1cb4d9c420a540f06ba8971a2cebf230f33 100644
--- a/nav2_map_server/test/component/test_occ_grid_node.cpp
+++ b/nav2_map_server/test/component/test_occ_grid_node.cpp
@@ -13,12 +13,18 @@
// limitations under the License.
#include <gtest/gtest.h>
+#include <experimental/filesystem>
#include <rclcpp/rclcpp.hpp>
#include <memory>
#include "test_constants/test_constants.h"
#include "nav2_map_server/occ_grid_loader.hpp"
#include "nav2_util/lifecycle_service_client.hpp"
+#include "nav2_msgs/srv/load_map.hpp"
+
+#define TEST_DIR TEST_DIRECTORY
+
+using std::experimental::filesystem::path;
using lifecycle_msgs::msg::Transition;
@@ -39,9 +45,13 @@ public:
node_ = rclcpp::Node::make_shared("map_client_test");
lifecycle_client_ =
std::make_shared<nav2_util::LifecycleServiceClient>("map_server", node_);
+ RCLCPP_INFO(node_->get_logger(), "Creating Test Node");
+
- lifecycle_client_->change_state(Transition::TRANSITION_CONFIGURE);
- lifecycle_client_->change_state(Transition::TRANSITION_ACTIVATE);
+ std::this_thread::sleep_for(std::chrono::seconds(1)); // allow node to start up
+ const std::chrono::seconds timeout(5);
+ lifecycle_client_->change_state(Transition::TRANSITION_CONFIGURE, timeout);
+ lifecycle_client_->change_state(Transition::TRANSITION_ACTIVATE, timeout);
}
~TestNode()
@@ -75,11 +85,13 @@ protected:
std::shared_ptr<nav2_util::LifecycleServiceClient> lifecycle_client_;
};
-TEST_F(TestNode, ResultReturned)
+TEST_F(TestNode, GetMap)
{
+ RCLCPP_INFO(node_->get_logger(), "Testing GetMap service");
auto req = std::make_shared<nav_msgs::srv::GetMap::Request>();
auto client = node_->create_client<nav_msgs::srv::GetMap>("map");
+ RCLCPP_INFO(node_->get_logger(), "Waiting for map service");
ASSERT_TRUE(client->wait_for_service());
auto resp = send_request<nav_msgs::srv::GetMap>(node_, client, req);
@@ -92,3 +104,72 @@ TEST_F(TestNode, ResultReturned)
ASSERT_EQ(g_valid_image_content[i], resp->map.data[i]);
}
}
+
+TEST_F(TestNode, LoadMap)
+{
+ RCLCPP_INFO(node_->get_logger(), "Testing LoadMap service");
+ auto req = std::make_shared<nav2_msgs::srv::LoadMap::Request>();
+ auto client = node_->create_client<nav2_msgs::srv::LoadMap>("load_map");
+
+ RCLCPP_INFO(node_->get_logger(), "Waiting for load_map service");
+ ASSERT_TRUE(client->wait_for_service());
+
+ req->map_url = path(TEST_DIR) / path(g_valid_yaml_file);
+ auto resp = send_request<nav2_msgs::srv::LoadMap>(node_, client, req);
+
+ ASSERT_EQ(resp->result, nav2_msgs::srv::LoadMap::Response::RESULT_SUCCESS);
+ ASSERT_FLOAT_EQ(resp->map.info.resolution, g_valid_image_res);
+ ASSERT_EQ(resp->map.info.height, g_valid_image_height);
+
+ for (unsigned int i = 0; i < resp->map.info.width * resp->map.info.height; i++) {
+ ASSERT_EQ(g_valid_image_content[i], resp->map.data[i]);
+ }
+}
+
+TEST_F(TestNode, LoadMapNull)
+{
+ RCLCPP_INFO(node_->get_logger(), "Testing LoadMap service");
+ auto req = std::make_shared<nav2_msgs::srv::LoadMap::Request>();
+ auto client = node_->create_client<nav2_msgs::srv::LoadMap>("load_map");
+
+ RCLCPP_INFO(node_->get_logger(), "Waiting for load_map service");
+ ASSERT_TRUE(client->wait_for_service());
+
+ req->map_url = "";
+ RCLCPP_INFO(node_->get_logger(), "Sending load_map request with null file name");
+ auto resp = send_request<nav2_msgs::srv::LoadMap>(node_, client, req);
+
+ ASSERT_EQ(resp->result, nav2_msgs::srv::LoadMap::Response::RESULT_MAP_DOES_NOT_EXIST);
+}
+
+TEST_F(TestNode, LoadMapInvalidYaml)
+{
+ RCLCPP_INFO(node_->get_logger(), "Testing LoadMap service");
+ auto req = std::make_shared<nav2_msgs::srv::LoadMap::Request>();
+ auto client = node_->create_client<nav2_msgs::srv::LoadMap>("load_map");
+
+ RCLCPP_INFO(node_->get_logger(), "Waiting for load_map service");
+ ASSERT_TRUE(client->wait_for_service());
+
+ req->map_url = "invalid_file.yaml";
+ RCLCPP_INFO(node_->get_logger(), "Sending load_map request with invalid yaml file name");
+ auto resp = send_request<nav2_msgs::srv::LoadMap>(node_, client, req);
+
+ ASSERT_EQ(resp->result, nav2_msgs::srv::LoadMap::Response::RESULT_INVALID_MAP_METADATA);
+}
+
+TEST_F(TestNode, LoadMapInvalidImage)
+{
+ RCLCPP_INFO(node_->get_logger(), "Testing LoadMap service");
+ auto req = std::make_shared<nav2_msgs::srv::LoadMap::Request>();
+ auto client = node_->create_client<nav2_msgs::srv::LoadMap>("load_map");
+
+ RCLCPP_INFO(node_->get_logger(), "Waiting for load_map service");
+ ASSERT_TRUE(client->wait_for_service());
+
+ req->map_url = path(TEST_DIR) / "invalid_image.yaml";
+ RCLCPP_INFO(node_->get_logger(), "Sending load_map request with invalid image file name");
+ auto resp = send_request<nav2_msgs::srv::LoadMap>(node_, client, req);
+
+ ASSERT_EQ(resp->result, nav2_msgs::srv::LoadMap::Response::RESULT_INVALID_MAP_DATA);
+}
diff --git a/nav2_map_server/test/invalid_image.yaml b/nav2_map_server/test/invalid_image.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..bcdc605c26b3b93b2f651b20636ace931840f900
--- /dev/null
+++ b/nav2_map_server/test/invalid_image.yaml
@@ -0,0 +1,8 @@
+map_type: "occupancy"
+image: "invalid.png"
+resolution: 0.1
+origin: [2.0, 3.0, 1.0]
+occupied_thresh: 0.65
+free_thresh: 0.196
+negate: 0
+mode: "trinary"
diff --git a/nav2_map_server/test/test_constants.cpp b/nav2_map_server/test/test_constants.cpp
index 0a49c713e862a6e0f13dc9329e5f0ba23ebfcf3b..5d17cb8b312e8dff5ffddce249d116130e11605e 100644
--- a/nav2_map_server/test/test_constants.cpp
+++ b/nav2_map_server/test/test_constants.cpp
@@ -57,5 +57,6 @@ const char g_valid_image_content[] = {
const char * g_valid_png_file = "testmap.png";
const char * g_valid_bmp_file = "testmap.bmp";
+const char * g_valid_yaml_file = "testmap.yaml";
const float g_valid_image_res = 0.1;
diff --git a/nav2_map_server/test/test_constants/test_constants.h b/nav2_map_server/test/test_constants/test_constants.h
index 7aeafc8708f38546b41d60d7f5f05065dd1dc6a1..08c7da9d8df797713b922977ec398641768fcb3b 100644
--- a/nav2_map_server/test/test_constants/test_constants.h
+++ b/nav2_map_server/test/test_constants/test_constants.h
@@ -38,6 +38,7 @@ extern const unsigned int g_valid_image_height;
extern const char g_valid_image_content[];
extern const char * g_valid_png_file;
extern const char * g_valid_bmp_file;
+extern const char * g_valid_yaml_file;
extern const float g_valid_image_res;
diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt
index 8a592ee440173db8dac854532583da04b8aee9d4..2c44ec1707d93745abf8e4cdc4c9ff6f1e892186 100644
--- a/nav2_msgs/CMakeLists.txt
+++ b/nav2_msgs/CMakeLists.txt
@@ -21,6 +21,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
"srv/ClearCostmapAroundRobot.srv"
"srv/ClearEntireCostmap.srv"
"srv/ManageLifecycleNodes.srv"
+ "srv/LoadMap.srv"
"action/BackUp.action"
"action/ComputePathToPose.action"
"action/FollowPath.action"
diff --git a/nav2_msgs/package.xml b/nav2_msgs/package.xml
index 3caab138ef9b3b18fc877d6adf924d5cbcae8ea7..24a21429d91fd9735bb189afce94e88be243d923 100644
--- a/nav2_msgs/package.xml
+++ b/nav2_msgs/package.xml
@@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>nav2_msgs</name>
- <version>0.2.4</version>
+ <version>0.3.0</version>
<description>Messages and service files for the navigation2 stack</description>
<maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
<maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>
diff --git a/nav2_msgs/srv/LoadMap.srv b/nav2_msgs/srv/LoadMap.srv
new file mode 100644
index 0000000000000000000000000000000000000000..3b9caaad47004e34d800ef84aae0553a8e2e5077
--- /dev/null
+++ b/nav2_msgs/srv/LoadMap.srv
@@ -0,0 +1,15 @@
+# URL of map resource
+# Can be an absolute path to a file: file:///path/to/maps/floor1.yaml
+# Or, relative to a ROS package: package://my_ros_package/maps/floor2.yaml
+string map_url
+---
+# Result code defintions
+uint8 RESULT_SUCCESS=0
+uint8 RESULT_MAP_DOES_NOT_EXIST=1
+uint8 RESULT_INVALID_MAP_DATA=2
+uint8 RESULT_INVALID_MAP_METADATA=3
+uint8 RESULT_UNDEFINED_FAILURE=255
+
+# Returned map is only valid if result equals RESULT_SUCCESS
+nav_msgs/OccupancyGrid map
+uint8 result
diff --git a/nav2_navfn_planner/package.xml b/nav2_navfn_planner/package.xml
index f7f7d7e5ed47bf3268949b8c247b88250688b5e0..16af1fd054ecbd70dc203ec29e631ef84eb35a5e 100644
--- a/nav2_navfn_planner/package.xml
+++ b/nav2_navfn_planner/package.xml
@@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>nav2_navfn_planner</name>
- <version>0.2.4</version>
+ <version>0.3.0</version>
<description>TODO</description>
<maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>
<maintainer email="carlos.a.orduno@intel.com">Carlos Orduno</maintainer>
diff --git a/nav2_planner/package.xml b/nav2_planner/package.xml
index 761266ed41e82e678a964a8a75ac02ccecc570e6..aef985bafb5de2f0fcda0f8a1b8b4fecc7f9891a 100644
--- a/nav2_planner/package.xml
+++ b/nav2_planner/package.xml
@@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>nav2_planner</name>
- <version>0.2.4</version>
+ <version>0.3.0</version>
<description>TODO</description>
<maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>
<license>Apache-2.0</license>
diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp
index 185c1ba2245316e9ab9d286b566a9f551c2bc84e..aad2e4f094f1a800591c644cfdd4a070b443d838 100644
--- a/nav2_planner/src/planner_server.cpp
+++ b/nav2_planner/src/planner_server.cpp
@@ -165,10 +165,7 @@ PlannerServer::on_cleanup(const rclcpp_lifecycle::State & state)
for (it = planners_.begin(); it != planners_.end(); ++it) {
it->second->cleanup();
}
-
- for (it = planners_.begin(); it != planners_.end(); ++it) {
- it->second.reset();
- }
+ planners_.clear();
return nav2_util::CallbackReturn::SUCCESS;
}
@@ -207,7 +204,7 @@ PlannerServer::computePlan()
if (action_server_->is_cancel_requested()) {
RCLCPP_INFO(get_logger(), "Goal was canceled. Canceling planning action.");
- action_server_->terminate_goals();
+ action_server_->terminate_all();
return;
}
@@ -247,8 +244,7 @@ PlannerServer::computePlan()
RCLCPP_WARN(get_logger(), "Planning algorithm %s failed to generate a valid"
" path to (%.2f, %.2f)", goal->planner_id.c_str(),
goal->pose.pose.position.x, goal->pose.pose.position.y);
- // TODO(orduno): define behavior if a preemption is available
- action_server_->terminate_goals();
+ action_server_->terminate_current();
return;
}
@@ -267,19 +263,17 @@ PlannerServer::computePlan()
RCLCPP_WARN(get_logger(), "%s plugin failed to plan calculation to (%.2f, %.2f): \"%s\"",
goal->planner_id.c_str(), goal->pose.pose.position.x,
goal->pose.pose.position.y, ex.what());
-
// TODO(orduno): provide information about fail error to parent task,
// for example: couldn't get costmap update
- action_server_->terminate_goals();
+ action_server_->terminate_current();
return;
} catch (...) {
RCLCPP_WARN(get_logger(), "Plan calculation failed, "
"An unexpected error has occurred. The planner server"
" may not be able to continue operating correctly.");
-
- // TODO(orduno): provide information about the failure to the parent task,
+ // TODO(orduno): provide information about fail error to parent task,
// for example: couldn't get costmap update
- action_server_->terminate_goals();
+ action_server_->terminate_current();
return;
}
}
diff --git a/nav2_recoveries/include/nav2_recoveries/recovery.hpp b/nav2_recoveries/include/nav2_recoveries/recovery.hpp
index bf628149e321a827c21126d171c58730be01b5be..404fd9ba880d396faf195a76ab3c6ebf6f237bf4 100644
--- a/nav2_recoveries/include/nav2_recoveries/recovery.hpp
+++ b/nav2_recoveries/include/nav2_recoveries/recovery.hpp
@@ -148,7 +148,7 @@ protected:
if (onRun(action_server_->get_current_goal()) != Status::SUCCEEDED) {
RCLCPP_INFO(node_->get_logger(), "Initial checks failed for %s", recovery_name_.c_str());
- action_server_->terminate_goals();
+ action_server_->terminate_current();
return;
}
@@ -162,7 +162,7 @@ protected:
if (action_server_->is_cancel_requested()) {
RCLCPP_INFO(node_->get_logger(), "Canceling %s", recovery_name_.c_str());
stopRobot();
- action_server_->terminate_goals();
+ action_server_->terminate_all();
return;
}
@@ -172,7 +172,7 @@ protected:
" however feature is currently not implemented. Aborting and stopping.",
recovery_name_.c_str());
stopRobot();
- action_server_->terminate_goals();
+ action_server_->terminate_current();
return;
}
@@ -184,7 +184,7 @@ protected:
case Status::FAILED:
RCLCPP_WARN(node_->get_logger(), "%s failed", recovery_name_.c_str());
- action_server_->terminate_goals();
+ action_server_->terminate_current();
return;
case Status::RUNNING:
diff --git a/nav2_recoveries/package.xml b/nav2_recoveries/package.xml
index eb14c3c99bcff52b6db521480b19b570f91f4139..2b178daf19a9dc1cdec368fa838705b1677b6eb9 100644
--- a/nav2_recoveries/package.xml
+++ b/nav2_recoveries/package.xml
@@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>nav2_recoveries</name>
- <version>0.2.4</version>
+ <version>0.3.0</version>
<description>TODO</description>
<maintainer email="carlos.a.orduno@intel.com">Carlos Orduno</maintainer>
<maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>
diff --git a/nav2_rviz_plugins/CMakeLists.txt b/nav2_rviz_plugins/CMakeLists.txt
index b6e4276e87c5f1be58de3e45cd1569dc7a2ff940..65c32d8e852a20c227300bba7990b3e52837afa0 100644
--- a/nav2_rviz_plugins/CMakeLists.txt
+++ b/nav2_rviz_plugins/CMakeLists.txt
@@ -30,6 +30,7 @@ find_package(rviz_ogre_vendor REQUIRED)
find_package(rviz_rendering REQUIRED)
find_package(std_msgs REQUIRED)
find_package(tf2_geometry_msgs REQUIRED)
+find_package(visualization_msgs REQUIRED)
set(nav2_rviz_plugins_headers_to_moc
include/nav2_rviz_plugins/goal_pose_updater
diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp
index 3e9e4271426fc7f9c3151eebcc3bf9a3d9b6c42d..af51df6d285d8812d3fcd64619623877b82860b5 100644
--- a/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp
+++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp
@@ -30,6 +30,7 @@
#include "rclcpp_action/rclcpp_action.hpp"
#include "rviz_common/panel.hpp"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
+#include "visualization_msgs/msg/marker_array.hpp"
#include "nav2_util/geometry_utils.hpp"
class QPushButton;
@@ -123,6 +124,11 @@ private:
QState * accumulated_{nullptr};
std::vector<geometry_msgs::msg::PoseStamped> acummulated_poses_;
+
+ // Publish the visual markers with the waypoints
+ void updateWpNavigationMarkers();
+ // Waypoint navigation visual markers publisher
+ rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr wp_navigation_markers_pub_;
};
class InitialThread : public QThread
diff --git a/nav2_rviz_plugins/package.xml b/nav2_rviz_plugins/package.xml
index 3b356e895c710ac6b1bf08e2a0823e7ef1c03481..5790637342cc80bb4fd4f75d5b4d5cbdf7b3e144 100644
--- a/nav2_rviz_plugins/package.xml
+++ b/nav2_rviz_plugins/package.xml
@@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>nav2_rviz_plugins</name>
- <version>0.2.4</version>
+ <version>0.3.0</version>
<description>Navigation 2 plugins for rviz</description>
<maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
<license>Apache-2.0</license>
@@ -25,7 +25,8 @@
<depend>rviz_rendering</depend>
<depend>std_msgs</depend>
<depend>tf2_geometry_msgs</depend>
-
+ <depend>visualization_msgs</depend>
+
<exec_depend>libqt5-core</exec_depend>
<exec_depend>libqt5-gui</exec_depend>
<exec_depend>libqt5-opengl</exec_depend>
diff --git a/nav2_rviz_plugins/src/nav2_panel.cpp b/nav2_rviz_plugins/src/nav2_panel.cpp
index 4507dd85d9650f69e900402be8df0c5c1d33aa92..a75ea6338eb89d34563b7988ef2159b2b47a1e11 100644
--- a/nav2_rviz_plugins/src/nav2_panel.cpp
+++ b/nav2_rviz_plugins/src/nav2_panel.cpp
@@ -233,6 +233,10 @@ Nav2Panel::Nav2Panel(QWidget * parent)
navigation_goal_ = nav2_msgs::action::NavigateToPose::Goal();
waypoint_follower_goal_ = nav2_msgs::action::FollowWaypoints::Goal();
+ wp_navigation_markers_pub_ =
+ client_node_->create_publisher<visualization_msgs::msg::MarkerArray>("waypoints",
+ rclcpp::QoS(1).transient_local());
+
QObject::connect(&GoalUpdater, SIGNAL(updateGoal(double,double,double,QString)), // NOLINT
this, SLOT(onNewGoal(double,double,double,QString))); // NOLINT
}
@@ -312,6 +316,8 @@ Nav2Panel::onNewGoal(double x, double y, double theta, QString frame)
std::cout << "Start navigation" << std::endl;
startNavigation(pose);
}
+
+ updateWpNavigationMarkers();
}
void
@@ -499,6 +505,60 @@ Nav2Panel::load(const rviz_common::Config & config)
Panel::load(config);
}
+void
+Nav2Panel::updateWpNavigationMarkers()
+{
+ visualization_msgs::msg::MarkerArray marker_array;
+
+ for (size_t i = 0; i < acummulated_poses_.size(); i++) {
+ // Draw a green ball at the waypoint pose
+ visualization_msgs::msg::Marker marker;
+ marker.header = acummulated_poses_[i].header;
+ marker.id = i * 2;
+ marker.type = visualization_msgs::msg::Marker::SPHERE;
+ marker.action = visualization_msgs::msg::Marker::ADD;
+ marker.pose = acummulated_poses_[i].pose;
+ marker.scale.x = 0.1;
+ marker.scale.y = 0.1;
+ marker.scale.z = 0.1;
+ marker.color.r = 0;
+ marker.color.g = 255;
+ marker.color.b = 0;
+ marker.color.a = 1.0f;
+ marker.lifetime = rclcpp::Duration(0);
+ marker.frame_locked = false;
+ marker_array.markers.push_back(marker);
+
+ // Draw the waypoint number
+ visualization_msgs::msg::Marker marker_text;
+ marker_text.header = acummulated_poses_[i].header;
+ marker_text.id = i * 2 + 1;
+ marker_text.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
+ marker_text.action = visualization_msgs::msg::Marker::ADD;
+ marker_text.pose = acummulated_poses_[i].pose;
+ marker_text.pose.position.z += 0.2; // draw it on top of the waypoint
+ marker_text.scale.x = 0.1;
+ marker_text.scale.y = 0.1;
+ marker_text.scale.z = 0.1;
+ marker_text.color.r = 0;
+ marker_text.color.g = 255;
+ marker_text.color.b = 0;
+ marker_text.color.a = 1.0f;
+ marker_text.lifetime = rclcpp::Duration(0);
+ marker_text.frame_locked = false;
+ marker_text.text = "wp_" + std::to_string(i + 1);
+ marker_array.markers.push_back(marker_text);
+ }
+
+ if (marker_array.markers.empty()) {
+ visualization_msgs::msg::Marker clear_all_marker;
+ clear_all_marker.action = visualization_msgs::msg::Marker::DELETEALL;
+ marker_array.markers.push_back(clear_all_marker);
+ }
+
+ wp_navigation_markers_pub_->publish(marker_array);
+}
+
} // namespace nav2_rviz_plugins
#include <pluginlib/class_list_macros.hpp> // NOLINT
diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt
index afb3f68d184b640ffff51334631928423526b838..03891cebe46e35ee4dcd1e434c47169fd3261e32 100644
--- a/nav2_system_tests/CMakeLists.txt
+++ b/nav2_system_tests/CMakeLists.txt
@@ -53,6 +53,7 @@ if(BUILD_TESTING)
add_subdirectory(src/system)
add_subdirectory(src/updown)
add_subdirectory(src/waypoint_follower)
+ install(DIRECTORY maps DESTINATION share/${PROJECT_NAME})
endif()
diff --git a/nav2_system_tests/maps/empty_room.yaml b/nav2_system_tests/maps/empty_room.yaml
deleted file mode 120000
index 6d6af16497357511bb47ef43c19c54dd69ebd815..0000000000000000000000000000000000000000
--- a/nav2_system_tests/maps/empty_room.yaml
+++ /dev/null
@@ -1,6 +0,0 @@
-image: empty.pgm
-resolution: 0.1
-origin: [-10.000000, -10.000000, 0.000000]
-negate: 0
-occupied_thresh: 0.65
-free_thresh: 0.196
diff --git a/nav2_system_tests/maps/empty_room.yaml b/nav2_system_tests/maps/empty_room.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..6d6af16497357511bb47ef43c19c54dd69ebd815
--- /dev/null
+++ b/nav2_system_tests/maps/empty_room.yaml
@@ -0,0 +1,6 @@
+image: empty.pgm
+resolution: 0.1
+origin: [-10.000000, -10.000000, 0.000000]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
diff --git a/nav2_system_tests/models/turtlebot3_waffle_depth_camera/turtlebot3_waffle_realsense/model.config b/nav2_system_tests/models/turtlebot3_waffle_depth_camera/model.config
similarity index 78%
rename from nav2_system_tests/models/turtlebot3_waffle_depth_camera/turtlebot3_waffle_realsense/model.config
rename to nav2_system_tests/models/turtlebot3_waffle_depth_camera/model.config
index acf1a68e90358a9b345af12d88658cbd3f8fb8da..f82c96e58b20a598d5e8f6cf708a7c4f47253a80 100644
--- a/nav2_system_tests/models/turtlebot3_waffle_depth_camera/turtlebot3_waffle_realsense/model.config
+++ b/nav2_system_tests/models/turtlebot3_waffle_depth_camera/model.config
@@ -1,7 +1,7 @@
<?xml version="1.0"?>
<model>
- <name>TurtleBot3(Waffle)</name>
+ <name>TurtleBot3_Waffle_DepthCamera</name>
<version>2.0</version>
<sdf version="1.5">model.sdf</sdf>
@@ -14,6 +14,6 @@
</author>
<description>
- TurtleBot3 Waffle
+ TurtleBot3 Waffle with Depth Camera
</description>
</model>
diff --git a/nav2_system_tests/models/turtlebot3_waffle_depth_camera/turtlebot3_waffle_realsense/model.sdf b/nav2_system_tests/models/turtlebot3_waffle_depth_camera/model.sdf
similarity index 99%
rename from nav2_system_tests/models/turtlebot3_waffle_depth_camera/turtlebot3_waffle_realsense/model.sdf
rename to nav2_system_tests/models/turtlebot3_waffle_depth_camera/model.sdf
index d7fe0f8b4f24a6e32dfdb1487b391800e5cbba64..6dd576dfe6bdfe5d003b0633d3ac3e0834a3d5d1 100644
--- a/nav2_system_tests/models/turtlebot3_waffle_depth_camera/turtlebot3_waffle_realsense/model.sdf
+++ b/nav2_system_tests/models/turtlebot3_waffle_depth_camera/model.sdf
@@ -1,6 +1,6 @@
<?xml version="1.0" ?>
<sdf version="1.5">
- <model name="turtlebot3_waffle">
+ <model name="TurtleBot3_Waffle_DepthCamera">
<pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
<link name="base_footprint"/>
@@ -335,7 +335,7 @@
<sensor name="intel_realsense_r200_depth" type="depth">
<always_on>1</always_on>
- <update_rate>1</update_rate>
+ <update_rate>15</update_rate>
<pose>0.064 -0.047 0.107 0 0 0</pose>
<camera name="realsense_depth_camera">
</camera>
diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml
index afd4b166743466c7500bc3de38cc46ecaca35885..378ad372c2415932446a6858ff1eb1eadd2e4081 100644
--- a/nav2_system_tests/package.xml
+++ b/nav2_system_tests/package.xml
@@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>nav2_system_tests</name>
- <version>0.2.4</version>
+ <version>0.3.0</version>
<description>TODO</description>
<maintainer email="carlos.a.orduno@intel.com">Carlos Orduno</maintainer>
<license>Apache-2.0</license>
@@ -53,6 +53,7 @@
<test_depend>ament_lint_common</test_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_cmake_gtest</test_depend>
+ <test_depend>ament_cmake_pytest</test_depend>
<test_depend>launch</test_depend>
<test_depend>launch_ros</test_depend>
<test_depend>launch_testing</test_depend>
diff --git a/nav2_system_tests/src/updown/CMakeLists.txt b/nav2_system_tests/src/updown/CMakeLists.txt
index 1dc1da51431ba859db19c894b96bceb95c60f4fc..a56bff34b2300a1518cacd6c6b377eca372c58b3 100644
--- a/nav2_system_tests/src/updown/CMakeLists.txt
+++ b/nav2_system_tests/src/updown/CMakeLists.txt
@@ -6,6 +6,6 @@ ament_target_dependencies(test_updown
${dependencies}
)
-install(TARGETS test_updown
- RUNTIME DESTINATION lib/${PROJECT_NAME}
-)
+install(TARGETS test_updown RUNTIME DESTINATION share/${PROJECT_NAME})
+install(FILES test_updown_launch.py DESTINATION share/${PROJECT_NAME})
+
diff --git a/nav2_system_tests/src/updown/nav2_params.yaml b/nav2_system_tests/src/updown/nav2_params.yaml
deleted file mode 100644
index 46312f8eec84c50528077fadf52ac551f91a9504..0000000000000000000000000000000000000000
--- a/nav2_system_tests/src/updown/nav2_params.yaml
+++ /dev/null
@@ -1,179 +0,0 @@
-amcl:
- ros__parameters:
- use_sim_time: True
- alpha1: 0.2
- alpha2: 0.2
- alpha3: 0.2
- alpha4: 0.2
- alpha5: 0.2
- base_frame_id: "base_footprint"
- beam_skip_distance: 0.5
- beam_skip_error_threshold: 0.9
- beam_skip_threshold: 0.3
- do_beamskip: false
- global_frame_id: "map"
- lambda_short: 0.1
- laser_likelihood_max_dist: 2.0
- laser_max_range: 100.0
- laser_min_range: -1.0
- laser_model_type: "likelihood_field"
- set_initial_pose: true
- initial_pose.x: -2.0
- initial_pose.y: -0.5
- initial_pose.z: 0.0
- initial_pose.yaw: 0.0
- max_beams: 60
- max_particles: 2000
- min_particles: 500
- odom_frame_id: "odom"
- pf_err: 0.05
- pf_z: 0.99
- recovery_alpha_fast: 0.0
- recovery_alpha_slow: 0.0
- resample_interval: 1
- robot_model_type: "differential"
- save_pose_rate: 0.5
- sigma_hit: 0.2
- tf_broadcast: true
- transform_tolerance: 1.0
- update_min_a: 0.2
- update_min_d: 0.25
- z_hit: 0.5
- z_max: 0.05
- z_rand: 0.5
- z_short: 0.05
-
-amcl_map_client:
- ros__parameters:
- use_sim_time: True
-
-amcl_rclcpp_node:
- ros__parameters:
- use_sim_time: True
-
-bt_navigator:
- ros__parameters:
- use_sim_time: True
- bt_xml_filename: "navigate_w_replanning_and_recovery.xml"
-
-bt_navigator_rclcpp_node:
- ros__parameters:
- use_sim_time: True
-
-nav2_controller:
- ros__parameters:
- use_sim_time: True
- debug_trajectory_details: True
- min_vel_x: -0.26
- min_vel_y: 0.0
- max_vel_x: 0.26
- max_vel_y: 0.0
- max_vel_theta: 1.0
- min_speed_xy: 0.0
- # Set max XY speed to a large value so noisy Y data from IMU doesn't
- # prevent operation. Since this robot has no Y velocity capability, any Y
- # velocity reported in /odom data is due to slippage or noise, and there is
- # no need to reduce the robot's operating envelope when generating possible
- # trajectories.
- max_speed_xy: 100.0
- min_speed_theta: 0.0
- acc_lim_x: 2.5
- acc_lim_y: 0.0
- acc_lim_theta: 3.2
- decel_lim_x: -2.5
- decel_lim_y: 0.0
- decel_lim_theta: -3.2
- ObstacleFootprint.scale: 1.0
- ObstacleFootprint.max_scaling_factor: 0.2
- ObstacleFootprint.scaling_speed: 0.25
- PathAlign.scale: 32.0
- GoalAlign.scale: 24.0
- PathDist.scale: 32.0
- GoalDist.scale: 24.0
- RotateToGoal.scale: 32.0
-
-nav2_controller_rclcpp_node:
- ros__parameters:
- use_sim_time: True
-
-local_costmap:
- local_costmap:
- ros__parameters:
- use_sim_time: True
- robot_radius: 0.092
- obstacle_layer:
- enabled: False
- always_send_full_costmap: True
- observation_sources: scan
- scan:
- topic: /scan
- max_obstacle_height: 2.0
- clearing: True
- marking: True
- local_costmap_rclcpp_node:
- ros__parameters:
- use_sim_time: True
-
-local_costmap_client:
- ros__parameters:
- use_sim_time: True
-
-lifecycle_manager:
- ros__parameters:
- use_sim_time: True
-
-lifecycle_manager_client_service_client:
- ros__parameters:
- use_sim_time: True
-
-lifecycle_manager_service_client:
- ros__parameters:
- use_sim_time: True
-
-map_server:
- ros__parameters:
- use_sim_time: True
- yaml_filename: "turtlebot3_world.yaml"
-
-navfn_planner:
- ros__parameters:
- use_sim_time: True
- tolerance: 0.0
- use_astar: false
-
-navfn_planner_GetCostmap_client:
- ros__parameters:
- use_sim_time: True
-
-navfn_planner_rclcpp_node:
- ros__parameters:
- use_sim_time: True
-
-robot_state_publisher:
- ros__parameters:
- use_sim_time: True
-
-global_costmap:
- global_costmap:
- ros__parameters:
- use_sim_time: True
- obstacle_layer:
- enabled: False
- always_send_full_costmap: True
- observation_sources: scan
- scan:
- topic: /scan
- max_obstacle_height: 2.0
- clearing: True
- marking: True
- global_costmap_rclcpp_node:
- ros__parameters:
- use_sim_time: True
-
-global_costmap_client:
- ros__parameters:
- use_sim_time: True
-
-transform_listener_impl:
- ros__parameters:
- use_sim_time: True
diff --git a/nav2_system_tests/src/updown/test_updown.cpp b/nav2_system_tests/src/updown/test_updown.cpp
index 822df9ddb1d4ada3cffb82b0f0f3191933d34ff6..48d276d93b98e9b31f5ea664cc5715ee4c3e837f 100644
--- a/nav2_system_tests/src/updown/test_updown.cpp
+++ b/nav2_system_tests/src/updown/test_updown.cpp
@@ -22,27 +22,12 @@
using namespace std::chrono_literals;
-struct xytheta
-{
- double x;
- double y;
- double theta;
-};
-
int main(int argc, char ** argv)
{
rclcpp::init(argc, argv);
+ RCLCPP_INFO(rclcpp::get_logger("test_updown"), "Initializing test");
nav2_lifecycle_manager::LifecycleManagerClient client;
-
- // Create a set of target poses across the map
- std::vector<xytheta> target_poses;
- target_poses.push_back({-2.0, -0.5, 0});
- target_poses.push_back({0.94, -0.55, 0});
- target_poses.push_back({1.7, 0.5, 1.4});
- target_poses.push_back({0.97, 1.68, 2.94});
- target_poses.push_back({0.02, 1.74, -2.9});
-
- xytheta & initial_pose = target_poses[0];
+ bool test_passed = true;
// Wait for a few seconds to let all of the nodes come up
std::this_thread::sleep_for(5s);
@@ -50,61 +35,35 @@ int main(int argc, char ** argv)
// Start the nav2 system, bringing it to the ACTIVE state
client.startup();
- // Set the robot's starting pose (approximately where it comes up in gazebo)
- client.set_initial_pose(initial_pose.x, initial_pose.y, initial_pose.theta);
-
// Wait for a couple secs to make sure the nodes have processed all discovery
// info before starting
+ RCLCPP_INFO(rclcpp::get_logger("test_updown"), "Waiting for nodes to be active");
std::this_thread::sleep_for(2s);
- // Parse the command line options
- char * nav_type_arg = rcutils_cli_get_option(argv, argv + argc, "-t");
- if (nav_type_arg != nullptr) {
- std::string nav_type(nav_type_arg);
-
- if (nav_type == "iterative") {
- // In the iterative case, navigate through all of the poses (but skip the
- // first one, which is the initial pose)
- for (std::vector<xytheta>::size_type i = 1; i < target_poses.size(); i++) {
- auto pose = target_poses[i];
- if (!client.navigate_to_pose(pose.x, pose.y, pose.theta)) {
- RCLCPP_ERROR(rclcpp::get_logger("test_updown"), "Navigation failed!");
- break;
- }
- }
- } else if (nav_type == "random") {
- // In the random case, navigate to randomly-selected poses from the target_poses
- // collection
-
- // Get set up to generate random indices
- std::random_device r;
- std::default_random_engine e1(r());
- std::uniform_int_distribution<int> uniform_dist(0, target_poses.size() - 1);
-
- for (int i = 0, cur_index = 0; i < 10; i++) {
- // Get a random index that is not the current one (so we can navigate
- // to a pose different than our current location)
- int next_index = 0;
- do {
- next_index = uniform_dist(r);
- } while (next_index == cur_index);
-
- // Grab the pose for that index and start the navigation
- auto pose = target_poses[next_index];
- if (!client.navigate_to_pose(pose.x, pose.y, pose.theta)) {
- RCLCPP_ERROR(rclcpp::get_logger("test_updown"), "Navigation failed!");
- break;
- }
- }
- } else {
- RCLCPP_ERROR(rclcpp::get_logger("test_updown"),
- "Unrecognized test type: %s, running simple up/down test\n", nav_type);
- }
+ // The system should now be active
+ int retries = 0;
+ while ((client.is_active() != nav2_lifecycle_manager::SystemStatus::ACTIVE) &&
+ (retries < 10))
+ {
+ std::this_thread::sleep_for(2s);
+ retries++;
+ }
+ if (retries == 10) {
+ // the system isn't active
+ RCLCPP_ERROR(rclcpp::get_logger("test_updown"), "System startup failed");
+ test_passed = false;
}
// Shut down the nav2 system, bringing it to the FINALIZED state
client.shutdown();
+ if (test_passed) {
+ RCLCPP_INFO(rclcpp::get_logger("test_updown"),
+ "**************************************************** TEST PASSED!");
+ } else {
+ RCLCPP_INFO(rclcpp::get_logger("test_updown"),
+ "**************************************************** TEST FAILED!");
+ }
rclcpp::shutdown();
return 0;
}
diff --git a/nav2_system_tests/src/updown/test_updown_launch.py b/nav2_system_tests/src/updown/test_updown_launch.py
index 8f75db644eb0919d57484aa7b5eb6b08da91c961..231d120902ab785c356fe48607667fb7c0c12ead 100755
--- a/nav2_system_tests/src/updown/test_updown_launch.py
+++ b/nav2_system_tests/src/updown/test_updown_launch.py
@@ -18,127 +18,74 @@ from ament_index_python.packages import get_package_prefix
from ament_index_python.packages import get_package_share_directory
import launch.actions
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import Node
def generate_launch_description():
# Configuration parameters for the launch
-
- world = launch.substitutions.LaunchConfiguration('world')
- urdf = launch.substitutions.LaunchConfiguration('urdf')
-
- params_file = launch.substitutions.LaunchConfiguration(
- 'params',
- default=[launch.substitutions.ThisLaunchFileDir(), '/nav2_params.yaml'])
-
- declare_world_cmd = launch.actions.DeclareLaunchArgument(
- 'world',
- default_value=[launch.substitutions.ThisLaunchFileDir(),
- '/../../worlds/turtlebot3_ros2_demo.world'],
- description='Full path to world file to load')
-
- declare_urdf_cmd = launch.actions.DeclareLaunchArgument(
- 'urdf',
- default_value=[launch.substitutions.ThisLaunchFileDir(),
- '/../../urdf/turtlebot3_waffle.urdf'],
- description='Full path to model file to load')
-
launch_dir = os.path.join(
get_package_share_directory('nav2_bringup'), 'launch')
- # Specify the actions
-
- start_gazebo_cmd = launch.actions.ExecuteProcess(
- cmd=['gzserver', '-s', 'libgazebo_ros_init.so',
- world,
- '--ros-args', '--params-file', params_file],
- cwd=[launch_dir], output='screen')
-
- start_robot_state_publisher_cmd = launch.actions.ExecuteProcess(
- cmd=[
- os.path.join(
- get_package_prefix('robot_state_publisher'),
- 'lib/robot_state_publisher/robot_state_publisher'),
- urdf,
- '--ros-args', '--params-file', params_file],
- cwd=[launch_dir], output='screen')
-
- start_map_server_cmd = launch.actions.ExecuteProcess(
- cmd=[
- os.path.join(
- get_package_prefix('nav2_map_server'),
- 'lib/nav2_map_server/map_server'),
- '--ros-args', '--params-file', params_file],
- cwd=[launch_dir], output='screen')
-
- start_localizer_cmd = launch.actions.ExecuteProcess(
- cmd=[
- os.path.join(
- get_package_prefix('nav2_amcl'),
- 'lib/nav2_amcl/amcl'),
- '--ros-args', '--params-file', params_file],
- cwd=[launch_dir], output='screen')
-
- start_dwb_cmd = launch.actions.ExecuteProcess(
- cmd=[
- os.path.join(
- get_package_prefix('nav2_controller'),
- 'lib/nav2_controller/controller_server'),
- '--ros-args', '--params-file', params_file],
- cwd=[launch_dir], output='screen')
-
- start_planner_cmd = launch.actions.ExecuteProcess(
- cmd=[
- os.path.join(
- get_package_prefix('nav2_planner'),
- 'lib/nav2_planner/planner_server'),
- '--ros-args', '--params-file', params_file],
- cwd=[launch_dir], output='screen')
+ map_yaml_file = os.path.join(
+ get_package_share_directory('nav2_system_tests'), 'maps/map_circular.yaml')
- start_navigator_cmd = launch.actions.ExecuteProcess(
- cmd=[
- os.path.join(
- get_package_prefix('nav2_bt_navigator'),
- 'lib/nav2_bt_navigator/bt_navigator'),
- '--ros-args', '--params-file', params_file],
- cwd=[launch_dir], output='screen')
-
- start_controller_cmd = launch.actions.ExecuteProcess(
- cmd=[
- os.path.join(
- get_package_prefix('nav2_lifecycle_manager'),
- 'lib/nav2_lifecycle_manager/lifecycle_manager'),
- '--ros-args', '--params-file', params_file],
- cwd=[launch_dir], output='screen')
-
- startup_cmd = launch.actions.ExecuteProcess(
+ # Specify the actions
+ start_tf_cmd_1 = Node(
+ package='tf2_ros',
+ node_executable='static_transform_publisher',
+ output='screen',
+ arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom'])
+
+ start_tf_cmd_2 = Node(
+ package='tf2_ros',
+ node_executable='static_transform_publisher',
+ output='screen',
+ arguments=['0', '0', '0', '0', '0', '0', 'odom', 'base_footprint'])
+
+ start_tf_cmd_3 = Node(
+ package='tf2_ros',
+ node_executable='static_transform_publisher',
+ output='screen',
+ arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link'])
+
+ start_tf_cmd_4 = Node(
+ package='tf2_ros',
+ node_executable='static_transform_publisher',
+ output='screen',
+ arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan'])
+
+ nav2_bringup = launch.actions.IncludeLaunchDescription(
+ PythonLaunchDescriptionSource(
+ os.path.join(launch_dir, 'nav2_bringup_launch.py')),
+ launch_arguments={'map': map_yaml_file,
+ 'use_sim_time': 'True',
+ 'autostart': 'False'}.items())
+
+ start_test = launch.actions.ExecuteProcess(
cmd=[
os.path.join(
get_package_prefix('nav2_system_tests'),
- 'lib/nav2_system_tests/test_updown'),
- '--ros-args', '--params-file', params_file],
+ 'lib/nav2_system_tests/test_updown')],
cwd=[launch_dir], output='screen')
- startup_exit_event_handler = launch.actions.RegisterEventHandler(
+ test_exit_event_handler = launch.actions.RegisterEventHandler(
event_handler=launch.event_handlers.OnProcessExit(
- target_action=startup_cmd,
+ target_action=start_test,
on_exit=launch.actions.EmitEvent(event=launch.events.Shutdown(reason='Done!'))))
# Compose the launch description
ld = launch.LaunchDescription()
- ld.add_action(declare_world_cmd)
- ld.add_action(declare_urdf_cmd)
- ld.add_action(start_controller_cmd)
- ld.add_action(start_gazebo_cmd)
- ld.add_action(start_robot_state_publisher_cmd)
- ld.add_action(start_map_server_cmd)
- ld.add_action(start_localizer_cmd)
- ld.add_action(start_dwb_cmd)
- ld.add_action(start_planner_cmd)
- ld.add_action(start_navigator_cmd)
- ld.add_action(startup_cmd)
- ld.add_action(startup_exit_event_handler)
+ ld.add_action(start_tf_cmd_1)
+ ld.add_action(start_tf_cmd_2)
+ ld.add_action(start_tf_cmd_3)
+ ld.add_action(start_tf_cmd_4)
+
+ ld.add_action(nav2_bringup)
+ ld.add_action(start_test)
+ ld.add_action(test_exit_event_handler)
return ld
diff --git a/nav2_util/include/nav2_util/parameter_events_subscriber.hpp b/nav2_util/include/nav2_util/parameter_events_subscriber.hpp
deleted file mode 100644
index 3332940071d71dd5cbf4a10385da6cfd19e62592..0000000000000000000000000000000000000000
--- a/nav2_util/include/nav2_util/parameter_events_subscriber.hpp
+++ /dev/null
@@ -1,239 +0,0 @@
-// Copyright 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_UTIL__PARAMETER_EVENTS_SUBSCRIBER_HPP_
-#define NAV2_UTIL__PARAMETER_EVENTS_SUBSCRIBER_HPP_
-
-#include <string>
-#include <utility>
-#include <unordered_map>
-#include <vector>
-
-#include "rclcpp/rclcpp.hpp"
-#include "rclcpp/parameter_events_filter.hpp"
-
-namespace nav2_util
-{
-
-struct ParameterEventsCallbackHandle
-{
- RCLCPP_SMART_PTR_DEFINITIONS(ParameterEventsCallbackHandle)
-
- using ParameterEventsCallbackType = std::function<void (const rclcpp::Parameter &)>;
-
- std::string parameter_name;
- std::string node_name;
- ParameterEventsCallbackType callback;
-};
-
-class ParameterEventsSubscriber
-{
-public:
- ParameterEventsSubscriber(
- rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
- rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics,
- rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
- const rclcpp::QoS & qos = rclcpp::ParameterEventsQoS());
-
- template<typename NodeT>
- ParameterEventsSubscriber(
- NodeT node,
- const rclcpp::QoS & qos =
- rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_parameter_events)))
- : ParameterEventsSubscriber(
- node->get_node_base_interface(),
- node->get_node_topics_interface(),
- node->get_node_logging_interface(),
- qos)
- {}
-
- /// Set a custom callback for parameter events.
- /**
- * If no namespace is provided, a subscription will be created for the current namespace.
- * Repeated calls to this function will overwrite the callback.
- * If more than one namespace already has a subscription to its parameter events topic, then the
- * provided callback will be applied to all of them.
- *
- * \param[in] callback Function callback to be evaluated on event.
- * \param[in] node_namespaces Vector of namespaces for which a subscription will be created.
- */
- void
- set_event_callback(
- std::function<void(const rcl_interfaces::msg::ParameterEvent::SharedPtr &)> callback,
- const std::vector<std::string> & node_namespaces = {""});
-
- /// Remove parameter event callback.
- /**
- * Calling this function will set the event callback to nullptr. This function will also remove
- * event subscriptions on the namespaces for which there are no other callbacks (from parameter
- * callbacks) active on that namespace.
- */
- void
- remove_event_callback();
-
- using ParameterEventsCallbackType = ParameterEventsCallbackHandle::ParameterEventsCallbackType;
-
- /// Add a custom callback for a specified parameter.
- /**
- * If a node_name is not provided, defaults to the current node.
- *
- * \param[in] parameter_name Name of parameter.
- * \param[in] callback Function callback to be evaluated upon parameter event.
- * \param[in] node_name Name of node which hosts the parameter.
- */
- ParameterEventsCallbackHandle::SharedPtr
- add_parameter_callback(
- const std::string & parameter_name,
- ParameterEventsCallbackType callback,
- const std::string & node_name = "");
-
- /// Remove a custom callback for a specified parameter given its callback handle.
- /**
- * The parameter name and node name are inspected from the callback handle. The callback handle
- * is erased from the list of callback handles on the {parameter_name, node_name} in the map.
- * An error is thrown if the handle does not exist and/or was already removed.
- *
- * \param[in] handle Pointer to callback handle to be removed.
- */
- void
- remove_parameter_callback(
- const ParameterEventsCallbackHandle * const handle);
-
- /// Remove a custom callback for a specified parameter given its name and respective node.
- /**
- * If a node_name is not provided, defaults to the current node.
- * The {parameter_name, node_name} key on the parameter_callbacks_ map will be erased, removing
- * all callback associated with that parameter.
- *
- * \param[in] parameter_name Name of parameter.
- * \param[in] node_name Name of node which hosts the parameter.
- */
- void
- remove_parameter_callback(
- const std::string & parameter_name,
- const std::string & node_name = "");
-
- /// Get a rclcpp::Parameter from parameter event, return true if parameter name & node in event.
- /**
- * If a node_name is not provided, defaults to the current node.
- *
- * \param[in] event Event msg to be inspected.
- * \param[out] parameter Reference to rclcpp::Parameter to be assigned.
- * \param[in] parameter_name Name of parameter.
- * \param[in] node_name Name of node which hosts the parameter.
- * \returns true if requested parameter name & node is in event, false otherwise
- */
- static bool
- get_parameter_from_event(
- const rcl_interfaces::msg::ParameterEvent::SharedPtr event,
- rclcpp::Parameter & parameter,
- const std::string parameter_name,
- const std::string node_name = "");
-
- /// Get a rclcpp::Parameter from parameter event
- /**
- * If a node_name is not provided, defaults to the current node.
- *
- * The user is responsible to check if the returned parameter has been properly assigned.
- * By default, if the requested parameter is not found in the event, the returned parameter
- * has parameter value of type rclcpp::PARAMETER_NOT_SET.
- *
- * \param[in] event Event msg to be inspected.
- * \param[in] parameter_name Name of parameter.
- * \param[in] node_name Name of node which hosts the parameter.
- * \returns The resultant rclcpp::Parameter from the event
- */
- static rclcpp::Parameter
- get_parameter_from_event(
- const rcl_interfaces::msg::ParameterEvent::SharedPtr event,
- const std::string parameter_name,
- const std::string node_name = "");
-
- using CallbacksContainerType = std::list<ParameterEventsCallbackHandle::WeakPtr>;
-
-protected:
- /// Add a subscription (if unique) to a namespace parameter events topic.
- void
- add_namespace_event_subscriber(const std::string & node_namespace);
-
- /// Remove a subscription to a namespace parameter events topic.
- void
- remove_namespace_event_subscriber(const std::string & node_namespace);
-
- /// Return true if any callbacks still exist on a namespace event topic, otherwise return false
- bool
- should_unsubscribe_to_namespace(const std::string & node_namespace);
-
- /// Callback for parameter events subscriptions.
- void
- event_callback(const rcl_interfaces::msg::ParameterEvent::SharedPtr event);
-
- // Utility functions for string and path name operations.
- std::string resolve_path(const std::string & path);
- std::pair<std::string, std::string> split_path(const std::string & str);
- std::string join_path(std::string path, std::string name);
-
- // Node Interfaces used for logging and creating subscribers.
- rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_;
- rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_;
- rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_;
-
- rclcpp::QoS qos_;
-
- // *INDENT-OFF* Uncrustify doesn't handle indented public/private labels
- // Hash function for string pair required in std::unordered_map
- class StringPairHash
- {
- public:
- template<typename T>
- inline void hash_combine(std::size_t & seed, const T & v) const
- {
- std::hash<T> hasher;
- seed ^= hasher(v) + 0x9e3779b9 + (seed << 6) + (seed >> 2);
- }
-
- inline size_t operator()(const std::pair<std::string, std::string> & s) const
- {
- size_t seed = 0;
- hash_combine(seed, s.first);
- hash_combine(seed, s.second);
- return seed;
- }
- };
- // *INDENT-ON*
-
- // Map container for registered parameters.
- std::unordered_map<
- std::pair<std::string, std::string>,
- CallbacksContainerType,
- StringPairHash
- > parameter_callbacks_;
-
- // Vector of unique namespaces added.
- std::vector<std::string> subscribed_namespaces_;
- // Vector of event callback namespaces
- std::vector<std::string> event_namespaces_;
-
- // Vector of event subscriptions for each namespace.
- std::vector<rclcpp::Subscription
- <rcl_interfaces::msg::ParameterEvent>::SharedPtr> event_subscriptions_;
-
- std::function<void(const rcl_interfaces::msg::ParameterEvent::SharedPtr &)> event_callback_;
-
- std::recursive_mutex mutex_;
-};
-
-} // namespace nav2_util
-
-#endif // NAV2_UTIL__PARAMETER_EVENTS_SUBSCRIBER_HPP_
diff --git a/nav2_util/include/nav2_util/simple_action_server.hpp b/nav2_util/include/nav2_util/simple_action_server.hpp
index eb1e118b461798e7860965c64ff5e14dd57f0c71..371f887eed747c73a06d1db809eb6f83b5c2bac5 100644
--- a/nav2_util/include/nav2_util/simple_action_server.hpp
+++ b/nav2_util/include/nav2_util/simple_action_server.hpp
@@ -18,6 +18,9 @@
#include <memory>
#include <mutex>
#include <string>
+#include <thread>
+#include <future>
+#include <chrono>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
@@ -35,13 +38,14 @@ public:
typename nodeT::SharedPtr node,
const std::string & action_name,
ExecuteCallback execute_callback,
- bool autostart = true)
+ bool autostart = true,
+ std::chrono::milliseconds server_timeout = std::chrono::milliseconds(500))
: SimpleActionServer(
node->get_node_base_interface(),
node->get_node_clock_interface(),
node->get_node_logging_interface(),
node->get_node_waitables_interface(),
- action_name, execute_callback, autostart)
+ action_name, execute_callback, autostart, server_timeout)
{}
explicit SimpleActionServer(
@@ -51,76 +55,21 @@ public:
rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr node_waitables_interface,
const std::string & action_name,
ExecuteCallback execute_callback,
- bool autostart = true)
+ bool autostart = true,
+ std::chrono::milliseconds server_timeout = std::chrono::milliseconds(500))
: node_base_interface_(node_base_interface),
node_clock_interface_(node_clock_interface),
node_logging_interface_(node_logging_interface),
node_waitables_interface_(node_waitables_interface),
- action_name_(action_name), execute_callback_(execute_callback)
+ action_name_(action_name),
+ execute_callback_(execute_callback),
+ server_timeout_(server_timeout)
{
if (autostart) {
server_active_ = true;
}
- auto handle_goal =
- [this](const rclcpp_action::GoalUUID &, std::shared_ptr<const typename ActionT::Goal>)
- {
- std::lock_guard<std::recursive_mutex> lock(update_mutex_);
-
- if (!server_active_) {
- return rclcpp_action::GoalResponse::REJECT;
- }
-
- debug_msg("Received request for goal acceptance");
- return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
- };
-
- auto handle_cancel =
- [this](std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>>)
- {
- std::lock_guard<std::recursive_mutex> lock(update_mutex_);
- // TODO(orduno) could goal handle be aborted (and on a terminal state) before reaching here?
- debug_msg("Received request for goal cancellation");
- return rclcpp_action::CancelResponse::ACCEPT;
- };
-
- auto handle_accepted =
- [this](std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>> handle)
- {
- std::lock_guard<std::recursive_mutex> lock(update_mutex_);
- debug_msg("Receiving a new goal");
-
- if (is_active(current_handle_)) {
- debug_msg("An older goal is active, moving the new goal to a pending slot.");
-
- if (is_active(pending_handle_)) {
- debug_msg("The pending slot is occupied."
- " The previous pending goal will be aborted and replaced.");
-
- pending_handle_->abort(empty_result());
- pending_handle_.reset();
- preempt_requested_ = false;
- }
-
- debug_msg("Setting flag so the action server can grab the preempt request.");
- preempt_requested_ = true;
- pending_handle_ = handle;
- } else {
- if (is_active(pending_handle_)) {
- // Shouldn't reach a state with a pending goal but no current one.
- error_msg("Forgot to handle a preemption. Aborting the pending goal.");
-
- pending_handle_->abort(empty_result());
- pending_handle_.reset();
- preempt_requested_ = false;
- }
-
- debug_msg("Starting a thread to process the goals");
-
- current_handle_ = handle;
- std::thread{execute_callback_}.detach();
- }
- };
+ using namespace std::placeholders; // NOLINT
action_server_ = rclcpp_action::create_server<ActionT>(
node_base_interface_,
@@ -128,31 +77,144 @@ public:
node_logging_interface_,
node_waitables_interface_,
action_name_,
- handle_goal,
- handle_cancel,
- handle_accepted);
+ std::bind(&SimpleActionServer::handle_goal, this, _1, _2),
+ std::bind(&SimpleActionServer::handle_cancel, this, _1),
+ std::bind(&SimpleActionServer::handle_accepted, this, _1));
+ }
+
+ rclcpp_action::GoalResponse handle_goal(
+ const rclcpp_action::GoalUUID & /*uuid*/,
+ std::shared_ptr<const typename ActionT::Goal>/*goal*/)
+ {
+ std::lock_guard<std::recursive_mutex> lock(update_mutex_);
+
+ if (!server_active_) {
+ return rclcpp_action::GoalResponse::REJECT;
+ }
+
+ debug_msg("Received request for goal acceptance");
+ return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+ }
+
+ rclcpp_action::CancelResponse handle_cancel(
+ const std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>>/*handle*/)
+ {
+ std::lock_guard<std::recursive_mutex> lock(update_mutex_);
+ debug_msg("Received request for goal cancellation");
+ return rclcpp_action::CancelResponse::ACCEPT;
+ }
+
+ void handle_accepted(const std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>> handle)
+ {
+ std::lock_guard<std::recursive_mutex> lock(update_mutex_);
+ debug_msg("Receiving a new goal");
+
+ if (is_active(current_handle_)) {
+ debug_msg("An older goal is active, moving the new goal to a pending slot.");
+
+ if (is_active(pending_handle_)) {
+ debug_msg("The pending slot is occupied."
+ " The previous pending goal will be terminated and replaced.");
+ terminate(pending_handle_);
+ }
+ pending_handle_ = handle;
+ preempt_requested_ = true;
+ } else {
+ if (is_active(pending_handle_)) {
+ // Shouldn't reach a state with a pending goal but no current one.
+ error_msg("Forgot to handle a preemption. Terminating the pending goal.");
+ terminate(pending_handle_);
+ preempt_requested_ = false;
+ }
+
+ current_handle_ = handle;
+
+ // Return quickly to avoid blocking the executor, so spin up a new thread
+ debug_msg("Executing goal asynchronously.");
+ execution_future_ = std::async(std::launch::async, [this]() {work();});
+ }
+ }
+
+ void work()
+ {
+ while (rclcpp::ok() && !stop_execution_ && is_active(current_handle_)) {
+ debug_msg("Executing the goal...");
+ try {
+ execute_callback_();
+ } catch (std::exception & ex) {
+ RCLCPP_ERROR(node_logging_interface_->get_logger(),
+ "Action server failed while executing action callback: \"%s\"", ex.what());
+ terminate_all();
+ return;
+ }
+
+ debug_msg("Blocking processing of new goal handles.");
+ std::lock_guard<std::recursive_mutex> lock(update_mutex_);
+
+ if (stop_execution_) {
+ warn_msg("Stopping the thread per request.");
+ terminate_all();
+ break;
+ }
+
+ if (is_active(current_handle_)) {
+ warn_msg("Current goal was not completed successfully.");
+ terminate(current_handle_);
+ }
+
+ if (is_active(pending_handle_)) {
+ debug_msg("Executing a pending handle on the existing thread.");
+ accept_pending_goal();
+ } else {
+ debug_msg("Done processing available goals.");
+ break;
+ }
+ }
+ debug_msg("Worker thread done.");
}
void activate()
{
std::lock_guard<std::recursive_mutex> lock(update_mutex_);
server_active_ = true;
+ stop_execution_ = false;
}
void deactivate()
{
- std::lock_guard<std::recursive_mutex> lock(update_mutex_);
- server_active_ = false;
+ debug_msg("Deactivating...");
- if (is_active(current_handle_)) {
- warn_msg("Taking action server to deactive state with an active goal.");
+ {
+ std::lock_guard<std::recursive_mutex> lock(update_mutex_);
+ server_active_ = false;
+ stop_execution_ = true;
+ }
+
+ if (!execution_future_.valid()) {
+ return;
}
- if (is_active(pending_handle_)) {
- warn_msg("Taking action server to deactive state with a pending preemption.");
+ if (is_running()) {
+ warn_msg("Requested to deactivate server but goal is still executing."
+ " Should check if action server is running before deactivating.");
}
- terminate_goals();
+ using namespace std::chrono; //NOLINT
+ auto start_time = steady_clock::now();
+ while (execution_future_.wait_for(milliseconds(100)) != std::future_status::ready) {
+ info_msg("Waiting for async process to finish.");
+ if (steady_clock::now() - start_time >= server_timeout_) {
+ terminate_all();
+ throw std::runtime_error("Action callback is still running and missed deadline to stop");
+ }
+ }
+
+ debug_msg("Deactivation completed.");
+ }
+
+ bool is_running()
+ {
+ return execution_future_.wait_for(std::chrono::milliseconds(0)) == std::future_status::ready;
}
bool is_server_active()
@@ -220,39 +282,22 @@ public:
return current_handle_->is_canceling();
}
- void terminate_goals(
+ void terminate_all(
typename std::shared_ptr<typename ActionT::Result> result =
std::make_shared<typename ActionT::Result>())
{
std::lock_guard<std::recursive_mutex> lock(update_mutex_);
+ terminate(current_handle_, result);
+ terminate(pending_handle_, result);
+ preempt_requested_ = false;
+ }
- if (is_active(current_handle_)) {
- if (current_handle_->is_canceling()) {
- debug_msg("Client requested to cancel the current goal. Cancelling.");
- current_handle_->canceled(result);
- current_handle_.reset();
- } else {
- debug_msg("Aborting the current goal.");
- current_handle_->abort(result);
- current_handle_.reset();
- }
- }
-
- if (is_active(pending_handle_)) {
- if (pending_handle_->is_canceling()) {
- warn_msg("Client requested to cancel the pending goal."
- " Cancelling. Should check for pre-empt requests before terminating the goal.");
- pending_handle_->canceled(result);
- pending_handle_.reset();
- preempt_requested_ = false;
- } else {
- warn_msg("Aborting a pending goal. "
- " Should check for pre-empt requests before terminating the goal.");
- pending_handle_->abort(result);
- pending_handle_.reset();
- preempt_requested_ = false;
- }
- }
+ void terminate_current(
+ typename std::shared_ptr<typename ActionT::Result> result =
+ std::make_shared<typename ActionT::Result>())
+ {
+ std::lock_guard<std::recursive_mutex> lock(update_mutex_);
+ terminate(current_handle_, result);
}
void succeeded_current(
@@ -266,13 +311,6 @@ public:
current_handle_->succeed(result);
current_handle_.reset();
}
-
- if (is_active(pending_handle_)) {
- warn_msg("A preemption request was available before succeeding on the current goal.");
- pending_handle_->abort(empty_result());
- pending_handle_.reset();
- preempt_requested_ = false;
- }
}
void publish_feedback(typename std::shared_ptr<typename ActionT::Feedback> feedback)
@@ -293,10 +331,14 @@ protected:
std::string action_name_;
ExecuteCallback execute_callback_;
+ std::future<void> execution_future_;
+ bool stop_execution_;
mutable std::recursive_mutex update_mutex_;
bool server_active_{false};
bool preempt_requested_{false};
+ std::chrono::milliseconds server_timeout_;
+
std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>> current_handle_;
std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>> pending_handle_;
@@ -313,6 +355,31 @@ protected:
return handle != nullptr && handle->is_active();
}
+ void terminate(
+ std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>> handle,
+ typename std::shared_ptr<typename ActionT::Result> result =
+ std::make_shared<typename ActionT::Result>())
+ {
+ std::lock_guard<std::recursive_mutex> lock(update_mutex_);
+
+ if (is_active(handle)) {
+ if (handle->is_canceling()) {
+ warn_msg("Client requested to cancel the goal. Cancelling.");
+ handle->canceled(result);
+ } else {
+ warn_msg("Aborting handle.");
+ handle->abort(result);
+ }
+ handle.reset();
+ }
+ }
+
+ void info_msg(const std::string & msg) const
+ {
+ RCLCPP_INFO(node_logging_interface_->get_logger(),
+ "[%s] [ActionServer] %s", action_name_.c_str(), msg.c_str());
+ }
+
void debug_msg(const std::string & msg) const
{
RCLCPP_DEBUG(node_logging_interface_->get_logger(),
diff --git a/nav2_util/package.xml b/nav2_util/package.xml
index cd4a41956bb12e315e187b92f04a7ffc4ec1034a..eb3c097680e4161fe8d68f43a4825d79ae1eb888 100644
--- a/nav2_util/package.xml
+++ b/nav2_util/package.xml
@@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>nav2_util</name>
- <version>0.2.4</version>
+ <version>0.3.0</version>
<description>TODO</description>
<maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
<maintainer email="mohammad.haghighipanah@intel.com">Mohammad Haghighipanah</maintainer>
diff --git a/nav2_util/src/CMakeLists.txt b/nav2_util/src/CMakeLists.txt
index bf5aae60c53037dd737606ae04d88ecf09dea6ae..70f1c7b62479d049f524dacf0787e953a8553007 100644
--- a/nav2_util/src/CMakeLists.txt
+++ b/nav2_util/src/CMakeLists.txt
@@ -7,7 +7,6 @@ add_library(${library_name} SHARED
lifecycle_node.cpp
robot_utils.cpp
node_thread.cpp
- parameter_events_subscriber.cpp
)
ament_target_dependencies(${library_name}
diff --git a/nav2_util/src/parameter_events_subscriber.cpp b/nav2_util/src/parameter_events_subscriber.cpp
deleted file mode 100644
index 5d1b9c8053fbc034936154fe6c7a325e41f46949..0000000000000000000000000000000000000000
--- a/nav2_util/src/parameter_events_subscriber.cpp
+++ /dev/null
@@ -1,308 +0,0 @@
-// Copyright 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.
-
-#include <functional>
-#include <memory>
-#include <string>
-#include <unordered_map>
-#include <utility>
-#include <vector>
-
-#include "nav2_util/parameter_events_subscriber.hpp"
-
-namespace nav2_util
-{
-
-ParameterEventsSubscriber::ParameterEventsSubscriber(
- rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
- rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics,
- rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
- const rclcpp::QoS & qos)
-: node_base_(node_base),
- node_topics_(node_topics),
- node_logging_(node_logging),
- qos_(qos)
-{}
-
-void
-ParameterEventsSubscriber::add_namespace_event_subscriber(const std::string & node_namespace)
-{
- std::lock_guard<std::recursive_mutex> lock(mutex_);
- if (std::find(subscribed_namespaces_.begin(), subscribed_namespaces_.end(),
- node_namespace) == subscribed_namespaces_.end())
- {
- subscribed_namespaces_.push_back(node_namespace);
- auto topic = join_path(node_namespace, "parameter_events");
- RCLCPP_DEBUG(node_logging_->get_logger(), "Subscribing to topic: %s", topic.c_str());
-
- auto event_sub = rclcpp::create_subscription<rcl_interfaces::msg::ParameterEvent>(
- node_topics_, topic, qos_,
- std::bind(&ParameterEventsSubscriber::event_callback, this, std::placeholders::_1));
-
- event_subscriptions_.push_back(event_sub);
- }
-}
-
-void
-ParameterEventsSubscriber::remove_namespace_event_subscriber(const std::string & node_namespace)
-{
- std::lock_guard<std::recursive_mutex> lock(mutex_);
- auto check_sub_cb = [this, &node_namespace](
- rclcpp::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr sub) {
- return sub->get_topic_name() == join_path(node_namespace, "parameter_events");
- };
-
- auto it = std::find_if(
- event_subscriptions_.begin(),
- event_subscriptions_.end(),
- check_sub_cb);
-
- if (it != event_subscriptions_.end()) {
- event_subscriptions_.erase(it);
- subscribed_namespaces_.erase(
- std::remove(subscribed_namespaces_.begin(), subscribed_namespaces_.end(), node_namespace));
- }
-}
-
-void
-ParameterEventsSubscriber::set_event_callback(
- std::function<void(const rcl_interfaces::msg::ParameterEvent::SharedPtr &)> callback,
- const std::vector<std::string> & node_namespaces)
-{
- std::lock_guard<std::recursive_mutex> lock(mutex_);
- remove_event_callback();
- std::string full_namespace;
- for (auto & ns : node_namespaces) {
- if (ns == "") {
- full_namespace = node_base_->get_namespace();
- } else {
- full_namespace = resolve_path(ns);
- }
- add_namespace_event_subscriber(full_namespace);
- event_namespaces_.push_back(full_namespace);
- }
-
- event_callback_ = callback;
-}
-
-void
-ParameterEventsSubscriber::remove_event_callback()
-{
- std::lock_guard<std::recursive_mutex> lock(mutex_);
- // Clear current vector of event namespaces and remove subscriptions
- auto temp_namespaces = event_namespaces_;
- event_namespaces_.clear();
- for (auto temp_ns : temp_namespaces) {
- if (should_unsubscribe_to_namespace(temp_ns)) {
- remove_namespace_event_subscriber(temp_ns);
- }
- }
-
- event_callback_ = nullptr;
-}
-
-ParameterEventsCallbackHandle::SharedPtr
-ParameterEventsSubscriber::add_parameter_callback(
- const std::string & parameter_name,
- ParameterEventsCallbackType callback,
- const std::string & node_name)
-{
- std::lock_guard<std::recursive_mutex> lock(mutex_);
- auto full_node_name = resolve_path(node_name);
- add_namespace_event_subscriber(split_path(full_node_name).first);
-
- auto handle = std::make_shared<ParameterEventsCallbackHandle>();
- handle->callback = callback;
- handle->parameter_name = parameter_name;
- handle->node_name = full_node_name;
- // the last callback registered is executed first.
- parameter_callbacks_[{parameter_name, full_node_name}].emplace_front(handle);
-
- return handle;
-}
-
-struct HandleCompare
- : public std::unary_function<ParameterEventsCallbackHandle::WeakPtr, bool>
-{
- explicit HandleCompare(const ParameterEventsCallbackHandle * const base)
- : base_(base) {}
- bool operator()(const ParameterEventsCallbackHandle::WeakPtr & handle)
- {
- auto shared_handle = handle.lock();
- if (base_ == shared_handle.get()) {
- return true;
- }
- return false;
- }
- const ParameterEventsCallbackHandle * const base_;
-};
-
-void
-ParameterEventsSubscriber::remove_parameter_callback(
- const ParameterEventsCallbackHandle * const handle)
-{
- std::lock_guard<std::recursive_mutex> lock(mutex_);
- auto & container = parameter_callbacks_[{handle->parameter_name, handle->node_name}];
- auto it = std::find_if(
- container.begin(),
- container.end(),
- HandleCompare(handle));
- if (it != container.end()) {
- container.erase(it);
- if (container.empty()) {
- parameter_callbacks_.erase({handle->parameter_name, handle->node_name});
- if (should_unsubscribe_to_namespace(split_path(handle->node_name).first)) {
- remove_namespace_event_subscriber(split_path(handle->node_name).first);
- }
- }
- } else {
- throw std::runtime_error("Callback doesn't exist");
- }
-}
-
-bool
-ParameterEventsSubscriber::should_unsubscribe_to_namespace(const std::string & node_namespace)
-{
- auto it1 = std::find(event_namespaces_.begin(), event_namespaces_.end(), node_namespace);
- if (it1 != event_namespaces_.end()) {
- return false;
- }
-
- auto it2 = parameter_callbacks_.begin();
- while (it2 != parameter_callbacks_.end()) {
- if (split_path(it2->first.second).first == node_namespace) {
- return false;
- }
- ++it2;
- }
- return true;
-}
-
-void
-ParameterEventsSubscriber::remove_parameter_callback(
- const std::string & parameter_name,
- const std::string & node_name)
-{
- std::lock_guard<std::recursive_mutex> lock(mutex_);
- auto full_node_name = resolve_path(node_name);
- parameter_callbacks_.erase({parameter_name, full_node_name});
- if (should_unsubscribe_to_namespace(split_path(full_node_name).first)) {
- RCLCPP_INFO(node_logging_->get_logger(), "Removing namespace event subscription");
- remove_namespace_event_subscriber(split_path(full_node_name).first);
- }
-}
-
-bool
-ParameterEventsSubscriber::get_parameter_from_event(
- const rcl_interfaces::msg::ParameterEvent::SharedPtr event,
- rclcpp::Parameter & parameter,
- const std::string parameter_name,
- const std::string node_name)
-{
- if (event->node == node_name) {
- rclcpp::ParameterEventsFilter filter(event, {parameter_name},
- {rclcpp::ParameterEventsFilter::EventType::NEW,
- rclcpp::ParameterEventsFilter::EventType::CHANGED});
- if (!filter.get_events().empty()) {
- const auto & events = filter.get_events();
- auto param_msg = events.back().second;
- parameter = rclcpp::Parameter::from_parameter_msg(*param_msg);
- return true;
- }
- }
- return false;
-}
-
-rclcpp::Parameter
-ParameterEventsSubscriber::get_parameter_from_event(
- const rcl_interfaces::msg::ParameterEvent::SharedPtr event,
- const std::string parameter_name,
- const std::string node_name)
-{
- rclcpp::Parameter p(parameter_name);
- get_parameter_from_event(event, p, parameter_name, node_name);
- return p;
-}
-
-void
-ParameterEventsSubscriber::event_callback(
- const rcl_interfaces::msg::ParameterEvent::SharedPtr event)
-{
- std::lock_guard<std::recursive_mutex> lock(mutex_);
- const std::string & node_name = event->node;
- RCLCPP_DEBUG(node_logging_->get_logger(), "Parameter event received for node: %s",
- node_name.c_str());
-
- for (auto it = parameter_callbacks_.begin(); it != parameter_callbacks_.end(); ++it) {
- rclcpp::Parameter p;
- if (get_parameter_from_event(event, p, it->first.first, it->first.second)) {
- for (auto cb = it->second.begin(); cb != it->second.end(); ++cb) {
- auto shared_handle = cb->lock();
- if (nullptr != shared_handle) {
- shared_handle->callback(p);
- } else {
- cb = it->second.erase(cb);
- }
- }
- }
- }
-
- if (event_callback_) {
- event_callback_(event);
- }
-}
-
-std::string
-ParameterEventsSubscriber::resolve_path(const std::string & path)
-{
- std::string full_path;
-
- if (path == "") {
- full_path = node_base_->get_fully_qualified_name();
- } else {
- full_path = path;
- if (*full_path.begin() != '/') {
- full_path = join_path(node_base_->get_namespace(), full_path);
- }
- }
-
- return full_path;
-}
-
-std::pair<std::string, std::string>
-ParameterEventsSubscriber::split_path(const std::string & str)
-{
- std::string path;
- std::size_t found = str.find_last_of("/\\");
- if (found == 0) {
- path = str.substr(0, found + 1);
- } else {
- path = str.substr(0, found);
- }
- std::string name = str.substr(found + 1);
- return {path, name};
-}
-
-std::string
-ParameterEventsSubscriber::join_path(std::string path, std::string name)
-{
- std::string joined_path = path;
- if (*joined_path.rbegin() != '/' && *name.begin() != '/') {
- joined_path = joined_path + "/";
- }
-
- return joined_path + name;
-}
-
-} // namespace nav2_util
diff --git a/nav2_util/test/test_actions.cpp b/nav2_util/test/test_actions.cpp
index 1143587c05c68e109cf1283f52734f7624b98687..a306832e31da6a37f8b3649bba4f3d75a40c5b24 100644
--- a/nav2_util/test/test_actions.cpp
+++ b/nav2_util/test/test_actions.cpp
@@ -20,6 +20,7 @@
#include "nav2_util/node_utils.hpp"
#include "nav2_util/simple_action_server.hpp"
#include "test_msgs/action/fibonacci.hpp"
+#include "std_msgs/msg/empty.hpp"
using Fibonacci = test_msgs::action::Fibonacci;
using GoalHandle = rclcpp_action::ServerGoalHandle<Fibonacci>;
@@ -45,6 +46,30 @@ public:
shared_from_this(),
"fibonacci",
std::bind(&FibonacciServerNode::execute, this));
+
+ deactivate_subs_ = create_subscription<std_msgs::msg::Empty>(
+ "deactivate_server",
+ 1,
+ [this](std_msgs::msg::Empty::UniquePtr /*msg*/) {
+ RCLCPP_INFO(this->get_logger(), "Deactivating");
+ action_server_->deactivate();
+ });
+
+ activate_subs_ = create_subscription<std_msgs::msg::Empty>(
+ "activate_server",
+ 1,
+ [this](std_msgs::msg::Empty::UniquePtr /*msg*/) {
+ RCLCPP_INFO(this->get_logger(), "Activating");
+ action_server_->activate();
+ });
+
+ omit_preempt_subs_ = create_subscription<std_msgs::msg::Empty>(
+ "omit_preemption",
+ 1,
+ [this](std_msgs::msg::Empty::UniquePtr /*msg*/) {
+ RCLCPP_INFO(this->get_logger(), "Ignoring preemptions");
+ do_premptions_ = false;
+ });
}
void on_term()
@@ -68,15 +93,15 @@ preempted:
sequence.push_back(1);
for (int i = 1; (i < goal->order) && rclcpp::ok(); ++i) {
- // Check if this action has been canceled
- if (action_server_->is_cancel_requested()) {
+ // Should be check periodically if this action has been canceled
+ // or if the server has been deactivated.
+ if (action_server_->is_cancel_requested() || !action_server_->is_server_active()) {
result->sequence = sequence;
- action_server_->terminate_goals(result);
return;
}
// Check if we've gotten an new goal, pre-empting the current one
- if (action_server_->is_preempt_requested()) {
+ if (do_premptions_ && action_server_->is_preempt_requested()) {
action_server_->accept_pending_goal();
goto preempted;
}
@@ -98,6 +123,11 @@ preempted:
private:
std::unique_ptr<nav2_util::SimpleActionServer<Fibonacci>> action_server_;
+ rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr deactivate_subs_;
+ rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr activate_subs_;
+ rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr omit_preempt_subs_;
+
+ bool do_premptions_{true};
};
class RclCppFixture
@@ -142,6 +172,10 @@ public:
{
action_client_ = rclcpp_action::create_client<Fibonacci>(shared_from_this(), "fibonacci");
action_client_->wait_for_action_server();
+
+ deactivate_pub_ = this->create_publisher<std_msgs::msg::Empty>("deactivate_server", 1);
+ activate_pub_ = this->create_publisher<std_msgs::msg::Empty>("activate_server", 1);
+ omit_prempt_pub_ = this->create_publisher<std_msgs::msg::Empty>("omit_preemption", 1);
}
void on_term()
@@ -149,7 +183,25 @@ public:
action_client_.reset();
}
+ void deactivate_server()
+ {
+ deactivate_pub_->publish(std_msgs::msg::Empty());
+ }
+
+ void activate_server()
+ {
+ activate_pub_->publish(std_msgs::msg::Empty());
+ }
+
+ void omit_server_preemptions()
+ {
+ omit_prempt_pub_->publish(std_msgs::msg::Empty());
+ }
+
rclcpp_action::Client<Fibonacci>::SharedPtr action_client_;
+ rclcpp::Publisher<std_msgs::msg::Empty>::SharedPtr deactivate_pub_;
+ rclcpp::Publisher<std_msgs::msg::Empty>::SharedPtr activate_pub_;
+ rclcpp::Publisher<std_msgs::msg::Empty>::SharedPtr omit_prempt_pub_;
};
class ActionTest : public ::testing::Test
@@ -245,3 +297,155 @@ TEST_F(ActionTest, test_simple_action_with_feedback)
ASSERT_EQ(sum, 143);
ASSERT_GE(feedback_sum, 0); // We should have received *some* feedback
}
+
+TEST_F(ActionTest, test_simple_action_activation_cycling)
+{
+ // The goal for this invocation
+ auto goal = Fibonacci::Goal();
+
+ // Sending a goal that will take a long time to calculate
+ goal.order = 12'000'000;
+
+ // Start by sending goal on an active server
+
+ // Send the goal
+ auto future_goal_handle = node_->action_client_->async_send_goal(goal);
+ ASSERT_EQ(rclcpp::spin_until_future_complete(node_,
+ future_goal_handle), rclcpp::executor::FutureReturnCode::SUCCESS);
+
+ // Deactivate while running
+ node_->deactivate_server();
+
+ auto goal_handle = future_goal_handle.get();
+
+ // Wait for the result
+ auto future_result = node_->action_client_->async_get_result(goal_handle);
+ ASSERT_EQ(rclcpp::spin_until_future_complete(node_, future_result),
+ rclcpp::executor::FutureReturnCode::SUCCESS);
+
+ // The action should be reported as aborted.
+ ASSERT_EQ(future_result.get().code, rclcpp_action::ResultCode::ABORTED);
+
+ // Cycle back to active
+ node_->activate_server();
+
+ goal.order = 12;
+
+ // Send the goal
+ future_goal_handle = node_->action_client_->async_send_goal(goal);
+ ASSERT_EQ(rclcpp::spin_until_future_complete(node_,
+ future_goal_handle), rclcpp::executor::FutureReturnCode::SUCCESS);
+
+ goal_handle = future_goal_handle.get();
+
+ // Wait for the result
+ future_result = node_->action_client_->async_get_result(goal_handle);
+ ASSERT_EQ(rclcpp::spin_until_future_complete(node_, future_result),
+ rclcpp::executor::FutureReturnCode::SUCCESS);
+
+ // Now the action should have been successfully executed.
+ ASSERT_EQ(future_result.get().code, rclcpp_action::ResultCode::SUCCEEDED);
+}
+
+TEST_F(ActionTest, test_simple_action_preemption)
+{
+ // The goal for this invocation
+ auto goal = Fibonacci::Goal();
+
+ // Sending a goal that will take a long time to calculate
+ goal.order = 12'000'000;
+
+ // Send the goal
+ auto future_goal_handle = node_->action_client_->async_send_goal(goal);
+ ASSERT_EQ(rclcpp::spin_until_future_complete(node_,
+ future_goal_handle), rclcpp::executor::FutureReturnCode::SUCCESS);
+
+ // Preempt the goal
+ auto preemption_goal = Fibonacci::Goal();
+ preemption_goal.order = 1;
+
+ // Send the goal
+ future_goal_handle = node_->action_client_->async_send_goal(preemption_goal);
+ ASSERT_EQ(rclcpp::spin_until_future_complete(node_,
+ future_goal_handle), rclcpp::executor::FutureReturnCode::SUCCESS);
+
+ auto goal_handle = future_goal_handle.get();
+
+ // Wait for the result
+ auto future_result = node_->action_client_->async_get_result(goal_handle);
+ ASSERT_EQ(rclcpp::spin_until_future_complete(node_, future_result),
+ rclcpp::executor::FutureReturnCode::SUCCESS);
+
+ // The final result
+ rclcpp_action::ClientGoalHandle<Fibonacci>::WrappedResult result = future_result.get();
+ ASSERT_EQ(result.code, rclcpp_action::ResultCode::SUCCEEDED);
+
+ // Sum all of the values in the requested fibonacci series
+ int sum = 0;
+ for (auto number : result.result->sequence) {
+ sum += number;
+ }
+
+ ASSERT_EQ(sum, 1);
+}
+
+TEST_F(ActionTest, test_simple_action_preemption_after_succeeded)
+{
+ // Test race condition between successfully completing an action and receiving a preemption.
+ auto goal = Fibonacci::Goal();
+ goal.order = 20;
+
+ auto preemption = Fibonacci::Goal();
+ preemption.order = 1;
+
+ // Send the goal
+ auto future_goal_handle = node_->action_client_->async_send_goal(goal);
+ ASSERT_EQ(rclcpp::spin_until_future_complete(node_,
+ future_goal_handle), rclcpp::executor::FutureReturnCode::SUCCESS);
+
+ node_->omit_server_preemptions();
+
+ auto future_preempt_handle = node_->action_client_->async_send_goal(preemption);
+ ASSERT_EQ(rclcpp::spin_until_future_complete(node_,
+ future_goal_handle), rclcpp::executor::FutureReturnCode::SUCCESS);
+
+ // Get the results
+ auto goal_handle = future_goal_handle.get();
+
+ // Wait for the result of initial goal
+ auto future_result = node_->action_client_->async_get_result(goal_handle);
+ ASSERT_EQ(rclcpp::spin_until_future_complete(node_, future_result),
+ rclcpp::executor::FutureReturnCode::SUCCESS);
+
+ // The final result
+ rclcpp_action::ClientGoalHandle<Fibonacci>::WrappedResult result = future_result.get();
+ ASSERT_EQ(result.code, rclcpp_action::ResultCode::SUCCEEDED);
+
+ // Sum all of the values in the requested fibonacci series
+ int sum = 0;
+ for (auto number : result.result->sequence) {
+ sum += number;
+ }
+
+ ASSERT_EQ(sum, 17710);
+
+ // Now get the preemption result
+ goal_handle = future_preempt_handle.get();
+
+ // Wait for the result of initial goal
+ future_result = node_->action_client_->async_get_result(goal_handle);
+ ASSERT_EQ(rclcpp::spin_until_future_complete(node_, future_result),
+ rclcpp::executor::FutureReturnCode::SUCCESS);
+
+ // The final result
+ result = future_result.get();
+ ASSERT_EQ(result.code, rclcpp_action::ResultCode::SUCCEEDED);
+
+ // Sum all of the values in the requested fibonacci series
+ sum = 0;
+ for (auto number : result.result->sequence) {
+ sum += number;
+ }
+
+ ASSERT_EQ(sum, 1);
+}
diff --git a/nav2_voxel_grid/package.xml b/nav2_voxel_grid/package.xml
index 44dc7cbb24d7ac7dec9296d2a7619b7d66714d23..62b2699545e3ba7387b48a3ad738a1e6ab5a41ff 100644
--- a/nav2_voxel_grid/package.xml
+++ b/nav2_voxel_grid/package.xml
@@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>nav2_voxel_grid</name>
- <version>0.2.4</version>
+ <version>0.3.0</version>
<description>
voxel_grid provides an implementation of an efficient 3D voxel grid. The occupancy grid can support 3 different representations for the state of a cell: marked, free, or unknown. Due to the underlying implementation relying on bitwise and and or integer operations, the voxel grid only supports 16 different levels per voxel column. However, this limitation yields raytracing and cell marking performance in the grid comparable to standard 2D structures making it quite fast compared to most 3D structures.
</description>
diff --git a/nav2_waypoint_follower/package.xml b/nav2_waypoint_follower/package.xml
index 43f12b0ca9330be6af8571d66d1338b6cb2e694b..cb8a46d4be71f9efc1174f4a277fda6b7ec691a8 100644
--- a/nav2_waypoint_follower/package.xml
+++ b/nav2_waypoint_follower/package.xml
@@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>nav2_waypoint_follower</name>
- <version>0.2.4</version>
+ <version>0.3.0</version>
<description>A waypoint follower navigation server</description>
<maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>
<license>Apache-2.0</license>
diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp
index d4d0b546d148609de38832931ee63b599f81d08e..949c7d63aff7d5643003b64b384141f38075b723 100644
--- a/nav2_waypoint_follower/src/waypoint_follower.cpp
+++ b/nav2_waypoint_follower/src/waypoint_follower.cpp
@@ -132,7 +132,7 @@ WaypointFollower::followWaypoints()
// Check if asked to stop processing action
if (action_server_->is_cancel_requested()) {
RCLCPP_INFO(get_logger(), "Cancelling action.");
- action_server_->terminate_goals();
+ action_server_->terminate_all();
return;
}
@@ -171,7 +171,7 @@ WaypointFollower::followWaypoints()
"list and stop on failure is enabled."
" Terminating action.", goal_index);
result->missed_waypoints = failed_ids_;
- action_server_->terminate_goals(result);
+ action_server_->terminate_current(result);
failed_ids_.clear();
return;
} else {
diff --git a/navigation2/package.xml b/navigation2/package.xml
index f552f2166df74d14d31c241f20cee081feb73375..17d97667b7324a8451aac52f36e963f2284307c4 100644
--- a/navigation2/package.xml
+++ b/navigation2/package.xml
@@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>navigation2</name>
- <version>0.2.4</version>
+ <version>0.3.0</version>
<description>
ROS2 Navigation Stack
</description>
diff --git a/release_branch.Dockerfile b/release_branch.Dockerfile
index 4be39e3462a7504b88b8abb0b080fa0e06dfcf65..3a03113420bf3bf4c3354cdb8c498b5c5133c36a 100644
--- a/release_branch.Dockerfile
+++ b/release_branch.Dockerfile
@@ -11,6 +11,8 @@
ARG FROM_IMAGE=dashing
FROM ros:$FROM_IMAGE
+RUN rosdep update
+
# copy ros package repo
ENV NAV2_WS /opt/nav2_ws
RUN mkdir -p $NAV2_WS/src
diff --git a/sphinx_doc/Makefile b/sphinx_doc/Makefile
index b0c0f239375c3719b1d59db495626a0263d1aa0a..3ffb456a7cf38ae1255ed8b1161f73c39969ef8b 100644
--- a/sphinx_doc/Makefile
+++ b/sphinx_doc/Makefile
@@ -52,7 +52,10 @@ publish:
rm -fr $(PUBLISHDIR)/*
cp -r $(BUILDDIR)/html/* $(PUBLISHDIR)
cp scripts/.nojekyll $(PUBLISHDIR)/.nojekyll
- cd $(PUBLISHDIR) && git add -A && git commit -s -m "[skip ci] publish $(RELEASE)" && git push origin
+ cd $(PUBLISHDIR) && \
+ git add -A && \
+ git diff-index --quiet HEAD || \
+ (git commit -s -m "[skip ci] publish $(RELEASE)" && git push origin)
# Catch-all target: route all unknown targets to Sphinx using the new
diff --git a/sphinx_doc/howtos/index.rst b/sphinx_doc/howtos/index.rst
index a2e068d9eb060378693dd574a2042791e14744e1..b69ccd03e6331965db28d9b945504bc750ace9b8 100644
--- a/sphinx_doc/howtos/index.rst
+++ b/sphinx_doc/howtos/index.rst
@@ -6,5 +6,6 @@ How-Tos
.. toctree::
:maxdepth: 1
+ tutorials/index.rst
docs/index.rst
tunning/index.rst
diff --git a/sphinx_doc/howtos/tunning/params/tunable-params.rst b/sphinx_doc/howtos/tunning/params/tunable-params.rst
index 315d4346a7b8db25d89b9999e8619031d7d19eac..e932fd37a9eb7bde57da012969177144b7f1d297 100644
--- a/sphinx_doc/howtos/tunning/params/tunable-params.rst
+++ b/sphinx_doc/howtos/tunning/params/tunable-params.rst
@@ -137,7 +137,6 @@ Hosts the `DWB` controller.
sim_time: 1.7
trajectory_generator_name: dwb_plugins::StandardTrajectoryGenerator
transform_tolerance: 0.2
- use_dwa: false
use_sim_time: true
vx_samples: 20
vy_samples: 5
diff --git a/sphinx_doc/howtos/tutorials/docs/images/Navigation2_on_real_Turtlebot3/rviz_after_launch_view.png b/sphinx_doc/howtos/tutorials/docs/images/Navigation2_on_real_Turtlebot3/rviz_after_launch_view.png
new file mode 100644
index 0000000000000000000000000000000000000000..8c91a80d5880939cdf184dbdf7db219671276cee
Binary files /dev/null and b/sphinx_doc/howtos/tutorials/docs/images/Navigation2_on_real_Turtlebot3/rviz_after_launch_view.png differ
diff --git a/sphinx_doc/howtos/tutorials/docs/images/Navigation2_on_real_Turtlebot3/rviz_robot_navigating.png b/sphinx_doc/howtos/tutorials/docs/images/Navigation2_on_real_Turtlebot3/rviz_robot_navigating.png
new file mode 100644
index 0000000000000000000000000000000000000000..ed5f5d2080d3a627d8d3272279f4c1746eada254
Binary files /dev/null and b/sphinx_doc/howtos/tutorials/docs/images/Navigation2_on_real_Turtlebot3/rviz_robot_navigating.png differ
diff --git a/sphinx_doc/howtos/tutorials/docs/images/Navigation2_on_real_Turtlebot3/rviz_send_goal.png b/sphinx_doc/howtos/tutorials/docs/images/Navigation2_on_real_Turtlebot3/rviz_send_goal.png
new file mode 100644
index 0000000000000000000000000000000000000000..1cb3a59609d7aaeee6d9633992580c56e70b3ce9
Binary files /dev/null and b/sphinx_doc/howtos/tutorials/docs/images/Navigation2_on_real_Turtlebot3/rviz_send_goal.png differ
diff --git a/sphinx_doc/howtos/tutorials/docs/images/Navigation2_on_real_Turtlebot3/rviz_set_initial_pose.png b/sphinx_doc/howtos/tutorials/docs/images/Navigation2_on_real_Turtlebot3/rviz_set_initial_pose.png
new file mode 100644
index 0000000000000000000000000000000000000000..2475fa6e9cddd2d9d6fdbceca7f45c282637040e
Binary files /dev/null and b/sphinx_doc/howtos/tutorials/docs/images/Navigation2_on_real_Turtlebot3/rviz_set_initial_pose.png differ
diff --git a/sphinx_doc/howtos/tutorials/docs/images/Navigation2_on_real_Turtlebot3/rviz_slam_map_view.png b/sphinx_doc/howtos/tutorials/docs/images/Navigation2_on_real_Turtlebot3/rviz_slam_map_view.png
new file mode 100644
index 0000000000000000000000000000000000000000..42fa031eb8fc10ffe64405d6d580201b912164ad
Binary files /dev/null and b/sphinx_doc/howtos/tutorials/docs/images/Navigation2_on_real_Turtlebot3/rviz_slam_map_view.png differ
diff --git a/sphinx_doc/howtos/tutorials/docs/images/Navigation2_with_Turtlebot3_in_Gazebo/gazebo_robot_reached_goal.png b/sphinx_doc/howtos/tutorials/docs/images/Navigation2_with_Turtlebot3_in_Gazebo/gazebo_robot_reached_goal.png
new file mode 100644
index 0000000000000000000000000000000000000000..a14a4261f66b102ba668ac7196b72409f95395f9
Binary files /dev/null and b/sphinx_doc/howtos/tutorials/docs/images/Navigation2_with_Turtlebot3_in_Gazebo/gazebo_robot_reached_goal.png differ
diff --git a/sphinx_doc/howtos/tutorials/docs/images/Navigation2_with_Turtlebot3_in_Gazebo/gazebo_turlebot3.png b/sphinx_doc/howtos/tutorials/docs/images/Navigation2_with_Turtlebot3_in_Gazebo/gazebo_turlebot3.png
new file mode 100644
index 0000000000000000000000000000000000000000..b5776b16f4079886429c01ecf31b5009f3d9be7c
Binary files /dev/null and b/sphinx_doc/howtos/tutorials/docs/images/Navigation2_with_Turtlebot3_in_Gazebo/gazebo_turlebot3.png differ
diff --git a/sphinx_doc/howtos/tutorials/docs/images/Navigation2_with_Turtlebot3_in_Gazebo/navigation_with_recovery_behaviours.gif b/sphinx_doc/howtos/tutorials/docs/images/Navigation2_with_Turtlebot3_in_Gazebo/navigation_with_recovery_behaviours.gif
new file mode 100644
index 0000000000000000000000000000000000000000..58fd51cb783392bcd4f3c304dd894eed987a9b11
Binary files /dev/null and b/sphinx_doc/howtos/tutorials/docs/images/Navigation2_with_Turtlebot3_in_Gazebo/navigation_with_recovery_behaviours.gif differ
diff --git a/sphinx_doc/howtos/tutorials/docs/images/Navigation2_with_Turtlebot3_in_Gazebo/rviz_initial_view.png b/sphinx_doc/howtos/tutorials/docs/images/Navigation2_with_Turtlebot3_in_Gazebo/rviz_initial_view.png
new file mode 100644
index 0000000000000000000000000000000000000000..5a1a4e505b1111f8df03526bf722e1b26bfdf668
Binary files /dev/null and b/sphinx_doc/howtos/tutorials/docs/images/Navigation2_with_Turtlebot3_in_Gazebo/rviz_initial_view.png differ
diff --git a/sphinx_doc/howtos/tutorials/docs/images/Navigation2_with_Turtlebot3_in_Gazebo/rviz_robot_navigating.png b/sphinx_doc/howtos/tutorials/docs/images/Navigation2_with_Turtlebot3_in_Gazebo/rviz_robot_navigating.png
new file mode 100644
index 0000000000000000000000000000000000000000..aadbe2c040e9d266bf9383800613b415fdf3ef37
Binary files /dev/null and b/sphinx_doc/howtos/tutorials/docs/images/Navigation2_with_Turtlebot3_in_Gazebo/rviz_robot_navigating.png differ
diff --git a/sphinx_doc/howtos/tutorials/docs/images/Navigation2_with_Turtlebot3_in_Gazebo/rviz_send_goal_pose.png b/sphinx_doc/howtos/tutorials/docs/images/Navigation2_with_Turtlebot3_in_Gazebo/rviz_send_goal_pose.png
new file mode 100644
index 0000000000000000000000000000000000000000..5f67ba5190474f46b1b3d7c5ea1e915cf78a71e7
Binary files /dev/null and b/sphinx_doc/howtos/tutorials/docs/images/Navigation2_with_Turtlebot3_in_Gazebo/rviz_send_goal_pose.png differ
diff --git a/sphinx_doc/howtos/tutorials/docs/images/Navigation2_with_Turtlebot3_in_Gazebo/rviz_set_initial_pose.png b/sphinx_doc/howtos/tutorials/docs/images/Navigation2_with_Turtlebot3_in_Gazebo/rviz_set_initial_pose.png
new file mode 100644
index 0000000000000000000000000000000000000000..2b77d4ea680107f2beea1f6d46fa4a9f25cfd0e8
Binary files /dev/null and b/sphinx_doc/howtos/tutorials/docs/images/Navigation2_with_Turtlebot3_in_Gazebo/rviz_set_initial_pose.png differ
diff --git a/sphinx_doc/howtos/tutorials/docs/navigation2_on_real_turtlebot3.rst b/sphinx_doc/howtos/tutorials/docs/navigation2_on_real_turtlebot3.rst
new file mode 100644
index 0000000000000000000000000000000000000000..6ced5b45c9f6a243ce71ec38e9868275f5c56f98
--- /dev/null
+++ b/sphinx_doc/howtos/tutorials/docs/navigation2_on_real_turtlebot3.rst
@@ -0,0 +1,145 @@
+.. _navigation2-on-real-turtlebot3:
+
+Navigation2 on a Physical Turtlebot 3
+*************************************
+
+- `Overview`_
+- `Requirements`_
+- `Tutorial Steps`_
+- `Videos`_
+
+Overview
+========
+
+This tutorial shows how to control and navigate Turtlebot 3 using the ROS2 Navigation2 on a physical Turtlebot 3 robot.
+Before completing this tutorials, completing the :ref:`navigation2-with-turtlebot3-in-gazebo`. is highly recommended especially if you are new to ROS and Navigation2.
+
+This tutorial consists of two parts. In the first part, you learned how to use Turtlebot 3 robots in simulation (Gazebo).
+In this part, you will learn how to control a physical Turtlebot Waffle using Navigation2.
+
+``ROS2 Dashing`` and ``Navigation2 Dashing 0.2.4`` are used to create this tutorial.
+You should be able to do this tutorial using other Navigation2 versions as well.
+
+This tutorial may take about 1 hour to complete.
+It depends on your experience with ROS, robots, and what computer system you have.
+
+Requirements
+============
+
+You must install Navigation2, Turtelbot3.
+If you don't have them installed, please follow the steps in the previous tutorial. :ref:`navigation2-with-turtlebot3-in-gazebo`
+
+Tutorial Steps
+==============
+
+0- Setup Your Enviroment Variables
+----------------------------------
+
+Run the following commands first whenever you open a new terminal during this tutorial.
+
+- ``source /opt/ros/<ros2-distro>/setup.bash``
+- ``export TURTLEBOT3_MODEL=waffle``
+
+1- Launch Turtlebot 3 Robot State Publisher
+-------------------------------------------
+
+Launch Turtlebot 3 robot state publisher,
+
+ ``ros2 launch turtlebot3_bringup turtlebot3_state_publisher.launch.py use_sim_time:=False``
+
+Note: use_sim_time is set to **False** because we're on a physical robot.
+
+2- Launch Navigation2
+---------------------
+
+You need to have a map of the environment where you want to Navigate Turtlebot 3, or create one live with SLAM.
+
+In case you are interested, there is a use case tutorial which shows how to use Navigation2 with SLAM.
+`Navigation2 with SLAM <https://github.com/ros-planning/navigation2/blob/master/doc/use_cases/navigation_with_slam.md>`_
+
+Required files:
+
+ - ``your-map.map``
+ - ``your-map.yaml``
+
+``<your_map>.yaml`` is the configuration file for the map we want to provide Navigation2.
+In this case, it has the map resolution value, threshold values for obstacles and free spaces, and a map file location.
+You need to make sure these values are correct.
+More information about the map.yaml can be found `here <http://wiki.ros.org/map_server>`_.
+
+Launch Navigation 2. If you set autostart:=False, you need to click on the start button in RViz to initialize the nodes.
+Make sure `use_sim time` is set to **False**, because we want to use the system time instead of the time simulation time from Gazebo.
+
+``ros2 launch nav2_bringup nav2_bringup_launch.py use_sim_time:=False autostart:=False map:=/path/to/your-map.yaml``
+
+Note: Don't forget to change **/path/to/your-map.yaml** to the actual path to the your-map.yaml file.
+
+3- Launch RVIZ
+---------------
+
+Launch RVIZ with a pre-defined configuration file.
+
+ ``ros2 run rviz2 rviz2 -d $(ros2 pkg prefix nav2_bringup)/share/nav2_bringup/launch/nav2_default_view.rviz``
+
+Now, you should see a shadow of Turtlebot 3 robot model in the center of the plot in Rviz.
+Click on the Start button (Bottom Left) if you set the auto_start parameter to false.
+
+.. image:: images/Navigation2_on_real_Turtlebot3/rviz_after_launch_view.png
+ :height: 720px
+ :width: 1024px
+ :alt: RViz after launch, auto_start = False
+
+Now, the map should appear in RViz.
+
+.. image:: images/Navigation2_on_real_Turtlebot3/rviz_slam_map_view.png
+ :height: 720px
+ :width: 1024px
+ :alt: A map generated by using SLAM in RViz
+
+4- Initialize the Location of Turtlebot 3
+-----------------------------------------
+
+First, find where the robot is on the map. Check where your robot is in the room.
+
+Set the pose of the robot in RViz.
+Click on the 2D Pose Estimate button and point the location of the robot on the map.
+The direction of the green arrow is the orientation of Turtlebot.
+
+.. image:: images/Navigation2_on_real_Turtlebot3/rviz_set_initial_pose.png
+ :height: 720px
+ :width: 1024px
+ :alt: Set initial pose in RViz
+
+Now, the 3D model of Turtlebot should move to that location.
+A small error in the estimated location is tolerable.
+
+5- Send a Goal Pose
+--------------------
+
+Pick a target location for Turtlebot on the map.
+You can send Turtlebot 3 a goal position and a goal orientation by using the **Navigation2 Goal** or the **GoalTool** buttons.
+
+Note: Navigation2 Goal button uses a ROS2 Action to send the goal and the GoalTool publishes the goal to a topic.
+
+.. image:: images/Navigation2_on_real_Turtlebot3/rviz_send_goal.png
+ :height: 720px
+ :width: 1024px
+ :alt: Send goal pose in RViz
+
+Once you define the target pose, Navigation2 will find a global path and start navigating the robot on the map.
+
+.. image:: images/Navigation2_on_real_Turtlebot3/rviz_robot_navigating.png
+ :height: 720px
+ :width: 1024px
+ :alt: Robot navigating in RViz
+
+Now, you can see that Turtlebot 3 moves towards the goal position in the room. See the video below.
+
+Videos
+------
+
+.. raw:: html
+
+ <div style="position: relative; padding-bottom: 0%; overflow: hidden; max-width: 100%; height: auto;">
+ <iframe width="960" height="720" src="https://www.youtube.com/embed/ZeCds7Sv-5Q" frameborder="0" allow="accelerometer; autoplay; encrypted-media; gyroscope; picture-in-picture" allowfullscreen></iframe>
+ </div>
diff --git a/sphinx_doc/howtos/tutorials/docs/navigation2_with_turtlebot3_in_gazebo.rst b/sphinx_doc/howtos/tutorials/docs/navigation2_with_turtlebot3_in_gazebo.rst
new file mode 100644
index 0000000000000000000000000000000000000000..1396c5c73a222797261f7b93e3fb0d417650dd8d
--- /dev/null
+++ b/sphinx_doc/howtos/tutorials/docs/navigation2_with_turtlebot3_in_gazebo.rst
@@ -0,0 +1,192 @@
+.. _navigation2-with-turtlebot3-in-gazebo:
+
+Navigation2 with Turtlebot 3 in Gazebo
+**************************************
+
+- `Overview`_
+- `Requirements`_
+- `Tutorial Steps`_
+- `Videos`_
+
+Overview
+========
+
+This tutorial shows how to control and navigate Turtlebot 3 using the ROS2 Navigation2 in Gazebo.
+Turtlebot is a low-cost, educational robot kit with open-source software (ROS2).
+Turtblebot robots are widely supported by the ROS community.
+You can find many applications and examples of Turtlebot projects on the Internet.
+You can find more information about Turtlebot3 `here. <https://www.turtlebot.com/>`_
+
+Links to the robot kits and Github repo.
+
+ - `Turtlebot3 Kits <https://www.turtlebot.com/purchase/>`_
+ - `Turtlebot 3 Github <https://github.com/ROBOTIS-GIT/turtlebot3>`_
+
+This tutorial consists of two parts.
+In the first part, you will learn how to use Turtlebot 3 robots in simulation (Gazebo).
+And in the second part, you will learn how to control a real Turtlebot Waffle using Navigation2.
+
+``ROS2 Dashing`` and ``Navigation2 Dashing 0.2.4`` are used to create this tutorial.
+
+This tutorial may take about 30 minutes to complete. It depends on your experience with ROS, robots, and what computer system you have.
+
+Requirements
+============
+
+- [Install ROS2](https://index.ros.org/doc/ros2/Installation/)
+
+- Install Navigation2
+
+ - ``sudo apt install ros-<ros2-distro>-navigation2 ros-<ros2-distro>-nav2-bringup``
+
+- Install Turtlebot 3
+
+ - ``sudo apt install ros-<ros2-distro>-turtlebot3*``
+
+- `Setup Turtlebot 3 <http://emanual.robotis.com/docs/en/platform/turtlebot3/ros2/#setup>`_
+
+Tutorial Steps
+==============
+
+0- Setup Your Environment Variables
+-----------------------------------
+
+Run the following commands whenever you open a new terminal during this tutorial.
+
+- ``source /opt/ros/<ros2-distro>/setup.bash``
+- ``export TURTLEBOT3_MODEL=waffle``
+- ``export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:/opt/ros/dashing/share/turtlebot3_gazebo/models``
+
+Short-cut
+---------
+
+If you don't have the time to go through all the steps and only want to try Navigation2 in simulation, there is a solution for that.
+After setting up your system variables, if you don't want to go through the step by step launch instructions,
+you can use the launch file that does all the steps that are explained in the next sections for you.
+
+- ``ros2 launch nav2_bringup nav2_tb3_simulation_launch.py``
+
+This launch file is going to launch Gazebo, Turtlebot3 specific nodes, Navigation2 and RViz2.
+Once you see everything launced, you can jump to step 5.
+
+1- Launch Gazebo
+----------------
+
+Now, launch Gazebo with the world model, open a new terminal and type
+
+ ``gazebo --verbose -s libgazebo_ros_init.so /opt/ros/dashing/share/turtlebot3_gazebo/worlds/turtlebot3_worlds/waffle.model``
+
+Once, Gazebo is launched, you should see the Turtlebot3 world and Turtlebot 3 Waffle.
+
+If Gazebo fails to start, run the following commands and try to launch Gazebo again.
+
+ - ``killall gzserver``
+ - ``killall gzclient``
+
+.. image:: images/Navigation2_with_Turtlebot3_in_Gazebo/gazebo_turlebot3.png
+ :height: 720px
+ :width: 1024px
+ :alt: TB3 world and robot in Gazebo
+
+2- Launch Turtlebot 3 Robot State Publisher
+-------------------------------------------
+
+Launch Turtlebot 3 specific nodes,
+
+ ``ros2 launch turtlebot3_bringup turtlebot3_state_publisher.launch.py use_sim_time:=True``
+
+Note : Make sure ``use_sim_time`` is set to **True** because our robot Turtlebot 3 is in Gazebo.
+
+3- Launch Navigation2
+---------------------
+
+Launch Navigation 2. If you set ``autostart:=False``, you need to click on the start button in RVIZ to initialize the nodes.
+Make sure `use_sim time` is set to **True**, because we want to use the time simulation time in Gazebo instead of the system time.
+
+turtlebot3_world.yaml is the configuration file for the map we want to provide Navigation2.
+In this case, it has the map resolution value, threshold values for obstacles and free spaces, and a map file location.
+
+ ``ros2 launch nav2_bringup nav2_bringup_launch.py use_sim_time:=True autostart:=False map:=/opt/ros/dashing/share/nav2_bringup/launch/turtlebot3_world.yaml``
+
+4- Launch RViz
+---------------
+
+Launch RVIZ with a pre-defined configuration file.
+
+ ``ros2 run rviz2 rviz2 -d $(ros2 pkg prefix nav2_bringup)/share/nav2_bringup/launch/nav2_default_view.rviz``
+
+Now, you should see a shadow of Turtlebot 3 robot model in the center of the plot in RViz.
+Click on the Start button (Bottom Left) if you set the auto_start parameter to false.
+
+.. image:: images/Navigation2_on_real_Turtlebot3/rviz_after_launch_view.png
+ :height: 720px
+ :width: 1024px
+ :alt: Rviz after launch, auto_start = false
+
+Now, the map should appear in RViz.
+
+.. image:: images/Navigation2_with_Turtlebot3_in_Gazebo/rviz_initial_view.png
+ :height: 720px
+ :width: 1024px
+ :alt: Turtlebot 3 map in RViz
+
+5- Initialize the location of Turtlebot 3
+-----------------------------------------
+
+First, find where the robot is in Gazebo. You can see where the robot's initial position in Gazebo.
+
+.. image:: images/Navigation2_with_Turtlebot3_in_Gazebo/gazebo_turlebot3.png
+ :height: 720px
+ :width: 1024px
+ :alt: Turtlebot 3 world and robot in Gazebo
+
+Set the pose of the robot in RViz. Click on the 2D Pose Estimate button and point the location of the robot on the map.
+The direction of the green arrow is the orientation of Turtlebot.
+
+.. image:: images/Navigation2_with_Turtlebot3_in_Gazebo/rviz_set_initial_pose.png
+ :height: 720px
+ :width: 1024px
+ :alt: Set initial pose in RViz
+
+Now, the 3D model of Turtlebot should move to that location.
+A small error in the estimated location is tolerable.
+
+6- Send a Goal Pose
+--------------------
+
+Pick a target location for Turtlebot on the map.
+You can send Turtlebot 3 a goal position and a goal orientation by using the **Navigation2 Goal** or the **GoalTool** buttons.
+
+Note: Navigation2 Goal button uses a ROS2 Action to send the goal and the GoalTool publishes the goal to a topic.
+
+.. image:: images/Navigation2_with_Turtlebot3_in_Gazebo/rviz_send_goal_pose.png
+ :height: 720px
+ :width: 1024px
+ :alt: Send goal pose in RViz
+
+Once you define the target pose, Navigation2 will find a global path and start navigating the robot on the map.
+
+.. image:: images/Navigation2_with_Turtlebot3_in_Gazebo/rviz_robot_navigating.png
+ :height: 720px
+ :width: 1024px
+ :alt: Turtlebot 3 navigating on a map in RViz
+
+You can also observe that Turtlebot 3 moves in the simulated environment in Gazebo as well.
+
+.. image:: images/Navigation2_with_Turtlebot3_in_Gazebo/gazebo_robot_reached_goal.png
+ :height: 720px
+ :width: 1024px
+ :alt: Turtlebot 3 navigating in Gazebo
+
+Next Tutorial
+-------------
+
+:ref:`navigation2-on-real-turtlebot3`
+
+Videos
+------
+
+.. image:: images/Navigation2_with_Turtlebot3_in_Gazebo/navigation_with_recovery_behaviours.gif
+ :height: 480px
+ :width: 640px
+ :alt: Navigation2 with Turtlebot 3 Demo
diff --git a/sphinx_doc/howtos/tutorials/index.rst b/sphinx_doc/howtos/tutorials/index.rst
new file mode 100644
index 0000000000000000000000000000000000000000..b0ec0753dc091b898a801688dde8742bab40faaa
--- /dev/null
+++ b/sphinx_doc/howtos/tutorials/index.rst
@@ -0,0 +1,13 @@
+.. _tutorials:
+
+Tutorials
+#########
+
+Navigation2 Tutorials
+
+.. toctree::
+ :maxdepth: 1
+
+ docs/navigation2_with_turtlebot3_in_gazebo.rst
+ docs/navigation2_on_real_turtlebot3.rst
+