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