Ask Your Question
0

Not being able to build a point cloud out of pose and laser messages

asked 2011-12-25 04:39:11 -0500

alfa_80 gravatar image

updated 2011-12-28 02:58:52 -0500

dornhege gravatar image

I have been trying for a few weeks to build a point cloud out of pose and laser data that I have, however, it is still not working until now. The code snippet looks like below:

void PointCloudBuilder::computePointCloud()
{
    sensor_msgs::PointCloud cloud;

    pose3D_LD2.header.stamp = ros::Time::now();
    pose3D_LD2.header.frame_id = "pose3D_LD2";

    pose3D_LD2.pose.position.x = previous_poseLD_.pose.position.x;
    pose3D_LD2.pose.position.y = previous_poseLD_.pose.position.y ;
    pose3D_LD2.pose.position.z =  previous_poseLD_.pose.position.z ;

    pose3D_LD2.pose.orientation.x = previous_poseLD_.pose.orientation.x;
    pose3D_LD2.pose.orientation.y = previous_poseLD_.pose.orientation.y;
    pose3D_LD2.pose.orientation.z = previous_poseLD_.pose.orientation.z;
    pose3D_LD2.pose.orientation.w = previous_poseLD_.pose.orientation.w;

    static tf::TransformBroadcaster tfb;
    tf::Transform xform;
    xform.setOrigin(tf::Vector3(previous_poseLD_.pose.position.x, previous_poseLD_.pose.position.y, previous_poseLD_.pose.position.z));
    tf::Quaternion qt(previous_poseLD_.pose.orientation.x, previous_poseLD_.pose.orientation.y, previous_poseLD_.pose.orientation.z, previous_poseLD_.pose.orientation.w);
    xform.setRotation(qt);
    tfb.sendTransform(tf::StampedTransform(xform, ros::Time::now(), "base_frame", "target_frame"));

    projector_.transformLaserScanToPointCloud("base_link", previous_scan_, cloud, tfListener_);
      // Do something with cloud.


    pose3D_usinglocalizationdata_pub2.publish(pose3D_LD2);
    point_cloud_publisher_.publish(cloud);

}

When I issue a command of rostopic echo -c /cloud, it returns nothing there. Also from the terminal, I got an error like below:

terminate called after throwing an instance of 'tf::ExtrapolationException'

  what():  Lookup would require extrapolation at time 1324837730.494437690, but only     time  1324837730.469437690 is in the buffer, when looking up transform from frame     [/laser] to frame [/base_link]
[pointcloud_builder_node-5] process has died [pid 29991, exit code -6].
log files: /home/shah/.ros/log/4995859a-2f26-11e1-84b5-c42c03199b29    /pointcloud_builder_node-5*.log

Am I missing something or the order is not correct? Hope somebody can guide me on this.

EDIT(After adding waitForTransform, need somebody to check the use of it):

    tf::StampedTransform transform;

    //static tf::TransformBroadcaster tfb;
    tf::Transform xform;
    xform.setOrigin(tf::Vector3(previous_poseLD_.pose.position.x, previous_poseLD_.pose.position.y, previous_poseLD_.pose.position.z));
    tf::Quaternion qt(previous_poseLD_.pose.orientation.x, previous_poseLD_.pose.orientation.y, previous_poseLD_.pose.orientation.z, previous_poseLD_.pose.orientation.w);
    xform.setRotation(qt);
    //tfb.sendTransform(tf::StampedTransform(xform, ros::Time::now(), "base_frame", "target_frame"));

    /*try{
            tfListener_.waitForTransform("/base_link", "/laser", ros::Time(0), ros::Duration(3.0));
            tfListener_.lookupTransform("/base_link", "/laser", ros::Time(0), transform);
        }
         catch (tf::TransformException ex){
             ROS_ERROR("%s",ex.what());
         }*/

    projector_.transformLaserScanToPointCloud("base_link", previous_scan_, cloud, xform);
      // Do something with cloud.


    pose3D_usinglocalizationdata_pub2.publish(pose3D_LD2);
    point_cloud_publisher_.publish(cloud);
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2011-12-25 12:47:26 -0500

dornhege gravatar image

I think the problem is that instead of using the transform directly you are broadcasting it via tf and the in the next line using the transformlistener. But the listener needs some (small) time to receive the transform, and thus gives the exception.

Whatever your project class does, might be easier to do direclty with the appropriate transform (without broadcasting it first). If you want to proceed this way, maybe a waitForTransform call fixes the issue, but be aware that the transform should actually have been broadcast to be able to wait for it.

edit flag offensive delete link more

Comments

Should "waitForTransform()" be called before or after "tfb.sendTransform()"? Sorry for not getting it.
alfa_80 gravatar image alfa_80  ( 2011-12-25 20:02:40 -0500 )edit
@dornhege: I've just included the "waitForTransform". Could you please check what's wrong the use of "waitForTransform" call there.
alfa_80 gravatar image alfa_80  ( 2011-12-25 20:19:28 -0500 )edit
I think you should use the same timestamp that you broadcast the tf with. But I would advise to use the transform directly instead of sending it.
dornhege gravatar image dornhege  ( 2011-12-26 12:23:10 -0500 )edit
About this one, "use the transform directly instead of sending it", i didn't get this. Maybe if you show me with a code snippet it will be helpful.
alfa_80 gravatar image alfa_80  ( 2011-12-26 16:48:40 -0500 )edit
Basically take your code, remove the broadcaster and listener altogether and use xform instead of the listener in your projector class.
dornhege gravatar image dornhege  ( 2011-12-27 02:48:08 -0500 )edit
If it's possible, could you please edit the above code, because I can easily misunderstand it, I'm really confused with this broadcaster and listener stuff.
alfa_80 gravatar image alfa_80  ( 2011-12-27 03:05:24 -0500 )edit
Just remove the broadcaster and listeners.
dornhege gravatar image dornhege  ( 2011-12-27 06:37:50 -0500 )edit
But then, the broadcaster relates the pose messages with the point cloud to be built. How do I make use of my pose message then?
alfa_80 gravatar image alfa_80  ( 2011-12-27 07:00:34 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

Stats

Asked: 2011-12-25 04:39:11 -0500

Seen: 377 times

Last updated: Dec 28 '11