ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

Pointcloud2 to depth map

asked 2020-06-19 14:28:03 -0500

Chao Chen gravatar image

updated 2020-06-19 14:29:05 -0500

I want to generate a 2D depth map _depth_map[x,y] = z from pointcloud2 where x,y,z are the dimension x,y,z

Currently my code is:

void callback(const sensor_msgs::PointCloud2ConstPtr& msg){

int width = msg->width;
int height = msg->height;
int v = 1;
int u = 1;
int arrayPosition = v * msg->row_step + u * msg->point_step;
int arrayPosX = arrayPosition + msg->fields[0].offset; // X has an offset of 0
int arrayPosY = arrayPosition + msg->fields[1].offset; // Y has an offset of 4
int arrayPosZ = arrayPosition + msg->fields[2].offset; // Z has an offset of 8

double X = 0.0;
double Y = 0.0;
double Z = 0.0;

memcpy(&X, &msg->data[arrayPosX], sizeof(double));
memcpy(&Y, &msg->data[arrayPosY], sizeof(double));
memcpy(&Z, &msg->data[arrayPosZ], sizeof(double));

ROS_INFO("X:%lf",X);
ROS_INFO("Y:%lf",Y);
ROS_INFO("Z:%lf",Z);

}

I am confused with the parameter u and v, how to obtain u and v from unordered point cloud. Cited from problem #191265 And why Z value = 0 all the time. Below is some sample result:

edit retag flag offensive close merge delete

Comments

Chao Chen gravatar image Chao Chen  ( 2020-06-19 14:28:46 -0500 )edit

Hi! i'm having a similar problem: i have to convert a "const sensor_msgs::PointCloud2ConstPtr& msg" to a occupacy gridmap. i need to obtain x,y,z informations as u wrote above. If i paste your code in my callback function it returns:

"[ INFO] [1631005633.103332744]: X:nan "

Did you resolved your problem you had or not yet?

christian.celestino.cc@gmail.com gravatar image christian.celestino.cc@gmail.com  ( 2021-09-07 04:18:54 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2020-06-19 14:29:20 -0500

Chao Chen gravatar image

edit flag offensive delete link more

Question Tools

2 followers

Stats

Asked: 2020-06-19 14:28:03 -0500

Seen: 257 times

Last updated: Jun 19 '20