Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

Removing Single Points from a sensor_msgs::PointCloud2

Hi,

I am trying to build a dynamic mask for robot systems using a Kinect2. My Idea is to use a naive euklidean distance search to remove the robot from the world. (I want to have a hole in the cloud where the robot is)

Unfortunately I don't know how to do that. Can you help me?

This is the part that needs help:

for (sensor_msgs::PointCloud2ConstIterator<float> it(output, "x"); it != it.end(); ++it) {
    // TODO: do something with the values of x, y, z


    try {
        transformStamped = tfBuffer.lookupTransform("kinect2_rgb_optical_frame", "RobotCenter",ros::Time(0),
                                                    ros::Duration(2));

        CamRobX = transformStamped.transform.translation.x;
        CamRobY = transformStamped.transform.translation.y;
        CamRobZ = transformStamped.transform.translation.z;
        CamPointX = it[0];
        CamPointY = it[1];
        CamPointZ = it[2];

        distance = sqrt(pow((CamPointX - CamRobX), 2) + pow((CamPointY - CamRobY), 2) + pow((CamPointZ - CamRobZ), 2));

    }
    catch (tf2::TransformException &ex) {
        ROS_WARN("%s", ex.what());
        ros::Duration(1.0).sleep();
        continue;
    }

    // distance refers to a radius of a frame in the middle of the room everything around this frame has to dissapear

    if(distance <0.4){

    }

    else{
    }
}

pub.publish(output);