Generate and publish PointCloud2 in ros2?

asked 2019-01-12 11:03:18 -0500

8729 ●137 ●239 ●256 https://github.com/lucasw

updated 2019-01-13 09:33:58 -0500

What's the most convenient way to generate a bunch of points in a loop, assign xyz (and possibly rgb), and then publish as a PointCloud2? I'm interested in a full C++ code example mainly but python would be useful to have here also.

With and without pcl would be useful too. pcl_ros isn't available in crystal but there is pcl_conversions ros-crystal-pcl-conversions. I encountered some oddities with linking to PCL in colcon/ament last time I tried, maybe that is better addressed in a separate question but CMakeLists.txt excerpts are additionally helpful.

edit retag flag offensive close merge delete

Comments

Cross referencing https://github.com/lucasw/imgui_ros/i...

lucasw ( 2019-01-12 11:13:23 -0500 )edit

add a comment

5

answered 2019-01-12 12:19:34 -0500

lucasw

8729 ●137 ●239 ●256 https://github.com/lucasw

updated 2019-01-13 08:56:33 -0500

c++ with pcl_conversions

https://github.com/lucasw/imgui_ros/b...

#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
...
pub_ = create_publisher<sensor_msgs::msg::PointCloud2>("point_cloud");
...
pcl::PointCloud<pcl::PointXYZRGB> cloud_;
// TODO(lucasw) generate a cube or sphere instead, more interesting than 2d
for (int i = 0; i < num_points_; ++i) {
  const float fr = static_cast<float>(i) / static_cast<float>(num_points_);
  pcl::PointXYZRGB pt;
  pt = pcl::PointXYZRGB(fr * 255, 255 - fr * 255, 18 + fr * 20);
  pt.x = cos(fr * M_PI * 2.0) * 1.0;
  pt.y = sin(fr * M_PI * 2.0) * 1.0;
  pt.z = 0.0;
  cloud_.points.push_back(pt);
}

pc2_msg_ = std::make_shared<sensor_msgs::msg::PointCloud2>();
pcl::toROSMsg(cloud_, *pc2_msg_);
pc2_msg_->header.frame_id = "map";
...

pc2_msg_->header.stamp = now();
pub_->publish(pc2_msg_);

And in cmake:

add_executable(generate_pointcloud2 src/test/generate_pointcloud2.cpp)
ament_target_dependencies(generate_pointcloud2
  "geometry_msgs"  # maybe don't need this
  "pcl_conversions"
  "rcl_interfaces"
  "rclcpp"
  "rcutils"
  "sensor_msgs"
  "std_msgs"
  "tf2_geometry_msgs"
)
target_link_libraries(generate_pointcloud2
  ${Boost_SYSTEM_LIBRARY}  # pcl_conversions requires this
)