Pointcloud2 to depth map
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:
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?