def add_name_marker(self, center_x, center_y, who): ''' This method will add name of the players under their scores who: 1, for user 0, for computer ''' marker = Marker() marker.header.frame_id = "/map" marker.header.stamp = rospy.Time.now() marker.action = marker.ADD marker.type = Marker.TEXT_VIEW_FACING marker.scale.x = 0.3 marker.scale.y = 0.3 marker.scale.z = 0.3 marker.color.a = 1 marker.color.r = 1 marker.color.g = 1 marker.color.b = 1 marker.pose.orientation.w = 0 marker.pose.orientation.x = 0 marker.pose.orientation.y = 0 marker.pose.orientation.z = 0 marker.pose.position.x = center_x marker.pose.position.y = center_y marker.pose.position.z = 0.5 if who == 0: marker.ns = 'computer_name' marker.id = 6 marker.text = 'Computer' else: marker.ns = 'player_name' marker.id = 7 marker.text = 'Player' self.marker_publisher.publish(marker)
def service_cb(req): marker = Marker() # Sphere text_marker = Marker() # Text marker.header.stamp = rospy.Time.now() marker.header.frame_id = "base_link" text_marker.header.stamp = rospy.Time.now() text_marker.header.frame_id = "base_link" marker.action = Marker.ADD marker.type = Marker.SPHERE text_marker.type = Marker.TEXT_VIEW_FACING marker.pose.position = req.point marker.pose.orientation.w = 1.0 marker.scale.x = marker.scale.y = marker.scale.z = 0.02 text_marker.pose.position.y = -0.9 text_marker.pose.position.x = 0.9 text_marker.scale.z = 0.06 if req.primitive == 0: # suck 1 marker.color.b = marker.color.a = 1.0 # blue elif req.primitive == 1: # suck 2 marker.color.g = marker.color.a = 1.0 # green else: # grasp marker.color.r = marker.color.a = 1.0 text_marker.color.r = text_marker.color.g = text_marker.color.b = text_marker.color.a = 1.0 if req.primitive == 0 and req.valid: # suck 1 text_marker.text = "suck_1" elif req.primitive == 1 and req.valid: # suck 2 text_marker.text = "suck_2" elif req.primitive >= 2 and req.valid: # GRASP angle = round(req.angle * 180 / pi) text_marker.text = "grasp," + str(int(angle)) elif not req.valid: # INVALID text_marker.text = "invalid target" pub_marker.publish(marker) pub_text_marker.publish(text_marker) return viz_markerResponse()
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 publish_waypoint_markers(self): pub_waypoint_markers = rospy.Publisher('waypoint_markers', MarkerArray) marker_array = MarkerArray() for i in range(len(self.waypoints)): waypoint = self.waypoints[i] waypoint_marker = Marker() waypoint_marker.id = i waypoint_marker.header.frame_id = "/map" waypoint_marker.header.stamp = rospy.Time.now() if (waypoint.type == 'P'): waypoint_marker.type = 5 # Line List waypoint_marker.text = 'waypoint_%s_point' % i waypoint_marker.color.r = 176.0 waypoint_marker.color.g = 224.0 waypoint_marker.color.b = 230.0 waypoint_marker.color.a = 0.5 waypoint_marker.scale.x = 0.5 c = waypoint.coordinate waypoint_marker.points.append(Point(c.x, c.y, c.z)) waypoint_marker.points.append(Point(c.x, c.y, c.z + 3.0)) else: waypoint_marker.type = 3 # Cylinder waypoint_marker.text = 'waypoint_%s_cone' % i waypoint_marker.color.r = 255.0 waypoint_marker.color.g = 69.0 waypoint_marker.color.b = 0.0 waypoint_marker.color.a = 1.0 waypoint_marker.scale.x = 0.3 waypoint_marker.scale.y = 0.3 waypoint_marker.scale.z = 0.5 waypoint_marker.pose.position = waypoint.coordinate marker_array.markers.append(waypoint_marker) if self.current_waypoint_idx != len(self.waypoints): current_waypoint_marker = Marker() current_waypoint_marker.id = 999 current_waypoint_marker.header.frame_id = "/map" current_waypoint_marker.header.stamp = rospy.Time.now() current_waypoint_marker.type = 2 # Sphere current_waypoint_marker.text = 'current_waypoint' current_waypoint_marker.color.r = 255.0 current_waypoint_marker.color.g = 0.0 current_waypoint_marker.color.b = 0.0 current_waypoint_marker.color.a = 1.0 current_waypoint_marker.scale.x = 0.3 current_waypoint_marker.scale.y = 0.3 current_waypoint_marker.scale.z = 0.3 current_waypoint = self.waypoints[self.current_waypoint_idx] current_waypoint_marker.pose.position.x = current_waypoint.coordinate.x current_waypoint_marker.pose.position.y = current_waypoint.coordinate.y current_waypoint_marker.pose.position.z = 1.0 marker_array.markers.append(current_waypoint_marker) pub_waypoint_markers.publish(marker_array)
def ArrowLabel(): global count global state global frame global start global goal global waypoint global label pub = rospy.Publisher('label', MarkerArray, queue_size = 10) marker_data = Marker() marker_data.header.frame_id = frame marker_data.header.stamp = rospy.Time.now() marker_data.ns = 'basic_shapes' marker_data.action = Marker.ADD marker_data.lifetime = rospy.Duration() marker_data.type = Marker.TEXT_VIEW_FACING marker_data.color.r = 1.0 marker_data.color.g = 0.2 marker_data.color.b = 0.5 marker_data.color.a = 1.0 marker_data.scale.x = 1.0 marker_data.scale.y = 1.0 marker_data.scale.z = 1.0 if state == 'goal' : pose = goal marker_data.text = 'Goal' marker_data.id = -1 elif state == 'start' : pose = start marker_data.text = 'Start' marker_data.id = -2 else : pose = waypoint.poses[count] marker_data.text = str(count) marker_data.id = count count +=1 marker_data.pose.position.x = pose.position.x marker_data.pose.position.y = pose.position.y marker_data.pose.orientation.z = pose.orientation.z marker_data.pose.orientation.w = pose.orientation.w label.markers.append(marker_data) pub.publish(label)
def text_marker(marker_id, _point, txt): #waypoints = list() #waypoints.append(Pose(Point(2.680, 1.600, 0.000), 0.1)) #waypoints.append(Pose(Point(2.680, -1.600, 0.000), 0.1)) #waypoints.append(Pose(Point(-2.680, 1.600, 0.000), 0.1)) #waypoints.append(Pose(Point(-2.680, -1.600, 0.000), 0.1)) #set up markeri marker = Marker() marker_scale = 0.6 marker_lifetime = 0 marker_ns = 'point name' marker_color= {'r':0.0, 'g':1.0, 'b':0.0, 'a':1.0} marker.ns = marker_ns marker.id = marker_id # marker.type = Marker.SPHERE marker.type = Marker.TEXT_VIEW_FACING marker.action = Marker.ADD marker.header.frame_id = '/map' marker.lifetime = rospy.Duration(marker_lifetime) marker.scale.x = marker_scale marker.scale.y = marker_scale marker.scale.z = marker_scale marker.color.r = marker_color['r'] marker.color.g = marker_color['g'] marker.color.b = marker_color['b'] marker.color.a = marker_color['a'] marker.text = txt marker.header.stamp = rospy.Time.now() marker.pose.position.x = _point.x marker.pose.position.y = _point.y marker.pose.position.z = _point.z print txt return marker
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 get_vehicle_marker(object, header, marker_id=0, is_player=False): """ Return a marker msg :param object: carla agent object (pb2 object (vehicle, pedestrian or traffic light)) :param header: ros header (stamp/frame_id) :param marker_id: a marker id (int32) :param is_player: True if player else False (usefull to change marker color) :return: a ros marker msg """ marker = Marker(header=header) marker.color.a = 0.3 if is_player: marker.color.g = 1 marker.color.r = 0 marker.color.b = 0 else: marker.color.r = 1 marker.color.g = 0 marker.color.b = 0 marker.id = marker_id marker.text = "id = {}".format(marker_id) update_marker_pose(object, marker) return marker
def control_points_callback(self, msg): subdiv_points = msg for i in range(self.subdivisions): subdiv_points = self.subdivide(subdiv_points) self.curve_pub.publish(subdiv_points) marker = Marker() marker.header.frame_id = "map" marker.header.stamp = rospy.Time.now() marker.ns = "subdivision" marker.id = 0 # self.subdivisions marker.action = Marker.ADD marker.type = Marker.LINE_STRIP marker.pose.orientation.w = 1.0 marker.scale.x = 0.05 marker.scale.y = 0.05 marker.scale.z = 0.05 marker.color.r = 1.0 marker.color.g = 1.0 marker.color.b = 1.0 marker.color.a = 1.0 marker.text = str(self.subdivisions) for pt in subdiv_points.points: gpt = Point() gpt.x = pt.x gpt.y = pt.y marker.points.append(gpt) marker.points.append(marker.points[0]) self.marker_pub.publish(marker)
def make_label(text, position, id = 0 ,duration = 5.0, color=[1.0,1.0,1.0]): """ Helper function for generating visualization markers. Args: text (str): Text string to be displayed. position (list): List containing [x,y,z] positions id (int): Integer identifying the label duration (rospy.Duration): How long the label will be displayed for color (list): List of label color floats from 0 to 1 [r,g,b] Returns: Marker: A text view marker which can be published to RViz """ marker = Marker() marker.header.frame_id = '/world' marker.id = id marker.type = marker.TEXT_VIEW_FACING marker.text = text marker.action = marker.ADD marker.scale.x = 0.05 marker.scale.y = 0.05 marker.scale.z = 0.05 marker.color.a = 1.0 marker.color.r = color[0] marker.color.g = color[1] marker.color.b = color[2] marker.lifetime = rospy.Duration(duration) marker.pose.orientation.w = 1.0 marker.pose.position.x = position[0] marker.pose.position.y = position[1] marker.pose.position.z = position[2] return marker
def addFlag(): # get the cooprations of turtlebot in map using tf # listener = tf.TransformListener() # listener.waitForTransform("/odom", "/base_link", rospy.Time(0), rospy.Duration(1)) # (position, rotation) = listener.lookupTransform("/odom", "/base_link", rospy.Time(0)) global poseX, poseY, poseZ marker_pub = rospy.Publisher("warning_marker",Marker,queue_size=1) shape = Marker.TEXT_VIEW_FACING for i in range(5): marker = Marker() marker.header.frame_id = 'map' marker.color.r, marker.color.b, marker.color.g = 1, 0, 0 marker.color.a = 1 marker.ns = "WarningFlag" marker.id = 1024 marker.pose.position.x, marker.pose.position.y, marker.pose.position.z = poseX, poseY , poseZ+0.5#position[0], position[1]-0.7, 0 marker.scale.x, marker.scale.y, marker.scale.z = 0.01, 0.01, 0.2 marker.type = shape marker.text = "WARNING" marker.lifetime = rospy.Duration(5) marker_pub.publish(marker) rospy.sleep(1)
def create_delay_text_marker(self, idx, header, dis): marker = Marker() marker.header.frame_id = header.frame_id marker.header.stamp = header.stamp marker.ns = self.inputTopic + "_d" marker.action = Marker.ADD marker.id = idx + 500 # message.track.id marker.type = Marker.TEXT_VIEW_FACING marker.scale.z = 1 marker.lifetime = rospy.Duration(1.0) marker.color.r = 0 marker.color.g = 1 marker.color.b = 0 marker.color.a = 1.0 marker.text = "%.3fms" % ((rospy.get_rostime() - header.stamp).to_sec() * 1000.0) marker.pose.position.x = dis marker.pose.position.y = 0 marker.pose.position.z = 7 marker.pose.orientation.x = 0.0 marker.pose.orientation.y = 0.0 marker.pose.orientation.z = 0.0 marker.pose.orientation.w = 1.0 return marker
def updateText(idx, pcd, theta, centroid, prediction): marker = Marker() marker.header.frame_id = "/base_link" marker.header.stamp = rospy.Time.now() marker.id = idx marker.type = Marker.TEXT_VIEW_FACING marker.action = Marker.ADD R = numpy.array([[numpy.cos(theta), -numpy.sin(theta)], [numpy.sin(theta), numpy.cos(theta)]]) new_pcd = R.dot( (pcd[:, :2] - centroid[:2]).transpose()).transpose() + centroid[:2] marker.pose.position.x = new_pcd[:, 0].mean() marker.pose.position.y = new_pcd[:, 1].mean() marker.pose.position.z = pcd[:, 2].max() marker.pose.orientation.x = 0.0 marker.pose.orientation.y = 0.0 marker.pose.orientation.z = 0.0 marker.pose.orientation.w = 1.0 marker.text = prediction marker.scale.x = marker.scale.y = marker.scale.z = 0.5 marker.color.r = class_to_color_rgb[prediction][0] / 255.0 marker.color.g = class_to_color_rgb[prediction][1] / 255.0 marker.color.b = class_to_color_rgb[prediction][2] / 255.0 marker.color.a = 1.0 pubMarker.publish(marker)
def __publish_marker_for_object(self, object, txt): marker = Marker() marker.header.frame_id = object.object.header.frame_id marker.header.stamp = rospy.Time(0) marker.ns = "scene_classifier_marker" self.marker_id = self.marker_id + 1 marker.id = self.marker_id marker.type = Marker.TEXT_VIEW_FACING marker.action = Marker.ADD marker.pose.position.x = object.c_centroid.x marker.pose.position.y = object.c_centroid.y marker.pose.position.z = object.c_centroid.z marker.pose.orientation.x = 0.0 marker.pose.orientation.y = 0.0 marker.pose.orientation.z = 0.0 marker.pose.orientation.w = 1.0 marker.scale.x = 0.5 marker.scale.y = 0.1 marker.scale.z = 0.1 marker.color.a = 1.0 marker.color.r = 0.0 marker.color.g = 1.0 marker.color.b = 0.0 marker.text = txt marker.lifetime = rospy.Duration.from_sec(10) self.marker_publisher.publish(marker)
def displayETA(self): marker = Marker() marker.header.frame_id = "base_footprint" marker.header.stamp = rospy.Time() marker.id = 1234 marker.type = Marker.TEXT_VIEW_FACING marker.pose.position.x = 0.0 marker.pose.position.y = 0.0 marker.pose.position.z = 0.0 marker.pose.orientation.x = 0.0 marker.pose.orientation.y = 0.0 marker.pose.orientation.z = 0.0 marker.pose.orientation.w = 1.0 marker.scale.x = 0.4 marker.scale.y = 0.1 marker.scale.z = 0.2 marker.color.a = 1.0 # Don't forget to set the alpha! marker.color.r = 0.02 marker.color.g = 0.84 marker.color.b = 1.0 marker.text = "ETA to destination: {}".format( self.current_plan_duration) self.vis_pub.publish(marker)
def create_person_label_marker(self, person, color): h = Header() h.frame_id = person.header.frame_id #tie marker visualization to laser it comes from h.stamp = rospy.Time.now() # Note you need to call rospy.init_node() before this will work #create marker:person_marker, modify a red cylinder, last indefinitely mark = Marker() mark.header = h mark.ns = "person_label_marker" ## simple hack to map persons name to integer value for unique marker id char_list = list(person.name) char_int_list = [str(ord(x)) for x in char_list] char_int_str = "".join(char_int_list) char_int = int(char_int_str) & 255 print print "str to int" print char_int_list print char_int print "Char int binary) ", bin(char_int) # mark.id = int(char_int / 10000) mark.id = int(float(person.name)) #char_int mark.type = Marker.TEXT_VIEW_FACING mark.action = 0 mark.scale = Vector3(self.scale_factor * 0.5, self.scale_factor * 0.5, 1) mark.color = color mark.lifetime = rospy.Duration(0.5,0) print "person name: ", person.name mark.text = person.name pose = Pose(Point(person.pose.position.x + 0.75, person.pose.position.y + 0.5, self.person_height + 0.75), Quaternion(0.0,0.0,1.0,cos(person.pose.position.z/2))) mark.pose = pose return mark
def __publish_marker_for_object(self, object, txt): marker = Marker() marker.header.frame_id = object.object.header.frame_id marker.header.stamp = rospy.Time(0) marker.ns = "scene_classifier_marker" self.marker_id = self.marker_id + 1 marker.id = self.marker_id marker.type = Marker.TEXT_VIEW_FACING marker.action = Marker.ADD marker.pose.position.x = object.c_centroid.x marker.pose.position.y = object.c_centroid.y marker.pose.position.z = object.c_centroid.z marker.pose.orientation.x = 0.0 marker.pose.orientation.y = 0.0 marker.pose.orientation.z = 0.0 marker.pose.orientation.w = 1.0 marker.scale.x = 0.5 marker.scale.y = 0.1 marker.scale.z = 0.1 marker.color.a = 1.0 marker.color.r = 0.0 marker.color.g = 1.0 marker.color.b = 0.0 marker.text = txt marker.lifetime = rospy.Duration.from_sec(10) self.marker_publisher.publish(marker)
def pub_body(self, pos, quat, scale, color, num, shape, text = ''): marker = Marker() marker.header.frame_id = self.frame marker.header.stamp = rospy.Time.now() marker.ns = "basic_shapes" marker.id = num marker.type = shape marker.action = Marker.ADD marker.pose.position.x = pos[0] marker.pose.position.y = pos[1] marker.pose.position.z = pos[2] marker.pose.orientation.x = quat[0] marker.pose.orientation.y = quat[1] marker.pose.orientation.z = quat[2] marker.pose.orientation.w = quat[3] marker.scale.x = scale[0] marker.scale.y = scale[1] marker.scale.z = scale[2] marker.color.r = color[0] marker.color.g = color[1] marker.color.b = color[2] marker.color.a = color[3] marker.lifetime = rospy.Duration() marker.text = text self.pub.publish(marker)
def 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 display_particles(poses_pub): markerArray = MarkerArray() #print(poses_pub[1]) #print('-------') for i in range(len(poses_pub)): marker = Marker() marker.header.frame_id = "map" marker.header.stamp = rospy.Time() marker.ns = "turtle_bot" marker.type = Marker.SPHERE marker.action = Marker.ADD marker.id = i marker.text = "testing" marker.pose.position.x = poses_pub[i][0] marker.pose.position.y = poses_pub[i][1] marker.pose.position.z = 1 marker.pose.orientation.w = 1 marker.color.r = 1.0 marker.color.g = 0.0 marker.color.b = 0.0 marker.color.a = 1.0 marker.scale.z = 0.1 marker.scale.y = 0.1 marker.scale.x = 0.1 markerArray.markers.append(marker) map_publisher.publish(markerArray)
def pub_arrow(self, pos1, pos2, color, mag_force): marker = Marker() marker.header.frame_id = self.frame marker.header.stamp = rospy.Time.now() marker.ns = "basic_shapes" marker.id = 99999 marker.type = Marker.ARROW marker.action = Marker.ADD pt1 = Point() pt2 = Point() pt1.x = pos1[0, 0] pt1.y = pos1[1, 0] pt1.z = pos1[2, 0] pt2.x = pos2[0, 0] pt2.y = pos2[1, 0] pt2.z = pos2[2, 0] marker.points.append(pt1) marker.points.append(pt2) marker.scale.x = 0.05 marker.scale.y = 0.1 #marker.scale.z = scale[2] marker.color.r = color[0] marker.color.g = color[1] marker.color.b = color[2] marker.color.a = color[3] marker.lifetime = rospy.Duration() marker.text = mag_force self.pub.publish(marker)
def get_current_position_marker(self, link, offset=None, root="", scale=1, color=(0,1,0,1), idx=0): (mesh, pose) = self.get_link_data(link) marker = Marker() if offset==None : marker.pose = pose else : marker.pose = toMsg(fromMsg(offset)*fromMsg(pose)) marker.header.frame_id = root marker.header.stamp = rospy.get_rostime() marker.ns = self.robot_name marker.mesh_resource = mesh marker.type = Marker.MESH_RESOURCE marker.action = Marker.MODIFY marker.scale.x = scale marker.scale.y = scale marker.scale.z = scale marker.color.r = color[0] marker.color.g = color[1] marker.color.b = color[2] marker.color.a = color[3] marker.text = link marker.id = idx marker.mesh_use_embedded_materials = True return marker
def __setTextMarker(self, id, waypoint, colors = [1,0,0,0]): try: marker = Marker() marker.header.frame_id = '/map' marker.header.stamp = rospy.Time.now() marker.ns = 'patrol' marker.id = id + len(self.__waypoints) marker.type = marker.TEXT_VIEW_FACING marker.action = marker.ADD marker.text = str(id) marker.scale.x = 0.2 marker.scale.y = 0.2 marker.scale.z = 0.2 marker.color.a = colors[0] marker.color.r = colors[1] marker.color.b = colors[2] marker.color.g = colors[3] marker.pose.orientation.w = 1.0 marker.pose.position.x = waypoint.target_pose.pose.position.x marker.pose.position.y = waypoint.target_pose.pose.position.y marker.pose.position.z = waypoint.target_pose.pose.position.z return marker except Exception as ex: rospy.logwarn('PatrolNode.__setMarker- ', ex.message)
def pub_arrow(self, pos1, pos2, color, mag_force): marker = Marker() marker.header.frame_id = self.frame marker.header.stamp = rospy.Time.now() marker.ns = "basic_shapes" marker.id = 99999 marker.type = Marker.ARROW marker.action = Marker.ADD pt1 = Point() pt2 = Point() pt1.x = pos1[0,0] pt1.y = pos1[1,0] pt1.z = pos1[2,0] pt2.x = pos2[0,0] pt2.y = pos2[1,0] pt2.z = pos2[2,0] marker.points.append(pt1) marker.points.append(pt2) marker.scale.x = 0.05 marker.scale.y = 0.1 #marker.scale.z = scale[2] marker.color.r = color[0] marker.color.g = color[1] marker.color.b = color[2] marker.color.a = color[3] marker.lifetime = rospy.Duration() marker.text = mag_force self.pub.publish(marker)
def publish_marker(cls, x_cord, y_cord, mid): new_marker = Marker(type = Marker.CUBE, action=Marker.ADD) new_marker.header.frame_id = "/base_laser_front_link" new_marker.header.stamp = rospy.Time.now() new_marker.ns = "basic_shapes" new_marker.id = mid new_marker.pose.position.x = x_cord new_marker.pose.position.y = y_cord new_marker.pose.position.z = 0.0 #pointxyz new_marker.pose.orientation.x = 0 new_marker.pose.orientation.y = 0 new_marker.pose.orientation.z = 0.0 new_marker.pose.orientation.w = 1 new_marker.scale.x = 0.005 new_marker.scale.y = 0.005 new_marker.scale.z = 0.005 if mid == 0: new_marker.color.r = 1 elif mid == 5: new_marker.color.g = 1 elif mid == 10: new_marker.color.b = 1 new_marker.color.a = 1 new_marker.text = "marker" new_marker.lifetime = rospy.Duration(1) SmachGlobalData.pub_marker.publish(new_marker)
def 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 publish_cluster(publisher, points, frame_id, namespace, cluster_id): """Publishes a marker representing a cluster. The x and y arguments specify the center of the target. Args: publisher: A visualization_msgs/Marker publisher points: A list of geometry_msgs/Point frame_id: The coordinate frame in which to interpret the points. namespace: string, a unique name for a group of clusters. cluster_id: int, a unique number for this cluster in the given namespace. """ marker = Marker() # TODO(jstn): Once the point clouds have the correct frame_id, # use them here. marker.header.frame_id = frame_id marker.header.stamp = rospy.Time().now() marker.ns = namespace marker.id = 2 * cluster_id marker.type = Marker.POINTS marker.action = Marker.ADD marker.color.r = random.random() marker.color.g = random.random() marker.color.b = random.random() marker.color.a = 0.5 marker.points = points marker.scale.x = 0.002 marker.scale.y = 0.002 marker.lifetime = rospy.Duration() center = [0, 0, 0] for point in points: center[0] += point.x center[1] += point.y center[2] += point.z center[0] /= len(points) center[1] /= len(points) center[2] /= len(points) text_marker = Marker() text_marker.header.frame_id = frame_id text_marker.header.stamp = rospy.Time().now() text_marker.ns = namespace text_marker.id = 2 * cluster_id + 1 text_marker.type = Marker.TEXT_VIEW_FACING text_marker.action = Marker.ADD text_marker.pose.position.x = center[0] - 0.1 text_marker.pose.position.y = center[1] text_marker.pose.position.z = center[2] text_marker.color.r = 1 text_marker.color.g = 1 text_marker.color.b = 1 text_marker.color.a = 1 text_marker.scale.z = 0.05 text_marker.text = '{}'.format(cluster_id) text_marker.lifetime = rospy.Duration() _publish(publisher, marker, "cluster") _publish(publisher, text_marker, "text_marker") return marker
def test(): rospy.init_node('intersect_plane_test') marker_pub = rospy.Publisher('table_marker', Marker) pose_pub = rospy.Publisher('pose', PoseStamped) int_pub = rospy.Publisher('intersected_points', PointCloud2) tfl = tf.TransformListener() # Test table table = Table() table.pose.header.frame_id = 'base_link' table.pose.pose.orientation.x, table.pose.pose.orientation.y, table.pose.pose.orientation.z, table.pose.pose.orientation.w = (0,0,0,1) table.x_min = -0.5 table.x_max = 0.5 table.y_min = -0.5 table.y_max = 0.5 # A marker for that table marker = Marker() marker.header.frame_id = table.pose.header.frame_id marker.id = 1 marker.type = Marker.LINE_STRIP marker.action = 0 marker.pose = table.pose.pose marker.scale.x, marker.scale.y, marker.scale.z = (0.005,0.005,0.005) marker.color.r, marker.color.g, marker.color.b, marker.color.a = (0.0,1.0,1.0,1.0) marker.frame_locked = False marker.ns = 'table' marker.points = [ Point(table.x_min,table.y_min, table.pose.pose.position.z), Point(table.x_min,table.y_max, table.pose.pose.position.z), Point(table.x_max,table.y_max, table.pose.pose.position.z), Point(table.x_max,table.y_min, table.pose.pose.position.z), Point(table.x_min,table.y_min, table.pose.pose.position.z), ] marker.colors = [] marker.text = '' # marker.mesh_resource = '' marker.mesh_use_embedded_materials = False marker.header.stamp = rospy.Time.now() # A test pose pose = PoseStamped() pose.header = table.pose.header pose.pose.position.x, pose.pose.position.y, pose.pose.position.z = (0,0,0.5) pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w = quaternion_from_euler(0,-pi/5,pi/5) intersection = cast_ray(pose, table, tfl) cloud = xyz_array_to_pointcloud2(np.array([[intersection.point.x, intersection.point.y, intersection.point.z]])) cloud.header = pose.header while not rospy.is_shutdown(): marker_pub.publish(marker) pose_pub.publish(pose) int_pub.publish(cloud) rospy.loginfo('published') rospy.sleep(0.1)
def visualize_cluster(self, cluster, label=None): points = pc2.read_points(cluster.pointcloud, skip_nans=True) point_list = [Point(x=x, y=y-0.3, z=z) for x, y, z, rgb in points] if len(point_list) == 0: rospy.logwarn('Point list was of size 0, skipping.') return marker_id = len(self._current_markers) marker = Marker() marker.header.frame_id = 'map' marker.header.stamp = rospy.Time().now() marker.ns = 'clusters' marker.id = marker_id marker.type = Marker.POINTS marker.action = Marker.ADD marker.color.r = random.random() marker.color.g = random.random() marker.color.b = random.random() marker.color.a = 0.5 + random.random() marker.points = point_list marker.scale.x = 0.002 marker.scale.y = 0.002 marker.lifetime = rospy.Duration() self.visualize_marker(marker) if label is not None: center = [0, 0, 0] for point in point_list: center[0] += point.x center[1] += point.y center[2] += point.z center[0] /= len(point_list) center[1] /= len(point_list) center[2] /= len(point_list) text_marker = Marker() text_marker.header.frame_id = 'map' text_marker.header.stamp = rospy.Time().now() text_marker.ns = 'labels' text_marker.id = marker_id + 1 text_marker.type = Marker.TEXT_VIEW_FACING text_marker.action = Marker.ADD text_marker.pose.position.x = center[1] - 0.05 text_marker.pose.position.y = center[1] text_marker.pose.position.z = center[2] text_marker.color.r = 1 text_marker.color.g = 1 text_marker.color.b = 1 text_marker.color.a = 1 text_marker.scale.z = 0.05 text_marker.text = label text_marker.lifetime = rospy.Duration() self.visualize_marker(text_marker)
def poseupdate(): #set up marker marker_scale = 0.4 marker_lifetime = 0 marker_ns = 'waypoint' marker_id = 0 marker_color= {'r':1.0, 'g':0.0, 'b':0.0, 'a':1.0} marker_pub = rospy.Publisher('waypoint_marker', Marker) marker = Marker() marker.ns = marker_ns marker.id = marker_id marker.type = Marker.CUBE_LIST marker.action = Marker.ADD marker.text = "marker" marker.lifetime = rospy.Duration(marker_lifetime) marker.scale.x = marker_scale marker.scale.y = marker_scale marker.scale.z = marker_scale marker.color.r = marker_color['r'] marker.color.g = marker_color['g'] marker.color.b = marker_color['b'] marker.color.a = marker_color['a'] marker.header.frame_id = '/map' marker.header.stamp = rospy.Time.now() marker.points = list() # for waypoint in waypoints: # p = Point() # p = waypoint.position # marker.points.append(p) # i = 0 # while i < 4 and not rospy.is_shutdown(): # marker_pub.publish(marker) # i += 1 p1 = Point(0.502, 11.492, 0.000) #room1 Position(0.718, -0.571, 0.000),Orientation(0.000, 0.000, -0.008, 1.000) = Angle: -0.015 p2 = Point(-3.66308784485,13.2560949326, 0.000) #room2 Orientation(0.000, 0.000, -0.741, 0.672) = Angle: -1.668 p3 = Point(-0.00961661338806, 9.34885692596, 0.000) #room3 Position(0.782, -2.700, 0.000), Orientation(0.000, 0.000, -0.052, 0.999) = Angle: -0.105 p4 = Point(-4.05501270294,10.2383213043, 0.000) #room4 Position(-0.343, -6.387, 0.000), Orientation(0.000, 0.000, -0.667, 0.745) = Angle: -1.459 p5 = Point(-3.27898073196, 6.35970401764, 0.000) #door1 , Position(-1.134, -1.524, 0.000), Orientation(0.000, 0.000, -0.704, 0.710) = Angle: -1.562 p6 = Point( -1.94060504436, 0.0072660446167, 0.000) #door2 Position(-0.203, -3.766, 0.000), Orientation(0.000, 0.000, -0.717, 0.697) = Angle: -1.599 p7 = Point(1.34751307964, 0.948370933533, 0.000) #desk Position(-1.579, -3.107, 0.000), Orientation(0.000, 0.000, 0.012, 1.000) = Angle: 0.023 p8 = Point(-1.64714360237,4.02495145798, 0.000) #cooridor Position(-0.175, -4.631, 0.000), Orientation(0.000, 0.000, -0.717, 0.697) = Angle: -1.599 p9 = Point(-3.74548912048,3.87098288536,0.000) marker.points.append(p1) marker.points.append(p2) marker.points.append(p3) marker.points.append(p4) marker.points.append(p5) marker.points.append(p6) marker.points.append(p7) marker.points.append(p8) marker.points.append(p9) while not rospy.is_shutdown(): marker_pub.publish(marker)
def publish(self, target_frame, timestamp): ma = MarkerArray() for id in self.marker_list: marker = Marker() marker.header.stamp = timestamp marker.header.frame_id = target_frame marker.ns = "landmark_kf" marker.id = id marker.type = Marker.CYLINDER marker.action = Marker.ADD Lkf = self.marker_list[id] marker.pose.position.x = Lkf.L[0,0] marker.pose.position.y = Lkf.L[1,0] marker.pose.position.z = 0 marker.pose.orientation.x = 0 marker.pose.orientation.y = 0 marker.pose.orientation.z = 1 marker.pose.orientation.w = 0 marker.scale.x = max(3*sqrt(Lkf.P[0,0]),0.05) marker.scale.y = max(3*sqrt(Lkf.P[1,1]),0.05) 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=3.0; ma.markers.append(marker) marker = Marker() marker.header.stamp = timestamp marker.header.frame_id = target_frame marker.ns = "landmark_kf" marker.id = 1000+id marker.type = Marker.TEXT_VIEW_FACING marker.action = Marker.ADD Lkf = self.marker_list[id] marker.pose.position.x = Lkf.L[0,0] marker.pose.position.y = Lkf.L[1,0] marker.pose.position.z = 1.0 marker.pose.orientation.x = 0 marker.pose.orientation.y = 0 marker.pose.orientation.z = 1 marker.pose.orientation.w = 0 marker.text = str(id) marker.scale.x = 1.0 marker.scale.y = 1.0 marker.scale.z = 0.2 marker.color.a = 1.0; marker.color.r = 1.0; marker.color.g = 1.0; marker.color.b = 1.0; marker.lifetime.secs=3.0; ma.markers.append(marker) self.marker_pub.publish(ma)
def transformStampedArrayToLabeledLineStripMarker(cls, tsdata_lst, fmt="%Y-%m-%d %H:%M:%S", zoffset=0.05, label_downsample=1, discrete=False): "[[transformStamped, meta],...] -> LineStrip / String" res = [] # make line strip points = [] colors = [] t_first = tsdata_lst[0][0] prev_t = t_first.transform.translation for ts, _ in tsdata_lst: t = ts.transform.translation dist = distanceOfVector3(prev_t, t) * 0.65 rgb = colorsys.hsv_to_rgb(dist, 1.0, 1.0) points.append(Point(x=t.x, y=t.y, z=t.z)) colors.append(ColorRGBA(rgb[0],rgb[1],rgb[2],1.0)) prev_t = t h = Header() h.stamp = rospy.Time(0) #rospy.Time.now() h.frame_id = "eng2" #t_first.child_frame_id if discrete: m_type = Marker.POINTS else: m_type = Marker.LINE_STRIP m = Marker(type=m_type, action=Marker.ADD, header=h, id=cls.marker_id) cls.marker_id += 1 m.scale.x = 0.1 m.scale.y = 0.1 m.points = points m.colors = colors m.ns = "labeled_line" m.lifetime = rospy.Time(3000) res.append(m) for t, t_meta in tsdata_lst[::label_downsample]: text = Marker(type=Marker.TEXT_VIEW_FACING, action=Marker.ADD, header=h, id=cls.marker_id) cls.marker_id += 1 text.scale.z = 0.1 text.pose = T.poseFromTransform(t.transform) text.pose.position.z += zoffset text.color = ColorRGBA(0.0,0.0,1.0,1.0) text.text = t_meta["inserted_at"].strftime(fmt) text.ns = "labeled_line_text" text.lifetime = rospy.Time(3000) res.append(text) return res
def publishState(self): self.pubState.publish(str(self.state)) marker = Marker() marker.header.frame_id = "/map" marker.header.stamp = rospy.get_rostime() marker.type = marker.TEXT_VIEW_FACING marker.action = marker.ADD marker.scale.x = 1.0 marker.scale.y = 0.2 marker.scale.z = 0.5 marker.color.a = 1.0 marker.color.r = 1.0 marker.color.g = 0.0 marker.color.b = 0.0 marker.pose.orientation.w = 1.0 marker.pose.position.x = self.infoLoc["x"] marker.pose.position.y = self.infoLoc["y"] marker.pose.position.z = 0.0 marker.id = 0 if self.state in [State.RUNNING]: if not self.sendStart: marker.text = "Start" self.sendStart = True self.timeSend = rospy.get_rostime() elif (rospy.get_rostime() - self.timeSend).to_sec() > 0.2: # marker.type = marker.SPHERE marker.action = marker.DELETE else: marker.text = "Start" if self.state == State.FLEEING: timeFleeing = TIME_FLEEING - (rospy.get_rostime() - self.timeStartFleeing).to_sec() if timeFleeing > 0: marker.text = str(timeFleeing) else: marker.action = marker.DELETE self.state = State.RUNNING if self.state in [State.GAME_OVER, State.STOPPED, State.SETUP]: marker.text = "GAME OVER!" if self.state in [State.INIT]: marker.text = "Initializing Game" if self.state in [State.PAUSED]: marker.text = "Game Paused" if self.state in [State.WON]: marker.text = "Congratulations, you have won the Game! Now get back to you real job!" self.pubInfo.publish(marker)
def create_status_marker(self, text): marker = Marker() marker.header.frame_id = 'base_footprint' marker.header.stamp = rospy.Time.now() marker.pose.position.z += 1 marker.pose.orientation.w = 1 marker.type = Marker.TEXT_VIEW_FACING marker.text = text marker.frame_locked = True marker.ns = 'status' marker.scale.z = 0.3 marker.color.r = 0.8 marker.color.g = 0.5 marker.color.b = 0.1 marker.color.a = 1.0 return marker
def create_marker(self, marker_id, all_tags): marker = Marker() marker.header.stamp = rospy.Time.now() marker.header.frame_id = "/map" marker.type = marker.TEXT_VIEW_FACING marker.id = marker_id marker.action = marker.ADD marker.ns = 'tags' #marker.lifetime = 0 marker.scale.x = 0.1 marker.scale.y = 0.1 marker.scale.z = 0.1 marker.color.a = 1 marker.pose = self.pp.pose tag_str = [all_tags[i] for i in self.pp.tags] marker.text = '\n'.join(tag_str) return marker
def init_markers(self): # Set up our waypoint markers marker_scale = 0.6 marker_lifetime = 0 # 0 is forever marker_ns = 'waypoints' marker_id = 0 marker_color = {'r': 1.0, 'g': 0.0, 'b': 0.0, 'a': 1.0} # Define a marker publisher list. location_list = list() location_list = self.locations.keys() self.marker_pub_list = list() for i in location_list: self.marker_pub_list.append(rospy.Publisher('waypoint_' + i, Marker, queue_size=5)) # Define a marker list. self.marker_list = list() for location in self.locations: marker = Marker() marker.ns = marker_ns marker.id = marker_id marker.type = Marker.TEXT_VIEW_FACING marker.action = Marker.ADD marker.lifetime = rospy.Duration(marker_lifetime) marker.scale.x = marker_scale marker.scale.y = marker_scale marker.scale.z = marker_scale marker.color.r = marker_color['r'] marker.color.g = marker_color['g'] marker.color.b = marker_color['b'] marker.color.a = marker_color['a'] marker.header.frame_id = 'map' marker.header.stamp = rospy.Time.now() # rospy.loginfo(self.locations['foyer']) marker.pose = copy.deepcopy(self.locations[location]) marker.pose.position.z = 0.65 marker.text = location # rospy.loginfo(self.locations['foyer']) self.marker_list.append(marker)
def offset_for_target(self): # Move off of center of shape to be centered with large target move = self.navigator.move.forward(self.target_offset_meters) marker = Marker() marker.scale.x = 1; marker.scale.y = .1; marker.scale.z = .1; marker.action = Marker.ADD; marker.header.frame_id = "enu" marker.header.stamp = self.navigator.nh.get_time() marker.pose = self.navigator.move.goal.goal marker.type = Marker.ARROW marker.text = "Offset goal" marker.ns = "detect_deliver" marker.id = 3 marker.color.b = 1 marker.color.a = 1 self.markers.markers.append(marker) yield move
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 place_marker_pose(posestamped, pub, _id=0, _type=Marker.CUBE, ns='basic_shapes',\ r=0, g=1, b=0, a=1, xscale=.03, yscale=.03, zscale=.03,\ orientation=Quaternion(0,0,0,1), text=''): marker = Marker(type=_type, action=Marker.ADD) marker.ns = ns marker.header.frame_id = posestamped.header.frame_id marker.header.stamp = rospy.Time.now() marker.pose = posestamped.pose marker.scale.x = xscale marker.scale.y = yscale marker.scale.z = zscale marker.color.r = r marker.color.g = g marker.color.b = b marker.color.a = a marker.id = _id marker.text = text marker.lifetime = rospy.Duration() pub.publish(marker)
def _visualize_location(self, base_pose, location): """ Visualize a marker on the base_pose with the text 'location' and rotated to the correct side. :param base_pose: kdl Frame (in map) where the waypoint is located :param location: The name of the location as a label :return: """ # Convert KDL object to geometry message base_pose = kdl_frame_to_pose_msg(base_pose) # Create the marker m = Marker() if location == "one": m.id = 1 m.color.r = 1 elif location == "two": m.id = 2 m.color.g = 1 elif location == "three": m.id = 3 m.color.b = 1 else: m.id = 4 m.color.r = 1 m.color.g = 1 m.color.b = 1 m.color.a = 1 m.pose = base_pose m.header.frame_id = "map" m.header.stamp = rospy.Time.now() m.type = 0 # Arrow m.scale.x = 1.0 m.scale.y = 0.2 m.scale.z = 0.2 m.action = 0 m.ns = "arrow" self._waypoint_pub.publish(m) m.type = 9 m.text = location m.ns = "text" m.pose.position.z = 0.5 self._waypoint_pub.publish(m)