def callback(self, cloud_msg):
'''Function that the subscriber calls when it gets a new message'''
# print 'callback fn from ptcloud_listener called'
# rospy.loginfo( rospy.get_name() + ": pointcloud rcvd from %s" \
# %str(cloud_msg.PointField.name) )
## rospy.loginfo( rospy.get_name() + ": pointcloud msg rcvd" )
# print 'rospy logging successful'
if self.cloud_arr == []:
self.cloud_arr = pointclouds.pointcloud2_to_array( cloud_msg,\
split_rgb=True )
self.cloud_arr = self.cloud_arr[np.isfinite(self.cloud_arr['z'])]
elif self.ctr % 10 == 0:
cloud_arr = pointclouds.pointcloud2_to_array(cloud_msg,
split_rgb=True)
cloud_arr = cloud_arr[np.isfinite(cloud_arr['z'])]
self.cloud_arr = np.concatenate((self.cloud_arr, cloud_arr))
self.ctr += 1
# if not os.path.exists( 'ptcloud.npy' ):
# print 'pointcloud conversion successful'
# print 'Frame id = ', str(cloud_msg.header.frame_id)
# if str( cloud_msg.header.frame_id ) == '1':
print 'Saving ptcld...'
np.save('/home/grim4/Desktop/ptcloud.npy', self.cloud_arr)
print 'Done.'
def callback( self, cloud_msg ):
'''Function that the subscriber calls when it gets a new message'''
# Get and format the point cloud
self.cloud_arr = pointclouds.pointcloud2_to_array( cloud_msg,\
split_rgb=True )
self.cloud_arr = self.cloud_arr[ np.isfinite( self.cloud_arr['z'] ) ]
self.ctr += 1
# Send the pcld out
print 'Sending ptcloud out on network...'
tic = rospy.get_time()
self.emitter.send_zipped_pickle( self.cloud_arr[::10], self.topic )
toc = rospy.get_time()
print 'Ptcld sent. Elapsed time = %s' %(toc-tic)
def cloud_cb(cloud_msg):
rospy.loginfo('Filtering cloud')
cloud_arr = pointclouds.pointcloud2_to_array(cloud_msg, split_rgb=True)
# filter points by color
filtered_cloud_arr = cloud_arr[(cloud_arr['r'] < 128) * (cloud_arr['g'] < 50) * (cloud_arr['b'] < 50)]
# filter out far away points
filtered_cloud_arr = filtered_cloud_arr[filtered_cloud_arr['z'] < 1.5]
filtered_cloud_msg = pointclouds.array_to_pointcloud2(
filtered_cloud_arr, stamp=cloud_msg.header.stamp, frame_id=cloud_msg.header.frame_id, merge_rgb=True)
cloud_pub.publish(filtered_cloud_msg)
def callback( self, cloud_msg ):
'''Function that the subscriber calls when it gets a new message'''
# print 'callback fn from ptcloud_listener called'
# rospy.loginfo( rospy.get_name() + ": pointcloud rcvd from %s" \
# %str(cloud_msg.PointField.name) )
## rospy.loginfo( rospy.get_name() + ": pointcloud msg rcvd" )
# print 'rospy logging successful'
if self.cloud_arr == []:
self.cloud_arr = pointclouds.pointcloud2_to_array( cloud_msg,\
split_rgb=True )
self.cloud_arr = self.cloud_arr[ np.isfinite( self.cloud_arr['z'] ) ]
elif self.ctr % 10 == 0:
cloud_arr = pointclouds.pointcloud2_to_array( cloud_msg, split_rgb=True )
cloud_arr = cloud_arr[ np.isfinite( cloud_arr['z'] ) ]
self.cloud_arr = np.concatenate( (self.cloud_arr, cloud_arr) )
self.ctr += 1
# if not os.path.exists( 'ptcloud.npy' ):
# print 'pointcloud conversion successful'
# print 'Frame id = ', str(cloud_msg.header.frame_id)
# if str( cloud_msg.header.frame_id ) == '1':
np.save( '/home/grim4/Desktop/ptcloud.npy', self.cloud_arr )
def callback(self, cloud_msg):
'''Function that the subscriber calls when it gets a new message'''
# Get and format the point cloud
self.cloud_arr = pointclouds.pointcloud2_to_array( cloud_msg,\
split_rgb=True )
self.cloud_arr = self.cloud_arr[np.isfinite(self.cloud_arr['z'])]
self.ctr += 1
# Send the pcld out
print 'Sending ptcloud out on network...'
tic = rospy.get_time()
self.emitter.send_zipped_pickle(self.cloud_arr[::10], self.topic)
toc = rospy.get_time()
print 'Ptcld sent. Elapsed time = %s' % (toc - tic)
def getImageOfObsObject(self, index):
#Move to the grabbing position
sm = smach.Sequence(outcomes=['succeeded', 'preempted', 'aborted'], connector_outcome='succeeded')
sm.userdata.goal_r1 = MA1400JointState()
sm.userdata.goal_r2 = MA1400JointState()
sm.userdata.goal_r2.s = -0.30
sm.userdata.goal_r2.l = 0.30
sm.userdata.goal_r2.u = -0.14
sm.userdata.goal_r2.r = 0.00
sm.userdata.goal_r2.b = 0.33
sm.userdata.goal_r2.t = 0.00
sm_go_home = gensm_plan_vis_exec(Plan2ToJointsState(), servo_off=True, input_keys=['goal_r1', 'goal_r2'])
with sm:
smach.StateMachine.add('GO_HOME', sm_go_home)
smach.StateMachine.add('PAUSE', PauseState(2))
smach.StateMachine.add('CLOSE_GRIPPERS', gensm_grippers(False, False))
outcome = sm.execute()
takenImage = None
self.lastImageIndex = index
logging.info("Get image - waiting for msg")
pcData = rospy.wait_for_message("/xtion2/depth_registered/points", PointCloud2)
self.pc_frame_id = pcData.header.frame_id
#convert it to format accepted by openCV
logging.info("Get image - converting")
arr = pointclouds.pointcloud2_to_array(pcData, split_rgb=self.has_rgb(pcData))
if arr == None:
rospy.logerr(rospy.get_name() + ": Failed to convert PoinCloud to numpy array!")
return False
if self.has_fields(['x', 'y', 'z'], arr):
xyz = np.zeros(list(arr.shape) + [3], dtype=np.float32)
xyz[..., 0] = arr['x']
xyz[..., 1] = arr['y']
xyz[..., 2] = arr['z']
self.pcMap = xyz
if self.has_fields(['r', 'g', 'b'], arr):
rgb = np.zeros(list(arr.shape) + [3], dtype=np.uint8)
rgb[..., 0] = arr['b']
rgb[..., 1] = arr['g']
rgb[..., 2] = arr['r']
takenImage = cv.fromarray(rgb)
logging.debug("Get image - completed")
return takenImage
def callback(self, cloud_msg):
'''Callback function for dealing with every new received message.'''
# Get Timestamp of cloud
# ts = cloud_msg.header.stamp
# # Get transform for batch cloud at given time (NEED TRY-EXCEPT?)
# (trans, rot) = self.tf_listener.lookupTransform('/map', '/camera_link', ts )
# # Convert Transformation to R|T matrix form
# RT = np.zeros( (3,4) )
# RT[0:3,0:3] = tf.transformations.quaternion_matrix( rot )[0:3,0:3]
# RT[:,-1] = trans
# # Convert cloud to numpy array
cloud_arr = pointclouds.pointcloud2_to_array(cloud_msg, split_rgb=True)
# Get rid of z=nan points
cloud_arr = cloud_arr[np.isfinite(cloud_arr['z'])]
# Get the coords of points in the pc
# xyz = np.ones( (cloud_arr.shape[0], 3) )
# xyz[:,0] = cloud_arr['x']
# xyz[:,1] = cloud_arr['y']
# xyz[:,2] = cloud_arr['z']
# # Transform the pointcloud
## nxyz = RT.dot( xyz.T )
## nxyz = nxyz.T
# # Get the colors
# clrs = 255 * np.ones( (xyz.shape[0],4) )
# clrs[:,0] = cloud_arr['r']
# clrs[:,1] = cloud_arr['g']
# clrs[:,2] = cloud_arr['b']
# clrs /= 255
# Increment counter
# xyz[:,[0,1]] = xyz[:,[1,0]]
# xyz[:,2] *= -1
# print 'imgPts', self.imgPts.shape, 'nxyz', nxyz.shape
# print 'colors', self.colors.shape, 'clrs', clrs.shape
# self.imgPts = np.concatenate( (self.imgPts, xyz) )
# self.colors = np.concatenate( (self.colors, clrs) )
# TEST:
# now = rospy.Time.now()
# print
# print '%s\t%s\t%s' %( ts.to_sec(), now.to_sec(), now.to_sec()-ts.to_sec() )
## print trans
# print RT
# print 'num points in cloud = (%s, %s)' %( xyz.shape[0], xyz.shape[1] )
## print 'Transformation time = %s' %(toc - tic)
# print
print 'cloud %s rcvd' % (self.ctr)
np.save('/home/grim4/Desktop/cloud_debug/cloud_%s.npy' % (self.ctr),
cloud_arr)
self.ctr += 1
def callback( self, cloud_msg ):
'''Callback function for dealing with every new received message.'''
# Get Timestamp of cloud
# ts = cloud_msg.header.stamp
# # Get transform for batch cloud at given time (NEED TRY-EXCEPT?)
# (trans, rot) = self.tf_listener.lookupTransform('/map', '/camera_link', ts )
# # Convert Transformation to R|T matrix form
# RT = np.zeros( (3,4) )
# RT[0:3,0:3] = tf.transformations.quaternion_matrix( rot )[0:3,0:3]
# RT[:,-1] = trans
# # Convert cloud to numpy array
cloud_arr = pointclouds.pointcloud2_to_array( cloud_msg, split_rgb=True )
# Get rid of z=nan points
cloud_arr = cloud_arr[ np.isfinite( cloud_arr['z'] ) ]
# Get the coords of points in the pc
# xyz = np.ones( (cloud_arr.shape[0], 3) )
# xyz[:,0] = cloud_arr['x']
# xyz[:,1] = cloud_arr['y']
# xyz[:,2] = cloud_arr['z']
# # Transform the pointcloud
## nxyz = RT.dot( xyz.T )
## nxyz = nxyz.T
# # Get the colors
# clrs = 255 * np.ones( (xyz.shape[0],4) )
# clrs[:,0] = cloud_arr['r']
# clrs[:,1] = cloud_arr['g']
# clrs[:,2] = cloud_arr['b']
# clrs /= 255
# Increment counter
# xyz[:,[0,1]] = xyz[:,[1,0]]
# xyz[:,2] *= -1
# print 'imgPts', self.imgPts.shape, 'nxyz', nxyz.shape
# print 'colors', self.colors.shape, 'clrs', clrs.shape
# self.imgPts = np.concatenate( (self.imgPts, xyz) )
# self.colors = np.concatenate( (self.colors, clrs) )
# TEST:
# now = rospy.Time.now()
# print
# print '%s\t%s\t%s' %( ts.to_sec(), now.to_sec(), now.to_sec()-ts.to_sec() )
## print trans
# print RT
# print 'num points in cloud = (%s, %s)' %( xyz.shape[0], xyz.shape[1] )
## print 'Transformation time = %s' %(toc - tic)
# print
print 'cloud %s rcvd' %(self.ctr)
np.save('/home/grim4/Desktop/cloud_debug/cloud_%s.npy'%(self.ctr), cloud_arr)
self.ctr += 1
def test_roundtrip():
from python_msg_conversions import pointclouds
import numpy as np
npoints = 100
points_arr = np.zeros((npoints,), dtype=[
('x', np.float32),
('y', np.float32),
('z', np.float32),
('r', np.uint8),
('g', np.uint8),
('b', np.uint8)])
points_arr['x'] = np.random.random((npoints,))
points_arr['y'] = np.random.random((npoints,))
points_arr['z'] = np.random.random((npoints,))
points_arr['r'] = np.floor(np.random.random((npoints,))*255)
points_arr['g'] = 0
points_arr['b'] = 255
cloud_msg = pointclouds.array_to_pointcloud2(points_arr, merge_rgb=True)
new_points_arr = pointclouds.pointcloud2_to_array(cloud_msg, split_rgb=True)
assert(point_arrays_equal(points_arr, new_points_arr))
