def make_target_marker(self): #, scale=.2, color=stdm.ColorRGBA(.5,.5,.5,.5)): if self.is_current_task_frame: scale = .3 color = stdm.ColorRGBA(1,0,0,.5) else: scale = .2 color = stdm.ColorRGBA(.5,.5,.5,.5) name = 'target_' + self.tagid if not self.tag_database.has_id(self.tagid): return p_ar = self.tag_database.get(self.tagid)['target_location'] m_ar = tfu.tf_as_matrix(p_ar) map_T_ar = tfu.transform('map', self.tagid, self.tf_listener, t=0) p_map = tfu.matrix_as_tf(map_T_ar * m_ar) pose = p_map int_marker = interactive_marker(name, (pose[0], (0,0,0,1)), scale) int_marker.header.frame_id = 'map' #self.tagid int_marker.description = '' int_marker.controls.append(make_sphere_control(name, scale/2.)) int_marker.controls[0].markers[0].color = color int_marker.controls += make_directional_controls(name) self.server_lock.acquire() self.marker_server.insert(int_marker, self.marker_cb) self.server_lock.release() self.target_marker = int_marker if self.menu != None: self._make_menu()
def straighline_planner(self, obs=None): waypoints = linear_interp_traj([[self.start_x, self.start_y], [self.goal_x, self.goal_y]], 0.1) waypoints = np.array(waypoints) lin_x, lin_y, angu_z, ind, obs_lin_x, obs_lin_y = self.holonomic_controller.control(waypoints, self.start_x, self.start_y, self.start_yaw, self.goal_yaw, self.vel_rob[0], self.vel_rob[1], obs) self.vel_msg.linear.x = lin_x self.vel_msg.linear.y = lin_y self.vel_msg.angular.z = 0.0 # self.vel_msg.angular.z = angu_z # self.velocity_pub.publish(self.vel_msg) robot_marker = vis_msg.Marker(type=vis_msg.Marker.SPHERE_LIST, action=vis_msg.Marker.ADD) robot_marker.header.frame_id = 'world' robot_marker.header.stamp = rospy.Time.now() robot_marker.scale.x = 0.5 robot_marker.scale.y = 0.5 robot_marker.points = [geom_msg.Point(self.start_x, self.start_y, 0.0)] robot_marker.colors = [std_msg.ColorRGBA(1.0, 0.0, 0.0, 1.0)] self.robot_marker_pub.publish(robot_marker) goal_marker = vis_msg.Marker(type=vis_msg.Marker.SPHERE_LIST, action=vis_msg.Marker.ADD) goal_marker.header.frame_id = 'world' goal_marker.header.stamp = rospy.Time.now() goal_marker.scale.x = 0.5 goal_marker.scale.y = 0.5 goal_marker.points = [geom_msg.Point(self.goal_x, self.goal_y, 0.0)] goal_marker.colors = [std_msg.ColorRGBA(0.0, 0.0, 1.0, 1.0)] self.goal_marker_pub.publish(goal_marker) return waypoints, lin_x, lin_y, ind, obs_lin_x, obs_lin_y
def publish_octomap(pub_ns, arr, carr, size=0.1, stamp=None, frame_id='map', flip_rb=False): """ Publish cubes list: """ marker = vis_msg.Marker(type=vis_msg.Marker.CUBE_LIST, ns=pub_ns, action=vis_msg.Marker.ADD) marker.header.frame_id = frame_id marker.header.stamp = stamp if stamp is not None else rospy.Time.now() # Point width, and height marker.scale.x = size marker.scale.y = size marker.scale.z = size N, D = arr.shape # XYZ inds, = np.where(~np.isnan(arr).any(axis=1)) marker.points = [geom_msg.Point(arr[j,0], arr[j,1], arr[j,2]) for j in inds] # RGB (optionally alpha) rax, bax = 0, 2 # carr = carr.astype(np.float32) * 1.0 / 255 if flip_rb: rax, bax = 2, 0 if D == 3: marker.colors = [std_msg.ColorRGBA(carr[j,rax], carr[j,1], carr[j,bax], 1.0) for j in inds] elif D == 4: marker.colors = [std_msg.ColorRGBA(carr[j,rax], carr[j,1], carr[j,bax], carr[j,3]) for j in inds] marker.lifetime = rospy.Duration() _publish_marker(marker)
def _make_marker(self): #, scale=.2, color=stdm.ColorRGBA(.5,.5,.5,.5)): if self.is_current_task_frame: scale = .3 color = stdm.ColorRGBA(1,0,0,.4) else: scale = .2 color = stdm.ColorRGBA(.5,.5,.5,.4) self.marker_name = 'action_' + self.actionid pose = self.location_in_frame #print 'made marker with pose', pose int_marker = interactive_marker(self.marker_name, (pose[0], pose[1]), scale) #print int_marker.pose.orientation int_marker.header.frame_id = self.frame #self.tagid int_marker.description = self._make_description() int_marker.controls.append(make_sphere_control(self.marker_name + '_1', scale/8.)) int_marker.controls.append(make_sphere_control(self.marker_name + '_2', scale)) int_marker.controls[1].markers[0].color = color int_marker.controls += make_directional_controls(self.marker_name) int_marker.controls += make_orientation_controls(self.marker_name) self.server_lock.acquire() self.marker_server.insert(int_marker, self.marker_cb) self.server_lock.release() self.marker_obj = int_marker if self.menu_handler != None: self._make_menu() else: self._make_empty_menu()
def publish_cloud_markers(pub_ns, _arr, c='b', stamp=None, flip_rb=False, frame_id='map'): """ Publish point cloud on: pub_ns: Namespace on which the cloud will be published arr: numpy array (N x 3) for point cloud data c: Option 1: 'c', 'b', 'r' etc colors accepted by matplotlibs color Option 2: float ranging from 0 to 1 via matplotlib's jet colormap Option 3: numpy array (N x 3) with r,g,b vals ranging from 0 to 1 s: supported only by matplotlib plotting alpha: supported only by matplotlib plotting """ arr, carr = copy_pointcloud_data(_arr, c, flip_rb=flip_rb) marker = vis_msg.Marker(type=vis_msg.Marker.SPHERE_LIST, ns=pub_ns, action=vis_msg.Marker.ADD) marker.header.frame_id = frame_id marker.header.stamp = stamp if stamp is not None else rospy.Time.now() marker.scale.x = 0.01 marker.scale.y = 0.01 # XYZ inds, = np.where(~np.isnan(arr).any(axis=1)) marker.points = [geom_msg.Point(arr[j,0], arr[j,1], arr[j,2]) for j in inds] # RGB (optionally alpha) N, D = arr.shape[:2] if D == 3: marker.colors = [std_msg.ColorRGBA(carr[j,0], carr[j,1], carr[j,2], 1.0) for j in inds] elif D == 4: marker.colors = [std_msg.ColorRGBA(carr[j,0], carr[j,1], carr[j,2], carr[j,3]) for j in inds] marker.lifetime = rospy.Duration() _publish_marker(marker) print 'Publishing marker', N
def publishTagMarkers(tags, publisher): """Republishes the tag_markers topic""" SHIFT = 0.05 h = std_msgs.Header(stamp=rospy.Time.now(), frame_id='map') ca = std_msgs.ColorRGBA(r=1, a=1) cb = std_msgs.ColorRGBA(g=1, a=1) ct = std_msgs.ColorRGBA(b=1, a=1) sa = geometry_msgs.Vector3(x=0.1, y=0.5, z=0.5) sb = geometry_msgs.Vector3(x=0.1, y=0.25, z=0.25) st = geometry_msgs.Vector3(x=1, y=1, z=1) ma = visualization_msgs.MarkerArray() ma.markers.append( visualization_msgs.Marker( header=h, id=-1, action=visualization_msgs.Marker.DELETEALL)) for i, t in enumerate(tags): th = t['th_deg'] * math.pi / 180. q = tf.transformations.quaternion_from_euler(0, 0, th) q = geometry_msgs.Quaternion(x=q[0], y=q[1], z=q[2], w=q[3]) ma.markers.append( visualization_msgs.Marker( header=h, id=i * 3, type=visualization_msgs.Marker.CUBE, action=visualization_msgs.Marker.ADD, pose=geometry_msgs.Pose( position=geometry_msgs.Point(x=t['x'], y=t['y']), orientation=q), scale=sa, color=ca)) ma.markers.append( visualization_msgs.Marker( header=h, id=i * 3 + 1, type=visualization_msgs.Marker.CUBE, action=visualization_msgs.Marker.ADD, pose=geometry_msgs.Pose( position=geometry_msgs.Point( x=t['x'] + SHIFT * math.cos(th), y=t['y'] + SHIFT * math.sin(th)), orientation=q), scale=sb, color=cb)) ma.markers.append( visualization_msgs.Marker( header=h, id=i * 3 + 2, type=visualization_msgs.Marker.TEXT_VIEW_FACING, action=visualization_msgs.Marker.ADD, pose=geometry_msgs.Pose( position=geometry_msgs.Point(x=t[1], y=t[2], z=0.5), orientation=q), scale=st, color=ct, text=str(t['id']))) publisher.publish(ma)
def __init__(self, tagid, pose_in_map_frame, action_marker_manager, server_lock, marker_server, scale=.2): self.tagid = tagid self.action_marker_manager = action_marker_manager self.server_lock = server_lock self.marker_server = marker_server self.marker_name = 'ar_marker_' + str(self.tagid) int_marker = interactive_marker(self.marker_name, pose_in_map_frame, scale) int_marker.scale = .6 self.marker_obj = int_marker int_marker.description = 'Tag #%d' % self.tagid #int_marker.controls += [] #int_marker.controls[0].markers[0].color = stdm.ColorRGBA(0,1,0,.5) sph = make_sphere_control(self.marker_name, scale) sph.markers[0].color = stdm.ColorRGBA(0,1,0,.5) self.menu = mh.MenuHandler() self.menu.insert('Create New Action', parent=None, callback=self.create_new_action_cb) menu_control = ims.InteractiveMarkerControl() menu_control.interaction_mode = ims.InteractiveMarkerControl.MENU menu_control.description="" menu_control.name = 'menu_tag_' + str(self.tagid) menu_control.markers.append(copy.deepcopy(sph.markers[0])) menu_control.always_visible = True int_marker.controls += [menu_control] self.server_lock.acquire() self.marker_server.insert(int_marker, self.marker_cb) self.menu.apply(self.marker_server, int_marker.name) self.server_lock.release()
def draw_marker(self, ps, id=None, type=Marker.CUBE, ns='default_ns', rgba=(0, 1, 0, 1), scale=(.03, .03, .03), text='', duration=0): """ If markers aren't showing up, it's probably because the id's are being overwritten. Make sure to set the ids and namespace. """ if id is None: id = self.get_unused_id() marker = Marker(type=type, action=Marker.ADD) marker.ns = ns marker.header = ps.header marker.pose = ps.pose marker.scale = gm.Vector3(*scale) marker.color = stdm.ColorRGBA(*rgba) marker.id = id marker.text = text marker.lifetime = rospy.Duration(duration) self.pub.publish(marker) self.ids.add(id) return MarkerHandle(marker, self.pub)
def circle_marker(center, radius, scale, color, mframe, z=.03, duration=10.0, m_id=0, resolution=1.): angles = np.matrix(np.arange(0, 360., resolution)).T xs = center[0, 0] + np.cos(angles) * radius ys = center[1, 0] + np.sin(angles) * radius z = .03 m = vm.Marker() m.header.frame_id = mframe m.id = m_id m.type = create_mdict()['line_strip'] m.action = vm.Marker.ADD m.points = [ gm.Point(xs[i, 0], ys[i, 0], z) for i in range(angles.shape[0]) ] m.colors = [ sdm.ColorRGBA(color[0, 0], color[1, 0], color[2, 0], color[3, 0]) for i in range(angles.shape[0]) ] m.scale.x = scale m.lifetime = rospy.Duration(duration) return m
def list_marker(points, colors, scale, mtype, mframe, duration=10.0, m_id=0): m = vm.Marker() m.header.frame_id = mframe m.id = m_id m.type = create_mdict()[mtype] m.action = vm.Marker.ADD m.points = [ gm.Point(points[0, i], points[1, i], points[2, i]) for i in range(points.shape[1]) ] #pdb.set_trace() m.colors = [ sdm.ColorRGBA(colors[0, i], colors[1, i], colors[2, i], colors[3, i]) for i in range(colors.shape[1]) ] m.color.r = 1. m.color.g = 0. m.color.b = 0. m.color.a = 1. m.scale.x = scale[0] m.scale.y = scale[1] m.scale.z = scale[2] m.lifetime = rospy.Duration(duration) return m
def make_rviz_marker(scale): marker = ims.Marker() marker.type = ims.Marker.SPHERE marker.scale.x = scale * 0.45 marker.scale.y = scale * 0.45 marker.scale.z = scale * 0.45 marker.color = stdm.ColorRGBA(.5,.5,.5,1) return marker
def make_ar_marker(self, scale=.2): name = 'ar_' + self.tagid pose = self.tag_database.get(self.tagid)['ar_location'] int_marker = interactive_marker(name, (pose[0], (0, 0, 0, 1)), scale) int_marker.description = 'Tag #%d' % self.get_id_numb() int_marker.controls += [make_sphere_control(name, int_marker.scale)] int_marker.controls[0].markers[0].color = stdm.ColorRGBA(0, 1, 0, .5) self.server.insert(int_marker, self.process_feedback) self.ar_marker = int_marker
def publish_line_segments(pub_ns, _arr1, _arr2, c='b', stamp=None, frame_id='camera', flip_rb=False, size=0.002): """ Publish point cloud on: pub_ns: Namespace on which the cloud will be published arr: numpy array (N x 3) for point cloud data c: Option 1: 'c', 'b', 'r' etc colors accepted by matplotlibs color Option 2: float ranging from 0 to 1 via matplotlib's jet colormap Option 3: numpy array (N x 3) with r,g,b vals ranging from 0 to 1 s: supported only by matplotlib plotting alpha: supported only by matplotlib plotting """ arr1, carr = copy_pointcloud_data(_arr1, c, flip_rb=flip_rb) arr2 = (deepcopy(_arr2)).reshape(-1, 3) marker = vis_msg.Marker(type=vis_msg.Marker.LINE_LIST, ns=pub_ns, action=vis_msg.Marker.ADD) if not arr1.shape == arr2.shape: raise AssertionError marker.header.frame_id = frame_id marker.header.stamp = stamp if stamp is not None else rospy.Time.now() marker.scale.x = size marker.scale.y = size marker.color.b = 1.0 marker.color.a = 1.0 marker.pose.position = geom_msg.Point(0, 0, 0) marker.pose.orientation = geom_msg.Quaternion(0, 0, 0, 1) # Handle 3D data: [ndarray or list of ndarrays] arr12 = np.hstack([arr1, arr2]) inds, = np.where(~np.isnan(arr12).any(axis=1)) marker.points = [] for j in inds: marker.points.extend([ geom_msg.Point(arr1[j, 0], arr1[j, 1], arr1[j, 2]), geom_msg.Point(arr2[j, 0], arr2[j, 1], arr2[j, 2]) ]) # RGB (optionally alpha) marker.colors = [ std_msg.ColorRGBA(carr[j, 0], carr[j, 1], carr[j, 2], 1.0) for j in inds ] marker.lifetime = rospy.Duration() _publish_marker(marker)
def make_target_marker(self, name, pose, scale=.2): int_marker = interactive_marker(name, (pose[0], (0, 0, 0, 1)), scale) int_marker.header.frame_id = 'map' #self.tagid int_marker.description = '' int_marker.controls.append(make_sphere_control(name, scale / 2.)) int_marker.controls[0].markers[0].color = stdm.ColorRGBA(1, 0, 0, .5) int_marker.controls += make_directional_controls(name) self.server.insert(int_marker, self.process_feedback) self.target_marker = int_marker if self.menu != None: self._make_menu()
def create_marker(w): global id_count m = viz_msgs.Marker() m.header.frame_id = w['frame_id'] m.ns = w['name'] m.id = id_count m.action = viz_msgs.Marker.ADD m.pose = create_geo_pose(w) m.scale = geometry_msgs.Vector3(1.0,0.3,0.3) m.color = std_msgs.ColorRGBA(0.0,1.0,0.0,1.0) id_count = id_count + 1 return m
def draw_curve(self, pose_array, id=None, rgba=(0, 1, 0, 1), width=.01, ns="default_ns", duration=0, type=Marker.LINE_STRIP): if id is None: id = self.get_unused_id() marker = Marker(type=type, action=Marker.ADD) marker.header = pose_array.header marker.points = [pose.position for pose in pose_array.poses] marker.lifetime = rospy.Duration(0) if isinstance(rgba, list): assert len(rgba) == len(pose_array.poses) marker.colors = [stdm.ColorRGBA(*c) for c in rgba] else: marker.color = stdm.ColorRGBA(*rgba) marker.scale = gm.Vector3(width, width, width) marker.id = id marker.ns = ns self.pub.publish(marker) self.ids.add(id) return MarkerHandle(marker, self.pub)
def make_ar_marker(self, scale=.2): name = 'ar_' + self.tagid entry = self.tag_database.get(self.tagid) pose = entry['ar_location'] int_marker = interactive_marker(name, (pose[0], (0,0,0,1)), scale) if entry['behavior'] != None: behavior_name = pt.split(entry['behavior'])[1] int_marker.description = tag_name(self.tagid, behavior_name) else: int_marker.description = 'Tag #%d' % get_id_numb(self.tagid) int_marker.controls += [make_sphere_control(name, int_marker.scale)] int_marker.controls[0].markers[0].color = stdm.ColorRGBA(0,1,0,.5) self.server_lock.acquire() self.marker_server.insert(int_marker, self.marker_cb) self.server_lock.release() self.ar_marker = int_marker
def publish_pose_list(pub_ns, _poses, texts=[], stamp=None, size=0.05, frame_id='camera', seq=1): """ Publish Pose List on: pub_channel: Channel on which the cloud will be published """ poses = deepcopy(_poses) if not len(poses): return arrs = np.vstack([pose.matrix[:3,:3].T.reshape((1,9)) for pose in poses]) * size arrX = np.vstack([pose.tvec.reshape((1,3)) for pose in poses]) arrx, arry, arrz = arrs[:,0:3], arrs[:,3:6], arrs[:,6:9] # Point width, and height N = len(poses) markers = vis_msg.Marker(type=vis_msg.Marker.LINE_LIST, ns=pub_ns, action=vis_msg.Marker.ADD) markers.header.frame_id = frame_id markers.header.stamp = stamp if stamp is not None else rospy.Time.now() markers.header.seq = seq markers.scale.x = size/20 # 0.01 markers.scale.y = size/20 # 0.01 markers.color.a = 1.0 markers.pose.position = geom_msg.Point(0,0,0) markers.pose.orientation = geom_msg.Quaternion(0,0,0,1) markers.points = [] markers.lifetime = rospy.Duration() for j in range(N): markers.points.extend([geom_msg.Point(arrX[j,0], arrX[j,1], arrX[j,2]), geom_msg.Point(arrX[j,0] + arrx[j,0], arrX[j,1] + arrx[j,1], arrX[j,2] + arrx[j,2])]) markers.colors.extend([std_msg.ColorRGBA(1.0, 0.0, 0.0, 1.0), std_msg.ColorRGBA(1.0, 0.0, 0.0, 1.0)]) markers.points.extend([geom_msg.Point(arrX[j,0], arrX[j,1], arrX[j,2]), geom_msg.Point(arrX[j,0] + arry[j,0], arrX[j,1] + arry[j,1], arrX[j,2] + arry[j,2])]) markers.colors.extend([std_msg.ColorRGBA(0.0, 1.0, 0.0, 1.0), std_msg.ColorRGBA(0.0, 1.0, 0.0, 1.0)]) markers.points.extend([geom_msg.Point(arrX[j,0], arrX[j,1], arrX[j,2]), geom_msg.Point(arrX[j,0] + arrz[j,0], arrX[j,1] + arrz[j,1], arrX[j,2] + arrz[j,2])]) markers.colors.extend([std_msg.ColorRGBA(0.0, 0.0, 1.0, 1.0), std_msg.ColorRGBA(0.0, 0.0, 1.0, 1.0)]) _publish_marker(markers)
def set_color(self, color): if color == 'black': self.marker.color = SM.ColorRGBA(*BLACK) elif color == 'blue': self.marker.color = SM.ColorRGBA(*BLUE) elif color == 'green': self.marker.color = SM.ColorRGBA(*GREEN) elif color == 'red': self.marker.color = SM.ColorRGBA(*RED) elif color == 'white': self.marker.color = SM.ColorRGBA(*WHITE) else: rospy.logwarn("String marker color not recognized!") self.marker.color = SM.ColorRGBA(*BLACK)
def make_kin_tree_from_link(ps, linkname, ns='default_ns', valuedict=None): markers = [] U = get_pr2_urdf() link = U.links[linkname] if link.visual and link.visual.geometry and isinstance( link.visual.geometry, urdf.Mesh): rospy.logdebug("%s is a mesh. drawing it.", linkname) marker = Marker(type=Marker.MESH_RESOURCE, action=Marker.ADD) marker.ns = ns marker.header = ps.header linkFromGeom = conversions.trans_rot_to_hmat( link.visual.origin.position, transformations.quaternion_from_euler( *link.visual.origin.rotation)) origFromLink = conversions.pose_to_hmat(ps.pose) origFromGeom = np.dot(origFromLink, linkFromGeom) marker.pose = conversions.hmat_to_pose(origFromGeom) marker.scale = gm.Vector3(1, 1, 1) marker.color = stdm.ColorRGBA(1, 1, 0, .5) marker.id = 0 marker.lifetime = rospy.Duration() marker.mesh_resource = str(link.visual.geometry.filename) marker.mesh_use_embedded_materials = False markers.append(marker) else: rospy.logdebug("%s is not a mesh", linkname) if U.child_map.has_key(linkname): for (cjoint, clink) in U.child_map[linkname]: markers.extend( make_kin_tree_from_joint(ps, cjoint, ns=ns, valuedict=valuedict)) return markers
def draw_traj_points(self, pose_array, rgba=(0, 1, 0, 1), arrow_scale=.05, ns="default_ns", duration=0): marker_array = MarkerArray() for pose in pose_array.poses: marker = Marker(type=Marker.ARROW, action=Marker.ADD) marker.header = pose_array.header marker.pose = pose marker.lifetime = rospy.Duration(0) marker.color = stdm.ColorRGBA(*rgba) marker.scale = gm.Vector3(arrow_scale, arrow_scale, arrow_scale) marker.id = self.get_unused_id() marker.ns = ns marker_array.markers.append(marker) self.ids.add(marker.id) self.array_pub.publish(marker_array) return MarkerListHandle([ MarkerHandle(marker, self.pub) for marker in marker_array.markers ])
def create_arrow(w): m = create_marker(w) m.type = viz_msgs.Marker.ARROW m.color = std_msgs.ColorRGBA(0.0,1.0,0.0,1.0) return m
def main(): # Initialization rospy.init_node('objrec_spoofer') # Get spoofer parameters frame_id = rospy.get_param('~frame_id','/world') publish_period = rospy.get_param('~publish_period',1.0) publish_rate = rospy.Rate(1.0/publish_period) # Get the model info models = rospy.get_param('~models') stl_uris = {} for model in models: stl_uris[model] = rospy.get_param('~stl_uris/'+model) # Get objects from rosparam spoofed_objects = rospy.get_param('~spoofed_objects') # Create the publishers obj_pub = rospy.Publisher('/recognized_objects',objrec_msgs.RecognizedObjects) obj_marker_pub = rospy.Publisher('/recognized_objects_markers',visualization_msgs.MarkerArray) while not rospy.is_shutdown(): # Initialize objects message objects_msg = objrec_msgs.RecognizedObjects() objects_msg.header.stamp = rospy.Time.now() objects_msg.header.frame_id = frame_id # Initialize marker array message marker_array = visualization_msgs.MarkerArray() marker_id = 0 for obj in spoofed_objects: # Construct and populate an object message pss_msg = objrec_msgs.PointSetShape() pss_msg.label = obj['label'] pss_msg.confidence = obj['confidence'] pss_msg.pose = geometry_msgs.Pose(geometry_msgs.Point(*obj['position']), geometry_msgs.Quaternion(*obj['orientation'])) # Store the message objects_msg.objects.append(pss_msg) # Construct and populate a marker message marker = visualization_msgs.Marker() marker.header = objects_msg.header marker.ns = "objrec" marker.type = visualization_msgs.Marker.MESH_RESOURCE marker.action = visualization_msgs.Marker.ADD marker.lifetime = rospy.Duration(publish_period) marker.scale = geometry_msgs.Vector3(0.001, 0.001, 0.001) marker.color = std_msgs.ColorRGBA(1.0, 0.2, 0.4, 0.5) marker.id = marker_id marker.pose = pss_msg.pose marker.mesh_resource = stl_uris[obj['label']] # Store the message marker_array.markers.append(marker) marker_id += 1 # Publish the object information obj_pub.publish(objects_msg) # Publish the markers obj_marker_pub.publish(marker_array) publish_rate.sleep()
def numpy_to_colorRGBA(color): return std_msgs.ColorRGBA(*color)