Ask Your Question
0

Subscribe to PointClud2 Ros2 [closed]

asked 2020-12-22 03:22:28 -0500

Schloern93 gravatar image

updated 2020-12-24 06:22:35 -0500

Hello together

I get a depth image from my intel d453 camera in a PointClud2 msgs. My problem is now the following. I do not understand how I can access the individual points, i.e. XYZ coordinates. It is only possible to access the whole data structure. Unfortunately it is not possible to sort the points correctly.

I need the following information for each point XYZ coordinates Color of the point

Is it possible to access the individual points within the data structure in this way?

Here is my Subscriber Callback:

void callback_depth_image_d435 (const sensor_msgs::msg::PointCloud2::SharedPtr depth_points)
{

  printf("Cloud: width = %u, height = %u\n", depth_points->width, depth_points->height);

 // Looking for Something like that
 //      BOOST_FOREACH(const Point& pt, depth_points->points)
 //      {
 //        printf("\tpos = (%f, %f, %f), w = %u, normal = (%f, %f, %f)\n",
 //               pt.pos.x, pt.pos.y, pt.pos.z, pt.w,
 //                 pt.normal[0], pt.normal[1], pt.normal[2]);
 //      }

  //test
  std::cout << "data_size" << depth_points->data.size() << "\n";
  std::cout << "point_step" << depth_points->point_step << "\n";
  //std::cout << "data_size" << depth_points-> << "\n";
  std::cout << "data_size" << depth_points->data.size() << "\n";
}

Many thanks in advance!

edit retag flag offensive reopen merge delete

Closed for the following reason the question is answered, right answer was accepted by Schloern93
close date 2020-12-31 00:36:50.778803

1 Answer

Sort by ยป oldest newest most voted
0

answered 2020-12-29 06:41:30 -0500

Schloern93 gravatar image

Here is a solution.

  1. Install pcl_conversion (sudo apt install ros-eloquent-pcl-conversions)

CMakeList

    find_package(PCL 1.3 REQUIRED)
    include_directories(${PCL_INCLUDE_DIRS})
    link_directories(${PCL_LIBRARY_DIRS})
    add_definitions(${PCL_DEFINITIONS})
    target_link_libraries("target" ${PCL_LIBRARIES})

Subscriber Callback

 void callback_depth_image_d435 (const sensor_msgs::msg::PointCloud2::SharedPtr point_cloud2_msgs)
 {

   pcl::PointCloud<pcl::PointXYZ> point_cloud;
   pcl::fromROSMsg(*point_cloud2_msgs, point_cloud);     

   BOOST_FOREACH(const pcl::PointXYZ& pt, point_cloud.points)
   {          
       std::cout  << "x: " << pt.x <<"\n";
       std::cout  << "y: " << pt.y <<"\n";
       std::cout  << "z: " << pt.z <<"\n";
       std::cout << "---------" << "\n";
   }
 }
edit flag offensive delete link more

Comments

I tried the inverse procedure (feeding data to pointcloud2) on Eloquent but I'm stuck since toROSMsg expects pcl::PointCloud<pointt> and pcl::PCLPointCloud2. This is the one defined in pcl/ros/conversions.h

SimoneBenatti gravatar image SimoneBenatti  ( 2021-04-06 11:45:47 -0500 )edit

Question Tools

2 followers

Stats

Asked: 2020-12-22 03:22:28 -0500

Seen: 508 times

Last updated: Dec 29 '20