Revision history [back]

Thank you all for your help! Now I know what I did wrong. First off: Yes Changing Single Coordinates to NAN works, but like somebody else pointed out, I used the wrong Iterator. Thankfully There was another Iterator which doesn't have the "const" tag in it, and with this one I am able to change points.

Here is the improved code:

for (sensor_msgs::PointCloud2Iterator<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;
    }
    if(distance <0.4){


        it[0] = std::numeric_limits<float>::quiet_NaN();
        it[1] = std::numeric_limits<float>::quiet_NaN();
        it[2] = std::numeric_limits<float>::quiet_NaN();


    }

    else{
    }
}

pub.publish(output);