def spherelist_marker(points, size=[0.1, 0.1, 0.1], rgba=[1.0, 1.0, 1.0, 1.0], ns="", id=0, frame_id="", lifetime=None, stamp=None): marker = Marker() if stamp is None: marker.header.stamp = rospy.Time.now() else: marker.header.stamp = stamp marker.header.frame_id = frame_id marker.ns = ns marker.id = id marker.type = Marker.SPHERE_LIST marker.action = Marker.ADD for i in range(len(points)): marker.points.append(points[i]) marker.scale.x = size[0] marker.scale.y = size[1] marker.scale.z = size[2] marker.color.r = rgba[0] marker.color.g = rgba[1] marker.color.b = rgba[2] marker.color.a = rgba[3] if lifetime is None: marker.lifetime = rospy.Duration() else: marker.lifetime = rospy.Duration(lifetime) return marker
def arrow_marker(position, direction, shaft_radius=0.02, head_radius=0.04, rgba=[1.0, 1.0, 1.0, 1.0], ns="", id=0, frame_id="", lifetime=None, stamp=None): marker = Marker() if stamp is None: marker.header.stamp = rospy.Time.now() else: marker.header.stamp = stamp marker.header.frame_id = frame_id marker.ns = ns marker.id = id marker.type = Marker.ARROW marker.action = Marker.ADD #marker.pose.position.x = position[0] #marker.pose.position.y = position[1] #marker.pose.position.z = position[2] marker.points = [Point(*tuple(position)), Point(*tuple(position+direction*1.0))] marker.scale.x = shaft_radius marker.scale.y = head_radius marker.scale.z = 1.0 marker.color.r = rgba[0] marker.color.g = rgba[1] marker.color.b = rgba[2] marker.color.a = rgba[3] if lifetime is None: marker.lifetime = rospy.Duration() else: marker.lifetime = rospy.Duration(lifetime) return marker
def ellipsoid_marker(position, size=[0.1, 0.1, 0.1], rgba=[1.0, 1.0, 1.0, 1.0], ns="", id=0, frame_id="", lifetime=None, stamp=None): marker = Marker() if stamp is None: marker.header.stamp = rospy.Time.now() else: marker.header.stamp = stamp marker.header.frame_id = frame_id marker.ns = ns marker.id = id marker.type = Marker.SPHERE marker.action = Marker.ADD marker.pose.position.x = position[0] marker.pose.position.y = position[1] marker.pose.position.z = position[2] marker.scale.x = size[0] marker.scale.y = size[1] marker.scale.z = size[2] marker.color.r = rgba[0] marker.color.g = rgba[1] marker.color.b = rgba[2] marker.color.a = rgba[3] if lifetime is None: marker.lifetime = rospy.Duration() else: marker.lifetime = rospy.Duration(lifetime) return marker
def poseStampedToLabeledSphereMarker(cls, psdata, label, fmt="{label} %Y-%m-%d %H:%M:%S", zoffset=0.05): "[poseStamped, meta] -> sphereMarker" ps, meta = psdata res = [] h = Header() h.stamp = rospy.Time.now() h.frame_id = ps.header.frame_id sphere = Marker(type=Marker.SPHERE, action=Marker.ADD, header=h, id=cls.marker_id) sphere.scale.x = 0.07 sphere.scale.y = 0.07 sphere.scale.z = 0.07 sphere.pose = ps.pose sphere.color = ColorRGBA(1.0,0,0,1.0) sphere.ns = "db_play" sphere.lifetime = rospy.Time(3) cls.marker_id += 1 res.append(sphere) text = Marker(type=Marker.TEXT_VIEW_FACING, action=Marker.ADD, header=h, id=cls.marker_id) text.scale.z = 0.1 text.pose = deepcopy(ps.pose) text.pose.position.z += zoffset text.color = ColorRGBA(1.0,1.0,1.0,1.0) text.text = meta["inserted_at"].strftime(fmt).format(label=label) text.ns = "db_play_text" text.lifetime = rospy.Time(300) cls.marker_id += 1 res.append(text) return res
def mesh_marker(position, orientation=None, mesh_resource="", rgba=[1.0, 1.0, 1.0, 1.0], ns="", id=0, frame_id="", lifetime=None, stamp=None): marker = Marker() if stamp is None: marker.header.stamp = rospy.Time.now() else: marker.header.stamp = stamp marker.header.frame_id = frame_id marker.ns = ns marker.id = id marker.type = Marker.MESH_RESOURCE marker.mesh_resource = mesh_resource marker.action = Marker.ADD marker.pose.position.x = position[0] marker.pose.position.y = position[1] marker.pose.position.z = position[2] if orientation is not None: marker.pose.orientation.x = orientation[0] marker.pose.orientation.y = orientation[1] marker.pose.orientation.z = orientation[2] marker.pose.orientation.w = orientation[3] #marker.points = [Point(*tuple(position)), Point(*tuple(position+direction*1.0))] marker.scale.x = 1.0 marker.scale.y = 1.0 marker.scale.z = 1.0 marker.color.r = rgba[0] marker.color.g = rgba[1] marker.color.b = rgba[2] marker.color.a = rgba[3] if lifetime is None: marker.lifetime = rospy.Duration() else: marker.lifetime = rospy.Duration(lifetime) return marker
def publish_cluster(publisher, points, frame_id, namespace, cluster_id): """Publishes a marker representing a cluster. The x and y arguments specify the center of the target. Args: publisher: A visualization_msgs/Marker publisher points: A list of geometry_msgs/Point frame_id: The coordinate frame in which to interpret the points. namespace: string, a unique name for a group of clusters. cluster_id: int, a unique number for this cluster in the given namespace. """ marker = Marker() # TODO(jstn): Once the point clouds have the correct frame_id, # use them here. marker.header.frame_id = frame_id marker.header.stamp = rospy.Time().now() marker.ns = namespace marker.id = 2 * cluster_id marker.type = Marker.POINTS marker.action = Marker.ADD marker.color.r = random.random() marker.color.g = random.random() marker.color.b = random.random() marker.color.a = 0.5 marker.points = points marker.scale.x = 0.002 marker.scale.y = 0.002 marker.lifetime = rospy.Duration() center = [0, 0, 0] for point in points: center[0] += point.x center[1] += point.y center[2] += point.z center[0] /= len(points) center[1] /= len(points) center[2] /= len(points) text_marker = Marker() text_marker.header.frame_id = frame_id text_marker.header.stamp = rospy.Time().now() text_marker.ns = namespace text_marker.id = 2 * cluster_id + 1 text_marker.type = Marker.TEXT_VIEW_FACING text_marker.action = Marker.ADD text_marker.pose.position.x = center[0] - 0.1 text_marker.pose.position.y = center[1] text_marker.pose.position.z = center[2] text_marker.color.r = 1 text_marker.color.g = 1 text_marker.color.b = 1 text_marker.color.a = 1 text_marker.scale.z = 0.05 text_marker.text = '{}'.format(cluster_id) text_marker.lifetime = rospy.Duration() _publish(publisher, marker, "cluster") _publish(publisher, text_marker, "text_marker") return marker
def visualize_cluster(self, cluster, label=None): points = pc2.read_points(cluster.pointcloud, skip_nans=True) point_list = [Point(x=x, y=y-0.3, z=z) for x, y, z, rgb in points] if len(point_list) == 0: rospy.logwarn('Point list was of size 0, skipping.') return marker_id = len(self._current_markers) marker = Marker() marker.header.frame_id = 'map' marker.header.stamp = rospy.Time().now() marker.ns = 'clusters' marker.id = marker_id marker.type = Marker.POINTS marker.action = Marker.ADD marker.color.r = random.random() marker.color.g = random.random() marker.color.b = random.random() marker.color.a = 0.5 + random.random() marker.points = point_list marker.scale.x = 0.002 marker.scale.y = 0.002 marker.lifetime = rospy.Duration() self.visualize_marker(marker) if label is not None: center = [0, 0, 0] for point in point_list: center[0] += point.x center[1] += point.y center[2] += point.z center[0] /= len(point_list) center[1] /= len(point_list) center[2] /= len(point_list) text_marker = Marker() text_marker.header.frame_id = 'map' text_marker.header.stamp = rospy.Time().now() text_marker.ns = 'labels' text_marker.id = marker_id + 1 text_marker.type = Marker.TEXT_VIEW_FACING text_marker.action = Marker.ADD text_marker.pose.position.x = center[1] - 0.05 text_marker.pose.position.y = center[1] text_marker.pose.position.z = center[2] text_marker.color.r = 1 text_marker.color.g = 1 text_marker.color.b = 1 text_marker.color.a = 1 text_marker.scale.z = 0.05 text_marker.text = label text_marker.lifetime = rospy.Duration() self.visualize_marker(text_marker)
def transformStampedArrayToLabeledLineStripMarker(cls, tsdata_lst, fmt="%Y-%m-%d %H:%M:%S", zoffset=0.05, label_downsample=1, discrete=False): "[[transformStamped, meta],...] -> LineStrip / String" res = [] # make line strip points = [] colors = [] t_first = tsdata_lst[0][0] prev_t = t_first.transform.translation for ts, _ in tsdata_lst: t = ts.transform.translation dist = distanceOfVector3(prev_t, t) * 0.65 rgb = colorsys.hsv_to_rgb(dist, 1.0, 1.0) points.append(Point(x=t.x, y=t.y, z=t.z)) colors.append(ColorRGBA(rgb[0],rgb[1],rgb[2],1.0)) prev_t = t h = Header() h.stamp = rospy.Time(0) #rospy.Time.now() h.frame_id = "eng2" #t_first.child_frame_id if discrete: m_type = Marker.POINTS else: m_type = Marker.LINE_STRIP m = Marker(type=m_type, action=Marker.ADD, header=h, id=cls.marker_id) cls.marker_id += 1 m.scale.x = 0.1 m.scale.y = 0.1 m.points = points m.colors = colors m.ns = "labeled_line" m.lifetime = rospy.Time(3000) res.append(m) for t, t_meta in tsdata_lst[::label_downsample]: text = Marker(type=Marker.TEXT_VIEW_FACING, action=Marker.ADD, header=h, id=cls.marker_id) cls.marker_id += 1 text.scale.z = 0.1 text.pose = T.poseFromTransform(t.transform) text.pose.position.z += zoffset text.color = ColorRGBA(0.0,0.0,1.0,1.0) text.text = t_meta["inserted_at"].strftime(fmt) text.ns = "labeled_line_text" text.lifetime = rospy.Time(3000) res.append(text) return res
def publish(anns, data): wall_list = WallList() marker_list = MarkerArray() marker_id = 1 for a, d in zip(anns, data): # Walls object = deserialize_msg(d.data, Wall) wall_list.obstacles.append(object) # Markers marker = Marker() marker.id = marker_id marker.header = a.pose.header marker.type = a.shape marker.ns = "wall_obstacles" marker.action = Marker.ADD marker.lifetime = rospy.Duration.from_sec(0) marker.pose = copy.deepcopy(a.pose.pose.pose) marker.scale = a.size marker.color = a.color marker_list.markers.append(marker) marker_id = marker_id + 1 marker_pub = rospy.Publisher('wall_marker', MarkerArray, latch=True, queue_size=1) wall_pub = rospy.Publisher('wall_pose_list', WallList, latch=True, queue_size=1) wall_pub.publish(wall_list) marker_pub.publish(marker_list) return
def pub_arrow(self, pos1, pos2, color, mag_force): marker = Marker() marker.header.frame_id = self.frame marker.header.stamp = rospy.Time.now() marker.ns = "basic_shapes" marker.id = 99999 marker.type = Marker.ARROW marker.action = Marker.ADD pt1 = Point() pt2 = Point() pt1.x = pos1[0,0] pt1.y = pos1[1,0] pt1.z = pos1[2,0] pt2.x = pos2[0,0] pt2.y = pos2[1,0] pt2.z = pos2[2,0] marker.points.append(pt1) marker.points.append(pt2) marker.scale.x = 0.05 marker.scale.y = 0.1 #marker.scale.z = scale[2] marker.color.r = color[0] marker.color.g = color[1] marker.color.b = color[2] marker.color.a = color[3] marker.lifetime = rospy.Duration() marker.text = mag_force self.pub.publish(marker)
def to_marker(self): """ :return: a marker representing the map. :type: Marker """ marker = Marker() marker.type = Marker.CUBE_LIST for x in range(0, len(self.field)): for y in range(0, len(self.field[0])): marker.header.frame_id = '/odom_combined' marker.ns = 'suturo_planning/map' marker.id = 23 marker.action = Marker.ADD (x_i, y_i) = self.index_to_coordinates(x, y) marker.pose.position.x = 0 marker.pose.position.y = 0 marker.pose.position.z = 0 marker.pose.orientation.w = 1 marker.scale.x = self.cell_size marker.scale.y = self.cell_size marker.scale.z = 0.01 p = Point() p.x = x_i p.y = y_i marker.points.append(p) c = self.get_cell_by_index(x, y) marker.colors.append(self.__cell_to_color(c)) marker.lifetime = rospy.Time(0) return marker
def __publish_marker_for_object(self, object, txt): marker = Marker() marker.header.frame_id = object.object.header.frame_id marker.header.stamp = rospy.Time(0) marker.ns = "scene_classifier_marker" self.marker_id = self.marker_id + 1 marker.id = self.marker_id marker.type = Marker.TEXT_VIEW_FACING marker.action = Marker.ADD marker.pose.position.x = object.c_centroid.x marker.pose.position.y = object.c_centroid.y marker.pose.position.z = object.c_centroid.z marker.pose.orientation.x = 0.0 marker.pose.orientation.y = 0.0 marker.pose.orientation.z = 0.0 marker.pose.orientation.w = 1.0 marker.scale.x = 0.5 marker.scale.y = 0.1 marker.scale.z = 0.1 marker.color.a = 1.0 marker.color.r = 0.0 marker.color.g = 1.0 marker.color.b = 0.0 marker.text = txt marker.lifetime = rospy.Duration.from_sec(10) self.marker_publisher.publish(marker)
def publish_markerArray(publisher, points, rgba=(1, 0, 0, 1), shape=Marker.CUBE, duration=rospy.Duration(360), ns='ns'): #points is expected to be a list of tuples (x,y) #It's recommended that the publisher is created with latch=True _id = 0 ma = MarkerArray() for p in points: m = Marker() m.header.frame_id = '/map' m.header.stamp = rospy.get_rostime() m.ns = ns m.id = _id m.type = shape m.action = m.ADD m.pose.position.x = p[0] m.pose.position.y = p[1] m.pose.orientation.w = 1 m.scale.x = 0.5 m.scale.y = 0.5 m.scale.z = 0.5 m.color.r = rgba[0] m.color.g = rgba[1] m.color.b = rgba[2] m.color.a = rgba[3] m.lifetime = duration ma.markers.append(m) _id += 1 publisher.publish(ma)
def draw_box_marker( self, id, x, y, z, dimx, dimy, dimz, color=(1.0, 1.0, 1.0, 0.0), duration=0.0, frame_id="base_link" ): marker = Marker() marker.header.stamp = rospy.Time.now() marker.ns = "object_detector" marker.type = Marker.CUBE marker.action = marker.ADD marker.id = id marker.header.frame_id = "base_link" marker.pose.position.x = x marker.pose.position.y = y marker.pose.position.z = z marker.pose.position.x = x marker.pose.orientation.x = 0.0 marker.pose.orientation.y = 0.0 marker.pose.orientation.z = 0.0 marker.pose.orientation.w = 1.0 marker.scale.x = dimx marker.scale.y = dimy marker.scale.z = dimz marker.color.a = color[0] marker.color.r = color[1] marker.color.g = color[2] marker.color.b = color[3] marker.lifetime = rospy.Duration(duration) self.box_drawer.publish(marker)
def set_position_callback(self,marker,joy): print self.position_marker.ns print joy.buttons[3] == 1 print marker.ns == "PEOPLE" if (self.position_marker.ns == "NONE" and joy.buttons[3] == 1 and marker.ns == "PEOPLE"): self.position_marker = marker print "set position" ref_marker = Marker() ref_marker.header.frame_id = "base_footprint" ref_marker.header.stamp = rospy.get_rostime() ref_marker.ns = "robot" ref_marker.type = 9 ref_marker.action = 0 ref_marker.pose.position.x = self.position_marker.pose.position.x ref_marker.pose.position.y = self.position_marker.pose.position.y ref_marker.pose.position.z = self.position_marker.pose.position.z ref_marker.scale.x = .25 ref_marker.scale.y = .25 ref_marker.scale.z = .25 ref_marker.color.r = 1.0 ref_marker.color.g = 0.0 ref_marker.color.a = 1.0 ref_marker.lifetime = rospy.Duration(0) self.ref_viz_pub.publish(ref_marker) else: pass
def draw_bounding_box(self, id, box_msg, color=(1.0, 1.0, 0.0, 0.0), duration=0.0): """ Draws a bounding box as detectd by detect_bounding_box. Parameters: box_msg is a FindClusterBoundingBoxResponse msg. color: a quadruple with alpha, r,g,b duration: how long should the bounding box last. 0 means forever. """ marker = Marker() marker.header.stamp = rospy.Time.now() marker.ns = "object_detector" marker.type = Marker.CUBE marker.action = marker.ADD marker.id = id marker.header.frame_id = box_msg.pose.header.frame_id marker.pose = box_msg.pose.pose marker.scale.x = box_msg.box_dims.x marker.scale.y = box_msg.box_dims.y marker.scale.z = box_msg.box_dims.z marker.color.a = color[0] marker.color.r = color[1] marker.color.g = color[2] marker.color.b = color[3] marker.lifetime = rospy.Duration(duration) self.box_drawer.publish(marker)
def _makeMarker(self,tf,num): marker = Marker() marker.header.frame_id = "map" marker.header.stamp = rospy.Time.now() marker.ns = "kinect_challenge" marker.id = num marker.type= marker.ARROW marker.action = marker.ADD marker.pose.position.x = tf[0][0] marker.pose.position.y = tf[0][1] marker.pose.position.z = 0.5 marker.pose.orientation.x = tf[1][0] marker.pose.orientation.y = tf[1][1] marker.pose.orientation.z = tf[1][2] marker.pose.orientation.w = tf[1][3] marker.scale.x = 0.5 marker.scale.y = 0.05 marker.scale.z = 0.05 marker.color.r = 1.0 marker.color.g = 0.5 marker.color.b = 0.3 marker.color.a = 1.0 marker.lifetime = rospy.Duration() return marker
def get_marker(self, line_seg, frame_id, scale=0.3): p0, p1 = line_seg v = np.asarray([p1.position.x - p0.position.x, p1.position.y - p0.position.y, p1.position.z - p0.position.z]) vnorm = np.linalg.norm(v) if vnorm > 0.0: v /= np.linalg.norm(v) p2 = copy.deepcopy(p0) p2.position.x += scale * v[0] p2.position.y += scale * v[1] p2.position.z += scale * v[2] m = Marker() m.header.stamp = rospy.Time.now() m.header.frame_id = frame_id m.ns = "beam" m.type = Marker.ARROW m.action = Marker.MODIFY m.lifetime = rospy.Duration(1) m.points = [p0.position, p1.position] m.scale.x = 0.02 m.scale.y = 0.04 m.color.r = 1.0 m.color.a = 1.0 return m
def __init__(self): self.layout = rospy.get_param('/thruster_layout/thrusters') # Add thruster layout from ROS param set by mapper assert self.layout is not None, 'Could not load thruster layout from ROS param' ''' Create MarkerArray with an arrow marker for each thruster at index node_id. The position of the marker is as specified in the layout, but the length of the arrow will be set at each thrust callback. ''' self.markers = MarkerArray() for i in xrange(len(self.layout)): # Initialize each marker (color, scale, namespace, frame) m = Marker() m.header.frame_id = '/base_link' m.type = Marker.ARROW m.ns = 'thrusters' m.color.a = 0.8 m.scale.x = 0.01 # Shaft diameter m.scale.y = 0.05 # Head diameter m.action = Marker.DELETE m.lifetime = rospy.Duration(0.1) self.markers.markers.append(m) for key, layout in self.layout.iteritems(): # Set position and id of marker from thruster layout idx = layout['node_id'] pt = numpy_to_point(layout['position']) self.markers.markers[idx].points.append(pt) self.markers.markers[idx].points.append(pt) self.markers.markers[idx].id = idx # Create publisher for marker and subscribe to thrust self.pub = rospy.Publisher('/thrusters/markers', MarkerArray, queue_size=5) self.thrust_sub = rospy.Subscriber('/thrusters/thrust', Thrust, self.thrust_cb, queue_size=5)
def to_marker_array(self): markers = [] for x in range(0, len(self.field)): for y in range(0, len(self.field[0])): marker = Marker() marker.header.stamp = rospy.get_rostime() marker.header.frame_id = '/odom_combined' marker.ns = 'suturo_planning/search_grid' marker.id = x + (y * len(self.field)) marker.action = Marker.ADD marker.type = Marker.CUBE marker.pose.position.x = self.coordinates[x, y][0] marker.pose.position.y = self.coordinates[x, y][1] marker.pose.position.z = self.coordinates[x, y][2] marker.pose.orientation.w = 1 marker.scale.x = 2.0 / len(self.coordinates) marker.scale.y = marker.scale.x marker.scale.z = 0.01 marker.color = SearchGrid._get_color_for_value(self.field[x][y]) marker.lifetime = rospy.Time(0) markers.append(marker) return MarkerArray(markers)
def _create_add_line(self, points, edges, point_group_name, id, red, green, blue): all_points = [] for i, end_points in enumerate(edges): for endpoint_index in end_points: pt1, pt2 = Point(), Point() pt1.x, pt1.y, pt1.z = points[i][0], points[i][1], points[i][2] all_points.append(pt1) pt2.x, pt2.y, pt2.z = points[endpoint_index][0], points[endpoint_index][1], points[endpoint_index][2] all_points.append(pt2) marker = Marker() marker.header.frame_id = "odom_combined" marker.header.stamp = rospy.get_rostime() marker.ns = point_group_name marker.id = id marker.type = Marker.LINE_STRIP marker.action = Marker.ADD marker.points = all_points marker.pose.position.x = 0.0 marker.pose.position.y = 0.0 marker.pose.position.z = 0.0 marker.pose.orientation.x = 0.0 marker.pose.orientation.y = 0.0 marker.pose.orientation.z = 0.0 marker.pose.orientation.w = 1.0 marker.scale.x = 0.003 marker.color.r = red marker.color.g = green marker.color.b = blue marker.color.a = 1.0 marker.lifetime = rospy.Duration() return marker
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 _create_add_point(self, coords, point_group_name, id, red, green, blue, point_radius): marker = Marker() marker.header.frame_id = "odom_combined" marker.header.stamp = rospy.get_rostime() marker.ns = point_group_name marker.id = id marker.type = Marker.SPHERE marker.action = Marker.ADD marker.pose.position.x = coords[0] marker.pose.position.y = coords[1] marker.pose.position.z = coords[2] marker.pose.orientation.x = 0.0 marker.pose.orientation.y = 0.0 marker.pose.orientation.z = 0.0 marker.pose.orientation.w = 1.0 marker.scale.x = point_radius marker.scale.y = point_radius marker.scale.z = point_radius marker.color.r = red marker.color.g = green marker.color.b = blue marker.color.a = 1.0 marker.lifetime = rospy.Duration() return marker
def publish(anns, data): ar_mk_list = AlvarMarkers() marker_list = MarkerArray() marker_id = 1 for a, d in zip(anns, data): # AR markers object = deserialize_msg(d.data, AlvarMarker) ar_mk_list.markers.append(object) # Visual markers marker = Marker() marker.id = marker_id marker.header = a.pose.header marker.type = a.shape marker.ns = "ar_mk_obstacles" marker.action = Marker.ADD marker.lifetime = rospy.Duration.from_sec(0) marker.pose = copy.deepcopy(a.pose.pose.pose) marker.scale = a.size marker.color = a.color marker_list.markers.append(marker) marker_id = marker_id + 1 marker_pub = rospy.Publisher('ar_mk_marker', MarkerArray, latch=True, queue_size=1) ar_mk_pub = rospy.Publisher('ar_mk_pose_list', AlvarMarkers,latch=True, queue_size=1) ar_mk_pub.publish(ar_mk_list) marker_pub.publish(marker_list) return
def pub_at_position(self, odom_msg): """ Handles necessary information for displaying things at ACCEPTED (NOVATEL) POSITION SOLUTION: -vehicle mesh !!!this should only be invoked when the accepted (novatel) position is updated """ ### G35 Mesh ############################################################# marker = Marker() pub = self.curpos_publisher marker.header = odom_msg.header marker.id = 0 # enumerate subsequent markers here marker.action = Marker.MODIFY # can be ADD, REMOVE, or MODIFY marker.pose = odom_msg.pose.pose marker.pose.position.z = 1.55 marker.lifetime = rospy.Duration() # will last forever unless modified marker.ns = "vehicle_model" marker.type = Marker.MESH_RESOURCE marker.scale.x = 0.0254 # artifact of sketchup export marker.scale.y = 0.0254 # artifact of sketchup export marker.scale.z = 0.0254 # artifact of sketchup export marker.color.r = .05 marker.color.g = .05 marker.color.b = .05 marker.color.a = .2 marker.mesh_resource = self.veh_mesh_resource marker.mesh_use_embedded_materials = False pub.publish(marker)
def drawlandmark(pub): global pt_count points_tag = Marker() points_tag.header.frame_id = "/base_link" points_tag.action = Marker.ADD points_tag.header.stamp = rospy.Time.now() points_tag.lifetime = rospy.Time(0) points_tag.scale.x = 0.1; points_tag.scale.y = 0.1; points_tag.scale.z = 0.1; points_tag.color.a = 1.0; points_tag.color.r = 1.0; points_tag.color.g = 0.0; points_tag.color.b = 0.0; points_tag.ns = "pts_line" pt_count+=1 points_tag.id = pt_count points_tag.type = Marker.POINTS for x in range(6): p1 = Point() p1.x = tagnum_x[x]/100 p1.y = tagnum_y[x]/100 p1.z = 0 points_tag.points.append(p1) pub.publish(points_tag)
def pub_body(self, pos, quat, scale, color, num, shape, text = ''): marker = Marker() marker.header.frame_id = self.frame marker.header.stamp = rospy.Time.now() marker.ns = "basic_shapes" marker.id = num marker.type = shape marker.action = Marker.ADD marker.pose.position.x = pos[0] marker.pose.position.y = pos[1] marker.pose.position.z = pos[2] 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.scale.x = scale[0] marker.scale.y = scale[1] marker.scale.z = scale[2] marker.color.r = color[0] marker.color.g = color[1] marker.color.b = color[2] marker.color.a = color[3] marker.lifetime = rospy.Duration() marker.text = text self.pub.publish(marker)
def publish_marker(cls, x_cord, y_cord, mid): new_marker = Marker(type = Marker.CUBE, action=Marker.ADD) new_marker.header.frame_id = "/base_laser_front_link" new_marker.header.stamp = rospy.Time.now() new_marker.ns = "basic_shapes" new_marker.id = mid new_marker.pose.position.x = x_cord new_marker.pose.position.y = y_cord new_marker.pose.position.z = 0.0 #pointxyz new_marker.pose.orientation.x = 0 new_marker.pose.orientation.y = 0 new_marker.pose.orientation.z = 0.0 new_marker.pose.orientation.w = 1 new_marker.scale.x = 0.005 new_marker.scale.y = 0.005 new_marker.scale.z = 0.005 if mid == 0: new_marker.color.r = 1 elif mid == 5: new_marker.color.g = 1 elif mid == 10: new_marker.color.b = 1 new_marker.color.a = 1 new_marker.text = "marker" new_marker.lifetime = rospy.Duration(1) SmachGlobalData.pub_marker.publish(new_marker)
def draw_table_rviz(self, id, table_msg, color = (1.0, 1.0, 1.0, 0.0), duration = 0.0): marker = Marker() marker.header.stamp = rospy.Time.now() marker.ns = "object_detector" marker.type = Marker.CUBE marker.action = marker.ADD marker.id = id marker.header.frame_id = table_msg.pose.header.frame_id marker.pose = table_msg.pose.pose marker.pose.position.x = (table_msg.x_max-table_msg.x_min)/2 \ + table_msg.x_min marker.pose.position.y = (table_msg.y_max-table_msg.y_min)/2 \ + table_msg.y_min marker.scale.x = table_msg.x_max - table_msg.x_min marker.scale.y = table_msg.y_max - table_msg.y_min marker.scale.z = 0.01 marker.color.a = color[0] marker.color.r = color[1] marker.color.g = color[2] marker.color.b = color[3] marker.lifetime = rospy.Duration(duration) self.box_drawer.publish(marker)
def create_person_label_marker(self, person, color): h = Header() h.frame_id = person.header.frame_id #tie marker visualization to laser it comes from h.stamp = rospy.Time.now() # Note you need to call rospy.init_node() before this will work #create marker:person_marker, modify a red cylinder, last indefinitely mark = Marker() mark.header = h mark.ns = "person_label_marker" ## simple hack to map persons name to integer value for unique marker id char_list = list(person.name) char_int_list = [str(ord(x)) for x in char_list] char_int_str = "".join(char_int_list) char_int = int(char_int_str) & 255 print print "str to int" print char_int_list print char_int print "Char int binary) ", bin(char_int) # mark.id = int(char_int / 10000) mark.id = int(float(person.name)) #char_int mark.type = Marker.TEXT_VIEW_FACING mark.action = 0 mark.scale = Vector3(self.scale_factor * 0.5, self.scale_factor * 0.5, 1) mark.color = color mark.lifetime = rospy.Duration(0.5,0) print "person name: ", person.name mark.text = person.name pose = Pose(Point(person.pose.position.x + 0.75, person.pose.position.y + 0.5, self.person_height + 0.75), Quaternion(0.0,0.0,1.0,cos(person.pose.position.z/2))) mark.pose = pose return mark
def _visualize_breadcrumbs(self): breadcrumbs_msg = Marker() breadcrumbs_msg.type = Marker.POINTS breadcrumbs_msg.scale.x = 0.05 breadcrumbs_msg.header.stamp = rospy.get_rostime() breadcrumbs_msg.header.frame_id = "/map" breadcrumbs_msg.color.a = 1 breadcrumbs_msg.color.r = 0 breadcrumbs_msg.color.g = 1 breadcrumbs_msg.color.b = 1 breadcrumbs_msg.lifetime = rospy.Time(1.0) breadcrumbs_msg.id = 0 breadcrumbs_msg.action = Marker.ADD for crumb in self._breadcrumbs: breadcrumbs_msg.points.append( kdl_conversions.kdl_vector_to_point_msg(crumb.pose.frame.p)) self._breadcrumb_pub.publish(breadcrumbs_msg)
def publish_marker(self, poses, quaternion=[0, 0, 0, 1.0], clear=False): ''' erase all and re-append ''' self.array = MarkerArray() for pose in poses: m = Marker() m.lifetime = rospy.Duration.from_sec(0.1) m.header.frame_id = 'map' m.type = self.marker_type m.action = m.ADD m.scale.x = self.scale[0] m.scale.y = self.scale[1] m.scale.z = self.scale[2] m.color.r = self.rgba[0] m.color.g = self.rgba[1] m.color.b = self.rgba[2] m.color.a = self.rgba[3] m.pose.orientation.x = quaternion[0] m.pose.orientation.y = quaternion[1] m.pose.orientation.z = quaternion[2] m.pose.orientation.w = quaternion[3] m.pose.position.x = pose[0] m.pose.position.y = pose[1] m.pose.position.z = 0.0 self.array.markers.append(m) # ''' add new marker and remove the old one ''' # self.MARKER_MAX = len(poses) # if self.COUNT > self.MARKER_MAX: # self.array.markers.pop(0) # self.array.markers.append(m) # self.COUNT += 1 ''' Renumber the marker IDs ''' id = 0 for ma in self.array.markers: ma.id = id id += 1 self.marker_pub.publish(self.array)
def rviz_visualization_stl(str): str1 = 'package://aubo_demo/stl_test/' str = str1 + str color = ColorRGBA() color.r = 1.0 color.g = 0.0 color.b = 0.0 color.a = 1.0 position = Point() position.x = 0.0 position.y = 0.0 position.z = 0.0 marker = Marker() marker.header.frame_id = "/world" marker.type = Marker.MESH_RESOURCE marker.mesh_resource = "package://aubo_demo/stl_test/stl_document/0015.STL" # marker.mesh_resource=str marker.action = Marker.ADD marker.pose.orientation.w = 1.0 marker.scale.x = 1.0 marker.scale.y = 1.0 marker.scale.z = 1.0 marker.ns = 'arrow' marker.id = 1 marker.lifetime = rospy.Duration() marker.color.r = color.r marker.color.g = color.g marker.color.b = color.b marker.color.a = color.a marker.pose.position.x = position.x marker.pose.position.y = position.y marker.pose.position.z = position.z marker_pub = rospy.Publisher("visualization_marker", Marker, queue_size=10) rospy.init_node('markers') rate = rospy.Rate(10) while not rospy.is_shutdown(): marker_pub.publish(marker) rate.sleep()
def make_circle_marker(point, scale, color, frame_id, namespace, sid, duration=0): marker = Marker() marker.header = make_header(frame_id) marker.ns = namespace marker.id = sid marker.type = 2 # sphere marker.lifetime = rospy.Duration.from_sec(duration) marker.action = 0 marker.pose.position.x = point[0] marker.pose.position.y = point[1] marker.pose.orientation.w = 1.0 marker.scale.x = scale marker.scale.y = scale marker.scale.z = scale marker.color.r = float(color[0]) marker.color.g = float(color[1]) marker.color.b = float(color[2]) marker.color.a = 1.0 return marker
def delete_callback(self, msg): print("Deleting last face") del detected[self.marker_id_counter - 1] markers2 = MarkerArray() self.marker_id_counter -= 1 for i, detec in enumerate(detected): marker = Marker() marker.header.stamp = rospy.Time.now() marker.header.frame_id = "map" marker.pose = detec.pose marker.type = Marker.CUBE marker.action = Marker.ADD marker.frame_locked = False marker.lifetime = rospy.Duration.from_sec(1) marker.id = i marker.scale = Vector3(0.1, 0.1, 0.1) marker.color = ColorRGBA(1, 0, 0, 1) markers2.markers.append(marker) self.markers = markers2
def construct_landmark(lm_new, lm_pub): pos = curr_global_pose.position #tfPub = tf2_ros.StaticTransformBroadcaster() t = TransformStamped() t.header.stamp = rospy.Time.now() t.header.frame_id = "map" t.child_frame_id = lm_new t.transform.translation.x = pos.y t.transform.translation.y = pos.x t.transform.translation.z = 0 t.transform.rotation.x = 0.0 t.transform.rotation.y = 0.0 t.transform.rotation.z = 0.0 t.transform.rotation.w = 1 #tfPub.sendTransform(t) if already_known(lm_known, lm_new, pos): return False else: lm_known[lm_new] = pos # make and publish the marker mrk = Marker() mrk.header.frame_id = lm_new mrk.header.stamp = rospy.Time.now() mrk.ns = "/landmarkers" mrk.id = int(random.random()*1000) mrk.type = 9 mrk.action = 0 mrk.pose = globalPose mrk.pose.orientation.w = 1 mrk.scale.x = 5 mrk.scale.y = 0 mrk.scale.z = 0 mrk.color.r = 0.2 mrk.color.g = 0.4 mrk.color.b = 1.0 mrk.color.a = 0.7 mrk.lifetime= rospy.Duration(0) mrk.frame_locked = True mrk.text = lm_new lm_pub.publish(mrk) return True
def _publish(self, time): assert time != rospy.Time(0) out_msg = ObstacleArray() out_msg.header.stamp = time out_msg.header.frame_id = 'map' marker_array = MarkerArray() for f in self._filters: obstacle = f['filter'].get_state(time) out_msg.obstacles.append(obstacle) marker = Marker() marker.header.stamp = time marker.header.frame_id = 'map' marker.ns = 'obstacles' marker.id = uuid.UUID('{'+obstacle.odom.child_frame_id[len('obstacle-'):-len('/base_link')]+'}').int % (2**31) marker.type = Marker.CYLINDER marker.action = Marker.ADD marker.pose.orientation.w = 1.0 marker.scale.x = obstacle.pipe_radius marker.scale.y = obstacle.pipe_radius marker.scale.z = obstacle.pipe_height marker.color.r = 0.0 marker.color.g = 1.0 marker.color.b = 0.0 marker.color.a = 1.0 marker.lifetime = rospy.Duration(0.2) marker.pose.position.x = obstacle.odom.pose.pose.position.x marker.pose.position.y = obstacle.odom.pose.pose.position.y marker.pose.position.z = obstacle.pipe_height / 2. marker_array.markers.append(marker) self._marker_pub.publish(marker_array) self._pub.publish(out_msg)
def append_marker(pos, color): marker = Marker() marker.header.frame_id = "/simmap" marker.type = marker.SPHERE marker.id = pos[0] * pos[1] marker.lifetime = rospy.Duration(0) marker.action = marker.ADD marker.scale.x = 0.2 marker.scale.y = 0.2 marker.scale.z = 0.2 marker.color.a = 1.0 marker.color.r = color[0] marker.color.g = color[1] marker.color.b = color[2] marker.pose.orientation.w = 1.0 marker.pose.position.x = pos[0] marker.pose.position.y = pos[1] marker.pose.position.z = 0 buoy_array.markers.append(marker)
def place_marker_pose(posestamped, pub, _id=0, _type=Marker.CUBE, ns='basic_shapes',\ r=0, g=1, b=0, a=1, xscale=.03, yscale=.03, zscale=.03,\ orientation=Quaternion(0,0,0,1), text=''): marker = Marker(type=_type, action=Marker.ADD) marker.ns = ns marker.header.frame_id = posestamped.header.frame_id marker.header.stamp = rospy.Time.now() marker.pose = posestamped.pose marker.scale.x = xscale marker.scale.y = yscale marker.scale.z = zscale marker.color.r = r marker.color.g = g marker.color.b = b marker.color.a = a marker.id = _id marker.text = text marker.lifetime = rospy.Duration() pub.publish(marker)
def publish_grid(self): marker = Marker() marker.header.frame_id = 'pegasus_map' marker.ns = 'unvisited' marker.id = 0 marker.type = marker.POINTS marker.action = marker.ADD marker.pose.orientation.w = 1 marker.points = self.unvisited_points marker.lifetime = rospy.Duration() marker.scale.x = 5 marker.scale.y = 5 marker.scale.z = 5 marker.color.g = 1 marker.color.r = 1 marker.color.b = 0 marker.color.a = 1 self.publisher_marker.publish(marker)
def evaluateObject_sim(objectData): # evaluate the object classification marker = Marker() r, g, b, typ = evaluateColor( evaluateClassification(objectData.classification)) #assign frame id marker.header.frame_id = "/ego" # assign the geometric shape of the object e.g. Cube marker.type = typ # assign the marker option ADD marker.action = marker.ADD # assign the size of the object marker.scale.x = objectData.dimension.length marker.scale.y = objectData.dimension.width marker.scale.z = objectData.dimension.height # assign the color of the object to the marker marker.color.a = 1.0 # alpha Channel marker.color.r = r # red marker.color.g = g # green marker.color.b = b # blue # Convert RPY to quaternion q = quaternion_from_euler(0, 0, np.deg2rad(objectData.geometric.yaw)) marker.pose.orientation.x = q[0] marker.pose.orientation.y = q[1] marker.pose.orientation.z = q[2] marker.pose.orientation.w = q[3] # Evaluate Position of each Object marker.pose.position.x = car_ego_x + objectData.geometric.x marker.pose.position.y = car_ego_y + objectData.geometric.y * ( -1) # change the coordinate-system marker.pose.position.z = objectData.dimension.height / 2 # Lifetime of the object, after 0.1 second the object will disappear marker.lifetime = rospy.Duration(0.1) return marker
def set_up_wheel(self): self.markers_wheel = MarkerArray() it_id = 0 wheel_part = Marker() wheel_part.header.frame_id = "/robotiq_force_torque_frame_id" wheel_part.ns = "ur5" wheel_part.id = it_id wheel_part.type = Marker().CYLINDER wheel_part.action = Marker().ADD # wheel_part.scale = Vector3(0.08, 0.08, 0.035) wheel_part.scale = Vector3(0.08, 0.08, 0.06) wheel_part.color = ColorRGBA(100. / 265, 100. / 265, 100. / 265, 1) wheel_part.pose.orientation = Quaternion(0, 0, 0, 1) wheel_part.lifetime = rospy.Duration.from_sec(0) wheel_part.frame_locked = True self.markers_wheel.markers.append(wheel_part) dz = wheel_part.scale.z / 2 wheel_part.pose.position = Point(0, 0, dz) dz += wheel_part.scale.z / 2 it_id += 1 wheel_part = copy.deepcopy(wheel_part) wheel_part.id = it_id wheel_part.scale = Vector3(0.40, 0.40, 0.001) wheel_part.color = ColorRGBA(70. / 265, 70. / 265, 70. / 265, 1.) dz += wheel_part.scale.z / 2 wheel_part.pose.position = Point(0, 0, dz) dz += wheel_part.scale.z / 2 self.markers_wheel.markers.append(wheel_part) it_id += 1 wheel_part = copy.deepcopy(wheel_part) wheel_part.id = it_id wheel_part.scale = Vector3(0.40, 0.40, 0.014) wheel_part.color = ColorRGBA(130. / 265, 130. / 265, 130. / 265, 1.) dz += wheel_part.scale.z / 2 wheel_part.pose.position = Point(0, 0, dz) dz += wheel_part.scale.z / 2 self.markers_wheel.markers.append(wheel_part)
def footPoseToMarker(pose,leg): m = Marker() m.header=Header() m.header.frame_id = strings.map_frame m.header.stamp = rospy.Time.now() m.ns = ' ' m.type = Marker.CUBE m.action = Marker.ADD cos_theta = math.cos(pose.theta) sin_theta = math.sin(pose.theta) x_shift = cos_theta * foot_origin_shift_x - sin_theta * foot_origin_shift_y if leg == strings.left: y_shift = sin_theta * foot_origin_shift_x + cos_theta * foot_origin_shift_y else: y_shift = sin_theta * foot_origin_shift_x - cos_theta * foot_origin_shift_y m.pose.position.x = pose.x + x_shift m.pose.position.y = pose.y + y_shift m.pose.position.z = footsize_z / 2.0 q=tf.transformations.quaternion_from_euler(0.0, 0.0, pose.theta) m.pose.orientation.x = q[0] m.pose.orientation.y = q[1] m.pose.orientation.z = q[2] m.pose.orientation.w = q[3] m.scale.x = footsize_x m.scale.y = footsize_y m.scale.z = footsize_z if leg == strings.right: m.color.r = 0.0 m.color.g = 1.0 else: m.color.r = 1.0 m.color.g = 0.0 m.color.b = 0.0 m.color.a = 0.6 m.lifetime = rospy.Duration() return m
def ref_callback(self, msg): self._ref[0, 0] = msg.x self._ref[1, 0] = msg.xdot1 self._ref[2, 0] = msg.xdot2 self._ref[3, 0] = msg.xdot3 self._ref[4, 0] = msg.y self._ref[5, 0] = msg.ydot1 self._ref[6, 0] = msg.ydot2 self._ref[7, 0] = msg.ydot3 self._ref[8, 0] = msg.z self._ref[9, 0] = msg.zdot1 self._ref[10, 0] = msg.zdot2 self._ref[11, 0] = msg.zdot3 self._ref[12, 0] = msg.psi self._ref[13, 0] = msg.psidot1 # Can mess with the viz later. marker = Marker() marker.header.frame_id = self._fixed_frame marker.header.stamp = rospy.Time.now() marker.ns = "reference" marker.id = self._ref_viz_last_id marker.type = Marker.SPHERE marker.action = Marker.ADD marker.pose.position.x = msg.x marker.pose.position.y = msg.y marker.pose.position.z = msg.z marker.pose.orientation.x = 0.0 marker.pose.orientation.y = 0.0 marker.pose.orientation.z = 0.0 marker.pose.orientation.w = 1.0 marker.scale.x = 0.1 marker.scale.y = 0.1 marker.scale.z = 0.1 marker.color.a = 1.0 marker.color.r = 0.0 marker.color.g = 0.75 marker.color.b = 1.0 marker.lifetime = rospy.Duration(5) self._ref_viz_pub.publish(marker) self._ref_viz_last_id += 1
def publish_trajectory_markers(self, duration): """ Publishes markers to visualize the current trajectory. Paremeters: duration: how long should the markers visualization last. If this function is called from a loop they last at least the loop rate. """ if len(self.trajectory.poses) == 0: return msg = MarkerArray() marker_id = 0 #creating the path connecting the axes path = Marker() path.header.frame_id = self.trajectory.header.frame_id path.ns = "path" path.action = Marker.ADD path.type = Marker.LINE_STRIP path.lifetime = rospy.Duration(duration) path.color.r = 1 path.color.g = 0 path.color.b = 1 path.color.a = 1 path.scale.x = 0.01 path.id = marker_id marker_id += 1 for pose in self.trajectory.poses: pos = PoseStamped() pos.header.frame_id = self.trajectory.header.frame_id pos.pose = pose markers = utils.axis_marker(pos, marker_id, "axes") msg.markers.extend(markers) path.points.append(pose.position) marker_id += 3 #3 axes msg.markers.append(path) self.visualizer_pub.publish(msg)
def publish_Bottle_Pos_Marker(posbouteille_x, posbouteille_y, id_num, cutted): global ObjectCut #setup de l'objet marqueur rospy.loginfo(" ") _cmd_pub = rospy.Publisher('bottle', Marker, queue_size=10) marker_msg = Marker() marker_msg.header.frame_id = "map" marker_msg.header.stamp = rospy.Time.now() marker_msg.ns = "my_point" marker_msg.id = id_num #changement de format de marqueur dans le cas d'une bouteille coupee if cutted: marker_msg.color.r = 1.0 marker_msg.color.g = 0.0 marker_msg.color.b = 0.0 ObjectCut = False else: marker_msg.color.r = 0.0 marker_msg.color.g = 1.0 marker_msg.color.b = 0.0 marker_msg.type = Marker().CUBE marker_msg.action = Marker().ADD #placement du marqueur a une distance d du robot dependant de la fonction detection() marker_msg.pose.position.x = posbouteille_x marker_msg.pose.position.y = posbouteille_y marker_msg.pose.position.z = 0.0 #affichage marqueur RVIZ marker_msg.color.a = 1.0 #definition de la taille du marqueur marker_msg.scale.x = 0.15 marker_msg.scale.y = 0.15 marker_msg.scale.z = 0.15 marker_msg.lifetime = rospy.Duration(0) _cmd_pub.publish(marker_msg)
def plot_blocked_edges(self, node_list): """ Plot blocked branches in the rrt tree. """ tree = MarkerArray() id = 1 for node in self.node_list: if node.parent and node.parent.cost == float('Inf'): # edge between nodes path = Marker() path.header.frame_id = "map" path.header.stamp = rospy.get_rostime() path.ns = "markers" path.id = id id += 1 path.type = path.LINE_STRIP path.action = path.ADD path.scale.x = self.rviz_tuning_plt path.color.a = 1.0 path.color.r = 1.0 path.color.g = 0.0 path.color.b = 0.0 path.lifetime = rospy.Duration() path.pose.orientation.w = 1.0 p1 = Point() p1.x = node.parent.x p1.y = node.parent.y p1.z = 0.03 path.points.append(p1) p2 = Point() p2.x = node.x p2.y = node.y p2.z = 0.03 path.points.append(p2) tree.markers.append(path) self.pub_blocked_edges.publish(tree)
def set_target_action_marker(action_list, target=False): goal_marker = Marker() goal_marker.action = goal_marker.ADD goal_marker.header.frame_id = "table_gym" # goal_marker.header.stamp = rospy.get_rostime() # goal_marker.ns = "goal_marker" goal_marker.lifetime = rospy.Duration(60) # goal_marker.id = 0 goal_marker.type = goal_marker.LINE_STRIP goal_marker.pose.position.x = 0.0 goal_marker.pose.position.y = 0.0 goal_marker.pose.position.z = 0.00 goal_marker.pose.orientation.x = 0.0 goal_marker.pose.orientation.y = 0.0 goal_marker.pose.orientation.z = 0.0 goal_marker.pose.orientation.w = 1.0 goal_marker.scale.x = 0.001 goal_marker.color.r = 1.0 goal_marker.color.g = 0.0 goal_marker.color.b = 0.0 goal_marker.color.a = 1.0 #### if this marker is target action if target is True: # goal_marker.id = 100000 goal_marker.color.r = 0.0 goal_marker.color.g = 0.0 goal_marker.color.b = 1.0 goal_marker.scale.x = 0.005 goal_marker.pose.position.z = 0.02 goal_marker.points = [] n = len(action_list) for i in range(n): point = geometry_msgs.msg.Point() point.x = action_list[i][0] point.y = action_list[i][1] point.z = 0.0475 goal_marker.points.append(point) return goal_marker
def PublishTreeArray(self, tree): markarray = MarkerArray() for i in range(1,len(tree)): x_index = tree[i].x y_index = tree[i].y parent_index = tree[i].parent parent_x = tree[parent_index].x parent_y = tree[parent_index].y node_x = self.occ_grid[x_index][y_index][1] node_y = self.occ_grid[x_index][y_index][2] pnode_x = self.occ_grid[parent_x][parent_y][1] pnode_y = self.occ_grid[parent_x][parent_y][2] z = 0.025 points = self.ConstructPointsArray(node_x, node_y, pnode_x, pnode_y, z) marker = Marker() marker.ns = "tree" marker.action = marker.MODIFY marker.header.frame_id = "/map" marker.type = marker.LINE_STRIP marker.points = points marker.scale.x = 0.05 marker.color.g = 1.0 marker.color.a = 1.0 marker.pose.orientation.x = 0.0 marker.pose.orientation.y = 0.0 marker.pose.orientation.z = 0.0 marker.pose.orientation.w = 1.0 marker.lifetime = rospy.Duration(0.1) markarray.markers.append(marker) id = 0 for m in markarray.markers: m.id = id id += 1 self.tree_pub.publish(markarray)
def viz_cars(self, relevant_msg, header): marker_msg = MarkerArray() count = 0 for obj in relevant_msg.objects: mk = Marker() mk.header = header mk.type = Marker.CUBE mk.scale.x = 3.7 mk.scale.y = 1.6 mk.scale.z = 0.5 mk.color.a = 1 mk.color.r = 1 mk.color.g = 0 mk.color.b = 0 mk.pose = obj.pose mk.id = count mk.lifetime = rospy.Duration(0.05) marker_msg.markers.append(mk) count += 1 self.marker_pub.publish(marker_msg)
def publishTrace(time): global viz_pub global i msg = Marker() msg.header.frame_id = 'manipulator' msg.header.stamp = rospy.Time.now() msg.ns = "xd" i+=1 msg.id = i msg.lifetime = rospy.Duration(time+3.) msg.type = Marker.SPHERE msg.action = Marker.ADD msg.scale.x = 0.01 msg.scale.y = 0.01 msg.scale.z = 0.01 msg.color.r = 0. msg.color.g = 0. msg.color.b = 1. msg.color.a = 175./255. viz_pub.publish(msg)
def np_array_to_marker(self, id, p, v=0, a=0): marker = Marker() marker.header.frame_id = "map" marker.type = marker.CUBE marker.action = marker.ADD marker.id = id marker.scale.x = self.resolution marker.scale.y = self.resolution marker.scale.z = 0.1 marker.color.r = v / 72.0 marker.color.g = 0 marker.color.b = 0.5 marker.color.a = a marker.pose.orientation.w = 1.0 marker.pose.position.x = p[0] marker.pose.position.y = p[1] marker.pose.position.z = p[2] marker.lifetime = rospy.Time(10) return marker
def publishCube(pos, ori, scale, id): m = Marker() m.id = id m.ns = 'table' m.action = m.ADD m.type = m.CUBE m.header.stamp = rospy.Time.now() m.header.frame_id = frame_name m.lifetime = rospy.Duration(4.0) m.color = ColorRGBA(0.6, 0.4, 0.2, 1) m.pose.position = Point(*pos) m.pose.orientation = Quaternion(*ori) m.scale = Vector3(*scale) pub.publish(m)
def create_marker_point(ns='debug', frame_id='map'): """ creation of a rviz marker of type point :param ns: namespace to use :param frame_id: reference frame to state :return: """ point_marker = Marker() point_marker.header.frame_id = frame_id point_marker.ns = ns point_marker.id = 1 point_marker.type = Marker.POINTS point_marker.action = Marker.ADD point_marker.color.r = 1.0 point_marker.color.a = 1.0 point_marker.scale.x = 0.005 point_marker.scale.y = 0.005 point_marker.scale.z = 0.005 point_marker.lifetime = rospy.Duration(0.1) return point_marker
def pubMarker(self, pose): if self.markers: marker = Marker() marker.header.frame_id = "odom" marker.header.stamp = rospy.Time.now() marker.ns = "wps" marker.id = self.marker_id marker.type = Marker.SPHERE marker.action = Marker.ADD marker.pose = pose marker.scale.x = 1 marker.scale.y = 1 marker.scale.z = 1 marker.color.a = 1.0 marker.color.r = 0.0 marker.color.g = 1.0 marker.color.b = 0.0 marker.lifetime = rospy.Duration(60) self.vis_pub.publish(marker) self.marker_id += 1
def publishCylinder(pos, ori, scale, color, id): m = Marker() m.id = id m.ns = 'baker' m.action = m.ADD m.type = m.CYLINDER m.header.stamp = rospy.Time.now() m.header.frame_id = frame_name m.lifetime = rospy.Duration(4.0) m.color = ColorRGBA(*color) m.pose.position = Point(*pos) m.pose.orientation = Quaternion(*ori) m.scale = Vector3(*scale) pub.publish(m)
def visualize(self): m_array = MarkerArray() for i, g in enumerate(self.gates): m = Marker() m.header = Header(frame_id='/enu', stamp=rospy.Time.now()) m.ns = 'gates' m.id = i m.type = Marker.LINE_STRIP m.action = Marker.ADD m.scale.x = 0.1 m.color.r = 0.5 m.color.b = 0.75 m.color.g = 0.1 m.color.a = 1.0 m.points.append(g[1].buoy1.position) m.points.append(g[1].buoy2.position) m.lifetime = rospy.Duration(self.lifetime) m_array.markers.append(m) self.gate_viz_pub.publish(m_array)
def publish_closest_person_marker(self, pose, frame_id): if self.vis_marker_topic_pub.get_num_connections() > 0: m = Marker() m.header.stamp = rospy.Time.now() m.header.frame_id = frame_id m.ns = "velocity_costmaps" m.id = 0 m.type = m.SPHERE m.action = m.MODIFY m.pose = pose m.pose.position.z = 2.0 m.scale.x = .25 m.scale.y = .25 m.scale.z = .25 m.color.a = 1.0 m.color.r = 0.0 m.color.g = 1.0 m.color.b = 0.0 m.lifetime = rospy.Duration(1.) self.vis_marker_topic_pub.publish(m)
def get_path_markers(self): markers = [] for i, pose_stamped in enumerate(self.path.poses): marker = Marker() marker.header.frame_id = 'pegasus_map' marker.ns = 'path_markers' marker.id = 0 marker.type = marker.TEXT_VIEW_FACING marker.action = marker.ADD marker.pose.orientation.w = 1 marker.text = '%s' % (i, ) marker.lifetime = rospy.Duration() marker.scale.z = 10 marker.color.g = 0 marker.color.r = 1 marker.color.b = 1 marker.color.a = 1 marker.pose = pose_stamped.pose markers.append(marker) return markers
def node_to_marker(self, id, node): marker = Marker() marker.header.frame_id = "map" marker.type = marker.SPHERE marker.action = marker.ADD marker.id = id marker.scale.x = 0.4 marker.scale.y = 0.4 marker.scale.z = 0.4 marker.color.r = node.gain / 72.0 marker.color.g = 0.0 marker.color.b = 0.5 marker.color.a = 1.0 marker.pose.orientation.w = 1.0 marker.pose.position.x = node.position.x marker.pose.position.y = node.position.y marker.pose.position.z = node.position.z marker.lifetime = rospy.Time(1.2) return marker