diff --git a/nav2_bringup/bringup/params/nav2_params.yaml b/nav2_bringup/bringup/params/nav2_params.yaml
index 4fd94c03dcd48356f054345c1db7ec55a019ddeb..30762e8c945ad6b55dd348d9ce9d10069f43d8e7 100644
--- a/nav2_bringup/bringup/params/nav2_params.yaml
+++ b/nav2_bringup/bringup/params/nav2_params.yaml
@@ -152,7 +152,7 @@ controller_server:
RotateToGoal.scale: 32.0
RotateToGoal.slowing_factor: 5.0
RotateToGoal.lookahead_time: -1.0
-
+
controller_server_rclcpp_node:
ros__parameters:
use_sim_time: True
diff --git a/nav2_bringup/bringup/rviz/nav2_default_view.rviz b/nav2_bringup/bringup/rviz/nav2_default_view.rviz
index dfdda36ba12199306f642f8dfa84a018623ee668..904e51cb62310eb0633104eb0938b0ab409f6ea1 100644
--- a/nav2_bringup/bringup/rviz/nav2_default_view.rviz
+++ b/nav2_bringup/bringup/rviz/nav2_default_view.rviz
@@ -293,7 +293,7 @@ Visualization Manager:
Value: true
Axis: Z
Channel Name: intensity
- Class: rviz_default_plugins/PointCloud
+ Class: rviz_default_plugins/PointCloud2
Color: 125; 125; 125
Color Transformer: FlatColor
Decay Time: 0
@@ -407,7 +407,7 @@ Visualization Manager:
Value: true
Axis: Z
Channel Name: intensity
- Class: rviz_default_plugins/PointCloud
+ Class: rviz_default_plugins/PointCloud2
Color: 255; 255; 255
Color Transformer: RGB8
Decay Time: 0
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 22cf4269863308356a59ea689547a00d0435ba36..376ae45be8ef01325bd3d17399bd482ab91442cf 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp
@@ -99,7 +99,8 @@ private:
nav2_voxel_grid::VoxelGrid voxel_grid_;
double z_resolution_, origin_z_;
int unknown_threshold_, mark_threshold_, size_z_;
- rclcpp::Publisher<sensor_msgs::msg::PointCloud>::SharedPtr clearing_endpoints_pub_;
+ rclcpp_lifecycle::LifecyclePublisher<sensor_msgs::msg::PointCloud2>::SharedPtr
+ clearing_endpoints_pub_;
inline bool worldToMap3DFloat(
double wx, double wy, double wz, double & mx, double & my,
diff --git a/nav2_costmap_2d/plugins/voxel_layer.cpp b/nav2_costmap_2d/plugins/voxel_layer.cpp
index 40840426e3fb09b88ca9b1986a041197431d07d1..b8e5261bc148f53bf8b45925b30ea5c07c4f1127 100644
--- a/nav2_costmap_2d/plugins/voxel_layer.cpp
+++ b/nav2_costmap_2d/plugins/voxel_layer.cpp
@@ -96,8 +96,9 @@ void VoxelLayer::onInitialize()
voxel_pub_->on_activate();
}
- clearing_endpoints_pub_ = node->create_publisher<sensor_msgs::msg::PointCloud>(
+ clearing_endpoints_pub_ = node->create_publisher<sensor_msgs::msg::PointCloud2>(
"clearing_endpoints", custom_qos);
+ clearing_endpoints_pub_->on_activate();
unknown_threshold_ += (VOXEL_BITS - size_z_);
matchSize();
@@ -302,11 +303,9 @@ void VoxelLayer::raytraceFreespace(
double * max_x,
double * max_y)
{
- auto clearing_endpoints_ = std::make_unique<sensor_msgs::msg::PointCloud>();
+ auto clearing_endpoints_ = std::make_unique<sensor_msgs::msg::PointCloud2>();
- size_t clearing_observation_cloud_size = clearing_observation.cloud_->height *
- clearing_observation.cloud_->width;
- if (clearing_observation_cloud_size == 0) {
+ if (clearing_observation.cloud_->height == 0 || clearing_observation.cloud_->width == 0) {
return;
}
@@ -334,10 +333,23 @@ void VoxelLayer::raytraceFreespace(
}
if (publish_clearing_points) {
- clearing_endpoints_->points.clear();
- clearing_endpoints_->points.reserve(clearing_observation_cloud_size);
+ clearing_endpoints_->data.clear();
+ clearing_endpoints_->width = clearing_observation.cloud_->width;
+ clearing_endpoints_->height = clearing_observation.cloud_->height;
+ clearing_endpoints_->is_dense = true;
+ clearing_endpoints_->is_bigendian = false;
}
+ sensor_msgs::PointCloud2Modifier modifier(*clearing_endpoints_);
+ modifier.setPointCloud2Fields(
+ 3, "x", 1, sensor_msgs::msg::PointField::FLOAT32,
+ "y", 1, sensor_msgs::msg::PointField::FLOAT32,
+ "z", 1, sensor_msgs::msg::PointField::FLOAT32);
+
+ sensor_msgs::PointCloud2Iterator<float> clearing_endpoints_iter_x(*clearing_endpoints_, "x");
+ sensor_msgs::PointCloud2Iterator<float> clearing_endpoints_iter_y(*clearing_endpoints_, "y");
+ sensor_msgs::PointCloud2Iterator<float> clearing_endpoints_iter_z(*clearing_endpoints_, "z");
+
// we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later
double map_end_x = origin_x_ + getSizeInMetersX();
double map_end_y = origin_y_ + getSizeInMetersY();
@@ -414,11 +426,13 @@ void VoxelLayer::raytraceFreespace(
max_y);
if (publish_clearing_points) {
- geometry_msgs::msg::Point32 point;
- point.x = wpx;
- point.y = wpy;
- point.z = wpz;
- clearing_endpoints_->points.push_back(point);
+ *clearing_endpoints_iter_x = wpx;
+ *clearing_endpoints_iter_y = wpy;
+ *clearing_endpoints_iter_z = wpz;
+
+ ++clearing_endpoints_iter_x;
+ ++clearing_endpoints_iter_y;
+ ++clearing_endpoints_iter_z;
}
}
}
diff --git a/nav2_costmap_2d/src/costmap_2d_cloud.cpp b/nav2_costmap_2d/src/costmap_2d_cloud.cpp
index 52157b3b7a5e104f4311e7bf80c2080819798a7b..8c40709be6d794367a624c4467f9de5c640068f3 100644
--- a/nav2_costmap_2d/src/costmap_2d_cloud.cpp
+++ b/nav2_costmap_2d/src/costmap_2d_cloud.cpp
@@ -31,8 +31,8 @@
#include <utility>
#include "rclcpp/rclcpp.hpp"
-#include "sensor_msgs/msg/point_cloud.hpp"
-#include "sensor_msgs/msg/channel_float32.hpp"
+#include "sensor_msgs/msg/point_cloud2.hpp"
+#include "sensor_msgs/point_cloud2_iterator.hpp"
#include "nav2_voxel_grid/voxel_grid.hpp"
#include "nav2_msgs/msg/voxel_grid.hpp"
#include "nav2_util/execution_timer.hpp"
@@ -70,8 +70,63 @@ V_Cell g_unknown;
rclcpp::Node::SharedPtr g_node;
-rclcpp::Publisher<sensor_msgs::msg::PointCloud>::SharedPtr pub_marked;
-rclcpp::Publisher<sensor_msgs::msg::PointCloud>::SharedPtr pub_unknown;
+rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_marked;
+rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_unknown;
+
+/**
+ * @brief An helper function to fill pointcloud2 of both the marked and unknown points from voxel_grid
+ * @param cloud PointCloud2 Ptr which needs to be filled
+ * @param num_channels Represents the total number of points that are going to be filled
+ * @param header Carries the header information that needs to be assigned to PointCloud2 header
+ * @param g_cells contains the x, y, z values that needs to be added to the PointCloud2
+ */
+void pointCloud2Helper(
+ std::unique_ptr<sensor_msgs::msg::PointCloud2> & cloud,
+ uint32_t num_channels,
+ std_msgs::msg::Header header,
+ V_Cell & g_cells)
+{
+ cloud->header = header;
+ cloud->width = num_channels;
+ cloud->height = 1;
+ cloud->is_dense = true;
+ cloud->is_bigendian = false;
+ sensor_msgs::PointCloud2Modifier modifier(*cloud);
+
+ modifier.setPointCloud2Fields(
+ 6, "x", 1, sensor_msgs::msg::PointField::FLOAT32,
+ "y", 1, sensor_msgs::msg::PointField::FLOAT32,
+ "z", 1, sensor_msgs::msg::PointField::FLOAT32,
+ "r", 1, sensor_msgs::msg::PointField::UINT8,
+ "g", 1, sensor_msgs::msg::PointField::UINT8,
+ "b", 1, sensor_msgs::msg::PointField::UINT8);
+
+ sensor_msgs::PointCloud2Iterator<float> iter_x(*cloud, "x");
+ sensor_msgs::PointCloud2Iterator<float> iter_y(*cloud, "y");
+ sensor_msgs::PointCloud2Iterator<float> iter_z(*cloud, "z");
+
+ sensor_msgs::PointCloud2Iterator<uint8_t> iter_r(*cloud, "r");
+ sensor_msgs::PointCloud2Iterator<uint8_t> iter_g(*cloud, "g");
+ sensor_msgs::PointCloud2Iterator<uint8_t> iter_b(*cloud, "b");
+
+ for (uint32_t i = 0; i < num_channels; ++i) {
+ Cell & c = g_cells[i];
+ // assigning value to the point cloud2's iterator
+ *iter_x = c.x;
+ *iter_y = c.y;
+ *iter_z = c.z;
+ *iter_r = g_colors_r[c.status] * 255.0;
+ *iter_g = g_colors_g[c.status] * 255.0;
+ *iter_b = g_colors_b[c.status] * 255.0;
+
+ ++iter_x;
+ ++iter_y;
+ ++iter_z;
+ ++iter_r;
+ ++iter_g;
+ ++iter_b;
+ }
+}
void voxelCallback(const nav2_msgs::msg::VoxelGrid::ConstSharedPtr grid)
{
@@ -133,65 +188,19 @@ void voxelCallback(const nav2_msgs::msg::VoxelGrid::ConstSharedPtr grid)
}
}
- {
- auto cloud = std::make_unique<sensor_msgs::msg::PointCloud>();
- cloud->points.resize(num_marked);
- cloud->channels.resize(1);
- cloud->channels[0].values.resize(num_marked);
- cloud->channels[0].name = "rgb";
- cloud->header.frame_id = frame_id;
- cloud->header.stamp = stamp;
-
- sensor_msgs::msg::ChannelFloat32 & chan = cloud->channels[0];
- for (uint32_t i = 0; i < num_marked; ++i) {
- geometry_msgs::msg::Point32 & p = cloud->points[i];
- float & cval = chan.values[i];
- Cell & c = g_marked[i];
-
- p.x = c.x;
- p.y = c.y;
- p.z = c.z;
-
- uint32_t r = g_colors_r[c.status] * 255.0;
- uint32_t g = g_colors_g[c.status] * 255.0;
- uint32_t b = g_colors_b[c.status] * 255.0;
- // uint32_t a = g_colors_a[c.status] * 255.0;
-
- uint32_t col = (r << 16) | (g << 8) | b;
- memcpy(&cval, &col, sizeof col);
- }
+ std_msgs::msg::Header pcl_header;
+ pcl_header.frame_id = frame_id;
+ pcl_header.stamp = stamp;
+ {
+ auto cloud = std::make_unique<sensor_msgs::msg::PointCloud2>();
+ pointCloud2Helper(cloud, num_marked, pcl_header, g_marked);
pub_marked->publish(std::move(cloud));
}
{
- auto cloud = std::make_unique<sensor_msgs::msg::PointCloud>();
- cloud->points.resize(num_unknown);
- cloud->channels.resize(1);
- cloud->channels[0].values.resize(num_unknown);
- cloud->channels[0].name = "rgb";
- cloud->header.frame_id = frame_id;
- cloud->header.stamp = stamp;
-
- sensor_msgs::msg::ChannelFloat32 & chan = cloud->channels[0];
- for (uint32_t i = 0; i < num_unknown; ++i) {
- geometry_msgs::msg::Point32 & p = cloud->points[i];
- float & cval = chan.values[i];
- Cell & c = g_unknown[i];
-
- p.x = c.x;
- p.y = c.y;
- p.z = c.z;
-
- uint32_t r = g_colors_r[c.status] * 255.0;
- uint32_t g = g_colors_g[c.status] * 255.0;
- uint32_t b = g_colors_b[c.status] * 255.0;
- // uint32_t a = g_colors_a[c.status] * 255.0;
-
- uint32_t col = (r << 16) | (g << 8) | b;
- memcpy(&cval, &col, sizeof col);
- }
-
+ auto cloud = std::make_unique<sensor_msgs::msg::PointCloud2>();
+ pointCloud2Helper(cloud, num_unknown, pcl_header, g_unknown);
pub_unknown->publish(std::move(cloud));
}
@@ -208,9 +217,9 @@ int main(int argc, char ** argv)
RCLCPP_DEBUG(g_node->get_logger(), "Starting up costmap_2d_cloud");
- pub_marked = g_node->create_publisher<sensor_msgs::msg::PointCloud>(
+ pub_marked = g_node->create_publisher<sensor_msgs::msg::PointCloud2>(
"voxel_marked_cloud", 1);
- pub_unknown = g_node->create_publisher<sensor_msgs::msg::PointCloud>(
+ pub_unknown = g_node->create_publisher<sensor_msgs::msg::PointCloud2>(
"voxel_unknown_cloud", 1);
auto sub = g_node->create_subscription<nav2_msgs::msg::VoxelGrid>(
"voxel_grid", rclcpp::SystemDefaultsQoS(), voxelCallback);
diff --git a/nav2_dwb_controller/dwb_core/include/dwb_core/publisher.hpp b/nav2_dwb_controller/dwb_core/include/dwb_core/publisher.hpp
index aba5354c652d1b7e97771ae80a31431ece887676..bd2b210e033cb75fe34d37179808a16e84f33e0f 100644
--- a/nav2_dwb_controller/dwb_core/include/dwb_core/publisher.hpp
+++ b/nav2_dwb_controller/dwb_core/include/dwb_core/publisher.hpp
@@ -124,7 +124,7 @@ protected:
std::shared_ptr<LifecyclePublisher<nav_msgs::msg::Path>> transformed_pub_;
std::shared_ptr<LifecyclePublisher<nav_msgs::msg::Path>> local_pub_;
std::shared_ptr<LifecyclePublisher<visualization_msgs::msg::MarkerArray>> marker_pub_;
- std::shared_ptr<LifecyclePublisher<sensor_msgs::msg::PointCloud>> cost_grid_pc_pub_;
+ std::shared_ptr<LifecyclePublisher<sensor_msgs::msg::PointCloud2>> cost_grid_pc_pub_;
rclcpp_lifecycle::LifecycleNode::WeakPtr node_;
rclcpp::Clock::SharedPtr clock_;
diff --git a/nav2_dwb_controller/dwb_core/include/dwb_core/trajectory_critic.hpp b/nav2_dwb_controller/dwb_core/include/dwb_core/trajectory_critic.hpp
index 2724bfd8a10af06f67a13aafd33ed06f0c2726f8..17899a29947c2f58a57298e7e1f9993066be3e8c 100644
--- a/nav2_dwb_controller/dwb_core/include/dwb_core/trajectory_critic.hpp
+++ b/nav2_dwb_controller/dwb_core/include/dwb_core/trajectory_critic.hpp
@@ -38,6 +38,7 @@
#include <string>
#include <vector>
#include <memory>
+#include <utility>
#include "rclcpp/rclcpp.hpp"
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
@@ -166,7 +167,7 @@ public:
*
* @param pc PointCloud to add channels to
*/
- virtual void addCriticVisualization(sensor_msgs::msg::PointCloud &) {}
+ virtual void addCriticVisualization(std::vector<std::pair<std::string, std::vector<float>>> &) {}
std::string getName()
{
diff --git a/nav2_dwb_controller/dwb_core/src/publisher.cpp b/nav2_dwb_controller/dwb_core/src/publisher.cpp
index 70adb22d8196b0b87d065943decab18430f21449..7b860fef65a28667659b0929cdcd51ff8f84314c 100644
--- a/nav2_dwb_controller/dwb_core/src/publisher.cpp
+++ b/nav2_dwb_controller/dwb_core/src/publisher.cpp
@@ -40,6 +40,7 @@
#include <vector>
#include <utility>
+#include "sensor_msgs/point_cloud2_iterator.hpp"
#include "nav_2d_utils/conversions.hpp"
#include "nav2_util/node_utils.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"
@@ -105,7 +106,7 @@ DWBPublisher::on_configure()
transformed_pub_ = node->create_publisher<nav_msgs::msg::Path>("transformed_global_plan", 1);
local_pub_ = node->create_publisher<nav_msgs::msg::Path>("local_plan", 1);
marker_pub_ = node->create_publisher<visualization_msgs::msg::MarkerArray>("marker", 1);
- cost_grid_pc_pub_ = node->create_publisher<sensor_msgs::msg::PointCloud>("cost_cloud", 1);
+ cost_grid_pc_pub_ = node->create_publisher<sensor_msgs::msg::PointCloud2>("cost_cloud", 1);
double marker_lifetime = 0.0;
node->get_parameter(plugin_name_ + ".marker_lifetime", marker_lifetime);
@@ -256,7 +257,7 @@ DWBPublisher::publishCostGrid(
if (!publish_cost_grid_pc_) {return;}
- auto cost_grid_pc = std::make_unique<sensor_msgs::msg::PointCloud>();
+ auto cost_grid_pc = std::make_unique<sensor_msgs::msg::PointCloud2>();
cost_grid_pc->header.frame_id = costmap_ros->getGlobalFrameID();
cost_grid_pc->header.stamp = clock_->now();
@@ -264,38 +265,75 @@ DWBPublisher::publishCostGrid(
double x_coord, y_coord;
unsigned int size_x = costmap->getSizeInCellsX();
unsigned int size_y = costmap->getSizeInCellsY();
- cost_grid_pc->points.resize(size_x * size_y);
- unsigned int i = 0;
- for (unsigned int cy = 0; cy < size_y; cy++) {
- for (unsigned int cx = 0; cx < size_x; cx++) {
- costmap->mapToWorld(cx, cy, x_coord, y_coord);
- cost_grid_pc->points[i].x = x_coord;
- cost_grid_pc->points[i].y = y_coord;
- i++;
- }
- }
- sensor_msgs::msg::ChannelFloat32 totals;
- totals.name = "total_cost";
- totals.values.resize(size_x * size_y, 0.0);
+ std::vector<std::pair<std::string, std::vector<float>>> cost_channels;
+ std::vector<float> total_cost(size_x * size_y, 0.0);
for (TrajectoryCritic::Ptr critic : critics) {
- unsigned int channel_index = cost_grid_pc->channels.size();
- critic->addCriticVisualization(*cost_grid_pc);
- if (channel_index == cost_grid_pc->channels.size()) {
+ unsigned int channel_index = cost_channels.size();
+ critic->addCriticVisualization(cost_channels);
+ if (channel_index == cost_channels.size()) {
// No channels were added, so skip to next critic
continue;
}
double scale = critic->getScale();
- for (i = 0; i < size_x * size_y; i++) {
- totals.values[i] += cost_grid_pc->channels[channel_index].values[i] * scale;
+ for (unsigned int i = 0; i < size_x * size_y; i++) {
+ total_cost[i] += cost_channels[channel_index].second[i] * scale;
+ }
+ }
+
+ cost_channels.push_back(std::make_pair("total_cost", total_cost));
+
+ cost_grid_pc->width = size_x * size_y;
+ cost_grid_pc->height = 1;
+ cost_grid_pc->fields.resize(3 + cost_channels.size()); // x,y,z, + cost channels
+ cost_grid_pc->is_dense = true;
+ cost_grid_pc->is_bigendian = false;
+
+ int offset = 0;
+ for (size_t i = 0; i < cost_grid_pc->fields.size(); ++i, offset += 4) {
+ cost_grid_pc->fields[i].offset = offset;
+ cost_grid_pc->fields[i].count = 1;
+ cost_grid_pc->fields[i].datatype = sensor_msgs::msg::PointField::FLOAT32;
+ if (i >= 3) {
+ cost_grid_pc->fields[i].name = cost_channels[i - 3].first;
+ }
+ }
+
+ cost_grid_pc->fields[0].name = "x";
+ cost_grid_pc->fields[1].name = "y";
+ cost_grid_pc->fields[2].name = "z";
+
+ cost_grid_pc->point_step = offset;
+ cost_grid_pc->row_step = cost_grid_pc->point_step * cost_grid_pc->width;
+ cost_grid_pc->data.resize(cost_grid_pc->row_step * cost_grid_pc->height);
+
+ std::vector<sensor_msgs::PointCloud2Iterator<float>> cost_grid_pc_iter;
+
+ for (size_t i = 0; i < cost_grid_pc->fields.size(); ++i) {
+ sensor_msgs::PointCloud2Iterator<float> iter(*cost_grid_pc, cost_grid_pc->fields[i].name);
+ cost_grid_pc_iter.push_back(iter);
+ }
+
+ unsigned int j = 0;
+ for (unsigned int cy = 0; cy < size_y; cy++) {
+ for (unsigned int cx = 0; cx < size_x; cx++) {
+ costmap->mapToWorld(cx, cy, x_coord, y_coord);
+ *cost_grid_pc_iter[0] = x_coord;
+ *cost_grid_pc_iter[1] = y_coord;
+ *cost_grid_pc_iter[2] = 0.0; // z value
+
+ for (size_t i = 3; i < cost_grid_pc_iter.size(); ++i) {
+ *cost_grid_pc_iter[i] = cost_channels[i - 3].second[j];
+ ++cost_grid_pc_iter[i];
+ }
+ ++cost_grid_pc_iter[0];
+ ++cost_grid_pc_iter[1];
+ ++cost_grid_pc_iter[2];
+ j++;
}
}
- cost_grid_pc->channels.push_back(totals);
- // TODO(crdelsey): convert pc to pc2
- // sensor_msgs::msg::PointCloud2 cost_grid_pc2;
- // convertPointCloudToPointCloud2(cost_grid_pc, cost_grid_pc2);
cost_grid_pc_pub_->publish(std::move(cost_grid_pc));
}
diff --git a/nav2_dwb_controller/dwb_critics/include/dwb_critics/base_obstacle.hpp b/nav2_dwb_controller/dwb_critics/include/dwb_critics/base_obstacle.hpp
index 5f33445995cc56bb59b422e891ef23bb18f782f8..9bd4675ccb886203a0084a6e90bdb195beeb0cf3 100644
--- a/nav2_dwb_controller/dwb_critics/include/dwb_critics/base_obstacle.hpp
+++ b/nav2_dwb_controller/dwb_critics/include/dwb_critics/base_obstacle.hpp
@@ -35,6 +35,10 @@
#ifndef DWB_CRITICS__BASE_OBSTACLE_HPP_
#define DWB_CRITICS__BASE_OBSTACLE_HPP_
+#include <string>
+#include <vector>
+#include <utility>
+
#include "dwb_core/trajectory_critic.hpp"
namespace dwb_critics
@@ -55,7 +59,8 @@ class BaseObstacleCritic : public dwb_core::TrajectoryCritic
public:
void onInit() override;
double scoreTrajectory(const dwb_msgs::msg::Trajectory2D & traj) override;
- void addCriticVisualization(sensor_msgs::msg::PointCloud & pc) override;
+ void addCriticVisualization(
+ std::vector<std::pair<std::string, std::vector<float>>> & cost_channels) override;
/**
* @brief Return the obstacle score for a particular pose
diff --git a/nav2_dwb_controller/dwb_critics/include/dwb_critics/map_grid.hpp b/nav2_dwb_controller/dwb_critics/include/dwb_critics/map_grid.hpp
index 4d77e5019c08344db8fd15446a42f2ed6339ad51..611aad46445a337c2f7ada04f25a1d255daa2888 100644
--- a/nav2_dwb_controller/dwb_critics/include/dwb_critics/map_grid.hpp
+++ b/nav2_dwb_controller/dwb_critics/include/dwb_critics/map_grid.hpp
@@ -37,6 +37,9 @@
#include <vector>
#include <memory>
+#include <string>
+#include <utility>
+
#include "dwb_core/trajectory_critic.hpp"
#include "costmap_queue/costmap_queue.hpp"
@@ -61,7 +64,8 @@ public:
// Standard TrajectoryCritic Interface
void onInit() override;
double scoreTrajectory(const dwb_msgs::msg::Trajectory2D & traj) override;
- void addCriticVisualization(sensor_msgs::msg::PointCloud & pc) override;
+ void addCriticVisualization(
+ std::vector<std::pair<std::string, std::vector<float>>> & cost_channels) override;
double getScale() const override {return costmap_->getResolution() * 0.5 * scale_;}
// Helper Functions
diff --git a/nav2_dwb_controller/dwb_critics/src/base_obstacle.cpp b/nav2_dwb_controller/dwb_critics/src/base_obstacle.cpp
index e409b86ce4e1f429e2f59b39034c2070ed55a8df..2dfa3d415a7c56ce3e815dfd4ea2eca3d1a30c08 100644
--- a/nav2_dwb_controller/dwb_critics/src/base_obstacle.cpp
+++ b/nav2_dwb_controller/dwb_critics/src/base_obstacle.cpp
@@ -31,6 +31,9 @@
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
+#include <vector>
+#include <string>
+#include <utility>
#include "dwb_critics/base_obstacle.hpp"
#include "dwb_core/exceptions.hpp"
@@ -92,22 +95,23 @@ bool BaseObstacleCritic::isValidCost(const unsigned char cost)
cost != nav2_costmap_2d::NO_INFORMATION;
}
-void BaseObstacleCritic::addCriticVisualization(sensor_msgs::msg::PointCloud & pc)
+void BaseObstacleCritic::addCriticVisualization(
+ std::vector<std::pair<std::string, std::vector<float>>> & cost_channels)
{
- sensor_msgs::msg::ChannelFloat32 grid_scores;
- grid_scores.name = name_;
+ std::pair<std::string, std::vector<float>> grid_scores;
+ grid_scores.first = name_;
unsigned int size_x = costmap_->getSizeInCellsX();
unsigned int size_y = costmap_->getSizeInCellsY();
- grid_scores.values.resize(size_x * size_y);
+ grid_scores.second.resize(size_x * size_y);
unsigned int i = 0;
for (unsigned int cy = 0; cy < size_y; cy++) {
for (unsigned int cx = 0; cx < size_x; cx++) {
- grid_scores.values[i] = costmap_->getCost(cx, cy);
+ grid_scores.second[i] = costmap_->getCost(cx, cy);
i++;
}
}
- pc.channels.push_back(grid_scores);
+ cost_channels.push_back(grid_scores);
}
} // namespace dwb_critics
diff --git a/nav2_dwb_controller/dwb_critics/src/map_grid.cpp b/nav2_dwb_controller/dwb_critics/src/map_grid.cpp
index c70f7125e4c544d9703407d2177ef8fdbd9c112c..030e77cd637933495ef6f4398ff290963216f6da 100644
--- a/nav2_dwb_controller/dwb_critics/src/map_grid.cpp
+++ b/nav2_dwb_controller/dwb_critics/src/map_grid.cpp
@@ -35,6 +35,8 @@
#include "dwb_critics/map_grid.hpp"
#include <cmath>
#include <string>
+#include <vector>
+#include <utility>
#include <algorithm>
#include <memory>
#include "dwb_core/exceptions.hpp"
@@ -164,23 +166,24 @@ double MapGridCritic::scorePose(const geometry_msgs::msg::Pose2D & pose)
return getScore(cell_x, cell_y);
}
-void MapGridCritic::addCriticVisualization(sensor_msgs::msg::PointCloud & pc)
+void MapGridCritic::addCriticVisualization(
+ std::vector<std::pair<std::string, std::vector<float>>> & cost_channels)
{
- sensor_msgs::msg::ChannelFloat32 grid_scores;
- grid_scores.name = name_;
+ std::pair<std::string, std::vector<float>> grid_scores;
+ grid_scores.first = name_;
nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap();
unsigned int size_x = costmap->getSizeInCellsX();
unsigned int size_y = costmap->getSizeInCellsY();
- grid_scores.values.resize(size_x * size_y);
+ grid_scores.second.resize(size_x * size_y);
unsigned int i = 0;
for (unsigned int cy = 0; cy < size_y; cy++) {
for (unsigned int cx = 0; cx < size_x; cx++) {
- grid_scores.values[i] = getScore(cx, cy);
+ grid_scores.second[i] = getScore(cx, cy);
i++;
}
}
- pc.channels.push_back(grid_scores);
+ cost_channels.push_back(grid_scores);
}
} // namespace dwb_critics
diff --git a/nav2_dwb_controller/dwb_critics/test/base_obstacle_test.cpp b/nav2_dwb_controller/dwb_critics/test/base_obstacle_test.cpp
index 7cf8de890bfa529a6500709d5b27094bbe96b041..15f5dd70a6e5cfda275a44892c8b76a97781b6c4 100644
--- a/nav2_dwb_controller/dwb_critics/test/base_obstacle_test.cpp
+++ b/nav2_dwb_controller/dwb_critics/test/base_obstacle_test.cpp
@@ -35,6 +35,7 @@
#include <vector>
#include <memory>
#include <string>
+#include <utility>
#include "gtest/gtest.h"
#include "rclcpp/rclcpp.hpp"
@@ -141,8 +142,8 @@ TEST(BaseObstacle, CriticVisualization)
costmap_ros->getCostmap()->setCost(10, 49, 24);
costmap_ros->getCostmap()->setCost(45, 2, 12);
- sensor_msgs::msg::PointCloud pointcloud;
- critic->addCriticVisualization(pointcloud);
+ std::vector<std::pair<std::string, std::vector<float>>> cost_channels;
+ critic->addCriticVisualization(cost_channels);
unsigned int size_x = costmap_ros->getCostmap()->getSizeInCellsX();
unsigned int size_y = costmap_ros->getCostmap()->getSizeInCellsY();
@@ -150,7 +151,7 @@ TEST(BaseObstacle, CriticVisualization)
// The values in the pointcloud should be equal to the values in the costmap
for (unsigned int y = 0; y < size_y; y++) {
for (unsigned int x = 0; x < size_x; x++) {
- float pointValue = pointcloud.channels[0].values[y * size_y + x];
+ float pointValue = cost_channels[0].second[y * size_y + x];
ASSERT_EQ(static_cast<int>(pointValue), costmap_ros->getCostmap()->getCost(x, y));
}
}
diff --git a/nav2_system_tests/src/costmap_filters/keepout_params.yaml b/nav2_system_tests/src/costmap_filters/keepout_params.yaml
index 72d93eddf9c4b47f66b146cab901f183719bf2c9..5d37d77ce2f97beaf858d6b20689a59f98d6a8ea 100644
--- a/nav2_system_tests/src/costmap_filters/keepout_params.yaml
+++ b/nav2_system_tests/src/costmap_filters/keepout_params.yaml
@@ -146,6 +146,8 @@ controller_server:
RotateToGoal.scale: 32.0
RotateToGoal.slowing_factor: 5.0
RotateToGoal.lookahead_time: -1.0
+ publish_cost_grid_pc: True
+
controller_server_rclcpp_node:
ros__parameters:
@@ -185,6 +187,10 @@ local_costmap:
clearing: True
marking: True
data_type: "LaserScan"
+ raytrace_max_range: 3.0
+ raytrace_min_range: 0.0
+ obstacle_max_range: 2.5
+ obstacle_min_range: 0.0
static_layer:
map_subscribe_transient_local: True
keepout_filter:
diff --git a/nav2_system_tests/src/costmap_filters/test_keepout_launch.py b/nav2_system_tests/src/costmap_filters/test_keepout_launch.py
index 9365f25771b05a9c56d56c12379a38727795d6bc..c2c7e093230473aca1e2cf2ff7d383a1df94a934 100755
--- a/nav2_system_tests/src/costmap_filters/test_keepout_launch.py
+++ b/nav2_system_tests/src/costmap_filters/test_keepout_launch.py
@@ -114,6 +114,13 @@ def generate_launch_description():
'params_file': new_yaml,
'bt_xml_file': bt_navigator_xml,
'autostart': 'True'}.items()),
+
+ Node(
+ package='nav2_costmap_2d',
+ executable='nav2_costmap_2d_cloud',
+ name='costmap_2d_cloud',
+ output='screen',
+ remappings=[('voxel_grid', 'local_costmap/voxel_grid')]),
])
diff --git a/nav2_system_tests/src/costmap_filters/tester_node.py b/nav2_system_tests/src/costmap_filters/tester_node.py
index c5ccef680d9c89f54c1413427ae29d48ffbcddef..ca0b585b6654451b51cc2649db39ba4c6374ab5e 100755
--- a/nav2_system_tests/src/costmap_filters/tester_node.py
+++ b/nav2_system_tests/src/costmap_filters/tester_node.py
@@ -1,4 +1,4 @@
-#! /usr/bin/env python3
+#!/usr/bin/env python3
# Copyright (c) 2018 Intel Corporation.
# Copyright (c) 2020 Samsung Research Russia
@@ -34,11 +34,11 @@ from nav_msgs.msg import OccupancyGrid
from nav_msgs.msg import Path
import rclpy
-
from rclpy.action import ActionClient
from rclpy.node import Node
from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy
from rclpy.qos import QoSProfile
+from sensor_msgs.msg import PointCloud2
class TestType(Enum):
@@ -96,26 +96,46 @@ class NavTester(Node):
self.goal_pub = self.create_publisher(PoseStamped, 'goal_pose', 10)
transient_local_qos = QoSProfile(
- durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
- reliability=QoSReliabilityPolicy.RELIABLE,
- history=QoSHistoryPolicy.KEEP_LAST,
- depth=1)
+ durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
+ reliability=QoSReliabilityPolicy.RELIABLE,
+ history=QoSHistoryPolicy.KEEP_LAST,
+ depth=1)
volatile_qos = QoSProfile(
- durability=QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_VOLATILE,
- reliability=QoSReliabilityPolicy.RELIABLE,
- history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST,
- depth=1)
+ durability=QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_VOLATILE,
+ reliability=QoSReliabilityPolicy.RELIABLE,
+ history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST,
+ depth=1)
self.model_pose_sub = self.create_subscription(PoseWithCovarianceStamped,
'amcl_pose', self.poseCallback,
transient_local_qos)
-
+ self.clearing_ep_sub = self.create_subscription(PointCloud2,
+ 'local_costmap/clearing_endpoints',
+ self.clearingEndpointsCallback,
+ transient_local_qos)
self.test_type = test_type
self.filter_test_result = True
+ self.clearing_endpoints_received = False
+ self.voxel_marked_received = False
+ self.voxel_unknown_received = False
+ self.cost_cloud_received = False
+
if self.test_type == TestType.KEEPOUT:
self.plan_sub = self.create_subscription(Path, 'plan',
self.planCallback, volatile_qos)
+ self.voxel_marked_sub = self.create_subscription(PointCloud2,
+ 'voxel_marked_cloud',
+ self.voxelMarkedCallback,
+ 1)
+ self.voxel_unknown_sub = self.create_subscription(PointCloud2,
+ 'voxel_unknown_cloud',
+ self.voxelUnknownCallback,
+ 1)
+ self.cost_cloud_sub = self.create_subscription(PointCloud2,
+ 'cost_cloud',
+ self.dwbCostCloudCallback,
+ 1)
elif self.test_type == TestType.SPEED:
self.speed_it = 0
# Expected chain of speed limits
@@ -132,7 +152,8 @@ class NavTester(Node):
self.initial_pose_received = False
self.initial_pose = initial_pose
self.goal_pose = goal_pose
- self.action_client = ActionClient(self, NavigateToPose, 'navigate_to_pose')
+ self.action_client = ActionClient(
+ self, NavigateToPose, 'navigate_to_pose')
def info_msg(self, msg: str):
self.get_logger().info('\033[1;37;44m' + msg + '\033[0m')
@@ -165,7 +186,8 @@ class NavTester(Node):
# Sends a `NavToPose` action request and waits for completion
self.info_msg("Waiting for 'NavigateToPose' action server")
while not self.action_client.wait_for_server(timeout_sec=1.0):
- self.info_msg("'NavigateToPose' action server not available, waiting...")
+ self.info_msg(
+ "'NavigateToPose' action server not available, waiting...")
self.goal_pose = goal_pose if goal_pose is not None else self.goal_pose
goal_msg = NavigateToPose.Goal()
@@ -208,7 +230,8 @@ class NavTester(Node):
self.warn_msg('Filter mask was not received')
elif self.isInKeepout(x, y):
self.filter_test_result = False
- self.error_msg('Pose (' + str(x) + ', ' + str(y) + ') belongs to keepout zone')
+ self.error_msg('Pose (' + str(x) + ', ' +
+ str(y) + ') belongs to keepout zone')
return False
return True
@@ -245,6 +268,23 @@ class NavTester(Node):
self.error_msg('Path plan intersects with keepout zone')
return
+ def clearingEndpointsCallback(self, msg):
+ if len(msg.data) > 0:
+ self.clearing_endpoints_received = True
+
+ def voxelMarkedCallback(self, msg):
+ if len(msg.data) > 0:
+ self.voxel_marked_received = True
+
+ def voxelUnknownCallback(self, msg):
+ if len(msg.data) > 0:
+ self.voxel_unknown_received = True
+
+ def dwbCostCloudCallback(self, msg):
+ self.info_msg('Received cost_cloud points')
+ if len(msg.data) > 0:
+ self.cost_cloud_received = True
+
def speedLimitCallback(self, msg):
self.info_msg('Received speed limit: ' + str(msg.speed_limit))
self.checkSpeed(self.speed_it, msg.speed_limit)
@@ -266,6 +306,21 @@ class NavTester(Node):
return False
return True
+ def wait_for_pointcloud_subscribers(self, timeout):
+ start_time = time.time()
+ while not self.voxel_unknown_received or not self.voxel_marked_received \
+ or not self.clearing_endpoints_received:
+ self.info_msg(
+ 'Waiting for voxel_marked_cloud/voxel_unknown_cloud/\
+ clearing_endpoints msg to be received ...')
+ rclpy.spin_once(self, timeout_sec=1)
+ if (time.time() - start_time) > timeout:
+ self.error_msg(
+ 'Time out to waiting for voxel_marked_cloud/voxel_unknown_cloud/\
+ clearing_endpoints msgs')
+ return False
+ return True
+
def reachesGoal(self, timeout, distance):
goalReached = False
start_time = time.time()
@@ -305,7 +360,8 @@ class NavTester(Node):
state = future.result().current_state.label
self.info_msg('Result of get_state: %s' % state)
else:
- self.error_msg('Exception while calling service: %r' % future.exception())
+ self.error_msg('Exception while calling service: %r' %
+ future.exception())
time.sleep(5)
def shutdown(self):
@@ -313,9 +369,11 @@ class NavTester(Node):
self.action_client.destroy()
transition_service = 'lifecycle_manager_navigation/manage_nodes'
- mgr_client = self.create_client(ManageLifecycleNodes, transition_service)
+ mgr_client = self.create_client(
+ ManageLifecycleNodes, transition_service)
while not mgr_client.wait_for_service(timeout_sec=1.0):
- self.info_msg(transition_service + ' service not available, waiting...')
+ self.info_msg(transition_service +
+ ' service not available, waiting...')
req = ManageLifecycleNodes.Request()
req.command = ManageLifecycleNodes.Request().SHUTDOWN
@@ -324,13 +382,16 @@ class NavTester(Node):
self.info_msg('Shutting down navigation lifecycle manager...')
rclpy.spin_until_future_complete(self, future)
future.result()
- self.info_msg('Shutting down navigation lifecycle manager complete.')
+ self.info_msg(
+ 'Shutting down navigation lifecycle manager complete.')
except Exception as e: # noqa: B902
self.error_msg('Service call failed %r' % (e,))
transition_service = 'lifecycle_manager_localization/manage_nodes'
- mgr_client = self.create_client(ManageLifecycleNodes, transition_service)
+ mgr_client = self.create_client(
+ ManageLifecycleNodes, transition_service)
while not mgr_client.wait_for_service(timeout_sec=1.0):
- self.info_msg(transition_service + ' service not available, waiting...')
+ self.info_msg(transition_service +
+ ' service not available, waiting...')
req = ManageLifecycleNodes.Request()
req.command = ManageLifecycleNodes.Request().SHUTDOWN
@@ -339,7 +400,8 @@ class NavTester(Node):
self.info_msg('Shutting down localization lifecycle manager...')
rclpy.spin_until_future_complete(self, future)
future.result()
- self.info_msg('Shutting down localization lifecycle manager complete')
+ self.info_msg(
+ 'Shutting down localization lifecycle manager complete')
except Exception as e: # noqa: B902
self.error_msg('Service call failed %r' % (e,))
@@ -382,16 +444,19 @@ def run_all_tests(robot_tester):
robot_tester.wait_for_initial_pose()
robot_tester.wait_for_node_active('bt_navigator')
result = robot_tester.wait_for_filter_mask(10)
-
if (result):
result = robot_tester.runNavigateAction()
+ if robot_tester.test_type == TestType.KEEPOUT:
+ result = result and robot_tester.wait_for_pointcloud_subscribers(10)
+
if (result):
result = test_RobotMovesToGoal(robot_tester)
if (result):
if robot_tester.test_type == TestType.KEEPOUT:
result = robot_tester.filter_test_result
+ result = result and robot_tester.cost_cloud_received
elif robot_tester.test_type == TestType.SPEED:
result = test_SpeedLimitsAllCorrect(robot_tester)
@@ -438,7 +503,8 @@ def get_tester(args):
def main(argv=sys.argv[1:]):
# The robot(s) positions from the input arguments
- parser = argparse.ArgumentParser(description='System-level costmap filters tester node')
+ parser = argparse.ArgumentParser(
+ description='System-level costmap filters tester node')
parser.add_argument('-t', '--type', type=str, action='store', dest='type',
help='Type of costmap filter being tested.')
group = parser.add_mutually_exclusive_group(required=True)