def _make_mesh_marker(self): '''Creates a mesh marker''' mesh = Marker() mesh.mesh_use_embedded_materials = False mesh.type = Marker.MESH_RESOURCE mesh.scale.x = 1.0 mesh.scale.y = 1.0 mesh.scale.z = 1.0 alpha = 0.6 if self.is_dimmed: alpha = 0.1 if self.is_fake: alpha = 0.4 mesh.color = ColorRGBA(0.3, 0.3, 0.3, alpha) return mesh if self._is_reachable(): # Original: some kinda orange # r,g,b = 1.0, 0.5, 0.0 # New: rainbow! See method comment for details. r,g,b = self.get_marker_color() mesh.color = ColorRGBA(r, g, b, alpha) else: mesh.color = ColorRGBA(0.5, 0.5, 0.5, alpha) return mesh
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 pubViz(self, ast, bst): rate = rospy.Rate(10) for i in range(self.winSize): msgs = MarkerArray() #use1について msg = Marker() #markerのプロパティ msg.header.frame_id = 'camera_link' msg.header.stamp = rospy.Time.now() msg.ns = 'j1' msg.action = 0 msg.id = 1 msg.type = 8 msg.scale.x = 0.1 msg.scale.y = 0.1 msg.color = self.carray[2] #ジョイントポイントを入れる処理 for j1 in range(self.jointSize): point = Point() point.x = self.pdata[0][ast+i][j1][0] point.y = self.pdata[0][ast+i][j1][1] point.z = self.pdata[0][ast+i][j1][2] msg.points.append(point) msg.pose.orientation.w = 1.0 msgs.markers.append(msg) msg = Marker() msg.header.frame_id = 'camera_link' msg.header.stamp = rospy.Time.now() msg.ns = 'j2' msg.action = 0 msg.id = 2 msg.type = 8 msg.scale.x = 0.1 msg.scale.y = 0.1 msg.color = self.carray[1] for j2 in range(self.jointSize): point = Point() point.x = self.pdata[1][bst+i][j2][0] point.y = self.pdata[1][bst+i][j2][1] point.z = self.pdata[1][bst+i][j2][2] msg.points.append(point) msg.pose.orientation.w = 1.0 msgs.markers.append(msg) self.mpub.publish(msgs) rate.sleep()
def _make_mesh_marker(self): '''Creates a mesh marker''' mesh = Marker() mesh.mesh_use_embedded_materials = False mesh.type = Marker.MESH_RESOURCE mesh.scale.x = 1.0 mesh.scale.y = 1.0 mesh.scale.z = 1.0 if self._is_reachable(): mesh.color = ColorRGBA(1.0, 0.5, 0.0, 0.6) else: mesh.color = ColorRGBA(0.5, 0.5, 0.5, 0.6) return mesh
def _make_mesh_marker(self): mesh = Marker() mesh.mesh_use_embedded_materials = False mesh.type = Marker.MESH_RESOURCE mesh.scale.x = 1.0 mesh.scale.y = 1.0 mesh.scale.z = 1.0 mesh.color = ColorRGBA(0.0, 0.5, 1.0, 0.6) ik_solution = self.ik.get_ik_for_ee(self.ee_pose) if (ik_solution == None): mesh.color = ColorRGBA(1.0, 0.0, 0.0, 0.6) else: mesh.color = ColorRGBA(0.0, 1.0, 0.0, 0.6) return mesh
def pubRviz(self, pos, joints): msgs = MarkerArray() for p in range(len(pos)): msg = Marker() msg.header.frame_id = 'camera_link' msg.header.stamp = rospy.Time.now() msg.ns = 'marker' msg.action = 0 msg.id = p msg.type = 4 msg.scale.x = 0.1 msg.scale.y = 0.1 msg.color = self.carray[2] for i in range(len(pos[p])): point = Point() point.x = pos[p][i][0] point.y = pos[p][i][1] point.z = pos[p][i][2] msg.points.append(point) msg.pose.orientation.w = 1.0 msgs.markers.append(msg) for j in range(len(joints)): msg = Marker() msg.header.frame_id = 'camera_link' msg.header.stamp = rospy.Time.now() msg.ns = 'joints' msg.action = 0 msg.id = j msg.type = 8 msg.scale.x = 0.1 msg.scale.y = 0.1 msg.color = self.carray[j] #print "joints len:"+str(len(joints[j])) for i in range(len(joints[j])): point = Point() point.x = joints[j][i][0] point.y = joints[j][i][1] point.z = joints[j][i][2] msg.points.append(point) msg.pose.orientation.w = 1.0 msgs.markers.append(msg) self.mpub.publish(msgs)
def _make_mesh_marker(self): mesh = Marker() mesh.mesh_use_embedded_materials = False mesh.type = Marker.MESH_RESOURCE mesh.scale.x = 1.0 mesh.scale.y = 1.0 mesh.scale.z = 1.0 target_pose = GripperMarkers._offset_pose(self.ee_pose) ik_sol = self._ik_service.get_ik_for_ee(target_pose) if ik_sol == None: mesh.color = ColorRGBA(1.0, 0.0, 0.0, 0.6) else: mesh.color = ColorRGBA(0.0, 1.0, 0.0, 0.6) return mesh
def _pub_sight_areas_cylinder(self, orders, target=True): """ Publish sight areas as cylinders """ color = ColorRGBA() if target: ns = 'sight.target.cyl' color = COL_TARGET else: ns = 'sight.current.cyl' color = COL_CURRENT x_coords, y_coords = extract_coords(orders, target=target) viz = Marker() viz.header.stamp = rospy.Time() viz.header.frame_id = '/map' viz.action = viz.MODIFY viz.ns = ns viz.type = viz.CYLINDER viz.color = color viz.scale.x = 2*orders.sight_radius viz.scale.y = 2*orders.sight_radius viz.scale.z = CYL_HEIGHT viz.lifetime = MARKERS_LIFETIME for i_drone in range(orders.num_drones): viz.id = i_drone viz.pose.position.x = x_coords[i_drone] viz.pose.position.y = y_coords[i_drone] viz.pose.position.z = CYL_HEIGHT/2. self.orders_viz_pub.publish(viz)
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 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 marker_from_point_radius(point, radius, index=0, linewidth=0.1, color=ColorRGBA(1, 0, 0, 1), z=0., lifetime=10.0): marker = Marker() marker.header = make_header("/map") marker.ns = "Speed_NS" marker.id = index marker.type = Marker.CYLINDER marker.action = 0 # action=0 add/modify object # marker.color.r = 1.0 # marker.color.g = 0.0 # marker.color.b = 0.0 # marker.color.a = 0.4 marker.color = color marker.lifetime = rospy.Duration.from_sec(lifetime) marker.pose = Pose() marker.pose.position.z = z marker.pose.position.x = point[0] marker.pose.position.y = point[1] marker.scale = Vector3(radius * 2.0, radius * 2.0, 0.001) return marker
def draw_wall(self, points, c=(1.0, 0.0, 0.0, 1.0)): ''' draws marker line in rviz based on list of points :param points: list of points :param c: RGBA color tuple; defaults to red ''' marker = Marker() marker.id = 0 marker.header.frame_id = 'base_link' marker.type = Marker.LINE_STRIP marker.lifetime = rospy.Duration(60) marker.color = ColorRGBA(*c) marker.scale.x = 0.1 marker.scale.y = 0.1 marker.scale.z = 0.1 marker.pose.position.x = 0 marker.pose.position.y = 0 marker.pose.position.z = 0 marker.pose.orientation.x = 0 marker.pose.orientation.y = 0 marker.pose.orientation.z = 0 marker.pose.orientation.w = 1 marker.points = points self.marker_pub.publish(marker)
def as_marker_msg(self, ns=u'', id=1): """ :param ns: :param id: :rtype: Marker """ if len(self.get_link_names()) > 1: raise TypeError( u'only urdfs objects with a single link can be turned into marker' ) link = self.get_urdf_link(self.get_link_names()[0]) m = Marker() m.ns = u'{}/{}'.format(ns, self.get_name()) m.id = id if link.visual: geometry = link.visual.geometry else: geometry = link.visuals[0].geometry if isinstance(geometry, up.Box): m.type = Marker.CUBE m.scale = Vector3(*geometry.size) elif isinstance(geometry, up.Sphere): m.type = Marker.SPHERE m.scale = Vector3(geometry.radius * 2, geometry.radius * 2, geometry.radius * 2) elif isinstance(geometry, up.Cylinder): m.type = Marker.CYLINDER m.scale = Vector3(geometry.radius * 2, geometry.radius * 2, geometry.length) else: raise Exception( u'world body type {} can\'t be converted to marker'.format( geometry.__class__.__name__)) m.color = ColorRGBA(0, 1, 0, 0.5) return m
def create_point_list(cls, ref_pose, points, colors, size=0.01, namespace='point_list'): """ Creates a point list referenced to a pose Args: ref_pose (PoseStamped): This is used for the header and the ref_points points (list): List of Point objects with the positions relative to ref_pose colors (list): The colors for the points size (float): size of the points in meters """ marker = Marker() marker.type = Marker.POINTS marker.header = ref_pose.header marker.pose = ref_pose.pose marker.ns = namespace marker.action = Marker.ADD marker.scale.x = size marker.scale.y = size if isinstance(colors[0], list): # colors defined for every element marker.colors = [cls.rgba_from_list(color) for color in colors] else: marker.color = cls.rgba_from_list(colors) marker.points = points return marker
def create_point_markers(cls, poses, size=0.01, color=None, namespace='point_marker'): """ create point markers from a list of poses Params: poses ([PoseStamped]): list of poses size (float): size of the markers in meter Returns: [Marker] """ color = cls.rgba_from_list(color) markers = list() for pose in poses: marker = Marker() marker.ns = namespace marker.type = Marker.SPHERE marker.header = pose.header marker.pose = pose.pose marker.scale.x = size marker.scale.y = size marker.scale.z = size marker.color = color marker.action = Marker.ADD markers.append(marker) return markers
def createSphereMarker(color, scale, offset=(0,0,0), orientation=(0,0,0,1), frame_id = "/map", marker_id = 1): marker = Marker() marker.header.frame_id = frame_id marker.type = marker.SPHERE marker.scale.x = scale[0] marker.scale.y = scale[1] marker.scale.z = scale[2] marker.id = marker_id p = ColorRGBA() p.r = color[0] p.g = color[1] p.b = color[2] p.a = color[3] marker.color = p 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.pose.position.x = offset[0] marker.pose.position.y = offset[1] marker.pose.position.z = offset[2] return marker
def get_particle_marker(self, timestamp, particle, marker_id): """Returns an rviz marker that visualizes a single particle""" msg = Marker() msg.header.stamp = timestamp msg.header.frame_id = 'map' msg.ns = 'particles' msg.id = marker_id msg.type = 0 # arrow msg.action = 0 # add/modify msg.lifetime = rospy.Duration(1) yaw_in_map = particle.theta vx = cos(yaw_in_map) vy = sin(yaw_in_map) msg.color = ColorRGBA(0, 1.0, 0, 1.0) msg.points.append(Point(particle.x, particle.y, 0.2)) msg.points.append( Point(particle.x + 0.3 * vx, particle.y + 0.3 * vy, 0.2)) msg.scale.x = 0.05 msg.scale.y = 0.15 msg.scale.z = 0.1 return msg
def makeViz(self, reachSets): # if self.skipfreq == self.skipcount: # self.skipcount = 0 # self.skipcount += 1 pointSets = [[tuple(p.p) for p in rs.set] for rs in reachSets.sets] #triangleSets = [tripy.earclip(ps) for ps in pointSets] header = Header() header.stamp = rospy.Time.now() header.frame_id = reachSets.header.frame_id origin = Pose(Point(0, 0, 0), Quaternion(0, 0, 0, 1)) lineMarkers = MarkerArray() lineMarkerArray = [] for ii in range(len(pointSets)): m = Marker() m.header = header m.id = self.outline_marker_id + ii m.action = 0 m.pose = origin m.type = 4 #LINE_STRIP m.color = self.colors[ii % len(self.colors)] m.points = [Point(p[0], p[1], 0) for p in pointSets[ii]] m.scale = Vector3(.05, 0, 0) lineMarkerArray.append(m) lineMarkers.markers = lineMarkerArray self.outlinePub.publish(lineMarkers) rospy.logdebug("Published Visualization")
def render_axes(self,track,marker_array): for i in range( len( track.pose ) ): for axis in range(3): marker = Marker() marker.header.stamp = track.header.stamp marker.header.frame_id = track.header.frame_id marker.ns = "track_visualizer-%d"%(track.id) marker.id = self.num_markers[track.id] marker.action = Marker.ADD marker.scale = Vector3(0.003,0.003,0.003) marker.color = self.generate_color_axis(track.id, i,axis) marker.type = Marker.LINE_STRIP marker.pose = track.pose[i] marker.points.append( Point(0,0,0) ) if axis==0: marker.points.append( Point(0.05,0,0) ) elif axis==1: marker.points.append( Point(0,0.05,0) ) elif axis==2: marker.points.append( Point(0,0,0.05) ) marker_array.markers.append(marker) self.num_markers[track.id] += 1
def create_cylinder(cx,cy,cz,ax,ay,az,radius, frame_id='/', obj_id=0, length=1, color=(0,1,0,0.3)): #The cylinder's axis if unrotated is originally pointing along the z axis of the referential #so that will be the up direction (0, 0 1). # #The cross product of the desired cylinder axis (normalized) and the up vector will give us #the axis of rotation, perpendicular to the plane defined by the cylinder axis and the up vector. # #The dot product of the cylinder axis and the up vector will provide the angle of rotation. axis = (ax, ay, az) rotation_axis = np.cross((0, 0, 1), axis) rotation_angle = simple_geom.angle_between_vectors(axis, (0, 0, 1)) cyl = Marker() cyl.id = obj_id cyl.header.frame_id = frame_id cyl.type = Marker.CYLINDER cyl.action = Marker.ADD cyl.pose.position = Point(x=cx,y=cy,z=cz) cyl.pose.orientation = Quaternion(*tf.transformations.quaternion_about_axis(rotation_angle, axis)) cyl.scale.x = 2*radius cyl.scale.y = 2*radius cyl.scale.z = length cyl.color = ColorRGBA(*color) return cyl
def publishSignalStrength(): with mutex: for mac in beaconLists: l = beaconLists[mac] #publish markers points = Marker() points.header.frame_id = "/map"; points.header.stamp = rospy.Time.now() points.ns = "points_and_lines"; points.action = Marker.ADD; points.pose.orientation.w = 1.0; points.id = 0; points.type = Marker.POINTS; # POINTS markers use x and y scale for width/height respectively points.scale.x = 0.2; points.scale.y = 0.2; # Points color points.color = getColorForMac(mac) # Create the vertices for the points and lines for b in l: p = Point(); p.x = b.x p.y = b.y p.z = b.strength points.points.append(p) # write to topic rospy.Publisher("signal"+str(mac), Marker, queue_size=100) .publish(points)
def make_sphere(x, y, radius, color, mark_id, off_x, off_y): """ Function to generate a spherical marker ARGUMENTS: x, y: coordinates of point in grid space radius: float color: ColorRGBA mark_id: unique id off_x, off_y: Origin of map coordinates in world frame """ global resolution marker = Marker() marker.header.stamp = rospy.Time.now() marker.header.frame_id = 'map' marker.ns = 'frontier' marker.id = mark_id marker.type = 2 marker.action = 0 marker.pose = geometry_msgs.msg.Pose() marker.pose.position.x = resolution * x + off_x marker.pose.position.y = resolution * y + off_y marker.scale = geometry_msgs.msg.Vector3(radius, radius, radius) marker.color = color marker.lifetime = rospy.rostime.Duration(0) marker.frame_locked = True return marker
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 scan_callback(msg): global pose, pub, seq, last_marker if pose is not None: angle = msg.angle * (np.pi / 200.0) # Convert grad to rad angle += np.pi # Transform to baselink frame ori = pose.orientation _, _, robot_yaw = tf.transformations.euler_from_quaternion( [ori.x, ori.y, ori.z, ori.w]) quat = tf.transformations.quaternion_from_euler( 0, 0, angle + robot_yaw) # Delete last marker m = last_marker m.action = m.DELETE m.pose.position = pose.position m.pose.orientation = Quaternion(quat[0], quat[1], quat[2], quat[3]) m.scale = Vector3(1, 0.1, 1) m.color = ColorRGBA(2, 0, 0, 1) pub.publish(m) last_marker = m # Publish new marker m = Marker() m.header.frame_id = "map" m.id = seq m.type = m.ARROW m.ns = "" m.pose.position = pose.position m.pose.orientation = Quaternion(quat[0], quat[1], quat[2], quat[3]) m.scale = Vector3(1, 0.1, 1) m.color = ColorRGBA(2, 0, 0, 1) pub.publish(m) last_marker = m
def _pub_order(self, orders, target=True): color = ColorRGBA() if target: color = COL_TARGET ns = "orders.target" else: color = COL_CURRENT ns = "orders.current" viz = Marker() viz.header.stamp = rospy.Time() viz.header.seq = self.seq viz.header.frame_id = '/map' viz.action = viz.MODIFY viz.ns = ns viz.id = 0 viz.type = viz.POINTS viz.color = color viz.scale = self.scale viz.lifetime = MARKERS_LIFETIME x_coords, y_coords = extract_coords(orders, target=target) for i_drone in range(orders.num_drones): loc = Point() loc.x = x_coords[i_drone] loc.y = y_coords[i_drone] loc.z = 0 viz.points.append(loc) rospy.loginfo("pub {} points".format(len(viz.points))) self.orders_viz_pub.publish(viz)
def _draw_sphere_list(self, uid, namespace, scale, points, color=None, colors=None): # type: (int, str, Vector3, list, ColorRGBA, list) -> None marker = Marker() # Header marker.header.frame_id = "base_link" marker.header.stamp = rospy.Time() # Body marker.ns = namespace marker.id = uid marker.type = Marker.SPHERE_LIST marker.action = Marker.ADD marker.scale = scale marker.pose.orientation = Quaternion(0, 0, 0, 1) marker.points = points if color is not None: marker.color = color if colors is not None: marker.colors = colors self._visualization_publisher.publish(marker)
def show_mean_left_in_rviz(self, opacity): marker = Marker() marker.header.frame_id = "map" marker.header.stamp = rospy.Time.now() marker.ns = "mean_left" # marker.id = self.count marker.id = 3 marker.type = Marker.LINE_STRIP marker.action = Marker.ADD for i in range(100): point = Point() # moved in to for loop point.x = self.all_means[2][i][0] point.y = self.all_means[2][i][1] point.z = 0 marker.points.append(point) marker.pose.orientation = Quaternion(0, 0, 0, 1) marker.scale = Vector3(self.all_std[2][i][0] * 2, self.all_std[2][i][1] * 2, 0.1) marker.color = ColorRGBA(0.0, 1.0, 0.0, opacity) self.count += 1 # marker.lifetime = rospy.Duration(0.25) self.marker_publisher_mean.publish(marker)
def publish(anns, data): table_list = TableList() marker_list = MarkerArray() marker_id = 1 for a, d in zip(anns, data): # Tables object = pickle.loads(d.data) table_list.tables.append(object) # Markers marker = Marker() marker.id = marker_id marker.header = a.pose.header marker.type = a.shape marker.ns = "table_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('table_marker', MarkerArray, latch=True, queue_size=1) table_pub = rospy.Publisher('table_pose_list', TableList, latch=True, queue_size=1) table_pub.publish(table_list) marker_pub.publish(marker_list) return
def MarkerTimerCB(self, event): markerArray = MarkerArray() ad_msg = AmmoDetect() ammobox_detected = 0 for ab in self.ammobox_list: if ab.vote >= 2: ad_msg.ammo_detect.append(ab.id) marker = Marker() marker.header.frame_id = "map" marker.type = marker.CUBE marker.action = marker.ADD marker.scale = Vector3(0.2, 0.2, 0.2) marker.color = ColorRGBA( 1, 0, 0, 1) if ab.vote == 1 else ColorRGBA(1, 1, 1, 0.5) marker.pose.orientation.w = 1.0 marker.pose.position.x = ab.x marker.pose.position.y = ab.y marker.pose.position.z = ab.z markerArray.markers.append(marker) if ab.vote == 1: ammobox_detected += 1 # renumber the markers id = 0 for m in markerArray.markers: m.id = id id += 1 self.pub_markers.publish(markerArray) self.pub_ammo_detect.publish(ad_msg) print ad_msg
def createTriangleListMarker(marker_id, points, rgba, frame_id="/camera_rgb_optical_frame"): marker = Marker() marker.header.frame_id = frame_id marker.type = marker.TRIANGLE_LIST marker.scale = Vector3(1, 1, 1) marker.id = marker_id n = len(points) if rgba is not None: marker.color = ColorRGBA(*rgba) o = Point(0, 0, 0) for i in xrange(n): p = Point(*points[i]) marker.points.append(p) p = Point(*points[(i + 1) % 4]) marker.points.append(p) marker.points.append(o) marker.pose = poselist2pose([0, 0, 0, 0, 0, 0, 1]) return marker
def pub_obj(self, obj_type, pos, dim): m = Marker() m.id = self.obj_types.index(obj_type) m.ns = "simple_tabletop" m.type = Marker.CUBE m.pose = Pose(Point(*pos), Quaternion(0, 0, 0, 1)) m.header = Header(0, rospy.Time(0), "base_link") m.scale = Vector3(*dim) m.color = self.colors[obj_type] mtext = deepcopy(m) mtext.type = Marker.TEXT_VIEW_FACING mtext.id += 100 mtext.text = obj_type M = MarkerArray([m, mtext]) self.pub.publish(M) #moveit collision objects co = CollisionObject() co.header = deepcopy(m.header) co.id = obj_type obj_shape = SolidPrimitive() obj_shape.type = SolidPrimitive.BOX obj_shape.dimensions = list(dim) co.primitives.append(obj_shape) obj_pose = Pose(Point(*pos), Quaternion(0, 0, 0, 1)) co.primitive_poses = [obj_pose] self.pub_co.publish(co)
def delete_all_markers(box_, color, index): marker = Marker() marker.id = index marker.ns = ns marker.header.frame_id = frame_id marker.type = marker.LINE_STRIP marker.action = 3 # marker.DELETEALL: deletes all objects # marker.frame_locked=False # marker scale marker.scale = Vector3(0.04, 0.04, 0.04) # x,yz # marker color marker.color = ColorRGBA(color[0], color[1], color[2], color[3]) # r,g,b,a # marker orientaiton marker.pose.orientation = Quaternion(0., 0., 0., 1.) # x,y,z,w # marker position marker.pose.position = Point(0., 0., 0.) # x,y,z # marker.lifetime = rospy.Duration(0.1) p0, p1, p2, p3, p4, p5, p6, p7 = box3d_2conner(box_, 0) # marker line points marker.points = [] for p in [ p0, p1, p2, p3, p0, p4, p5, p6, p7, p4, p5, p1, p2, p6, p7, p3 ]: marker.points.append(Point( p[0], p[1], p[2], )) return marker
def publish_marker_vector(start, end, diameter_shaft=0.01, diameter_head=0.02, id_=0): """ assumes points to be in frame map :type start: Point :type end: Point :type diameter_shaft: float :type diameter_head: float :type id_: int """ m = Marker() m.action = m.ADD m.ns = u'debug' m.id = id_ m.type = m.ARROW m.points.append(start) m.points.append(end) m.color = ColorRGBA(1, 0, 0, 1) m.scale.x = diameter_shaft m.scale.y = diameter_head m.scale.z = 0 pub = rospy.Publisher('/visualization_marker', Marker, queue_size=1) while pub.get_num_connections() < 1: # wait for a connection to publisher # you can do whatever you like here or simply do nothing pass rospy.sleep(0.3) pub.publish(m)
def visualize(self): lineVis = Marker() # Marker to visualize used lidar pts lineVis.header.frame_id = "odom" # Publishes it to the laser link, idk if it should be changed lineVis.header.stamp = rospy.Time.now() endVis = deepcopy(lineVis) startVis = deepcopy(lineVis) lineVis.points.append( Point(self.wp_1.point.x, self.wp_1.point.y, self.wp_1.point.z)) startVis.points.append( Point(self.wp_1.point.x, self.wp_1.point.y, self.wp_1.point.z)) lineVis.points.append( Point(self.wp_2.point.x, self.wp_2.point.y, self.wp_2.point.z)) endVis.points.append( Point(self.wp_2.point.x, self.wp_2.point.y, self.wp_2.point.z)) lineVis.color = ColorRGBA(1, 1, 1, 1) # Color is blue startVis.color = ColorRGBA(1, 0, 0, 1) # Color is blue endVis.color = ColorRGBA(0, 1, 0, 1) # Color is blue lineVis.type = 4 # makes it a list of points startVis.type = 8 endVis.type = 8 endVis.scale.x = 0.3 endVis.scale.y = 0.3 startVis.scale.x = 0.3 startVis.scale.y = 0.3 lineVis.scale.x = 0.05 # scale is about 2 cm lineVis.scale.y = 0.05 self.line_pub.publish(lineVis) self.end_pub.publish(endVis) self.start_pub.publish(startVis)
def create_interactive_mesh(name, resource, color=(1, 0, 0, 1), frame_id='base_link', transform=None, scale=1): if transform is None: transform = np.eye(4) # Start with the interactive control int_marker = create_interactive_6dof(name, color, frame_id, transform) # Mesh control control = InteractiveMarkerControl() control.always_visible = True control.interaction_mode = InteractiveMarkerControl.NONE # Mesh marker marker = Marker() marker.type = Marker.MESH_RESOURCE marker.scale = Vector3(*[scale for _ in range(3)]) marker.color = ColorRGBA(*color) marker.action = Marker.ADD marker.mesh_resource = resource # Add mesh control control.markers.append(marker) int_marker.controls.append(control) return int_marker
def marker_from_circle(circle, index=0, linewidth=0.1, color=ColorRGBA(1, 0, 0, 1), z=0., lifetime=10.0): marker = Marker() marker.header = make_header("/map") marker.ns = "Markers_NS" marker.id = index marker.type = Marker.CYLINDER marker.action = 0 # action=0 add/modify object # marker.color.r = 1.0 # marker.color.g = 0.0 # marker.color.b = 0.0 # marker.color.a = 0.4 marker.color = color marker.lifetime = rospy.Duration.from_sec(lifetime) marker.pose = Pose() marker.pose.position.z = z marker.pose.position.x = circle.center[0] marker.pose.position.y = circle.center[1] marker.scale = Vector3(circle.radius * 2.0, circle.radius * 2.0, 0) return marker
def addBoxes(self, in_objects, out_markers): for obj in in_objects.targets: box = Marker() # Message headers box.header = in_objects.header box.type = Marker.CUBE box.action = Marker.MODIFY if obj.uid in self._tracker_set else Marker.ADD box.ns = self._params.marker_namespace + "/boxs" box.id = obj.uid # Marker properties box.color = self._params.box_color box.scale.x = obj.bbox.dimension.length_x box.scale.y = obj.bbox.dimension.length_y box.scale.z = obj.bbox.dimension.length_z box.pose.position = obj.bbox.pose.pose.position box.pose.orientation = obj.bbox.pose.pose.orientation # Skip large boxes to prevent messy visualization if box.scale.x > self._params.box_max_size or box.scale.y > self._params.box_max_size or box.scale.z > self._params.box_max_size: rospy.logdebug("Object skipped with size %.2f x %.2f x %.2f!", box.scale.x, box.scale.y, box.scale.z) continue box_str = str(box) if 'nan' in box_str or 'inf' in box_str: print("----------------------------------") print(box_str) out_markers.markers.append(box)
def draw_speed_control_spline(self): HSV_min = 0 HSV_max = 240 / 360.0 marker = Marker() marker.header.stamp = rospy.get_rostime() marker.header.frame_id = "map" marker.id = self.MARKER_SPEED_SPLINE marker.type = Marker.LINE_STRIP marker.action = Marker.ADD marker.pose.orientation.x = 0.0 marker.pose.orientation.y = 0.0 marker.pose.orientation.z = 0.0 marker.pose.orientation.w = 1.0 color = self.max_lookahead_curv / \ (self.max_curv - self.min_curv) * (HSV_max - HSV_min) # Invert so the largest curvature maps to smallest HSV color = HSV_max - HSV_min - color r, g, b = colorsys.hsv_to_rgb(color, 1, 1) marker.color = ColorRGBA(r, g, b, 1) marker.scale.x = 0.5 index = self.traj_closest_index # Loop from the closest trajectory point to the speed lookahead point. while (index != self.speed_lookahead_index): x, y = self.traj_coords[index] marker.points.append(Point(x, y, 0)) index = (index + 1) % len(self.traj_coords) self.pub_visualiser.publish(marker)
def viz_cars(self, obj_list, header, color_rgba, publisher): ''' Publish objects to the provied rviz publisher. Parameters ---------- obj_list : [Objects] The list of Objects that should be visualized in rviz. header : Header The Header object containing the current ROS time and frame id. color_rgba : ColorRGBA The color in which the objects should be visualized. publisher : rospy.Publisher The ROS Publisher in which to publish the messages. ''' marker_msg = MarkerArray() count = 0 for obj in obj_list: 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 = color_rgba mk.pose = obj.pose mk.id = count mk.lifetime = rospy.Duration(1) marker_msg.markers.append(mk) count += 1 publisher.publish(marker_msg)
def add_marker(self, mesh_frame, marker_name, mesh_file, use_materials=False, color=None): marker_msg = Marker() marker_msg.frame_locked = True marker_msg.id = 0 marker_msg.action = Marker.ADD marker_msg.mesh_use_embedded_materials = use_materials if marker_msg.mesh_use_embedded_materials: marker_msg.color.r = 0 marker_msg.color.g = 0 marker_msg.color.b = 0 marker_msg.color.a = 0 elif color: marker_msg.color = color else: marker_msg.color.r = 0.6 marker_msg.color.g = 0.6 marker_msg.color.b = 0.6 marker_msg.color.a = 1.0 marker_msg.header.stamp = rospy.get_rostime() #marker_msg.lifetime = #marker_msg.pose = marker_msg.type = Marker.MESH_RESOURCE marker_msg.header.frame_id = mesh_frame marker_msg.ns = marker_name marker_msg.mesh_resource = 'file://%s' % (os.path.abspath(mesh_file)) scale = 1.0 marker_msg.scale.x, marker_msg.scale.y, marker_msg.scale.z = scale, scale, scale marker_msg.pose.position.x, marker_msg.pose.position.y, marker_msg.pose.position.z = 0, 0, 0 marker_msg.pose.orientation.x, marker_msg.pose.orientation.y, marker_msg.pose.orientation.z, marker_msg.pose.orientation.w = 0, 0, 0, 1 self.pub_marker_sync(marker_msg) if not self.marker_thread: self.marker_thread = thread.start_new_thread(MarkerPublisher.publish_markers, (self,))
def handle_image_msg(msg): assert msg._type == 'sensor_msgs/Image' with g_fusion_lock: g_fusion.filter(EmptyObservation(msg.header.stamp.to_sec())) if g_fusion.last_state_mean is not None: pose = g_fusion.lidar_observation_function( g_fusion.last_state_mean) yaw_q = ros_tf.transformations.quaternion_from_euler(0, 0, pose[3]) br = ros_tf.TransformBroadcaster() br.sendTransform(tuple(pose[:3]), tuple(yaw_q), rospy.Time.now(), 'obj_fuse_centroid', 'velodyne') if last_known_box_size is not None: # bounding box marker = Marker() marker.header.frame_id = "obj_fuse_centroid" marker.header.stamp = rospy.Time.now() marker.type = Marker.CUBE marker.action = Marker.ADD marker.scale.x = last_known_box_size[2] marker.scale.y = last_known_box_size[1] marker.scale.z = last_known_box_size[0] marker.color = ColorRGBA(r=0., g=1., b=0., a=0.5) marker.lifetime = rospy.Duration() pub = rospy.Publisher("obj_fuse_bbox", Marker, queue_size=10) pub.publish(marker)
def visualize_poop(x,y,z,color,frame,ns): msg = Marker() msg.header = Header(stamp=Time.now(), frame_id=frame) #msg.scale = Vector3(x=0.02, y=0.02, z=0.02) # for sphere msg.scale = Vector3(x=0.005, y=0.04, z=0.0) # for arrow #msg.pose.position = Point(x=x, y=y, z=z) #msg.pose.position = Point(x=x, y=y, z=z+0.15) # arrow #msg.pose.orientation = Quaternion(x=0, y=0, z=0, w=1) # arrow #msg.pose.orientation = Quaternion(x=0.707, y=0, z=0, w=0.707) msg.points = [Point(x=x, y=y,z=z+0.15), Point(x=x,y=y,z=z)] msg.ns = ns msg.id = 0 msg.action = 0 # add #msg.type = 2 # sphere msg.type = 0 # arrow msg.color = ColorRGBA(r=0, g=0, b=0, a=1) if color == 0: msg.color.g = 1; elif color == 1: msg.color.b = 1; elif color == 2: msg.color.r = 1; msg.color.g = 1; elif color == 3: msg.color.g = 1; msg.color.b = 1; #loginfo("Publishing %s marker at %0.3f %0.3f %0.3f",ns,x,y,z) viz_pub.publish(msg)
def __update_mesh(self, mesh): """ Refreshes mesh in rviz. :param mesh: Mesh object representing the space. """ m = Marker() m.header.frame_id = str(1) m.type = Marker.LINE_LIST m.action = Marker.ADD m.id = 1 m.color = RED m.scale.x = 0.001 edges_visited = set() for n1 in mesh.nodes: for n2 in n1.edges: if n1 < n2 and (n1, n2) not in edges_visited: m.points.append(n1.position()) m.points.append(n2.position()) edges_visited.add((n1, n2)) elif n2 < n1 and (n2, n1) not in edges_visited: m.points.append(n2.position()) m.points.append(n1.position()) edges_visited.add((n2, n1)) self.__mesh_publisher.publish(m)
def line(self, frame_id, p, r, t=[0.0, 0.0], key=None): """ line: t r + p This will be drawn for t[0] .. t[1] """ r = np.array(r) p = np.array(p) m = Marker() m.header.frame_id = frame_id m.ns = key or 'lines' m.id = 0 if key else len(self.lines) m.type = Marker.LINE_STRIP m.action = Marker.MODIFY m.pose = Pose(Point(0,0,0), Quaternion(0,0,0,1)) m.scale = Vector3(0.005, 0.005, 0.005) m.color = ColorRGBA(0,0.8,0.8,1) m.points = [Point(*(p+r*t)) for t in np.linspace(t[0], t[1], 10)] m.colors = [m.color] * 10 key = key or element_key(m) with self.mod_lock: self.lines[key] = m return key
def draw_marker( self, ps, id=None, type=Marker.CUBE, ns="default_ns", rgba=(0, 1, 0, 1), scale=(0.03, 0.03, 0.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 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 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 input_handler(space): global marker_color global marker_pub global matches for str in matches: if space.aux_payload.find(str) == -1: # print "FAIL: str='%s' aux_payload='%s'" % (str, space.aux_payload) return # print space # print marker = Marker() marker.header.frame_id = "/map" marker.header.stamp = rospy.Time() marker.ns = "" marker.id = 0 marker.type = Marker.POINTS marker.action = Marker.ADD marker.pose.position.x = 0 marker.pose.position.y = 0 marker.pose.position.z = 0 marker.pose.orientation.x = 0 marker.pose.orientation.y = 0 marker.pose.orientation.z = 0 marker.pose.orientation.w = 0 marker.scale.x = .1 marker.scale.y = .1 marker.scale.z = .1 marker.color = marker_color marker.points = [] # this line cuts off anything before the string "convex_set: " in the aux_payload convex_set = space.aux_payload[ space.aux_payload.find("convex_set: ") :] # this line cuts off that string at the next newline character convex_set = convex_set.split('\n')[0] # this line splits the string into pieces delineated by spaces convex_set = convex_set.split(' ') # print convex_set for candidate in convex_set: if len(candidate) == 0 or candidate[0] != '(': continue coords = candidate[1:-1].split(',') # print coords marker.points.append(Point(float(coords[0]), float(coords[1]), float(coords[2]))) # print marker.points[-1] # print # print marker.points # print marker_pub.publish(marker)
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 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 createSphereMarker(color, scale, offset=(0,0,0), orientation=(0,0,0,1)): marker = Marker() marker.header.frame_id = "/map" marker.type = marker.SPHERE marker.scale.x = scale[0] marker.scale.y = scale[1] marker.scale.z = scale[2] marker.id = 1 p = ColorRGBA() p.r = color[0] p.g = color[1] p.b = color[2] p.a = color[3] marker.color = p 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.pose.position.x = offset[0] marker.pose.position.y = offset[1] marker.pose.position.z = offset[2] return marker
def run(self): while self.is_looping(): navigation_tf_msg = TFMessage() try: motion_to_robot = almath.Pose2D(self.motion.getRobotPosition(True)) localization = self.navigation.getRobotPositionInMap() exploration_path = self.navigation.getExplorationPath() except Exception as e: navigation_tf_msg.transforms.append(self.get_navigation_tf(almath.Pose2D())) self.publishers["map_tf"].publish(navigation_tf_msg) self.rate.sleep() continue if len(localization) > 0 and len(localization[0]) == 3: navigation_to_robot = almath.Pose2D(localization[0][0], localization[0][1], localization[0][2]) navigation_to_motion = navigation_to_robot * almath.pinv(motion_to_robot) navigation_tf_msg.transforms.append(self.get_navigation_tf(navigation_to_motion)) self.publishers["map_tf"].publish(navigation_tf_msg) if len(localization) > 0: if self.publishers["uncertainty"].get_num_connections() > 0: uncertainty = Marker() uncertainty.header.frame_id = "/base_footprint" uncertainty.header.stamp = rospy.Time.now() uncertainty.type = Marker.CYLINDER uncertainty.scale = Vector3(localization[1][0], localization[1][1], 0.2) uncertainty.pose.position = Point(0, 0, 0) uncertainty.pose.orientation = self.get_ros_quaternion( almath.Quaternion_fromAngleAndAxisRotation(0, 0, 0, 1)) uncertainty.color = ColorRGBA(1, 0.5, 0.5, 0.5) self.publishers["uncertainty"].publish(uncertainty) if self.publishers["map"].get_num_connections() > 0: aggregated_map = None try: aggregated_map = self.navigation.getMetricalMap() except Exception as e: print("error " + str(e)) if aggregated_map is not None: map_marker = OccupancyGrid() map_marker.header.stamp = rospy.Time.now() map_marker.header.frame_id = "/map" map_marker.info.resolution = aggregated_map[0] map_marker.info.width = aggregated_map[1] map_marker.info.height = aggregated_map[2] map_marker.info.origin.orientation = self.get_ros_quaternion( almath.Quaternion_fromAngleAndAxisRotation(-1.57, 0, 0, 1)) map_marker.info.origin.position.x = aggregated_map[3][0] map_marker.info.origin.position.y = aggregated_map[3][1] map_marker.data = aggregated_map[4] self.publishers["map"].publish(map_marker) if self.publishers["exploration_path"].get_num_connections() > 0: path = Path() path.header.stamp = rospy.Time.now() path.header.frame_id = "/map" for node in exploration_path: current_node = PoseStamped() current_node.pose.position.x = node[0] current_node.pose.position.y = node[1] path.poses.append(current_node) self.publishers["exploration_path"].publish(path) self.rate.sleep()
def _make_mesh_marker(self): mesh = Marker() mesh.mesh_use_embedded_materials = False mesh.type = Marker.MESH_RESOURCE mesh.scale.x = 1.0 mesh.scale.y = 1.0 mesh.scale.z = 1.0 target_pose = GripperMarkers._offset_pose(self.marker_pose, GripperMarkers._offset) print target_pose ik_solution = self.ik.get_ik_for_ee(target_pose) if (ik_solution == None): mesh.color = ColorRGBA(1.0, 0.0, 0.0, 0.6) else: mesh.color = ColorRGBA(0.0, 1.0, 0.0, 0.6) return mesh
def rviz_obj(self, obj_id, obj_ns, obj_type, obj_size, obj_color=0, obj_life=0): obj = Marker() obj.header.frame_id, obj.header.stamp = "camera_link", rospy.Time.now() obj.ns, obj.action, obj.type = str(obj_ns), 0, obj_type obj.scale.x, obj.scale.y, obj.scale.z = obj_size[0], obj_size[1], obj_size[2] obj.color = self.carray[obj_color] obj.lifetime = rospy.Duration.from_sec(obj_life) obj.pose.orientation.w = 1.0 return obj
def create_base_marker(pose, id, color): marker = Marker() marker.header.frame_id = "base_link" marker.header.stamp = rospy.Time.now() marker.ns = "ar_servo" marker.id = id marker.pose = PoseConverter.to_pose_msg(pose) marker.color = ColorRGBA(*(color + (1.0,))) marker.scale.x = 0.7; marker.scale.y = 0.7; marker.scale.z = 0.2 return marker
def createMarker(self): marker = Marker() marker.header = Header(stamp=rospy.Time.now(), frame_id="odom") marker.id = 1 marker.type = Marker.SPHERE marker.action = Marker.ADD marker.pose.orientation = Quaternion(x=0, y=0, z=0, w=1) marker.scale = Vector3(x=.1, y=.1, z=.1) marker.color = ColorRGBA(a=1, r=0, g=1, b=0) return marker
def transformStampedArrayToLabeledArrayMarker(cls, tsdata_lst, fmt="%Y-%m-%d %H:%M:%S", zoffset=0.05, label_downsample=1, discrete=False): "[[transformStamped, meta],...] -> LineStrip / String" h = Header() h.stamp = rospy.Time(0) #rospy.Time.now() h.frame_id = "eng2" #t_first.child_frame_id res = [] t_first = tsdata_lst[0][0] prev_t = t_first.transform.translation for ts, _ in tsdata_lst: m = Marker(type=Marker.ARROW, action=Marker.ADD, header=h, id=cls.marker_id) cls.marker_id += 1 t = ts.transform.translation dist = distanceOfVector3(prev_t, t) * 0.65 rgb = colorsys.hsv_to_rgb(dist, 1.0, 1.0) m.points = [Point(x=prev_t.x,y=prev_t.y,z=prev_t.z), Point(x=(prev_t.x + t.x) / 2.,y=(prev_t.y + t.y) /2.,z=(prev_t.z + t.z) /2.)] m.ns = "labeled_line" m.lifetime = rospy.Time(3000) m.scale.x, m.scale.y, m.scale.z = 0.02, 0.06, 0.1 m.color = ColorRGBA(rgb[0],rgb[1],rgb[2],1.0) if dist < 0.65: res.append(m) prev_t = t 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 populate_marker(self): marker = Marker() marker.header.frame_id = "world" marker.header.stamp = rospy.Time.now() marker.id = 0 marker.ns = self.name marker.type = self.shape marker.action = Marker.ADD marker.color = self.color marker.lifetime = rospy.Duration(); return marker