From 49f0a124d742105ecccc0ac8b96ef1e549c50fac Mon Sep 17 00:00:00 2001
From: Yathartha Tuladhar <yathartha3@gmail.com>
Date: Mon, 4 Nov 2019 23:30:18 -0800
Subject: [PATCH] Enable voxel layer (#1291)
* Update params for obstacle layer using poincloud
* Enabled Voxel Layer
* voxel param file
* Update param file
* Remove duplicate param yaml & fix voxel reset
* Short voxel layer documentation
* observation sources in individual layer namespaces
* Added missed param change
* Updated documentation
* Documentation for multiple sensors
---
nav2_bringup/bringup/params/nav2_params.yaml | 45 +++++++++--
nav2_costmap_2d/CMakeLists.txt | 6 +-
nav2_costmap_2d/README.md | 79 +++++++++++++++++++
.../include/nav2_costmap_2d/voxel_layer.hpp | 2 +-
nav2_costmap_2d/plugins/obstacle_layer.cpp | 3 +-
nav2_costmap_2d/plugins/voxel_layer.cpp | 5 +-
nav2_costmap_2d/src/costmap_2d_markers.cpp | 4 +-
7 files changed, 134 insertions(+), 10 deletions(-)
diff --git a/nav2_bringup/bringup/params/nav2_params.yaml b/nav2_bringup/bringup/params/nav2_params.yaml
index d8c522f3..634fced8 100644
--- a/nav2_bringup/bringup/params/nav2_params.yaml
+++ b/nav2_bringup/bringup/params/nav2_params.yaml
@@ -102,25 +102,42 @@ local_costmap:
ros__parameters:
use_sim_time: True
global_frame: odom
- plugin_names: ["obstacle_layer", "inflation_layer"]
- plugin_types: ["nav2_costmap_2d::ObstacleLayer", "nav2_costmap_2d::InflationLayer"]
+ plugin_names: ["obstacle_layer", "voxel_layer", "inflation_layer"]
+ plugin_types: ["nav2_costmap_2d::ObstacleLayer", "nav2_costmap_2d::VoxelLayer", "nav2_costmap_2d::InflationLayer"]
rolling_window: true
width: 3
height: 3
resolution: 0.05
robot_radius: 0.22
- inflation_layer.cost_scaling_factor: 3.0
+ inflation_layer:
+ cost_scaling_factor: 3.0
obstacle_layer:
enabled: True
+ observation_sources: scan
scan:
topic: /scan
max_obstacle_height: 2.0
clearing: True
marking: True
+ data_type: "LaserScan"
+ voxel_layer:
+ enabled: True
+ publish_voxel_map: True
+ origin_z: 0.0
+ z_resolution: 0.2
+ z_voxels: 10
+ max_obstacle_height: 2.0
+ mark_threshold: 0
+ observation_sources: pointcloud
+ pointcloud:
+ topic: /intel_realsense_r200_depth/points
+ max_obstacle_height: 2.0
+ clearing: True
+ marking: True
+ data_type: "PointCloud2"
static_layer:
map_subscribe_transient_local: True
always_send_full_costmap: True
- observation_sources: scan
local_costmap_client:
ros__parameters:
use_sim_time: True
@@ -132,18 +149,36 @@ global_costmap:
global_costmap:
ros__parameters:
use_sim_time: True
+ plugin_names: ["static_layer", "obstacle_layer", "voxel_layer", "inflation_layer"]
+ plugin_types: ["nav2_costmap_2d::StaticLayer", "nav2_costmap_2d::ObstacleLayer", "nav2_costmap_2d::VoxelLayer", "nav2_costmap_2d::InflationLayer"]
robot_radius: 0.22
obstacle_layer:
enabled: True
+ observation_sources: scan
scan:
topic: /scan
max_obstacle_height: 2.0
clearing: True
marking: True
+ data_type: "LaserScan"
+ voxel_layer:
+ enabled: True
+ publish_voxel_map: True
+ origin_z: 0.0
+ z_resolution: 0.2
+ z_voxels: 10
+ max_obstacle_height: 2.0
+ mark_threshold: 0
+ observation_sources: pointcloud
+ pointcloud:
+ topic: /intel_realsense_r200_depth/points
+ max_obstacle_height: 2.0
+ clearing: True
+ marking: True
+ data_type: "PointCloud2"
static_layer:
map_subscribe_transient_local: True
always_send_full_costmap: True
- observation_sources: scan
global_costmap_client:
ros__parameters:
use_sim_time: True
diff --git a/nav2_costmap_2d/CMakeLists.txt b/nav2_costmap_2d/CMakeLists.txt
index ca42f6f9..56b40564 100644
--- a/nav2_costmap_2d/CMakeLists.txt
+++ b/nav2_costmap_2d/CMakeLists.txt
@@ -110,6 +110,10 @@ target_link_libraries(nav2_costmap_2d_markers
nav2_costmap_2d_core
)
+ament_target_dependencies(nav2_costmap_2d_markers
+ ${dependencies}
+)
+
add_executable(nav2_costmap_2d_cloud src/costmap_2d_cloud.cpp)
target_link_libraries(nav2_costmap_2d_cloud
nav2_costmap_2d_core
@@ -125,7 +129,7 @@ target_link_libraries(nav2_costmap_2d
layers
)
-install(TARGETS nav2_costmap_2d_core nav2_costmap_2d layers nav2_costmap_2d_client
+install(TARGETS nav2_costmap_2d_core nav2_costmap_2d layers nav2_costmap_2d_client nav2_costmap_2d_markers
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION lib/${PROJECT_NAME}
diff --git a/nav2_costmap_2d/README.md b/nav2_costmap_2d/README.md
index 405b9683..9dd12247 100644
--- a/nav2_costmap_2d/README.md
+++ b/nav2_costmap_2d/README.md
@@ -10,7 +10,86 @@ ROS2 port of the ROS1 navigation stack version, with minimal noteable changes ne
widespread discussion throughout the navigation stack (see issue https://github.com/ros-planning/navigation2/issues/177) and
general ROS2 community. A proposal temporary replacement has been submitted as a PR here: https://github.com/ros-planning/navigation2/pull/196
+## How to configure using Voxel Layer plugin:
+By default, the navigation stack uses the _Obstacle Layer_ to avoid obstacles in 2D. The _Voxel Layer_ allows for detecting obstacles in 3D using Pointcloud2 data. It requires Pointcloud2 data being published on some topic. For simulation, a Gazebo model of the robot with depth camera enabled will publish a pointcloud topic.
+
+The Voxel Layer plugin can be used to update the local costmap, glocal costmap or both, depending on how you define it in the `nav2_params.yaml` file in the nav2_bringup directory. The voxel layer plugin has to be added to the list of ```plugin_names``` and ```plugin_types``` in the global/local costmap scopes in the param file. If these are not defined in the param file, the default plugins set in nav2_costmap_2d will be used.
+
+Inside each costmap layer (voxel, obstacle, etc) define `observation_sources` param. Here you can define multiple sources to be used with the layer. The param configuration example below shows the way you can configure costmaps to use voxel layer.
+
+The `voxel_layer` param has `observation_source: pointcloud` in it's scope, and the param `pointcloud` has `topic`, and `data_type` inside it's scope. By default, the data_type is `LaserScan`, thus you need to specify `PointCloud2` if you are using PointCould2 data from a depth sensor.
+
+Example param configuration snippet for enabling voxel layer in local costmap is shown below (not all params are shown):
+```
+local_costmap:
+ local_costmap:
+ ros__parameters:
+ plugin_names: ["obstacle_layer", "voxel_layer", "inflation_layer"]
+ plugin_types: ["nav2_costmap_2d::ObstacleLayer", "nav2_costmap_2d::VoxelLayer", "nav2_costmap_2d::InflationLayer"]
+ obstacle_layer:
+ enabled: True
+ observation_sources: scan
+ scan:
+ topic: /scan
+ max_obstacle_height: 2.0
+ clearing: True
+ marking: True
+ data_type: "LaserScan"
+ voxel_layer:
+ enabled: True
+ publish_voxel_map: True
+ origin_z: 0.0
+ z_resolution: 0.2
+ z_voxels: 10
+ max_obstacle_height: 2.0
+ mark_threshold: 0
+ observation_sources: pointcloud
+ pointcloud:
+ topic: /intel_realsense_r200_depth/points
+ max_obstacle_height: 2.0
+ clearing: True
+ marking: True
+ data_type: "PointCloud2"
+```
+Please note that not all params needed to run the navigation stack are shown here. This example only shows how you can add different costmap layers, with multiple input sources of different types.
+
+### To visualize the voxels in RVIZ:
+- Make sure `publish_voxel_map` in `voxel_layer` param's scope is set to `True`.
+- Open a new terminal and run:
+ ```ros2 run nav2_costmap_2d nav2_costmap_2d_markers voxel_grid:=/local_costmap/voxel_grid visualization_marker:=/my_marker```
+ Here you can change `my_marker` to any topic name you like for the markers to be published on.
+
+- Then add `my_marker` to RVIZ using the GUI.
+
+
+####Errata:
+- To see the markers in 3D, you will need to change the _view_ in RVIZ to a 3 dimensional view (e.g. orbit) from the RVIZ GUI.
+- Currently due to some bug in rviz, you need to set the `fixed_frame` in the rviz display, to `odom` frame.
+- Using pointcloud data from a saved bag file while using gazebo simulation can be troublesome due to the clock time skipping to an earlier time.
+
+## How to use multiple sensor sources:
+Multiple sources can be added into a costmap layer (e.g., obstacle layer), by listing them under the 'observation_sources' for that layer.
+For example, to add laser scan and pointcloud as two different sources of inputs to the obstacle layer, you can define them in the params file as shown below for the local costmap:
+```
+local_costmap:
+ local_costmap:
+ ros__parameters:
+ plugin_names: ["obstacle_layer", "inflation_layer"]
+ plugin_types: ["nav2_costmap_2d::ObstacleLayer", "nav2_costmap_2d::InflationLayer"]
+ obstacle_layer:
+ enabled: True
+ observation_sources: scan pointcloud
+ scan:
+ topic: /scan
+ data_type: "LaserScan"
+ pointcloud:
+ topic: /intel_realsense_r200_depth/points
+ data_type: "PointCloud2"
+```
+In order to add multiple sources to the global costmap, follow the same procedure shown in the example above, but now adding the sources and their specific params under the `global_costmap` scope.
+
## Future Plans
- Conceptually, the costmap_2d model acts as a world model of what is known from the map, sensor, robot pose, etc. We'd like
to broaden this world model concept and use costmap's layer concept as motivation for providing a service-style interface to
potential clients needing information about the world (see issue https://github.com/ros-planning/navigation2/issues/18)
+
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 22dc74fd..5e963c4f 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp
@@ -94,7 +94,7 @@ private:
double * max_y);
bool publish_voxel_;
- rclcpp::Publisher<nav2_msgs::msg::VoxelGrid>::SharedPtr voxel_pub_;
+ rclcpp_lifecycle::LifecyclePublisher<nav2_msgs::msg::VoxelGrid>::SharedPtr voxel_pub_;
nav2_voxel_grid::VoxelGrid voxel_grid_;
double z_resolution_, origin_z_;
int unknown_threshold_, mark_threshold_, size_z_;
diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp
index 347a139b..e40b0203 100644
--- a/nav2_costmap_2d/plugins/obstacle_layer.cpp
+++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp
@@ -80,6 +80,7 @@ void ObstacleLayer::onInitialize()
rclcpp::ParameterValue(true));
declareParameter("max_obstacle_height", rclcpp::ParameterValue(2.0));
declareParameter("combination_method", rclcpp::ParameterValue(1));
+ declareParameter("observation_sources", rclcpp::ParameterValue(std::string("")));
node_->get_parameter(name_ + "." + "enabled", enabled_);
node_->get_parameter(name_ + "." + "footprint_clearing_enabled", footprint_clearing_enabled_);
@@ -87,7 +88,7 @@ void ObstacleLayer::onInitialize()
node_->get_parameter(name_ + "." + "combination_method", combination_method_);
node_->get_parameter("track_unknown_space", track_unknown_space);
node_->get_parameter("transform_tolerance", transform_tolerance);
- node_->get_parameter("observation_sources", topics_string);
+ node_->get_parameter(name_ + "." + "observation_sources", topics_string);
RCLCPP_INFO(node_->get_logger(), "Subscribed to Topics: %s", topics_string.c_str());
diff --git a/nav2_costmap_2d/plugins/voxel_layer.cpp b/nav2_costmap_2d/plugins/voxel_layer.cpp
index 105b88a1..526b6200 100644
--- a/nav2_costmap_2d/plugins/voxel_layer.cpp
+++ b/nav2_costmap_2d/plugins/voxel_layer.cpp
@@ -88,6 +88,8 @@ void VoxelLayer::onInitialize()
"voxel_grid", custom_qos);
}
+ voxel_pub_->on_activate();
+
clearing_endpoints_pub_ = node_->create_publisher<sensor_msgs::msg::PointCloud>(
"clearing_endpoints", custom_qos);
@@ -108,11 +110,12 @@ void VoxelLayer::matchSize()
void VoxelLayer::reset()
{
+ voxel_pub_->on_deactivate();
deactivate();
resetMaps();
- voxel_grid_.reset();
activate();
undeclareAllParameters();
+ voxel_pub_->on_activate();
}
void VoxelLayer::resetMaps()
diff --git a/nav2_costmap_2d/src/costmap_2d_markers.cpp b/nav2_costmap_2d/src/costmap_2d_markers.cpp
index 42357321..2f1af4f7 100644
--- a/nav2_costmap_2d/src/costmap_2d_markers.cpp
+++ b/nav2_costmap_2d/src/costmap_2d_markers.cpp
@@ -62,7 +62,8 @@ float g_colors_a[] = {0.0f, 0.5f, 1.0f};
V_Cell g_cells;
nav2_util::LifecycleNode::SharedPtr g_node;
-rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr pub;
+rclcpp_lifecycle::LifecyclePublisher<visualization_msgs::msg::Marker>::SharedPtr pub;
+
void voxelCallback(const nav2_msgs::msg::VoxelGrid::ConstSharedPtr grid)
{
if (grid->data.empty()) {
@@ -150,6 +151,7 @@ int main(int argc, char ** argv)
pub = g_node->create_publisher<visualization_msgs::msg::Marker>(
"visualization_marker", 1);
+ pub->on_activate();
auto sub = g_node->create_subscription<nav2_msgs::msg::VoxelGrid>(
"voxel_grid", rclcpp::SystemDefaultsQoS(), voxelCallback);
--
GitLab