Ask Your Question
0

pointcloud2 and path planning

asked 2018-05-11 01:31:47 -0600

lr101095 gravatar image

updated 2018-05-15 23:25:31 -0600

currently running ubuntu 16.04, ros kinetic, using gazebo 7 simulation with a sawyer (intera 5.2), running kinect camera.

I want to take a point from a point cloud (type PointCloud2) taken from the kinect frame. point would be of type PointXYZRGBNormal. I want to take the point, use a transformation to get the point in the world frame, then input that into a script i wrote that performs inverse kinematics to determine a pose for sawyer.

My script takes a point input in the form of a position (xyz) and a quaternion (xyzw). I also want to be able to subscribe to the pointcloud rostopic and index through the points to select a single point and its relevant data.

can anyone suggest how to take the PointXYZRGBNormal and convert the information to a position and quaternion in the world frame?

Please let me know if more information is required.

Any help/suggestions would be much appreciated.

~ Luke

EDIT:

I want to be able to transform the kinect point cloud from the camera frame to the world frame. At this stage, i can't even extract a point from the point cloud to transform. after transforming the point cloud, i want to be able to select a point, then perform inverse kinematics to move to that point (i'm already able to perform IK to determine a pose).

I've tried to use transformPointCloud in python. See code below:

    def transform_cloud(self):
    tfBuffer = tf2_ros.Buffer()
    listener = tf2_ros.TransformListener(tfBuffer)
    transformer = tf.TransformerROS(True,rospy.Duration(1.0))
    pcl_sub = rospy.Subscriber("/downsampled_cloud", PointCloud2)
    pub_trans = rospy.Publisher('pub_trans', TransformStamped, queue_size = 10)
    trans_cloud = rospy.Publisher('trans_cloud', PointCloud2, queue_size = 10)

    rate = rospy.Rate(1)
    while not rospy.is_shutdown():
        try:
            trans = tfBuffer.lookup_transform('base', 'depth_link', rospy.Time.now(), rospy.Duration(1.0))
            print type(trans)
            pub_trans.publish(trans)
            cloud_trans = transformer.transformPointCloud('base', pcl_sub)
            trans_cloud.publish(cloud_trans)
        except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
            print("lookup failed")
            pass
        rate.sleep()

Doing this yields the following in terminal:

File "/opt/ros/kinetic/lib/python2.7/dist-packages/tf/listener.py", line 325, in transformPointCloud
    r.header.stamp = point_cloud.header.stamp
AttributeError: 'Subscriber' object has no attribute 'header'

It is worth noting that the PointCloud i'm trying to transform (/downsampled_cloud) does have a header. So i'm not sure why it's saying the subscriber doesn't have a header attribute.

Any help is much appreciated.

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2018-05-11 03:02:36 -0600

One problem you will have is that a point and a normal vector do not fully define a 6 FOF pose. For this reason you cannot convert a normal into a quaternion without an addition constraint that defines the rotation about the axis of the normal.

Converting the point and normal into the a world frame is easier though. You can lookup the transform from the frame_id of the point cloud to the world frame. Then you'll need to apply it in two stages:

1) Make a point object from the x y z elements of the cloud point and transform it.

2) Zero the translation of the transform (so it's a pure rotation) then apply it to the normal.

Hope this helps.

edit flag offensive delete link more

Comments

hi, yes this really helps. i've got a better understanding of what i need to do now. I will reach out, however, if i need a bit more help. thanks again!

lr101095 gravatar imagelr101095 ( 2018-05-13 20:04:34 -0600 )edit

@PeteBlackerThe3rd i've been trying to look for a usable example within the tf and tf2 tutorials, but i cant seem to find one that explains it very well in the context that i require. Would you be able to suggest any examples that does what i need tf or tf2 to do?

lr101095 gravatar imagelr101095 ( 2018-05-13 22:19:50 -0600 )edit

Are you trying to transform a point and normal now or how to define a 6 DOF pose for the arm.

PeteBlackerThe3rd gravatar imagePeteBlackerThe3rd ( 2018-05-14 07:01:57 -0600 )edit

i want to be able to transform a point and normal from the camera frame to the base frame of a 7dof arm. i already have a working script that determines pose, so i just need an input for the ik service i'm using.

lr101095 gravatar imagelr101095 ( 2018-05-14 18:47:44 -0600 )edit

I don't think there are any examples of exactly this. You'll have to get your hands dirty and make it yourself. You need to start with a transform listener to find the TF from the camera frame to the base frame. ...

PeteBlackerThe3rd gravatar imagePeteBlackerThe3rd ( 2018-05-16 14:17:52 -0600 )edit

Then you'll need to perform steps 1 and 2 I described above. This will result in the point and normal now correctly transformed into the base frame.

PeteBlackerThe3rd gravatar imagePeteBlackerThe3rd ( 2018-05-16 14:18:38 -0600 )edit

I've been trying to use transformPointCloud in python, but this function isn't compatible with PointCloud2. Do you have any suggestions for how to transform PC2? I already have a tf_listener in the frames i need, so i'm okay with that part

lr101095 gravatar imagelr101095 ( 2018-05-16 20:09:32 -0600 )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

2 followers

Stats

Asked: 2018-05-11 01:31:47 -0600

Seen: 275 times

Last updated: May 15 '18