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 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)