def get_arrow_text_markers(p, f, frame, m_id, duration, c=(1.,0.,0.,1.0)):
t_now = rospy.Time.now()
q = hv.arrow_direction_to_quat(f)
arrow_len = np.linalg.norm(f) * 0.04
#arrow_len = np.linalg.norm(f) * 0.007
f_unit = f/np.linalg.norm(f)
#if 'electric' not in roslib.__path__[0]:
# # diamondback
# scale = (arrow_len, 0.1, 0.1)
#else:
# electric
scale = (0.20, 0.20, arrow_len)
m1 = hv.single_marker(p, q, 'arrow', frame, scale, c, m_id = m_id,
duration = duration)
m1.header.stamp = t_now
m2 = hv.single_marker(p + f_unit * arrow_len * 1.6, q, 'text_view_facing', frame,
#m2 = hv.single_marker(p, q, 'text_view_facing', frame,
(0.07, 0.07, 0.07), m_id = m_id+1,
duration = duration, color=c)
m2.text = '%.1fN'%(np.linalg.norm(f))
m2.header.stamp = t_now
return m1, m2
def publish_rviz_markers(self):
# publish the CEP marker.
o = np.matrix([0., 0., 0., 1.]).T
jep = self.get_ep()
cep, r = self.kinematics.FK(jep)
cep_marker = hv.single_marker(cep,
o,
'sphere',
'/torso_lift_link',
color=(0., 0., 1., 1.),
scale=(0.02, 0.02, 0.02),
duration=0.,
m_id=1)
cep_marker.header.stamp = rospy.Time.now()
self.marker_pub.publish(cep_marker)
q = self.get_joint_angles()
ee, r = self.kinematics.FK(q)
ee_marker = hv.single_marker(ee,
o,
'sphere',
'/torso_lift_link',
color=(0., 1., 0., 1.),
scale=(0.02, 0.02, 0.02),
duration=0.,
m_id=2)
ee_marker.header.stamp = rospy.Time.now()
self.marker_pub.publish(ee_marker)
def get_arrow_text_markers(p, f, frame, m_id, duration):
t_now = rospy.Time.now()
q = hv.arrow_direction_to_quat(f)
arrow_len = np.linalg.norm(f) * 0.04
scale = (arrow_len, 0.2, 0.2)
m1 = hv.single_marker(p, q, 'arrow', frame, scale, m_id = m_id,
duration = duration)
m1.header.stamp = t_now
m2 = hv.single_marker(p, q, 'text_view_facing', frame,
(0.1, 0.1, 0.1), m_id = m_id+1,
duration = duration, color=(1.,0.,0.,1.))
m2.text = '%.1f N'%(np.linalg.norm(f))
m2.header.stamp = t_now
return m1, m2
def set_jep(self, arm, q, duration=0.15):
if q is None or len(q) != 7:
raise RuntimeError("set_jep value is " + str(q))
self.arm_state_lock[arm].acquire()
jtg = JointTrajectoryGoal()
jtg.trajectory.joint_names = self.joint_names_list[arm]
jtp = JointTrajectoryPoint()
jtp.positions = q
#jtp.velocities = [0 for i in range(len(q))]
#jtp.accelerations = [0 for i in range(len(q))]
jtp.time_from_start = rospy.Duration(duration)
jtg.trajectory.points.append(jtp)
self.joint_action_client[arm].send_goal(jtg)
self.jep[arm] = q
cep, r = self.arms.FK_all(arm, q)
self.arm_state_lock[arm].release()
o = np.matrix([0.,0.,0.,1.]).T
cep_marker = hv.single_marker(cep, o, 'sphere',
'/torso_lift_link', color=(0., 0., 1., 1.),
scale = (0.02, 0.02, 0.02),
m_id = self.cep_marker_id)
cep_marker.header.stamp = rospy.Time.now()
self.marker_pub.publish(cep_marker)
def publish_goal_marker(self, goal_pos):
o = np.matrix([0.,0.,0.,1.]).T
g_m = hv.single_marker(goal_pos, o, 'sphere',
'/world', color=(0., 1., 1., 1.),
scale = (0.02, 0.02, 0.02),
m_id = 23, duration=0)
g_m.header.stamp = rospy.Time.now()
self.goal_marker_pub.publish(g_m)
def vel_vec_rviz_marker(p, v):
quat = hv.arrow_direction_to_quat(v)
am = hv.single_marker(p,
quat,
'arrow',
'torso_lift_link',
m_id=1,
duration=0.1)
return am
def publish_rviz_markers(self):
# publish the CEP marker.
tip_tt = np.matrix(self.tip_tt).reshape(3,1)
o = np.matrix([0.,0.,0.,1.]).T
teleop_marker = hv.single_marker(tip_tt, o, 'sphere',
'/torso_lift_link', color=(0.1, 1., .1, 1.),
scale = (0.03, 0.03, 0.03), duration=0.)
teleop_marker.header.stamp = rospy.Time.now()
self.teleop_marker_pub.publish(teleop_marker)
def publish_rviz_markers(self):
# publish the CEP marker.
o = np.matrix([0.,0.,0.,1.]).T
jep = self.get_ep()
cep, r = self.kinematics.FK(jep)
cep_marker = hv.single_marker(cep, o, 'sphere',
'/torso_lift_link', color=(0., 0., 1., 1.),
scale = (0.02, 0.02, 0.02), duration=0.,
m_id=1)
cep_marker.header.stamp = rospy.Time.now()
self.marker_pub.publish(cep_marker)
q = self.get_joint_angles()
ee, r = self.kinematics.FK(q)
ee_marker = hv.single_marker(ee, o, 'sphere',
'/torso_lift_link', color=(0., 1., 0., 1.),
scale = (0.02, 0.02, 0.02), duration=0.,
m_id=2)
ee_marker.header.stamp = rospy.Time.now()
self.marker_pub.publish(ee_marker)
def publish_goal_marker(self, goal_pos):
o = np.matrix([0., 0., 0., 1.]).T
g_m = hv.single_marker(goal_pos,
o,
'sphere',
'/world',
color=(0., 1., 1., 1.),
scale=(0.02, 0.02, 0.02),
m_id=23,
duration=0)
g_m.header.stamp = rospy.Time.now()
self.goal_marker_pub.publish(g_m)
def r_arm_jep_cb(self, msg):
self.cb_lock.acquire()
self.r_arm_jep = list(msg.data)
self.cb_lock.release()
# publish the CEP marker.
cep, r = self.arms.FK_all('right_arm', self.r_arm_jep)
o = np.matrix([0.,0.,0.,1.]).T
cep_marker = hv.single_marker(cep, o, 'sphere',
'/torso_lift_link', color=(0., 0., 1., 1.),
scale = (0.02, 0.02, 0.02), duration=0.)
cep_marker.header.stamp = msg.header.stamp
self.cep_marker_pub.publish(cep_marker)
def publish_rviz_markers(self):
# publish the CEP marker.
tip_tt = np.matrix(self.tip_tt).reshape(3, 1)
o = np.matrix([0., 0., 0., 1.]).T
teleop_marker = hv.single_marker(tip_tt,
o,
'sphere',
'/torso_lift_link',
color=(0.1, 1., .1, 1.),
scale=(0.03, 0.03, 0.03),
duration=0.)
teleop_marker.header.stamp = rospy.Time.now()
self.teleop_marker_pub.publish(teleop_marker)
def get_arrow_text_markers(p, f, frame, m_id, duration, c=(1., 0., 0., 1.0)):
t_now = rospy.Time.now()
q = hv.arrow_direction_to_quat(f)
arrow_len = np.linalg.norm(f) * 0.04
#arrow_len = np.linalg.norm(f) * 0.007
f_unit = f / np.linalg.norm(f)
#if 'electric' not in roslib.__path__[0]:
# # diamondback
# scale = (arrow_len, 0.1, 0.1)
#else:
# electric
scale = (0.20, 0.20, arrow_len)
m1 = hv.single_marker(p,
q,
'arrow',
frame,
scale,
c,
m_id=m_id,
duration=duration)
m1.header.stamp = t_now
m2 = hv.single_marker(
p + f_unit * arrow_len * 1.6,
q,
'text_view_facing',
frame,
#m2 = hv.single_marker(p, q, 'text_view_facing', frame,
(0.07, 0.07, 0.07),
m_id=m_id + 1,
duration=duration,
color=c)
m2.text = '%.1fN' % (np.linalg.norm(f))
m2.header.stamp = t_now
return m1, m2
def r_arm_jep_cb(self, msg):
self.cb_lock.acquire()
self.r_arm_jep = list(msg.data)
self.cb_lock.release()
# publish the CEP marker.
cep, r = self.arms.FK_all('right_arm', self.r_arm_jep)
o = np.matrix([0., 0., 0., 1.]).T
cep_marker = hv.single_marker(cep,
o,
'sphere',
'/torso_lift_link',
color=(0., 0., 1., 1.),
scale=(0.02, 0.02, 0.02),
duration=0.)
cep_marker.header.stamp = msg.header.stamp
self.cep_marker_pub.publish(cep_marker)
def vel_vec_rviz_marker(p, v):
quat = hv.arrow_direction_to_quat(v)
am = hv.single_marker(p, quat, 'arrow', 'torso_lift_link', m_id=1,
duration=0.1)
return am
def visualize_taxel_array(ta, marker_pub):
markers = MarkerArray()
stamp = ta.header.stamp
frame = ta.header.frame_id
pts = np.column_stack((ta.centers_x, ta.centers_y, ta.centers_z))
colors = np.zeros((4, pts.shape[0]))
colors[0, :] = 243 / 255.0
colors[1, :] = 132 / 255.0
colors[2, :] = 0.
colors[3, :] = 1.0
scale = (0.005, 0.005, 0.005)
duration = 0.
m = hv.list_marker(pts.T,
colors,
scale,
'points',
frame,
duration=duration,
m_id=0)
m.header.stamp = stamp
markers.markers.append(m)
# now draw non-zero forces as arrows.
nrmls = np.column_stack((ta.normals_x, ta.normals_y, ta.normals_z))
fs = np.column_stack((ta.values_x, ta.values_y, ta.values_z))
fmags = ut.norm(fs.T).flatten()
if hasattr(ta, 'link_name'):
idxs = np.where(fmags > 0.0)[0] #was .01
else:
# HACK. need to calibrate the skin patch so that something
# reasonable gets outputted.
idxs = np.where(fmags > 0.)[0] #was 0.2
#force_marker_scale = 0.04
force_marker_scale = 1.0
duration = 0.4
for i in idxs:
p = np.matrix(pts[i]).T
n1 = np.matrix(nrmls[i]).T
n2 = np.matrix(fs[i]).T
if False:
q1 = hv.arrow_direction_to_quat(n1)
l1 = (n2.T * n1)[0, 0] * force_marker_scale
if 'electric' not in roslib.__path__[0]:
scale = (l1, 0.2, 0.2)
else:
scale = (0.2, 0.2, 0.2
) #something weird here, was (0.2, 0.2, l1)
scale = (0.4, 0.4, l1)
m = hv.single_marker(p,
q1,
'arrow',
frame,
duration=duration,
scale=scale,
m_id=3 * i + 1)
m.header.stamp = stamp
#markers.markers.append(m)
if True:
if np.linalg.norm(n2) < 0.30:
q2 = hv.arrow_direction_to_quat(n2)
l2 = np.linalg.norm(n2) * force_marker_scale
m = Marker()
m.points.append(Point(p[0, 0], p[1, 0], p[2, 0]))
m.points.append(
Point(p[0, 0] + n2[0, 0], p[1, 0] + n2[1, 0],
p[2, 0] + n2[2, 0]))
m.scale = Point(0.02, 0.04, 0.0)
m.header.frame_id = frame
m.id = 3 * i + 2
m.type = Marker.ARROW
m.action = Marker.ADD
m.color.r = 0.
m.color.g = 0.8
m.color.b = 0.
m.color.a = 1.
m.lifetime = rospy.Duration(duration)
m.header.stamp = stamp
markers.markers.append(m)
m = hv.single_marker(p + n2 / np.linalg.norm(n2) * l2 * 1.2,
q2,
'text_view_facing',
frame, (0.07, 0.07, 0.07),
m_id=3 * i + 3,
duration=duration,
color=(0.5, 0.5, 0.5, 1.))
m.text = '%.2fm' % (np.linalg.norm(n2))
m.header.stamp = stamp
markers.markers.append(m)
marker_pub.publish(markers)
def visualize_taxel_array(ta, marker_pub):
markers = MarkerArray()
stamp = ta.header.stamp
frame = ta.header.frame_id
pts = np.column_stack((ta.centers_x, ta.centers_y, ta.centers_z))
colors = np.zeros((4, pts.shape[0]))
colors[0, :] = 243 / 255.0
colors[1, :] = 132 / 255.0
colors[2, :] = 0.
colors[3, :] = 1.0
scale = (0.005, 0.005, 0.005)
duration = 0.
m = hv.list_marker(pts.T,
colors,
scale,
'points',
frame,
duration=duration,
m_id=0)
m.header.stamp = stamp
markers.markers.append(m)
# now draw non-zero forces as arrows.
nrmls = np.column_stack((ta.normals_x, ta.normals_y, ta.normals_z))
fs = np.column_stack((ta.values_x, ta.values_y, ta.values_z))
fmags = ut.norm(fs.T).flatten()
if hasattr(ta, 'link_name'):
idxs = np.where(fmags > 0.01)[0]
else:
# HACK. need to calibrate the skin patch so that something
# reasonable gets outputted.
idxs = np.where(fmags > 0.2)[0]
force_marker_scale = 0.04
duration = 0.02
for i in idxs:
p = np.matrix(pts[i]).T
n1 = np.matrix(nrmls[i]).T
n2 = np.matrix(fs[i]).T
q1 = hv.arrow_direction_to_quat(n1)
l1 = (n2.T * n1)[0, 0] * force_marker_scale
#if 'electric' not in roslib.__path__[0]:
# scale = (l1, 0.2, 0.2)
#else:
scale = (0.2, 0.2, l1)
m = hv.single_marker(p,
q1,
'arrow',
frame,
duration=duration,
scale=scale,
m_id=3 * i + 1)
m.header.stamp = stamp
markers.markers.append(m)
q2 = hv.arrow_direction_to_quat(n2)
l2 = np.linalg.norm(n2) * force_marker_scale
#if 'electric' not in roslib.__path__[0]:
# scale = (l2, 0.2, 0.2)
#else:
scale = (0.2, 0.2, l2)
m = hv.single_marker(p,
q2,
'arrow',
frame,
duration=duration,
scale=scale,
color=(0., 0.5, 0., 1.0),
m_id=3 * i + 2)
m.header.stamp = stamp
markers.markers.append(m)
m = hv.single_marker(p + n2 / np.linalg.norm(n2) * l2 * 1.6,
q2,
'text_view_facing',
frame, (0.07, 0.07, 0.07),
m_id=3 * i + 3,
duration=duration,
color=(0., 0.5, 0., 1.))
m.text = '%.1fN' % (np.linalg.norm(n2))
m.header.stamp = stamp
markers.markers.append(m)
marker_pub.publish(markers)
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 )
[ pub_mark.publish( x ) for x in tm ]
rospy.sleep( 1.0 )
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)
[pub_mark.publish(x) for x in tm]
rospy.sleep(1.0)
def visualize_taxel_array(ta, marker_pub):
markers = MarkerArray()
stamp = ta.header.stamp
frame = ta.header.frame_id
pts = np.column_stack((ta.centers_x, ta.centers_y, ta.centers_z))
colors = np.zeros((4, pts.shape[0]))
colors[0,:] = 243/255.0
colors[1,:] = 132/255.0
colors[2,:] = 0.
colors[3,:] = 1.0
scale = (0.005, 0.005, 0.005)
duration = 0.
m = hv.list_marker(pts.T, colors, scale, 'points',
frame, duration=duration, m_id=0)
m.header.stamp = stamp
markers.markers.append(m)
# now draw non-zero forces as arrows.
nrmls = np.column_stack((ta.normals_x, ta.normals_y, ta.normals_z))
fs = np.column_stack((ta.values_x, ta.values_y, ta.values_z))
fmags = ut.norm(fs.T).flatten()
if hasattr(ta, 'link_name'):
idxs = np.where(fmags > 0.0)[0] #was .01
else:
# HACK. need to calibrate the skin patch so that something
# reasonable gets outputted.
idxs = np.where(fmags > 0.)[0] #was 0.2
#force_marker_scale = 0.04
force_marker_scale = 1.0
duration = 0.4
for i in idxs:
p = np.matrix(pts[i]).T
n1 = np.matrix(nrmls[i]).T
n2 = np.matrix(fs[i]).T
if False:
q1 = hv.arrow_direction_to_quat(n1)
l1 = (n2.T * n1)[0,0] * force_marker_scale
if 'electric' not in roslib.__path__[0]:
scale = (l1, 0.2, 0.2)
else:
scale = (0.2, 0.2, 0.2) #something weird here, was (0.2, 0.2, l1)
scale = (0.4, 0.4, l1)
m = hv.single_marker(p, q1, 'arrow', frame, duration=duration,
scale=scale, m_id=3*i+1)
m.header.stamp = stamp
#markers.markers.append(m)
if True:
if np.linalg.norm(n2) < 0.30:
q2 = hv.arrow_direction_to_quat(n2)
l2 = np.linalg.norm(n2) * force_marker_scale
m = Marker()
m.points.append(Point(p[0,0], p[1,0], p[2,0]))
m.points.append(Point(p[0,0]+n2[0,0], p[1,0]+n2[1,0], p[2,0]+n2[2,0]))
m.scale = Point(0.02, 0.04, 0.0)
m.header.frame_id = frame
m.id = 3*i+2
m.type = Marker.ARROW
m.action = Marker.ADD
m.color.r = 0.
m.color.g = 0.8
m.color.b = 0.
m.color.a = 1.
m.lifetime = rospy.Duration(duration)
m.header.stamp = stamp
markers.markers.append(m)
m = hv.single_marker(p + n2/np.linalg.norm(n2) * l2 * 1.2, q2, 'text_view_facing', frame,
(0.07, 0.07, 0.07), m_id = 3*i+3,
duration = duration, color=(0.5,0.5,0.5,1.))
m.text = '%.2fm'%(np.linalg.norm(n2))
m.header.stamp = stamp
markers.markers.append(m)
marker_pub.publish(markers)
def visualize_taxel_array(ta, marker_pub):
markers = MarkerArray()
stamp = ta.header.stamp
frame = ta.header.frame_id
pts = np.column_stack((ta.centers_x, ta.centers_y, ta.centers_z))
colors = np.zeros((4, pts.shape[0]))
colors[0,:] = 243/255.0
colors[1,:] = 132/255.0
colors[2,:] = 0.
colors[3,:] = 1.0
scale = (0.005, 0.005, 0.005)
duration = 0.
m = hv.list_marker(pts.T, colors, scale, 'points',
frame, duration=duration, m_id=0)
m.header.stamp = stamp
markers.markers.append(m)
# now draw non-zero forces as arrows.
nrmls = np.column_stack((ta.normals_x, ta.normals_y, ta.normals_z))
fs = np.column_stack((ta.values_x, ta.values_y, ta.values_z))
fmags = ut.norm(fs.T).flatten()
if hasattr(ta, 'link_name'):
idxs = np.where(fmags > 0.01)[0]
else:
# HACK. need to calibrate the skin patch so that something
# reasonable gets outputted.
idxs = np.where(fmags > 0.2)[0]
force_marker_scale = 0.04
duration = 0.02
for i in idxs:
p = np.matrix(pts[i]).T
n1 = np.matrix(nrmls[i]).T
n2 = np.matrix(fs[i]).T
q1 = hv.arrow_direction_to_quat(n1)
l1 = (n2.T * n1)[0,0] * force_marker_scale
#if 'electric' not in roslib.__path__[0]:
# scale = (l1, 0.2, 0.2)
#else:
scale = (0.2, 0.2, l1)
m = hv.single_marker(p, q1, 'arrow', frame, duration=duration,
scale=scale, m_id=3*i+1)
m.header.stamp = stamp
markers.markers.append(m)
q2 = hv.arrow_direction_to_quat(n2)
l2 = np.linalg.norm(n2) * force_marker_scale
#if 'electric' not in roslib.__path__[0]:
# scale = (l2, 0.2, 0.2)
#else:
scale = (0.2, 0.2, l2)
m = hv.single_marker(p, q2, 'arrow', frame, duration=duration,
scale=scale, color=(0.,0.5,0.,1.0),
m_id=3*i+2)
m.header.stamp = stamp
markers.markers.append(m)
m = hv.single_marker(p + n2/np.linalg.norm(n2) * l2 * 1.6, q2, 'text_view_facing', frame,
(0.07, 0.07, 0.07), m_id = 3*i+3,
duration = duration, color=(0.,0.5,0.,1.))
m.text = '%.1fN'%(np.linalg.norm(n2))
m.header.stamp = stamp
markers.markers.append(m)
marker_pub.publish(markers)