python PointCloud2 read_points() problem
I'm working on a kinect project. I'm trying to write a pair of files that test out the PointCloud2 sensor message. I don't know how to make the 'listener' python script read the value that I have created in the 'talktest' script. I am using the class defined here:
sensor_msgs/point_cloud2.py (I could not post a link)
This is my code. These are my includes. I use them pretty much the same in both files.
#!/usr/bin/env python
import rospy
import sensor_msgs.point_cloud2 as pc2
from sensor_msgs.msg import PointCloud2, PointField
This is the 'talktest' code.
#talktest
def test():
rospy.init_node('talktest', anonymous=True)
pub_cloud = rospy.Publisher("camera/depth_registered/points", PointCloud2)
while not rospy.is_shutdown():
pcloud = PointCloud2()
# make point cloud
cloud = [[33,22,11],[55,33,22],[33,22,11]]
pcloud = pc2.create_cloud_xyz32(pcloud.header, cloud)
pub_cloud.publish(pcloud)
rospy.loginfo(pcloud)
rospy.sleep(1.0)
if __name__ == '__main__':
try:
test()
except rospy.ROSInterruptException:
pass
Then I have written a script that listens for point cloud data. It always throws a AttributeError.
#listener
def listen():
rospy.init_node('listen', anonymous=True)
rospy.Subscriber("camera/depth_registered/points", PointCloud2, callback_kinect)
def callback_kinect(data) :
# pick a height
height = int (data.height / 2)
# pick x coords near front and center
middle_x = int (data.width / 2)
# examine point
middle = read_depth (middle_x, height, data)
# do stuff with middle
def read_depth(width, height, data) :
# read function
if (height >= data.height) or (width >= data.width) :
return -1
data_out = pc2.read_points(data, field_names=None, skip_nans=False, uvs=[width, height])
int_data = next(data_out)
rospy.loginfo("int_data " + str(int_data))
return int_data
if __name__ == '__main__':
try:
listen()
except rospy.ROSInterruptException:
pass
I always get this error:
[ERROR] [WallTime: 1389040976.560028] bad callback: <function callback_kinect="" at="" 0x2a51b18=""> Traceback (most recent call last):
File "/opt/ros/hydro/lib/python2.7/dist-packages/rospy/topics.py", line 681, in _invoke_callback cb(msg) File "./turtlebot_listen.py", line 98, in callback_kinect left = read_depth (left_x, height, data) File "./turtlebot_listen.py", line 126, in read_depth int_data = next(data_out) File "/opt/ros/hydro/lib/python2.7/dist-packages/sensor_msgs/point_cloud2.py", line 74, in read_points assert isinstance(cloud, roslib.message.Message) and cloud._type == 'sensor_msgs/PointCloud2', 'cloud is not a sensor_msgs.msg.PointCloud2' AttributeError: 'module' object has no attribute 'message'
Can anyone tell me how to use this library correctly?