def make_marker(self, barcode_msg): ean = barcode_msg.barcode m = Marker() m.header = barcode_msg.barcode_pose.header m.pose = barcode_msg.barcode_pose.pose m.action = Marker.ADD m.type = Marker.CUBE m.scale = Vector3(.06, .06, .06) m.color = ColorRGBA(1, 0, 0, 1.0) text = Marker() text.header = barcode_msg.barcode_pose.header text.action = Marker.ADD text.type = Marker.TEXT_VIEW_FACING text.text = ean text.scale = Vector3(0, 0, .06) text.color = ColorRGBA(1, 0, 0, 1) text.pose.position.x = barcode_msg.barcode_pose.pose.position.x text.pose.position.y = barcode_msg.barcode_pose.pose.position.y text.pose.position.z = barcode_msg.barcode_pose.pose.position.z + 0.05 m.text = ean m.ns = ean text.ns = ean m.id = 2 self.marker_pub.publish(m) self.marker_pub.publish(text)
def update_goal(self, setpoint_msg): array_msg = MarkerArray() array_msg.markers = [] marker_msg = Marker() marker_msg.header = setpoint_msg.header marker_msg.ns = "guidance/current" marker_msg.id = 0 marker_msg.type = 2 marker_msg.action = 0 marker_msg.pose = setpoint_msg.pose marker_msg.scale = Vector3(2 * self.distance_error_acceptance, 2 * self.distance_error_acceptance, 2 * self.distance_error_acceptance) marker_msg.color = ColorRGBA(0.77432878, 0.31884126, 0.54658502, 1.0) #HOT PINK array_msg.markers.append(marker_msg) marker_msg = Marker() marker_msg.header = setpoint_msg.header marker_msg.ns = "guidance/current" marker_msg.id = 1 marker_msg.type = 0 marker_msg.action = 0 marker_msg.pose = setpoint_msg.pose marker_msg.scale = Vector3(20, 2, 2) marker_msg.color = ColorRGBA(0.77432878, 0.31884126, 0.54658502, 1.0) #HOT PINK array_msg.markers.append(marker_msg) self.marker_pub.publish(array_msg)
def makeViz(self, reachSets): 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) triPoints = [xy for tri in triangleSets for xy in tri] triMarker = Marker() triMarker.header = header triMarker.id = self.tri_marker_id triMarker.type = 11 #TRIANGLE_LIST triMarker.action = 0 triMarker.pose = origin triMarker.color = ColorRGBA(1, 1, 1, 1) triMarker.scale = Vector3(1, 1, 1) triMarker.points = [ Point(p[0], p[1], 0) for tri in triPoints for p in tri ] #expand color array to cover all verts for all tris in each set with same color triFrequency = [len(ps) for ps in pointSets] triColors = [ self.colors[ii % len(self.colors)] for ii in range(len(pointSets)) ] triMarker.colors = [ c for cidx in range(len(triColors)) for c in repeat(triColors[cidx], triFrequency[cidx]) ] self.trisPub.publish(triMarker)
def viz_waypoints(): markers = MarkerArray() path_marker = Marker() path_marker.header = waypoints[0].header path_marker.ns = 'path' path_marker.id = len(markers.markers) path_marker.type = Marker.LINE_STRIP path_marker.action = path_marker.ADD path_marker.points = [p.pose.position for p in waypoints] path_marker.color.b = 1. path_marker.color.a = 1. path_marker.scale.x = .05 markers.markers.append(path_marker) wp_marker = Marker() wp_marker.header = waypoints[0].header wp_marker.ns = 'waypoints' wp_marker.id = len(markers.markers) wp_marker.type = Marker.SPHERE_LIST wp_marker.action = Marker.ADD wp_marker.points = [p.pose.position for p in waypoints] wp_marker.color.r = 1. wp_marker.color.a = 1. wp_marker.scale.x = .25 wp_marker.scale.y = .25 wp_marker.scale.z = .25 markers.markers.append(wp_marker) dir_marker_del = Marker() dir_marker_del.ns = 'direction' dir_marker_del.action = Marker.DELETEALL markers.markers.append(dir_marker_del) for wp in waypoints: dir_marker = Marker() dir_marker.ns = 'direction' dir_marker.header = wp.header dir_marker.action = Marker.ARROW dir_marker.action = Marker.ADD dir_marker.ns = 'direction' dir_marker.id = len(markers.markers) dir_marker.pose = wp.pose dir_marker.color.r = 1. dir_marker.color.g = 1. dir_marker.color.b = 0. dir_marker.color.a = 0.5 dir_marker.scale.x = .5 dir_marker.scale.y = .2 dir_marker.scale.z = .05 markers.markers.append(dir_marker) viz_pub.publish(markers)
def class_visualization(data): center_point = Marker() center_point.header = data.header center_point.ns = "segment_center" center_point.action = center_point.MODIFY center_point.pose.orientation.w = 1.0 center_point.id = 0 center_point.type = center_point.SPHERE_LIST center_point.scale.x = center_point.scale.y = 0.1 center_point.scale.z = 0.1 center_point.color.a = 0.5 center_point.color.r = 1.0 center_point.color.g = 1.0 center_point.color.b = 1.0 class_text_array = MarkerArray() class_text = Marker() class_text.header = data.header class_text.action = class_text.DELETEALL class_text_array.markers.append(class_text) text_marker_pub.publish(class_text_array) for index, item in enumerate(data.segments): class_text = Marker() class_text.header = data.header class_text.ns = "class_text" class_text.action = class_text.ADD class_text.pose.orientation.w = 1.0 class_text.id = index class_text.type = class_text.TEXT_VIEW_FACING class_text.scale.z = 0.05 class_text.color.a = 1.0 class_text.color.r = 1.0 class_text.color.g = 1.0 class_text.color.b = 1.0 class_text.text = str(round(item.class_id, 2)) class_text.pose.position.x = (item.center.x) class_text.pose.position.y = (item.center.y) class_text.pose.position.z = (item.center.z) class_text_array.markers.append(class_text) if (item.class_id > 0.7): center_point.points.append(item.center) marker_pub.publish(center_point) text_marker_pub.publish(class_text_array)
def sensed_objects_callback(data, ns): marker_array = MarkerArray() for id, sensed in enumerate(data.objects): type_marker = Marker() type_marker.header = sensed.header type_marker.ns = ns type_marker.lifetime = rospy.Duration(1.0) type_marker.id = 10 * id type_marker.type = Marker.TEXT_VIEW_FACING type_marker.action = Marker.ADD type_marker.pose = copy.deepcopy(sensed.pose.pose) type_marker.pose.position.z += 0.15 # A little bit up type_marker.scale.z = 0.1 type_marker.color.r = 1.0 type_marker.color.g = 1.0 type_marker.color.b = 1.0 type_marker.color.a = 1.0 type_marker.text = get_type(sensed.type) # TODO: Add id? marker_array.markers.append(type_marker) pose_marker = Marker() pose_marker.header = sensed.header pose_marker.ns = ns pose_marker.lifetime = rospy.Duration(1.0) pose_marker.id = 10 * id + 1 pose_marker.type = Marker.SPHERE pose_marker.action = Marker.ADD pose_marker.pose = sensed.pose.pose pose_marker.scale.x = sensed.pose.covariance[0] pose_marker.scale.y = sensed.pose.covariance[7] pose_marker.scale.z = sensed.pose.covariance[14] pose_marker.color.r = 1.0 pose_marker.color.g = 1.0 pose_marker.color.b = 1.0 pose_marker.color.a = 1.0 marker_array.markers.append(pose_marker) scale_marker = Marker() scale_marker.header = sensed.header scale_marker.ns = ns scale_marker.lifetime = rospy.Duration(1.0) scale_marker.id = 10 * id + 2 scale_marker.type = Marker.CUBE scale_marker.action = Marker.ADD scale_marker.pose = sensed.pose.pose scale_marker.scale = sensed.scale scale_marker.color = get_color(sensed.color, 0.5) marker_array.markers.append(scale_marker) marker_array_pub.publish(marker_array)
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 on_enter(self, userdata): ma = MarkerArray() self._path = MoveBaseActionPath() for i in range(len(userdata.path.poses)): marker = Marker(type=Marker.ARROW) marker.header = userdata.path.header marker.pose = userdata.path.poses[i].pose marker.scale.x = 0.2 marker.scale.y = 0.02 marker.scale.z = 0.02 marker.color.b = 1.0 marker.color.r = 0.9 - 0.7 * i / len(userdata.path.poses) marker.color.g = 0.9 - 0.7 * i / len(userdata.path.poses) marker.color.a = 0.8 - 0.5 * i / len(userdata.path.poses) marker.id = i ma.markers.append(marker) self._failed = False self._path.goal.target_path.poses = userdata.path.poses self._path.goal.target_path.header.frame_id = "map" self._pub.publish(self._pathTopic, self._path) self._pub.publish(self._marker_topic, ma) self._reached = True
def makeMarker(self, id_num, point, rgb=(1.0,1.0,1.0)): ## Make a std_msgs/Marker object # # @param id_num The id number corresponding to the marker. Note that these must be unique. # @param point An (x,y,z) tuple where the marker should be located in space # @param rgb A tuple corresponding to the RGB values on a scale from 0.0 to 1.0 # # @return A std_msgs/Marker object marker = Marker() marker.header = Header() marker.header.stamp = rospy.Time.now() marker.header.frame_id = self.frame_id marker.id = id_num marker.type = marker.SPHERE marker.action = marker.ADD marker.scale.x = 0.02 marker.scale.y = 0.02 marker.scale.z = 0.02 marker.color.r = rgb[0] marker.color.g = rgb[1] marker.color.b = rgb[2] marker.color.a = 1.0 marker.pose.orientation.w = 1.0 marker.pose.position.x = point[0] marker.pose.position.y = point[1] marker.pose.position.z = point[2] return marker
def publish(self, grasps, obj=None): msg = MarkerArray() self.marker_id = 0 # reset marker counter if obj and len(obj.primitives) > 0 and len(obj.primitive_poses) > 0: m = Marker() m.header = obj.header m.ns = "object" m.id = self.marker_id self.marker_id += 1 m.type = m.CUBE m.action = m.ADD m.pose = obj.primitive_poses[0] m.scale.x = obj.primitives[0].dimensions[0] m.scale.y = obj.primitives[0].dimensions[1] m.scale.z = obj.primitives[0].dimensions[2] m.color.r = 0 m.color.g = 0 m.color.b = 1 m.color.a = 0.8 msg.markers.append(m) for grasp in grasps: msg.markers.append(self.get_gripper_marker(grasp.grasp_pose, grasp.grasp_quality)) self.pub.publish(msg)
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 create_line_marker(p1, p2, frame_id): h = Header() h.frame_id = 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 = "unit_vector" mark.id = 0 mark.type = Marker.LINE_STRIP mark.action = 0 mark.scale.x = 0.2 mark.color = color = ColorRGBA(0.2, 0.5, 0.7, 1.0) mark.text = "unit_vector" points = [] pt1 = Point(p1[0], p1[1], p1[2]) pt2 = Point(p2[0], p2[1], p2[2]) # print "Pt 1 ", pt1 # print "Pt 2 ", pt2 points.append(pt1) points.append(pt2) mark.points = points 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 gpscallback(msg): marker = Marker() marker.header = msg.header marker.header.frame_id = "map" marker.ns = 'gps' marker.id = 0 marker.type = Marker.SPHERE_LIST marker.action = Marker.MODIFY marker.scale.x = 0.3 marker.scale.y = 0.3 marker.scale.z = 0.3 marker.color.r = 0 marker.color.g = 0 marker.color.b = 1 marker.color.a = 1 marker.lifetime = rospy.Duration(2000) marker.frame_locked = False point = Point() point.x = msg.pose.pose.position.x point.y = msg.pose.pose.position.y point.z = msg.pose.pose.position.z gpspoints.append(point) marker.points.extend(gpspoints) vis_pub.publish(marker)
def linelist(self): """Build line list marker from 8 points in 3d space.""" line_list = Marker() line_list.header = self._header line_list.type = Marker.LINE_LIST line_list.action = Marker.ADD line_list.scale.x = 0.005 line_list.color = self.YELLOW line_list.pose = deepcopy(self.POSE) line_list.pose.position.x = self._p1.x line_list.pose.position.y = self._p1.y line_list.pose.position.z = self._p1.z line_list.points.extend((self._p1, self._p2)) line_list.points.extend((self._p2, self._p3)) line_list.points.extend((self._p3, self._p4)) line_list.points.extend((self._p4, self._p1)) line_list.points.extend((self._p5, self._p6)) line_list.points.extend((self._p6, self._p7)) line_list.points.extend((self._p7, self._p8)) line_list.points.extend((self._p8, self._p5)) line_list.points.extend((self._p1, self._p5)) line_list.points.extend((self._p2, self._p6)) line_list.points.extend((self._p3, self._p7)) line_list.points.extend((self._p4, self._p8)) return line_list
def pose_cb(self, pose): pose = self.listener.transformPose(self.reference_frame,pose) q = pose.pose.orientation qvec = [q.x,q.y,q.z,q.w] euler = euler_from_quaternion(qvec) self.goal_x = pose.pose.position.x self.goal_y = pose.pose.position.y self.goal_theta = euler[2] (ex,ey,etheta) = self.compute_error() if ex < -self.dist_threshold: self.goal_theta = norm_angle(etheta + pi) print "New goal: %.2f %.2f %.2f" % (self.goal_x, self.goal_y, self.goal_theta) marker = Marker() marker.header = pose.header marker.ns = "goal_marker" marker.id = 10 marker.type = Marker.ARROW marker.action = Marker.ADD marker.pose = pose.pose marker.scale.x = 0.5 marker.scale.y = 0.5 marker.scale.z = 1.0; marker.color.a = 1.0; marker.color.r = 1.0; marker.color.g = 1.0; marker.color.b = 0.0; marker.lifetime.secs=-1.0 self.marker_pub.publish(marker)
def callback(msg): marker = Marker() marker.header = msg.header marker.header.frame_id = "map" marker.ns = 'obstacles' marker.id = 0 marker.type = Marker.SPHERE_LIST marker.action = Marker.MODIFY marker.scale.x = 0.1 marker.scale.y = 0.1 marker.scale.z = 0.1 marker.color.r = 0 marker.color.g = 1 marker.color.b = 0 marker.color.a = 1 marker.lifetime = rospy.Duration(2) marker.frame_locked = False for obstacle in msg.obstacles: point = Point() point.x = obstacle.odom.pose.pose.position.x point.y = obstacle.odom.pose.pose.position.y point.z = 0 marker.points.append(point) vis_pub.publish(marker)
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 create_marker(t, type, lifetime): global counter marker = Marker() marker.header = t.header marker.type = type marker.action = marker.ADD id = t.id if id == 0: id = counter counter += 1 marker.id = int(id) marker.lifetime = lifetime marker.ns = 'obstacle_tracking.%d' % type marker.color.a = 1. marker.color.r = marker.color.g = marker.color.b = 0.5 marker.pose = copy.deepcopy(t.pose.pose) marker.scale.x = marker.scale.y = marker.scale.z = 1. # default scale return marker
def addBoxes(self, in_objects, out_markers): for obj in in_objects.detections: box = Marker() box.lifetime = rospy.Duration(self._params.marker_lifetime) # Message headers box.header = in_objects.header box.type = Marker.CUBE box.action = Marker.ADD box.ns = self._params.marker_namespace + "/boxs" box.id = self._marker_id self._marker_id += 1 # 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 out_markers.markers.append(box)
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 callback(self, state): self._id = 0 m = self.create_arrow(state.base_pose) m.color.r = 1.0 self._out_pub.publish(m) m = self.create_arrow(state.bridge_pose) m.color.g = 1.0 m.scale.x = Constants.BRIDGE_TO_STRIKE_MIN self._out_pub.publish(m) m = Marker() m.header = state.bridge_pose.header m.ns = "billiards_shot_plan" m.id = self._id m.action = Marker.ADD m.type = Marker.CUBE m.pose = state.bridge_pose.pose m.pose.position.z += 0.055 m.scale.x = 0.005 m.scale.y = 0.05 m.scale.z = 0.11 m.color.a = 1.0 m.color.b = 1.0 self._out_pub.publish(m) self._id += 1
def addLabels(self, in_objects, out_markers): for obj in in_objects.detections: label_marker = Marker() label_marker.lifetime = rospy.Duration( self._params.marker_lifetime) # Message headers label_marker.header = in_objects.header label_marker.action = Marker.ADD label_marker.type = Marker.TEXT_VIEW_FACING label_marker.ns = self._params.marker_namespace + "/labels" label_marker.id = self._marker_id self._marker_id += 1 # Marker properties label_marker.scale.x = self._params.label_scale label_marker.scale.y = self._params.label_scale label_marker.scale.z = self._params.label_scale label_marker.color = self._params.label_color if len(obj.classes) > 0: # Use classification name if presented label_marker.text = "[" + str(obj.classes[0].classid) + "]" x = obj.bbox.pose.pose.position.x y = obj.bbox.pose.pose.position.y label_marker.text += "%.2f m" % math.sqrt(x * x + y * y) if obj.comments: label_marker.text += "\n" + obj.comments label_marker.pose.position.x = x label_marker.pose.position.y = y label_marker.pose.position.z = self._params.label_height out_markers.markers.append(label_marker)
def pose_cb(self, pose): pose = self.listener.transformPose(self.reference_frame, pose) q = pose.pose.orientation qvec = [q.x, q.y, q.z, q.w] euler = euler_from_quaternion(qvec) self.goal_x = pose.pose.position.x self.goal_y = pose.pose.position.y self.goal_theta = euler[2] (ex, ey, etheta) = self.compute_error() if ex < -self.dist_threshold: self.goal_theta = norm_angle(etheta + pi) print "New goal: %.2f %.2f %.2f" % (self.goal_x, self.goal_y, self.goal_theta) marker = Marker() marker.header = pose.header marker.ns = "goal_marker" marker.id = 10 marker.type = Marker.ARROW marker.action = Marker.ADD marker.pose = pose.pose marker.scale.x = 0.5 marker.scale.y = 0.5 marker.scale.z = 1.0 marker.color.a = 1.0 marker.color.r = 1.0 marker.color.g = 1.0 marker.color.b = 0.0 marker.lifetime.secs = -1.0 self.marker_pub.publish(marker)
def makeMarker(self, header, id_num, point): ## Make a std_msgs/Marker object # # @param header A ROS Header object for the marker # @param id_num The id number corresponding to the marker. Note that these must be unique. # @param point An (x,y,z) tuple where the marker should be located in space # # @return A std_msgs/Marker object marker = Marker() marker.header = header marker.id = id_num marker.type = marker.SPHERE marker.action = marker.ADD marker.scale.x = 0.03 marker.scale.y = 0.03 marker.scale.z = 0.03 marker.color.r = 1.0 marker.color.g = 0.2 marker.color.b = 0.3 marker.color.a = 1.0 marker.pose.orientation.w = 1.0 marker.pose.position.x = point[0] marker.pose.position.y = point[1] marker.pose.position.z = point[2] return marker
def pose_cb(self, pose): m_pose = PointStamped() m_pose.header = pose.header m_pose.point = pose.pose.position m_pose = self.listener.transformPoint(self.reference_frame,m_pose) self.goal_x = m_pose.point.x self.goal_y = m_pose.point.y print "New goal: %.2f %.2f" % (self.goal_x, self.goal_y) marker = Marker() marker.header = pose.header marker.ns = "goal_marker" marker.id = 10 marker.type = Marker.CYLINDER marker.action = Marker.ADD marker.pose = pose.pose marker.scale.x = 0.1 marker.scale.y = 0.1 marker.scale.z = 0.5; marker.color.a = 1.0; marker.color.r = 1.0; marker.color.g = 1.0; marker.color.b = 0.0; marker.lifetime.secs=-1.0 self.marker_pub.publish(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 make_marker_stub(self, stamped_pose, size=None, color=None): if color is None: color = (0.5, 0.5, 0.5, 1.0) if size is None: size = (1, 1, 1) marker = Marker() marker.header = stamped_pose.header # marker.ns = "collision_scene" marker.id = self.next_id marker.lifetime = Time(0) # forever marker.action = Marker.ADD marker.pose = stamped_pose.pose marker.color.r = color[0] marker.color.g = color[1] marker.color.b = color[2] if len(color) == 4: alpha = color[3] else: alpha = 1.0 marker.color.a = alpha marker.scale.x = size[0] marker.scale.y = size[1] marker.scale.z = size[2] self.next_id += 1 return 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 pose_cb(self, pose): m_pose = PointStamped() m_pose.header = pose.header m_pose.point = pose.pose.position m_pose = self.listener.transformPoint(self.reference_frame, m_pose) self.goal_x = m_pose.point.x self.goal_y = m_pose.point.y print "New goal: %.2f %.2f" % (self.goal_x, self.goal_y) marker = Marker() marker.header = pose.header marker.ns = "goal_marker" marker.id = 10 marker.type = Marker.CYLINDER marker.action = Marker.ADD marker.pose = pose.pose marker.scale.x = 0.1 marker.scale.y = 0.1 marker.scale.z = 0.5 marker.color.a = 1.0 marker.color.r = 1.0 marker.color.g = 1.0 marker.color.b = 0.0 marker.lifetime.secs = -1.0 self.marker_pub.publish(marker)
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_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 goal_parts_cb(self, msg: PoseWithCertaintyArray): arr = [] i = 0 for post in msg.poses: post_marker = Marker() pose = Pose() pose.position = post.pose.pose.position post_marker.pose = pose post_marker.pose.position.z = self.post_height / 2 post_marker.pose.orientation = Quaternion() post_marker.pose.orientation.x = 0 post_marker.pose.orientation.y = 0 post_marker.pose.orientation.z = 0 post_marker.pose.orientation.w = 1 post_marker.type = Marker.CYLINDER post_marker.action = Marker.MODIFY post_marker.id = i post_marker.ns = "rel_goal_parts" color = ColorRGBA() color.r = 1.0 color.g = 1.0 color.b = 1.0 color.a = 1.0 post_marker.color = color post_marker.lifetime = rospy.Duration(nsecs=self.goal_lifetime) scale = Vector3(self.post_diameter, self.post_diameter, self.post_height) post_marker.scale = scale post_marker.header = msg.header arr.append(post_marker) i += 1 self.goal_parts_marker.markers = arr self.marker_array_publisher.publish(self.goal_parts_marker)
def publish_trajectory(self, duration=0.0): should_publish = len(self.points) > 1 if self.visualize and self.traj_pub.get_num_connections() > 0: print "Publishing trajectory" marker = Marker() marker.header = self.make_header("/map") marker.ns = self.viz_namespace + "/trajectory" marker.id = 2 marker.type = marker.LINE_STRIP # line strip marker.lifetime = rospy.Duration.from_sec(duration) if should_publish: marker.action = marker.ADD marker.scale.x = 0.3 marker.color.r = 1.0 marker.color.g = 1.0 marker.color.b = 1.0 marker.color.a = 1.0 for p in self.points: pt = Point32() pt.x = p[0] pt.y = p[1] pt.z = 0.0 marker.points.append(pt) else: # delete marker.action = marker.DELETE self.traj_pub.publish(marker) print('publishing traj') elif self.traj_pub.get_num_connections() == 0: print "Not publishing trajectory, no subscribers"
def callback(self, data): marker = Marker() marker.header = data.header #_id = data.header.frame_id marker.header.stamp = rospy.Time.now() angle = 0 marker.id = 0 marker.ns = "scan_ds" marker.lifetime.secs = 100 marker.scale.x = self.scale marker.scale.y = self.scale marker.scale.z = self.scale for i in range(0,len(data.ranges), self.n): if np.abs(data.ranges[i]) < 20.0: p = Point() p.x = data.ranges[i] * np.cos(angle) p.y = data.ranges[i] * np.sin(angle) p.z = 0 marker.points.append(p) angle += self.n * data.angle_increment marker.type = marker.POINTS marker.color.a = 1.0 marker.color.r = 1.0 marker.color.b = 0.0 marker.color.g = 0.0 self.marker_pub.publish(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_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_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 create_circle_marker(self, pose_x, pose_y, ellipse_theta, ellipse_a, ellipse_b, marker_id=0, color=ColorRGBA(0, 1, 0, 1)): h = Header() # h.frame_id = self.scan_frame_id #tie marker visualization to laser it comes from h.frame_id = "base_laser_link" 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 = "ellipse_marker" mark.id = marker_id mark.type = 3 mark.action = 0 mark.scale = Vector3(ellipse_a * 2, ellipse_b * 2, 1) #scale, in meters mark.color = color pose = Pose(Point(pose_x, pose_y, 0.5), Quaternion(0.0, 0.0, 1.0, cos(ellipse_theta / 2))) mark.pose = pose return mark
def publish_end_point(self, duration=0.0): should_publish = len(self.points) > 1 if self.visualize and self.end_pub.get_num_connections() > 0: print "Publishing end point" marker = Marker() marker.header = self.make_header("/map") marker.ns = self.viz_namespace + "/trajectory" marker.id = 1 marker.type = 2 # sphere marker.lifetime = rospy.Duration.from_sec(duration) if should_publish: marker.action = 0 marker.pose.position.x = self.points[-1][0] marker.pose.position.y = self.points[-1][1] marker.pose.orientation.w = 1.0 marker.scale.x = 1.0 marker.scale.y = 1.0 marker.scale.z = 1.0 marker.color.r = 1.0 marker.color.g = 0.0 marker.color.b = 0.0 marker.color.a = 1.0 else: # delete marker marker.action = 2 self.end_pub.publish(marker)
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 publish_trajectory(self, duration=0.0, should_publish=True): rospy.loginfo('Publishing subsampled trajectory.') marker = Marker() marker.header = utils.make_header("/map") marker.ns = 'visualization/' marker.id = 2 marker.type = 4 # line strip marker.lifetime = rospy.Duration.from_sec(duration) if should_publish: marker.action = 0 marker.scale.x = 0.1 marker.scale.y = 0.1 marker.scale.z = 0.05 marker.color.r = 0.0 marker.color.g = 0.0 marker.color.b = 1.0 marker.color.a = 0.75 for p in self.traj_pts: pt = Point32() pt.x = p[0] pt.y = p[1] pt.z = -0.1 marker.points.append(pt) else: marker.action = 2 self.traj_pub.publish(marker) rospy.loginfo('Publishing a subsampled trajectory with %d points.' % len(marker.points))
def _build_marker_array(self, header, objects): """ Build MarkerArray from given header and list of ObjectItem. Args: header (std_msgs.msg.Header): Header of current processing frame objects (list): List of ObjectItem in which each item represents a merged result Returns: MarkerArray: Includes marker for cleaning preivous markers, marker for bouding box and marker for text. """ marker_array = MarkerArray() clean_all_marker = Marker() clean_all_marker.header = header clean_all_marker.action = Marker.DELETEALL marker_array.markers.append(clean_all_marker) markers = [] for obj in objects: markers.extend([obj.linelist()]) markers.extend([obj.min_text()]) markers.extend([obj.max_text()]) markers.extend([obj.name_id_text()]) for idx, marker in enumerate(markers): marker.id = idx marker_array.markers.extend(markers) return marker_array
def local_display(self, frame): object_list = self.objects.obstacles for i in range(len(object_list)): marker = Marker() marker.header = Header() marker.header.frame_id = frame marker.ns = "Object_NS" marker.id = i marker.type = Marker.CYLINDER marker.action = 0 marker.color.r = 0.0 marker.color.g = 0.0 marker.color.b = 1.0 marker.color.a = 0.5 marker.lifetime = rospy.Duration.from_sec(0.1) marker.pose.position.x = object_list[i].pos.point.x marker.pose.position.y = object_list[i].pos.point.y marker.pose.position.z = 0 radius = object_list[i].radius marker.scale.x = radius marker.scale.y = radius marker.scale.z = 0.01 self.display_pub.publish(marker)
def publish(self, grasps, obj = None): msg = MarkerArray() self.marker_id = 0 # reset marker counter if obj and len(obj.primitives) > 0 and len(obj.primitive_poses) > 0: m = Marker() m.header = obj.header m.ns = "object" m.id = self.marker_id self.marker_id += 1 m.type = m.CUBE m.action = m.ADD m.pose = obj.primitive_poses[0] m.scale.x = obj.primitives[0].dimensions[0] m.scale.y = obj.primitives[0].dimensions[1] m.scale.z = obj.primitives[0].dimensions[2] m.color.r = 0 m.color.g = 0 m.color.b = 1 m.color.a = 0.8 msg.markers.append(m) for grasp in grasps: msg.markers.append(self.get_gripper_marker(grasp.grasp_pose, grasp.grasp_quality)) self.pub.publish(msg)
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 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 make_arrow_marker(self, header, pose, rgba, arrow_scale, ns): marker = Marker(type=Marker.ARROW,action=Marker.ADD) marker.header = header marker.pose = pose marker.lifetime = rospy.Duration(0) marker.color = stdm.ColorRGBA(*rgba) marker.scale = gm.Vector3(6*arrow_scale, arrow_scale, arrow_scale) marker.id = self.get_unused_id() self.ids.add(marker.id) marker.ns = ns return marker
def send_marker(self, named_pt): m=Marker() m.header = copy.deepcopy(named_pt.header) m.type=Marker.CYLINDER m.pose.position = named_pt.point m.pose.orientation.x=0.707 m.pose.orientation.y=0.0 m.pose.orientation.z=0.0 m.pose.orientation.w=0.707 m.scale.x=0.2 m.scale.y=0.2 m.scale.z=0.2 m.color.r=0.8 m.color.g=0.8 m.color.b=0.8 m.color.a=1.0 m.id = self.count #m.text=named_pt.name self.marker_pub.publish(m) self.count += 1 t=Marker() t.header = copy.deepcopy(named_pt.header) m.type = Marker.TEXT_VIEW_FACING m.pose.position = named_pt.point m.pose.position.z += 0.1 m.pose.orientation.x=0.707 m.pose.orientation.y=0.0 m.pose.orientation.z=0.0 m.pose.orientation.w=0.707 m.scale.x=0.2 m.scale.y=0.2 m.scale.z=0.2 m.color.r=0.8 m.color.g=0.8 m.color.b=0.8 m.color.a=1.0 m.text = named_pt.name m.id = self.count self.marker_pub.publish(m) self.count += 1
def pub_marker(): marker = Marker(ns="car_model", id=0, type=Marker.MESH_RESOURCE, action=Marker.ADD, frame_locked=True, mesh_resource = "package://gazebo_drive_simulator/models/polaris.stl" , mesh_use_embedded_materials=False) marker.scale = Vector3(1, 1, 1) marker.header = std_msgs.msg.Header(frame_id="car", stamp=rospy.get_rostime()) marker.pose = Pose(position=Point(x=0, y=0, z=0), orientation=Quaternion(x=0, y=0, z=-numpy.sqrt(2.0)/2.0, w=numpy.sqrt(2.0)/2.0)) marker.color = std_msgs.msg.ColorRGBA(r=0.2, g=0.2, b=1.0, a=0.7) r = rospy.Rate(1) while not rospy.is_shutdown(): marker_pub.publish(MarkerArray([marker]+get_line_markers(rad))) r.sleep()
def _fit_ellipse(self, data, publisher): # print "fitting" ellipse_xy = [] # points = [] #array to hold all points - FOR DEBUGGING angle = data.angle_min incr = data.angle_increment max_range = data.range_max ranges = data.ranges # polar >> cartesian for r in ranges: if r < max_range: ellipse_xy.append([cos(angle) * r, sin(angle) * r]) # make xy angle += incr # eliminate outlying points # x_avg = sum([xy[0] for xy in ellipse_xy])/len(ellipse_xy) # y_avg = sum([xy[1] for xy in ellipse_xy])/len(ellipse_xy) # for xy in ellipse_xy: # if (abs(xy[0]-x_avg)<0.5 or abs(xy[1]-y_avg)<0.5): # ellipse_xy.remove(xy) # fit ellipse to points - constrain size if len(ellipse_xy) > 1: e2 = Ellipse2d() e2.fit(ellipse_xy) # apply alpha to smooth changes over time, if old data exists if self.last_a != None and self.last_b != None and self.last_center != None: e2.center = [ self.last_center[i] * self.center_alpha + e2.center[i] * (1 - self.center_alpha) for i in [0, 1] ] e2.a = self.last_a * self.axis_alpha + e2.a * (1 - self.axis_alpha) e2.b = self.last_b * self.axis_alpha + e2.b * (1 - self.axis_alpha) self.last_center = e2.center self.last_b = e2.b self.last_a = e2.a # Publish ellipse data as Marker message h = Header() h.frame_id = "laser" # 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 # publish marker:person_marker, modify a red cylinder, last indefinitely mark = Marker() mark.header = h mark.ns = "person_marker" mark.id = 0 mark.type = 3 mark.action = 0 mark.pose = geometry_msgs.msg.Pose( geometry_msgs.msg.Point(e2.center[0], e2.center[1], 0.5), geometry_msgs.msg.Quaternion(0.0, 0.0, 1.0, cos(e2.theta / 2)), ) mark.scale = geometry_msgs.msg.Vector3(e2.a * 2, e2.b * 2, 1) # scale, in meters mark.color = self.color # marker is red if clean, green if infected publisher.publish(mark) else: print "data not received"
def marker_at_point(point_stamped, ns='', mid=0, mtype=Marker.SPHERE, sx=0.05, sy=0.05, sz=0.05, r=0.0, g=0.0, b=1.0, a=0.8): ''' Returns a single marker at a point. Orientation is always (0, 0, 0, 1). See the visualization_msgs.msg.Marker documentation for more details on any of these fields. **Args:** **point_stamped (geometry_msgs.msg.PointStamped):** Point for marker *ns (string):* namespace *mid (int):* ID *mtype (int):* Shape type *sx (double):* Scale in x *sy (double):* Scale in y *sz (double):* scale in z *r (double):* Red (scale 0 to 1) *g (double):* Green (scale 0 to 1) *b (double):* Blue (scale 0 to 1) *a (double):* Alpha (scale 0 to 1) **Returns:** visualization_msgs.msg.Marker at point_stamped ''' marker = Marker() marker.header = copy.deepcopy(point_stamped.header) marker.ns = ns marker.id = mid marker.type = mtype marker.action = marker.ADD marker.pose.position = copy.deepcopy(point_stamped.point) marker.pose.orientation.w = 1.0 marker.scale.x = sx marker.scale.y = sy marker.scale.z = sz marker.color.r = r marker.color.g = g marker.color.b = b marker.color.a = a return marker
def create_arrow(self, pose_stamped): m = Marker() m.header = pose_stamped.header m.ns = "billiards_shot_plan" m.id = self._id m.action = Marker.ADD m.type = Marker.ARROW m.pose = pose_stamped.pose m.scale.x = 0.5 m.scale.y = 0.2 m.scale.z = 0.2 m.color.a = 1.0 self._id += 1 return m
def axis_marker(pose_stamped, id, ns): """ Create three Marker msgs from a PoseStamped set of arrows (x=red, y=green, z=blue). Parameters: id: the id number for the x-arrow (y is id+1, z is id+2) ns: the namespace for the marker """ marker = Marker() marker.header = pose_stamped.header marker.ns = ns marker.type = Marker.ARROW marker.action = Marker.ADD marker.scale.x = 0.01 marker.scale.y = 0.02 marker.color.a = 1.0 marker.lifetime = rospy.Duration(1/5.0) orientation = pose_stamped.pose.orientation quat = [orientation.x, orientation.y, orientation.z, orientation.w] mat = tf.transformations.quaternion_matrix(quat) start = (pose_stamped.pose.position.x, pose_stamped.pose.position.y, pose_stamped.pose.position.z) x_end = list(mat[:,0][0:3]*.05 + numpy.array(start)) y_end = list(mat[:,1][0:3]*.05 + numpy.array(start)) z_end = list(mat[:,2][0:3]*.05 + numpy.array(start)) marker.id = id marker.points = [pose_stamped.pose.position, Point(*x_end)] marker.color.r = 1.0 marker.color.g = 0.0 marker.color.b = 0.0 marker2 = copy.deepcopy(marker) marker2.id = id+1 marker2.points = [pose_stamped.pose.position, Point(*y_end)] marker2.color.r = 0.0 marker2.color.g = 1.0 marker2.color.b = 0.0 marker3 = copy.deepcopy(marker2) marker3.id = id+2 marker3.points = [pose_stamped.pose.position, Point(*z_end)] marker3.color.r = 0.0 marker3.color.g = 0.0 marker3.color.b = 1.0 return (marker, marker2, marker3)
def newCompositeDetectedPersonsAvailable(compositeDetectedPersons): global currentMarkerId, oldestActiveMarkerId markerArray = MarkerArray() markerZ = rospy.get_param("~marker_z", 2.0) markerScale = rospy.get_param("~marker_scale", 1.0) # Delete old markers for markerId in xrange(oldestActiveMarkerId, currentMarkerId): marker = Marker() marker.header = compositeDetectedPersons.header marker.id = markerId marker.action = Marker.DELETE markerArray.markers.append(marker) oldestActiveMarkerId = currentMarkerId # Create new markers for compositeDetectedPerson in compositeDetectedPersons.elements: if len(compositeDetectedPerson.original_detections) > 1: for originalDetectedPerson in compositeDetectedPerson.original_detections: marker = Marker() marker.header = compositeDetectedPersons.header marker.id = currentMarkerId marker.type = Marker.ARROW marker.color.r = marker.color.b = marker.color.a = 1.0 marker.color.g = 0.3 marker.points.append( Point(compositeDetectedPerson.pose.pose.position.x, compositeDetectedPerson.pose.pose.position.y, markerZ ) ) marker.points.append( Point(originalDetectedPerson.pose.pose.position.x, originalDetectedPerson.pose.pose.position.y, markerZ ) ) marker.scale.x = 0.01 * markerScale marker.scale.y = 0.05 * markerScale marker.scale.z = 0.05 * markerScale markerArray.markers.append(marker) currentMarkerId += 1 markerArrayPublisher.publish(markerArray)
def draw_traj_points(self, pose_array, rgba = (0,1,0,1), arrow_scale = .05, ns = "default_ns", duration=0): marker_array = MarkerArray() for pose in pose_array.poses: marker = Marker(type=Marker.ARROW,action=Marker.ADD) marker.header = pose_array.header marker.pose = pose marker.lifetime = rospy.Duration(0) marker.color = stdm.ColorRGBA(*rgba) marker.scale = gm.Vector3(arrow_scale, arrow_scale, arrow_scale) marker.id = self.get_unused_id() marker.ns = ns marker_array.markers.append(marker) self.ids.add(marker.id) self.array_pub.publish(marker_array) return MarkerListHandle([MarkerHandle(marker, self.pub) for marker in marker_array.markers])
def visualize_base_ray(): msg = Marker() msg.header = Header(stamp=Time.now(), frame_id="base_footprint") msg.scale = Vector3(x=0.005, y=0.0, z=0.0) # only x is used msg.pose.position = Point(x=0, y=0, z=0) # arrow msg.pose.orientation = Quaternion(x=0, y=0, z=0, w=1) msg.points = [Point(x=0, y=0,z=0.01), Point(x=WORKING_DIST_FROM_POOP,y=0,z=0.01)] msg.ns = "base_ray" msg.id = 0 msg.action = 0 # add msg.type = 4 # line strip msg.color = ColorRGBA(r=0, g=0, b=0, a=1) msg.color.g = 0.5; msg.color.b = 1; viz_pub.publish(msg)
def lineMarker(self, p, d): marker = Marker() header = Header() header.stamp = rospy.Time.now() header.frame_id = self.base_frame marker.header = header marker.ns = "rays" marker.id = self.recent_next marker.type = Marker.LINE_LIST marker.action = Marker.ADD marker.scale = Vector3(*[0.02 for i in range(3)]) marker.points = toPoints(np.column_stack([p, (p+2*d/np.linalg.norm(d))])) color = self.recent_color marker.color = color marker.colors = [color for i in range(len(marker.points))] return marker
def publish_markers(self, topic): ''' Publish RViz visualization markers for the current collection of annotations. @param topic: Where we must publish annotations markers. ''' if len(self._annotations) == 0: messages = "No annotations retrieved. Nothing to publish!" rospy.logwarn(messages) return # Advertise a topic for retrieved annotations' visualization markers markers_pub = rospy.Publisher(topic, MarkerArray, latch=True, queue_size=5) # Process retrieved data to build markers lists markers_list = MarkerArray() marker_id = 1 for a in self._annotations: marker = Marker() marker.id = marker_id marker.header = a.pose.header marker.type = a.shape marker.ns = a.type 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 markers_list.markers.append(marker) label = copy.deepcopy(marker) label.id = label.id + 1000000 # marker id must be unique label.type = Marker.TEXT_VIEW_FACING label.text = a.name + ' [' + a.type + ']' label.pose.position.z = label.pose.position.z + label.scale.z/2.0 + 0.12 # just above the visual label.scale.x = label.scale.y = label.scale.z = 0.3 label.color.a = 1.0 markers_list.markers.append(label) marker_id = marker_id + 1 markers_pub.publish(markers_list)