# pointcloud2 and path planning

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?

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
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 close merge delete

Sort by » oldest newest most voted

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.

more

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!

( 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?

( 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.

( 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.

( 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. ...

( 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.

( 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

( 2018-05-16 20:09:32 -0600 )edit