def joint_state_cb(self, msg):
if self.name_dict == None:
self.name_dict = {}
for i, n in enumerate(msg.name):
self.name_dict[n] = i
self.joint_groups = ut.load_pickle('link_names.pkl')
for group in self.joint_groups.keys():
self.joint_idx[group] = [self.name_dict[name] for name in self.joint_groups[group]]
dmat = np.matrix(msg.position).T
joint_angles = dmat[self.joint_idx['rarm'], 0].A1.tolist()
#print len(joint_angles)
#print dmat.shape, self.joint_idx['rarm']
rtip_pose = self.fkright.fk('base_footprint', 'r_wrist_roll_link', 'r_gripper_tool_frame', joint_angles)
position = rtip_pose[0:3,3]
#print position.T
pc = ru.np_to_pointcloud(position, 'base_footprint')
pc.header.stamp = rospy.get_rostime()
self.point_cloud_pub.publish(pc)
def joint_state_cb(self, msg):
if self.name_dict == None:
self.name_dict = {}
for i, n in enumerate(msg.name):
self.name_dict[n] = i
self.joint_groups = ut.load_pickle('link_names.pkl')
for group in self.joint_groups.keys():
self.joint_idx[group] = [
self.name_dict[name] for name in self.joint_groups[group]
]
dmat = np.matrix(msg.position).T
joint_angles = dmat[self.joint_idx['rarm'], 0].A1.tolist()
#print len(joint_angles)
#print dmat.shape, self.joint_idx['rarm']
rtip_pose = self.fkright.fk('base_footprint', 'r_wrist_roll_link',
'r_gripper_tool_frame', joint_angles)
position = rtip_pose[0:3, 3]
#print position.T
pc = ru.np_to_pointcloud(position, 'base_footprint')
pc.header.stamp = rospy.get_rostime()
self.point_cloud_pub.publish(pc)
# help='File name. Should be woot_150_x_reads.pkl', default='')
opt, args = p.parse_args()
fname = 'search_aware_home/woot_150_'+str(opt.trial)+'_reads.pkl'
f = open( fname, 'r' )
r = pkl.load(f)
f.close()
xyz = np.array([ [p.ps_base_map.pose.position.x,
p.ps_base_map.pose.position.y,
p.ps_base_map.pose.position.z ] for p in r ]).T
rospy.init_node( 'pub_traj' )
pts = ru.np_to_pointcloud( xyz, '/map' )
pub_pts = rospy.Publisher( '/robot_traj', sm.PointCloud )
pub_mark = rospy.Publisher( '/tag_poses', vm.Marker )
rospy.sleep( 0.5 )
tag_pts = np.array([ ahe.pts[k][1] for k in ahe.pts.keys() ]).T
tm = [ viz.single_marker( tag_pts[:,i].reshape([3,1]),
np.matrix([ [0.0], [0.0], [0.0], [1.0] ]),
'sphere', '/map',
color=[0.0, 1.0, 0.0, 1.0],
m_id=i ) for i in xrange( tag_pts.shape[1] )]
while not rospy.is_shutdown():
pts.header.stamp = rospy.Time.now()
pub_pts.publish( pts )
surf_closest3d = surf_loc3d[:, surf_closest_idx]
surf_closest_fea = sloc[surf_closest_idx]
#draw this surf feature in image
proc_surfed = fea.draw_surf(proc_surfed, [surf_closest_fea], (0, 0, 255))
import pdb
pdb.set_trace()
cv.SaveImage('proc_surfed.png', proc_surfed)
bridge = CvBridge()
image_message = bridge.cv_to_imgmsg(proc_surfed, "bgr8")
print 'init for viz'
#left_contact = np.column_stack(left_contact)
#right_contact = np.column_stack(right_contact)
left_con_pc = ru.np_to_pointcloud(left_contact, '/base_footprint')
right_con_pc = ru.np_to_pointcloud(right_contact, '/base_footprint')
print 'publishing to rviz'
r = rospy.Rate(2)
while not rospy.is_shutdown():
points_pub.publish(data_dict['points'])
touchll_pub.publish(left_con_pc)
touchlr_pub.publish(right_con_pc)
proc_pub.publish(image_message)
cam_info.publish(proc_cam_info)
r.sleep()
#image_gray = fea.grayscale(image)
#surf_keypoints, surf_descriptors = fea.surf(image_gray)
#pointcloud_base = et.pointcloud_transform * np.row_stack((point_cloud, np.matrix(np.zeros(1, point_cloud.shape[1]))))
surf_closest3d = surf_loc3d[:, surf_closest_idx]
surf_closest_fea = sloc[surf_closest_idx]
#draw this surf feature in image
proc_surfed = fea.draw_surf(proc_surfed, [surf_closest_fea], (0,0,255))
import pdb
pdb.set_trace()
cv.SaveImage('proc_surfed.png', proc_surfed )
bridge = CvBridge()
image_message = bridge.cv_to_imgmsg(proc_surfed, "bgr8")
print 'init for viz'
#left_contact = np.column_stack(left_contact)
#right_contact = np.column_stack(right_contact)
left_con_pc = ru.np_to_pointcloud(left_contact, '/base_footprint')
right_con_pc = ru.np_to_pointcloud(right_contact, '/base_footprint')
print 'publishing to rviz'
r = rospy.Rate(2)
while not rospy.is_shutdown():
points_pub.publish(data_dict['points'])
touchll_pub.publish(left_con_pc)
touchlr_pub.publish(right_con_pc)
proc_pub.publish(image_message)
cam_info.publish(proc_cam_info)
r.sleep()
#image_gray = fea.grayscale(image)
#surf_keypoints, surf_descriptors = fea.surf(image_gray)
#pointcloud_base = et.pointcloud_transform * np.row_stack((point_cloud, np.matrix(np.zeros(1, point_cloud.shape[1]))))
def publish_readings( trial, obj, pub_time = 30e3, screen_cap = False ):
loc = ( trial + obj ) % 9
fname = 'search_aware_home/woot_150_'+str(trial)+'_reads.pkl'
f = open( fname, 'r' )
r = pkl.load(f)
f.close()
rospy.init_node( 'starter_woot' )
vsm = viz.single_marker
# RFID readings
pos = [ vsm( np.matrix([ p.ps_ant_map.pose.position.x,
p.ps_ant_map.pose.position.y,
p.ps_ant_map.pose.position.z ]).T,
np.matrix([ p.ps_ant_map.pose.orientation.x,
p.ps_ant_map.pose.orientation.y,
p.ps_ant_map.pose.orientation.z,
p.ps_ant_map.pose.orientation.w ]).T,
'arrow', '/map',
color = [1.0, 0.0, 0.0, 0.8], # rgba,
duration = 10.0,
m_id = i )
for i,p in enumerate( r )
if p.read.rssi != -1 and p.read.tagID == ahe.tdb[obj][0] ]
neg = [ vsm( np.matrix([ p.ps_ant_map.pose.position.x,
p.ps_ant_map.pose.position.y,
p.ps_ant_map.pose.position.z ]).T,
np.matrix([ p.ps_ant_map.pose.orientation.x,
p.ps_ant_map.pose.orientation.y,
p.ps_ant_map.pose.orientation.z,
p.ps_ant_map.pose.orientation.w ]).T,
'arrow', '/map',
color = [0.2, 0.2, 0.2, 0.2], # rgba
duration = 10.0,
m_id = i + len(r) )
for i,p in enumerate( r )
if p.read.tagID != ahe.tdb[obj] ] # for no-read or other tag reads
print 'Pos: ', len(pos), '\nNeg: ', len(neg)
# Robot Trajectory
tm = [ vsm( np.matrix([ ahe.pts[loc][1][0],
ahe.pts[loc][1][1],
ahe.pts[loc][1][2] ]).T,
np.matrix([ [0.0], [0.0], [0.0], [1.0] ]),
'sphere', '/map',
color = [0.0, 1.0, 0.0, 1.0], # rgba
duration = 10.0,
m_id = 2*len(r) + 1 )]
xyz = np.array([ [p.ps_base_map.pose.position.x,
p.ps_base_map.pose.position.y,
p.ps_base_map.pose.position.z ] for p in r ]).T
pts = ru.np_to_pointcloud( xyz, '/map' )
pub_pts = rospy.Publisher( '/robot_traj', sm.PointCloud )
pub_mark = rospy.Publisher( '/tag_poses', vm.Marker )
# Search and Servo Positions
obj_name = ahe.tdb[obj][0] # tagID
tname = obj_name.replace( ' ', '' )
# "Best" Location determined by search
search_fname = 'search_aware_home/woot_150_' + str(trial) + '_tag_' + tname + '.yaml'
try:
f = open( search_fname )
except:
return
y = yaml.load( f )
f.close()
search = [ vsm( np.matrix([ y['pose']['position']['x'],
y['pose']['position']['y'],
y['pose']['position']['z'] ]).T,
np.matrix([ y['pose']['orientation']['x'],
y['pose']['orientation']['y'],
y['pose']['orientation']['z'],
y['pose']['orientation']['w'] ]).T,
'arrow', '/map',
scale = [0.5, 1.0, 1.0],
color = [255./255, 123./255, 1./255, 1.0], # rgba
duration = 10.0,
m_id = 2 * len(r) + 2 )]
# Location after Servoing
servo_fname = 'search_aware_home/woot_150_' + str(trial) + '_tag_' + tname + '_end.txt'
try:
f = open( servo_fname )
except:
return
y = f.readlines()
f.close()
# ['At time 1313069718.853\n',
# '- Translation: [2.811, 1.711, 0.051]\n',
# '- Rotation: in Quaternion [0.003, 0.001, -0.114, 0.993]\n',
# ' in RPY [0.005, 0.003, -0.229]\n',
# 'At time 1313069719.853\n',
# '- Translation: [2.811, 1.711, 0.051]\n',
# '- Rotation: in Quaternion [0.003, 0.001, -0.114, 0.993]\n',
# ' in RPY [0.005, 0.002, -0.229]\n']
quat = y[-2].find('Quaternion')+10
quat_list = json.loads( y[-2][quat:] )
sloc = y[-3].find('tion:')+5
sloc_list = json.loads( y[-3][sloc:] )
servo = [ vsm( np.matrix([ sloc_list ]).T,
np.matrix([ quat_list ]).T,
'arrow', '/map',
scale = [0.5, 1.0, 1.0],
color = [0./255, 205./255, 255./255, 1.0], # rgba
duration = 10.0,
m_id = 2 * len(r) + 3 )]
marks = neg + pos + tm + search + servo
t0 = time.time()
while time.time() - t0 < pub_time and not rospy.is_shutdown():
pts.header.stamp = rospy.Time.now()
pub_pts.publish( pts )
[ pub_mark.publish( x ) for x in marks ]
rospy.sleep( 1.0 )
if screen_cap:
os.system( 'scrot -d 2 -u Obj%d_Trial%d.png' % ( obj, trial ))
print 'Closing down... letting markers expire'
rospy.sleep( 15 )
def publish_readings(trial, obj, pub_time=30e3, screen_cap=False):
loc = (trial + obj) % 9
fname = 'search_aware_home/woot_150_' + str(trial) + '_reads.pkl'
f = open(fname, 'r')
r = pkl.load(f)
f.close()
rospy.init_node('starter_woot')
vsm = viz.single_marker
# RFID readings
pos = [
vsm(
np.matrix([
p.ps_ant_map.pose.position.x, p.ps_ant_map.pose.position.y,
p.ps_ant_map.pose.position.z
]).T,
np.matrix([
p.ps_ant_map.pose.orientation.x,
p.ps_ant_map.pose.orientation.y,
p.ps_ant_map.pose.orientation.z,
p.ps_ant_map.pose.orientation.w
]).T,
'arrow',
'/map',
color=[1.0, 0.0, 0.0, 0.8], # rgba,
duration=10.0,
m_id=i) for i, p in enumerate(r)
if p.read.rssi != -1 and p.read.tagID == ahe.tdb[obj][0]
]
neg = [
vsm(
np.matrix([
p.ps_ant_map.pose.position.x, p.ps_ant_map.pose.position.y,
p.ps_ant_map.pose.position.z
]).T,
np.matrix([
p.ps_ant_map.pose.orientation.x,
p.ps_ant_map.pose.orientation.y,
p.ps_ant_map.pose.orientation.z,
p.ps_ant_map.pose.orientation.w
]).T,
'arrow',
'/map',
color=[0.2, 0.2, 0.2, 0.2], # rgba
duration=10.0,
m_id=i + len(r)) for i, p in enumerate(r)
if p.read.tagID != ahe.tdb[obj]
] # for no-read or other tag reads
print 'Pos: ', len(pos), '\nNeg: ', len(neg)
# Robot Trajectory
tm = [
vsm(
np.matrix(
[ahe.pts[loc][1][0], ahe.pts[loc][1][1],
ahe.pts[loc][1][2]]).T,
np.matrix([[0.0], [0.0], [0.0], [1.0]]),
'sphere',
'/map',
color=[0.0, 1.0, 0.0, 1.0], # rgba
duration=10.0,
m_id=2 * len(r) + 1)
]
xyz = np.array([[
p.ps_base_map.pose.position.x, p.ps_base_map.pose.position.y,
p.ps_base_map.pose.position.z
] for p in r]).T
pts = ru.np_to_pointcloud(xyz, '/map')
pub_pts = rospy.Publisher('/robot_traj', sm.PointCloud)
pub_mark = rospy.Publisher('/tag_poses', vm.Marker)
# Search and Servo Positions
obj_name = ahe.tdb[obj][0] # tagID
tname = obj_name.replace(' ', '')
# "Best" Location determined by search
search_fname = 'search_aware_home/woot_150_' + str(
trial) + '_tag_' + tname + '.yaml'
try:
f = open(search_fname)
except:
return
y = yaml.load(f)
f.close()
search = [
vsm(
np.matrix([
y['pose']['position']['x'], y['pose']['position']['y'],
y['pose']['position']['z']
]).T,
np.matrix([
y['pose']['orientation']['x'], y['pose']['orientation']['y'],
y['pose']['orientation']['z'], y['pose']['orientation']['w']
]).T,
'arrow',
'/map',
scale=[0.5, 1.0, 1.0],
color=[255. / 255, 123. / 255, 1. / 255, 1.0], # rgba
duration=10.0,
m_id=2 * len(r) + 2)
]
# Location after Servoing
servo_fname = 'search_aware_home/woot_150_' + str(
trial) + '_tag_' + tname + '_end.txt'
try:
f = open(servo_fname)
except:
return
y = f.readlines()
f.close()
# ['At time 1313069718.853\n',
# '- Translation: [2.811, 1.711, 0.051]\n',
# '- Rotation: in Quaternion [0.003, 0.001, -0.114, 0.993]\n',
# ' in RPY [0.005, 0.003, -0.229]\n',
# 'At time 1313069719.853\n',
# '- Translation: [2.811, 1.711, 0.051]\n',
# '- Rotation: in Quaternion [0.003, 0.001, -0.114, 0.993]\n',
# ' in RPY [0.005, 0.002, -0.229]\n']
quat = y[-2].find('Quaternion') + 10
quat_list = json.loads(y[-2][quat:])
sloc = y[-3].find('tion:') + 5
sloc_list = json.loads(y[-3][sloc:])
servo = [
vsm(
np.matrix([sloc_list]).T,
np.matrix([quat_list]).T,
'arrow',
'/map',
scale=[0.5, 1.0, 1.0],
color=[0. / 255, 205. / 255, 255. / 255, 1.0], # rgba
duration=10.0,
m_id=2 * len(r) + 3)
]
marks = neg + pos + tm + search + servo
t0 = time.time()
while time.time() - t0 < pub_time and not rospy.is_shutdown():
pts.header.stamp = rospy.Time.now()
pub_pts.publish(pts)
[pub_mark.publish(x) for x in marks]
rospy.sleep(1.0)
if screen_cap:
os.system('scrot -d 2 -u Obj%d_Trial%d.png' % (obj, trial))
print 'Closing down... letting markers expire'
rospy.sleep(15)
opt, args = p.parse_args()
fname = 'search_aware_home/woot_150_' + str(opt.trial) + '_reads.pkl'
f = open(fname, 'r')
r = pkl.load(f)
f.close()
xyz = np.array([[
p.ps_base_map.pose.position.x, p.ps_base_map.pose.position.y,
p.ps_base_map.pose.position.z
] for p in r]).T
rospy.init_node('pub_traj')
pts = ru.np_to_pointcloud(xyz, '/map')
pub_pts = rospy.Publisher('/robot_traj', sm.PointCloud)
pub_mark = rospy.Publisher('/tag_poses', vm.Marker)
rospy.sleep(0.5)
tag_pts = np.array([ahe.pts[k][1] for k in ahe.pts.keys()]).T
tm = [
viz.single_marker(tag_pts[:, i].reshape([3, 1]),
np.matrix([[0.0], [0.0], [0.0], [1.0]]),
'sphere',
'/map',
color=[0.0, 1.0, 0.0, 1.0],
m_id=i) for i in xrange(tag_pts.shape[1])
]
while not rospy.is_shutdown():
def subsample(self, points3d, frame='base_link'):
req = fsrv.SubsampleCalcRequest()
req.input = ru.np_to_pointcloud(points3d, frame)
res = self.proxy(req)
return ru.pointcloud_to_np(res.output)
def subsample(self, points3d, frame="base_link"):
req = fsrv.SubsampleCalcRequest()
req.input = ru.np_to_pointcloud(points3d, frame)
res = self.proxy(req)
return ru.pointcloud_to_np(res.output)
