tf transformation glitching

asked 2015-07-18 19:08:45 -0500

Rodrigo gravatar image

updated 2016-04-22 10:34:15 -0500

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 ...
(more)
edit retag flag offensive close merge delete

Comments

1

Have you checked your tf tree? Also these kind of glitches are possible when several nodes are publishing to the same topic, but frame_id fields are different. So check the node graph as well.

Boris gravatar imageBoris ( 2015-07-19 09:16:44 -0500 )edit
1

You shouldn't need to create your own frame to publish clouds in; you should just be able to transform and publish them in the /odom or /map frame (depending on if you're using nav or not).

ahendrix gravatar imageahendrix ( 2015-07-19 12:07:02 -0500 )edit