tf transformation glitching

edit

Hi everybody,

I've been using Gazebo with the PR2 simulated robot. I'm working on a node which grabs a point cloud from PR2's Kinect (by subscribing to /head_mount_kinect/depth_registered/points), extract some points and publish a new cloud with the extracted data. To do this I transform the cloud from Kinect's coordinate system to PR2's base coordinate system, extract the points and publish new cloud to topic using the base's coordinate system (I don't convert them back).

Here is where the thing turns weird. If I want to view the extracted cloud using RViz I need to publish the coordinate system associated to it, since without it RViz can't show it correctly. So I publish cloud's coordinate system in the same node where the extraction is performed. This coordinate system is basically same as PR2 base's.

So far this sounds good (I think), but when I visualize the extracted cloud using RViz the cloud 'gliches' in its orientation, I mean sometimes it is oriented good, like if the coordinate transformation was done ok, that is objects are well placed according to the base. Nevertheless sometimes the cloud is oriented bad, like if the coordinate transformation weren't be done at all, I can notice this because if I display data according to base's coordinate system objects show inclined, exactly like if it weren't been transformed from Kinect's coordinate system.

My code is basically the following (I removed I few methods to make it shorter). I checked tf and the transformation is being published, at an awful slow rate (around 1Hz), but it is.

Has this happened to anybody?? So far I got no clue what might be the problem. Thanks in advance.

void cloudCallback(const sensor_msgs::PointCloud2ConstPtr &_msg)
{
    // Convert and clean input cloud
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
    fromROSMsg(*_msg, *cloud);
    Util::removeNANs(cloud);

    ROS_INFO("Transforming and filtering");
    if (!transformCloud(cloud, _msg->header.frame_id))
        return;
    removeFloor(cloud);

    // Get plane segmenting data
    ROS_INFO("Segmenting");
    pcl::PointIndices::Ptr inliers(new pcl::PointIndices());
    Eigen::Hyperplane<float, 3> plane = getSegmentationPlane(cloud, inliers);

    std::map<int, bool> inlierMap;
    for (size_t i = 0; i < inliers->indices.size(); ++i)
        inlierMap[inliers->indices[i]] = true;

    // Extract only the interesting data
    pcl::PointCloud<pcl::PointXYZ>::Ptr filteredCloud(new pcl::PointCloud<pcl::PointXYZ>());
    filteredCloud->reserve(cloud->size());
    for (size_t i = 0; i < cloud->size(); i++)
    {
        if (inlierMap.find(i) != inlierMap.end())
            continue;

        Eigen::Vector3f point = cloud->at(i).getVector3fMap();
        if (plane.signedDistance(point) > 0)
            filteredCloud->push_back(cloud->points[i]);
    }

    // Publish the extracted data
    ROS_INFO("Publishing");
    ros::Time now = ros::Time::now();
    sensor_msgs::PointCloud2 output;
    pcl::toROSMsg(*filteredCloud, output);
    output.header.stamp = now;
    output.header.frame_id = EXTRACTED_CLOUD_FRAME;
    publisher.publish(output);
}

int main(int argc, char** argv)
{
    // Initialize ROS
    ros::init(argc, argv, "basic_cloud_extractor");
    ros::NodeHandle handler;

    // Create a ROS publisher and subscriber
    publisher = handler.advertise<sensor_msgs::PointCloud2>("pr2_grasping/extracted_cloud", 2);
    ros::Subscriber sub = handler.subscribe("input", 1, cloudCallback);

    // Publish transformation
    static tf ...