def publish_cartesian_markers(arm, time_stamp, cep, rot, c1, c2, marker_id):
marker = Marker()
marker.header.frame_id = ar.link_tf_name(arm, 0)
marker.header.stamp = time_stamp
marker.ns = arm
marker.type = Marker.ARROW
marker.action = Marker.ADD
marker.pose.position.x = cep[0,0]
marker.pose.position.y = cep[1,0]
marker.pose.position.z = cep[2,0]
marker.scale.x = 0.1
marker.scale.y = 0.2
marker.scale.z = 0.2
marker.lifetime = rospy.Duration()
marker.id = marker_id*100 + 0
#rot1 = tr.Ry(math.radians(90.)) * rot.T
rot1 = rot * tr.rotY(math.pi/2)
quat = tr.matrix_to_quaternion(rot1)
marker.pose.orientation.x = quat[0]
marker.pose.orientation.y = quat[1]
marker.pose.orientation.z = quat[2]
marker.pose.orientation.w = quat[3]
marker.color.r = c1[0]
marker.color.g = c1[1]
marker.color.b = c1[2]
marker.color.a = 1.
marker_pub.publish(marker)
marker.id = marker_id*100 + 1
if arm == 'left_arm':
#rot2 = tr.Rz(math.radians(90.)) * rot.T
rot2 = rot * tr.rotZ(-math.pi/2)
else:
#rot2 = tr.Rz(math.radians(-90.)) * rot.T
rot2 = rot * tr.rotZ(math.pi/2)
quat = tr.matrix_to_quaternion(rot2)
marker.pose.orientation.x = quat[0]
marker.pose.orientation.y = quat[1]
marker.pose.orientation.z = quat[2]
marker.pose.orientation.w = quat[3]
marker.color.r = c2[0]
marker.color.g = c2[1]
marker.color.b = c2[2]
marker.color.a = 1.
marker_pub.publish(marker)
def publish_cartesian_markers(arm, time_stamp, cep, rot, c1, c2, marker_id):
marker = Marker()
marker.header.frame_id = ar.link_tf_name(arm, 0)
marker.header.stamp = time_stamp
marker.ns = arm
marker.type = Marker.ARROW
marker.action = Marker.ADD
marker.pose.position.x = cep[0, 0]
marker.pose.position.y = cep[1, 0]
marker.pose.position.z = cep[2, 0]
marker.scale.x = 0.1
marker.scale.y = 0.2
marker.scale.z = 0.2
marker.lifetime = rospy.Duration()
marker.id = marker_id * 100 + 0
#rot1 = tr.Ry(math.radians(90.)) * rot.T
rot1 = rot * tr.rotY(math.pi / 2)
quat = tr.matrix_to_quaternion(rot1)
marker.pose.orientation.x = quat[0]
marker.pose.orientation.y = quat[1]
marker.pose.orientation.z = quat[2]
marker.pose.orientation.w = quat[3]
marker.color.r = c1[0]
marker.color.g = c1[1]
marker.color.b = c1[2]
marker.color.a = 1.
marker_pub.publish(marker)
marker.id = marker_id * 100 + 1
if arm == 'left_arm':
#rot2 = tr.Rz(math.radians(90.)) * rot.T
rot2 = rot * tr.rotZ(-math.pi / 2)
else:
#rot2 = tr.Rz(math.radians(-90.)) * rot.T
rot2 = rot * tr.rotZ(math.pi / 2)
quat = tr.matrix_to_quaternion(rot2)
marker.pose.orientation.x = quat[0]
marker.pose.orientation.y = quat[1]
marker.pose.orientation.z = quat[2]
marker.pose.orientation.w = quat[3]
marker.color.r = c2[0]
marker.color.g = c2[1]
marker.color.b = c2[2]
marker.color.a = 1.
marker_pub.publish(marker)
def process_read( pf, p_start, m ):
# Note: p_start is numpy array (Nx3) and it's weights ([:,2]) will be modified in-place!
# m is form: RFIDread, PoseStamped (ant in map), PoseStamped (robot base in map)
# We assume it is a postive reading for the desired tagID!
xy_map = np.copy( p_start[:,0:2].T ) # 2xN
# Antenna is at position given by m.ps_ant_map.
# IMPORTANT NOTE: I'm assuming that the antenna XY plane is parallel to the map XY plane!
o = m.ps_ant_map.pose.orientation
d = m.ps_ant_map.pose.position
roll, pitch, yaw = tft.euler_from_quaternion([ o.x, o.y, o.z, o.w ])
dx, dy = d.x, d.y
# Transform xy_map points into antenna frame
t_new = tr.composeHomogeneousTransform( tr.rotZ( yaw ),
np.matrix([dx,dy,0.0]).T )
trans = tr.invertHomogeneousTransform( t_new )
xy_new = (trans * tr.xyToHomogenous( xy_map ))[0:2] # This is a matrix! (2xN)
xy_new = np.array( xy_new ) # Convert it back to array!
# Build new particles using old weights (Nx3)
p_new = np.column_stack([ xy_new.T, p_start[:,2] ]) # take weights from last iteration
rv = pf.step( 0.0, m.read.rssi, p_new, should_resample_func = pfilter.retFalse )
# Put back the new weights:
p_start[:,2] = rv[:,2]
return
def grid_lines(self, rotation_angle=0.):
grid_size = np.multiply(self.grid_shape,self.resolution)
rot_mat = tr.rotZ(rotation_angle)
p5 = self.tlb
p6 = p5+np.matrix([0.,-grid_size[1,0],0.]).T
p8 = p5+np.matrix([0.,0.,-grid_size[2,0]]).T
p7 = p8+np.matrix([0.,-grid_size[1,0],0.]).T
p3 = self.brf
p4 = p3+np.matrix([0.,grid_size[1,0],0.]).T
p2 = p3+np.matrix([0.,0.,grid_size[2,0]]).T
p1 = p2+np.matrix([0.,grid_size[1,0],0.]).T
p1 = rot_mat*p1
p2 = rot_mat*p2
p3 = rot_mat*p3
p4 = rot_mat*p4
p5 = rot_mat*p5
p6 = rot_mat*p6
p7 = rot_mat*p7
p8 = rot_mat*p8
l = [(p1,p2),(p1,p4),(p2,p3),(p3,p4),(p5,p6),(p6,p7),(p7,p8),(p8,p5),(p1,p5),(p2,p6),(p4,p8),(p3,p7)]
#l = [(p5,p6),(p5,p3),(p1,p2)]
return l
def grid_lines(self, rotation_angle=0.):
grid_size = np.multiply(self.grid_shape, self.resolution)
rot_mat = tr.rotZ(rotation_angle)
p5 = self.tlb
p6 = p5 + np.matrix([0., -grid_size[1, 0], 0.]).T
p8 = p5 + np.matrix([0., 0., -grid_size[2, 0]]).T
p7 = p8 + np.matrix([0., -grid_size[1, 0], 0.]).T
p3 = self.brf
p4 = p3 + np.matrix([0., grid_size[1, 0], 0.]).T
p2 = p3 + np.matrix([0., 0., grid_size[2, 0]]).T
p1 = p2 + np.matrix([0., grid_size[1, 0], 0.]).T
p1 = rot_mat * p1
p2 = rot_mat * p2
p3 = rot_mat * p3
p4 = rot_mat * p4
p5 = rot_mat * p5
p6 = rot_mat * p6
p7 = rot_mat * p7
p8 = rot_mat * p8
l = [(p1, p2), (p1, p4), (p2, p3), (p3, p4), (p5, p6), (p6, p7),
(p7, p8), (p8, p5), (p1, p5), (p2, p6), (p4, p8), (p3, p7)]
#l = [(p5,p6),(p5,p3),(p1,p2)]
return l
def process_read(pf, p_start, m):
# Note: p_start is numpy array (Nx3) and it's weights ([:,2]) will be modified in-place!
# m is form: RFIDread, PoseStamped (ant in map), PoseStamped (robot base in map)
# We assume it is a postive reading for the desired tagID!
xy_map = np.copy(p_start[:, 0:2].T) # 2xN
# Antenna is at position given by m.ps_ant_map.
# IMPORTANT NOTE: I'm assuming that the antenna XY plane is parallel to the map XY plane!
o = m.ps_ant_map.pose.orientation
d = m.ps_ant_map.pose.position
roll, pitch, yaw = tft.euler_from_quaternion([o.x, o.y, o.z, o.w])
dx, dy = d.x, d.y
# Transform xy_map points into antenna frame
t_new = tr.composeHomogeneousTransform(tr.rotZ(yaw),
np.matrix([dx, dy, 0.0]).T)
trans = tr.invertHomogeneousTransform(t_new)
xy_new = (trans *
tr.xyToHomogenous(xy_map))[0:2] # This is a matrix! (2xN)
xy_new = np.array(xy_new) # Convert it back to array!
# Build new particles using old weights (Nx3)
p_new = np.column_stack([xy_new.T,
p_start[:,
2]]) # take weights from last iteration
rv = pf.step(0.0,
m.read.rssi,
p_new,
should_resample_func=pfilter.retFalse)
# Put back the new weights:
p_start[:, 2] = rv[:, 2]
return
X,Y = np.meshgrid( np.arange(-10,10,0.1),
np.arange(-10,10,0.1) )
xyw = np.row_stack([ X.flatten(), # Build Nx3
Y.flatten(),
np.ones( X.shape ).flatten() ]).T # weights (multiplicative)
p_set = np.copy( xyw )
pf = pfilter.PFilter( rfid_model.NoMotion(), rfid_model.RfidModel( rfid_model.yaml_fname ))
t0 = time.time()
rv = pf.step( 0.0, 83, p_set, should_resample_func = pfilter.retFalse ) # rv are the new weights
# New antenna is at position (5,-5) and rotated 90-deg. relative to MAP frame
t_new = tr.composeHomogeneousTransform( tr.rotZ( math.radians(90) ), np.matrix([5,-5,0]).T )
# This transform takes points from map frame into t_new:
trans = tr.invertHomogeneousTransform( t_new )
xy_new = np.array((trans * tr.xyToHomogenous( xyw[:,0:2].T) )[0:2].T)
xyw_new = np.column_stack([ xy_new, rv[:,2] ]) # take weights from last iteration
rv = pf.step( 0.0, 83, xyw_new, should_resample_func = pfilter.retFalse ) # rv are the new weights
t1 = time.time()
print 'Time: %3.2f ' % (t1 - t0)
to_print = np.copy( xyw )
to_print[:,2] = rv[:,2] # Just keep weights, but use original points in "map" frame
np.ones(X.shape).flatten()
]).T # weights (multiplicative)
xyw_orig = np.copy(xyw)
rm = rfid_model.RfidModel(rfid_model.yaml_fname)
# pf = lib_pfilter.PFilter( lib_pfilter.NoMotion(), rm, xyw )
# pf.measurement( 80 )
# Antenna reading at origin (max near 4,0)
# rv = np.copy( xyw )
rv = rm.weight_set(83, xyw)
# Move antenna to 4,-4 and rotated 90 deg
xy = xyw[:, 0:2]
trans = tr.composeHomogeneousTransform(tr.rotZ(math.radians(-90)),
np.matrix([5, 5, 0]).T)
xy_new = np.array((trans * tr.xyToHomogenous(xy.T))[0:2].T)
xyw_new = np.column_stack([xy_new, rv[:,
2]]) # take weights from last iteration
rv_new = rm.weight_set(83, xyw_new)
to_print = np.copy(xyw_orig)
to_print[:,
2] = rv_new[:,
2] # Just keep weights, but use original points in "map" frame
dw = DisplayWeights()
while not rospy.is_shutdown():
np.ones( X.shape ).flatten() ]).T # weights (multiplicative)
xyw_orig = np.copy( xyw )
rm = rfid_model.RfidModel( rfid_model.yaml_fname )
# pf = lib_pfilter.PFilter( lib_pfilter.NoMotion(), rm, xyw )
# pf.measurement( 80 )
# Antenna reading at origin (max near 4,0)
# rv = np.copy( xyw )
rv = rm.weight_set( 83, xyw )
# Move antenna to 4,-4 and rotated 90 deg
xy = xyw[:,0:2]
trans = tr.composeHomogeneousTransform( tr.rotZ( math.radians(-90) ), np.matrix([5,5,0]).T )
xy_new = np.array((trans * tr.xyToHomogenous( xy.T) )[0:2].T)
xyw_new = np.column_stack([ xy_new, rv[:,2] ]) # take weights from last iteration
rv_new = rm.weight_set( 83, xyw_new )
to_print = np.copy( xyw_orig )
to_print[:,2] = rv_new[:,2] # Just keep weights, but use original points in "map" frame
dw = DisplayWeights()
while not rospy.is_shutdown():
print 'UPDATING'
dw.update( to_print )
