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 create_marker(self, message): # type: (Location) -> (Marker, Marker) r = random.random() g = random.random() b = random.random() scale = Vector3(0.5, 0.5, 0.5) frame_id = "map" ns = "location_markers" marker_str = Marker() marker_str.header.stamp = rospy.Time.now() marker_str.header.frame_id = frame_id marker_str.ns = ns marker_str.id = self.marker_id marker_str.type = Marker.TEXT_VIEW_FACING marker_str.text = message.name marker_str.action = Marker.ADD marker_str.pose.position = Point(message.pose.position.x, message.pose.position.y, 0.5) marker_str.scale = scale marker_str.color = ColorRGBA(r, g, b, 1) self.marker_id += 1 marker_sphere = Marker() marker_sphere.header.stamp = rospy.Time.now() marker_sphere.header.frame_id = frame_id marker_sphere.ns = ns marker_sphere.id = self.marker_id marker_sphere.type = Marker.SPHERE marker_sphere.action = Marker.ADD marker_sphere.pose.position = Point(message.pose.position.x, message.pose.position.y, 0) marker_sphere.scale = scale marker_sphere.color = ColorRGBA(r, g, b, 0.5) self.marker_id += 1 return [marker_sphere, marker_str]
def world_body_to_marker_msg(world_body, id=1, ns=u''): """ :type world_body: WorldBody :rtype: Marker """ m = Marker() m.ns = u'{}/{}'.format(ns, world_body.name) m.id = id if world_body.type == WorldBody.URDF_BODY: raise Exception( u'can\'t convert urdf body world object to marker array') elif world_body.type == WorldBody.PRIMITIVE_BODY: if world_body.shape.type == SolidPrimitive.BOX: m.type = Marker.CUBE m.scale = Vector3(*world_body.shape.dimensions) elif world_body.shape.type == SolidPrimitive.SPHERE: m.type = Marker.SPHERE m.scale = Vector3(world_body.shape.dimensions[0], world_body.shape.dimensions[0], world_body.shape.dimensions[0]) elif world_body.shape.type == SolidPrimitive.CYLINDER: m.type = Marker.CYLINDER m.scale = Vector3( world_body.shape.dimensions[SolidPrimitive.CYLINDER_RADIUS], world_body.shape.dimensions[SolidPrimitive.CYLINDER_RADIUS], world_body.shape.dimensions[SolidPrimitive.CYLINDER_HEIGHT]) else: raise Exception( u'world body type {} can\'t be converted to marker'.format( world_body.shape.type)) elif world_body.type == WorldBody.MESH_BODY: m.type = Marker.MESH_RESOURCE m.mesh_resource = world_body.mesh m.color = ColorRGBA(0, 1, 0, 0.8) return m
def make_individual_markers(self, msg): lpost = Marker() lpost.type = Marker.CYLINDER lpost.scale = Vector3(POST_DIAMETER, POST_DIAMETER, GOAL_HEIGHT) lpost.color.r = 1.0 lpost.color.g = 1.0 lpost.color.b = 1.0 lpost.color.a = 1.0 lpost.pose.position = Point(0, GOAL_WIDTH / 2, GOAL_HEIGHT / 2) rpost = Marker() rpost.type = Marker.CYLINDER rpost.scale = Vector3(POST_DIAMETER, POST_DIAMETER, GOAL_HEIGHT) rpost.color.r = 1.0 rpost.color.g = 1.0 rpost.color.b = 1.0 rpost.color.a = 1.0 rpost.pose.position = Point(0, -GOAL_WIDTH / 2, GOAL_HEIGHT / 2) bar = Marker() bar.type = Marker.CYLINDER bar.scale = Vector3(POST_DIAMETER, POST_DIAMETER, GOAL_WIDTH) bar.color.r = 1.0 bar.color.g = 1.0 bar.color.b = 1.0 bar.color.a = 1.0 bar.pose.position = Point(0, 0, GOAL_HEIGHT) bar.pose.orientation = Quaternion( math.sqrt(2) / 2, 0, 0, math.sqrt(2) / 2) return (lpost, rpost, bar)
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 visualize_pose(self,pos,orientation): # Yellow Box for Vehicle marker = Marker() marker.header.stamp = rospy.Time.now() marker.header.frame_id = 'map' marker.ns = 'agent' marker.id = 0 marker.type = marker.CUBE marker.action = marker.ADD marker.pose.position = pos marker.pose.orientation = orientation marker.scale = Vector3(x=0.7,y=0.42,z=1) marker.color = ColorRGBA(r=1.0,g=1.0,a=1.0) marker.lifetime = rospy.Duration(1.0) self.pub_pose_marker.publish(marker) # Red track for trajectory over time marker = Marker() marker.header.stamp = rospy.Time.now() marker.header.frame_id = 'map' marker.ns = 'agent' marker.id = self.num_poses marker.type = marker.CUBE marker.action = marker.ADD marker.pose.position = pos marker.pose.orientation = orientation marker.scale = Vector3(x=0.2,y=0.2,z=0.2) marker.color = ColorRGBA(r=1.0,a=1.0) marker.lifetime = rospy.Duration(10.0) self.pub_pose_marker.publish(marker)
def visualize_subgoal(self,subgoal, subgoal_options=None): markers = MarkerArray() # Display GREEN DOT at NN subgoal marker = Marker() marker.header.stamp = rospy.Time.now() marker.header.frame_id = 'map' marker.ns = 'subgoal' marker.id = 0 marker.type = marker.CYLINDER marker.action = marker.ADD marker.pose.position.x = subgoal[0] marker.pose.position.y = subgoal[1] marker.scale = Vector3(x=0.2,y=0.2,z=0) marker.color = ColorRGBA(r=0.0,g=0.0,b=0.0,a=1.0) marker.lifetime = rospy.Duration(2.0) self.pub_goal_path_marker.publish(marker) if subgoal_options is not None: for i in xrange(len(subgoal_options)): marker = Marker() marker.header.stamp = rospy.Time.now() marker.header.frame_id = 'map' marker.ns = 'subgoal' marker.id = i+1 # marker.type = marker.CUBE marker.type = marker.CYLINDER marker.action = marker.ADD marker.pose.position.x = subgoal_options[i][0] marker.pose.position.y = subgoal_options[i][1] marker.scale = Vector3(x=0.2,y=0.2,z=0.2) marker.color = ColorRGBA(r=0.0,g=0.0,b=255,a=1.0) marker.lifetime = rospy.Duration(1.0) self.pub_goal_path_marker.publish(marker)
def visualize_action(self, use_d_min): # Display BLUE ARROW from current position to NN desired position marker = Marker() marker.header.stamp = rospy.Time.now() marker.header.frame_id = 'map' marker.ns = 'path_arrow' marker.id = 0 marker.type = marker.ARROW marker.action = marker.ADD marker.points.append(self.pose.pose.position) marker.points.append(self.desired_position.pose.position) marker.scale = Vector3(x=0.1,y=0.2,z=0.2) marker.color = ColorRGBA(b=1.0,a=1.0) marker.lifetime = rospy.Duration(0.1) self.pub_goal_path_marker.publish(marker) # Display BLUE DOT at NN desired position marker = Marker() marker.header.stamp = rospy.Time.now() marker.header.frame_id = 'map' marker.ns = 'path_trail' marker.id = self.num_poses marker.type = marker.CUBE marker.action = marker.ADD marker.pose.position = copy.deepcopy(self.desired_position.pose.position) marker.scale = Vector3(x=0.2,y=0.2,z=0.4) marker.color = ColorRGBA(b=1.0,a=0.1) marker.lifetime = rospy.Duration(100) if self.desired_action[0] == 0.0: marker.pose.position.x += 2.0*np.cos(self.desired_action[1]) marker.pose.position.y += 2.0*np.sin(self.desired_action[1]) self.pub_goal_path_marker.publish(marker)
def get_marker( marker_type=None, frame_id="world", scale=1.0, color="#ffffffff", position=(0.0, 0.0, 0.0), orientation=(0.0, 0.0, 0.0, 1.0), ): """ Create a Marker visualization message. Parameters ---------- marker_type : int, default Marker.LINE_STRIP Type of the marker. frame_id : str, default "world" Name of the reference frame of the marker. scale : float or iterable of float, len 3, default 1.0 Scale of the marker. color : str, default "#ffffffff" Color of the marker. position : iterable, len 3, default (0.0, 0.0, 0.0) Position of the marker wrt its reference frame. orientation : iterable, len 4, default (0.0, 0.0, 0.0, 1.0) Orientation of the marker wrt its reference frame. Returns ------- marker: Marker Marker message. """ from geometry_msgs.msg import Point, Quaternion, Vector3 from visualization_msgs.msg import Marker marker = Marker() marker.type = marker_type or Marker.LINE_STRIP marker.header.frame_id = frame_id if isinstance(scale, float): marker.scale = Vector3(scale, scale, scale) else: marker.scale = Vector3(*scale) marker.color = hex_to_rgba(color) marker.pose.orientation = Quaternion(*orientation) marker.pose.position = Point(*position) # TOD0: make configurable via vector marker.points = [] return marker
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 callback_skeleton(self, data): mk = Marker() mk.header.frame_id = '/map' mk.ns = 'skeletro' mk.id = self.m_id mk.type = Marker.SPHERE_LIST mk.points = [] for j in data.joints: if self.point_is_valid(j): mk.points.append(j) mk.scale = geometry_msgs.msg.Vector3(3, 3, 3) mk.pose.position = geometry_msgs.msg.Point(0, 0, 0) mk.pose.orientation = geometry_msgs.msg.Quaternion(0.0, 0.0, 0.0, 1.0) # mk.lifetime = genpy.Duration(0.1) mk.color = std_msgs.msg.ColorRGBA(1.0, 0.0, 0.0, 1.0) linek = Marker() linek.header.frame_id = '/map' linek.ns = 'lines' linek.id = self.m_id linek.type = Marker.LINE_LIST linek.scale = geometry_msgs.msg.Vector3(0.3, 0.3, 0.3) linek.pose.position = geometry_msgs.msg.Point(0, 0, 0) linek.pose.orientation = geometry_msgs.msg.Quaternion( 0.0, 0.0, 0.0, 1.0) linek.points = [] self.vis_add_line(linek, data.joints, 0, 1) self.vis_add_line(linek, data.joints, 1, 2) self.vis_add_line(linek, data.joints, 3, 4) self.vis_add_line(linek, data.joints, 4, 5) self.vis_add_line(linek, data.joints, 6, 7) self.vis_add_line(linek, data.joints, 7, 8) self.vis_add_line(linek, data.joints, 9, 10) self.vis_add_line(linek, data.joints, 10, 11) self.vis_add_line(linek, data.joints, 12, 13) self.vis_add_line(linek, data.joints, 13, 14) # linek.lifetime = genpy.Duration(0.1) linek.color = std_msgs.msg.ColorRGBA(0.0, 1.0, 0.0, 1.0) msg = MarkerArray() msg.markers = [] msg.markers.append(mk) msg.markers.append(linek) self.visualization_pub.publish(msg)
def publish(self, particles, best_particle=None, weights=None): """Publish particles pose / visualization Args: particles: [N,3] numpy array formatted (x,y,h) best_particle : [3] numpy array formatted (x,y,h) Returns: None """ self.part_msg_.header.frame_id = 'map' self.part_msg_.header.seq += 1 self.part_msg_.header.stamp = rospy.Time.now() if best_particle is not None: self.best_msg_.header.frame_id = 'map' self.best_msg_.header.seq += 1 self.best_msg_.header.stamp = rospy.Time.now() if weights is not None: weights = (0.1 / weights.max()) * weights # populate poses #print 'length of particle : ', len(particles) self.part_msg_.poses = [self.particle_to_pose(p) for p in particles] self.part_pub_.publish(self.part_msg_) # poses v2 self.mark_msg_.markers = [] n = len(particles) for i in range(n): mk = Marker(header=Header(stamp=rospy.Time.now(), frame_id='map')) mk.ns = 'pcloud' mk.id = i mk.type = mk.ARROW mk.action = mk.ADD mk.pose = self.part_msg_.poses[i] if weights is not None: s = weights[i] mk.scale = Vector3(s * 2, s / 3., s / 3.) mk.color = ColorRGBA(1.0, 0.0, 1.0, s) else: mk.scale = Vector3(0.1, 0.02, 0.02) mk.color = ColorRGBA(1.0, 0.0, 1.0, 1.0) mk.lifetime = rospy.Duration(1.0) # stay alive for 1 sec self.mark_msg_.markers.append(mk) self.mark_pub_.publish(self.mark_msg_) if best_particle is not None: self.best_msg_.pose = self.particle_to_pose(best_particle) self.best_pub_.publish(self.best_msg_)
def callbackGeneratedActions(self,actions): zero_orientation = Quaternion(0, 0, 0, 1) zero_position = Point(0, 0, 0) zero_pose = Pose(zero_position, zero_orientation) counter = 0 marker_array = MarkerArray() for pose in actions.actions: marker = Marker() marker.header.stamp = rospy.get_rostime() marker.header.frame_id = "world" marker.ns = "model_visualizer_generated_actions" marker.id = counter counter = counter + 1 marker.action = Marker.ADD marker.scale = Vector3(0.02,0.02,0.02) marker.color.a = 1 marker.color.b = random.random() marker.color.r = random.random() marker.color.g = random.random() marker.type = Marker.ARROW marker.pose = zero_pose marker.points.append( Point(0,0,0) ) marker.points.append( Point(pose.position.x, pose.position.y, pose.position.z) ) marker_array.markers.append(marker) self.pub_array.publish(marker_array)
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 publish_goal(self): # Publish the reference topic msg = PoseStamped() msg.header.frame_id = self._arm_interface.base_link msg.header.stamp = rospy.Time.now() msg.pose.position.x = self._last_goal.p.x() msg.pose.position.y = self._last_goal.p.y() msg.pose.position.z = self._last_goal.p.z() msg.pose.orientation = Quaternion(*self._last_goal.M.GetQuaternion()) self._goal_pub.publish(msg) marker = Marker() marker.header.frame_id = self._arm_interface.base_link marker.header.stamp = rospy.Time.now() marker.ns = self._arm_interface.arm_name marker.id = 0 marker.type = Marker.SPHERE marker.action = Marker.MODIFY marker.pose.position = Vector3(self._last_goal.p.x(), self._last_goal.p.y(), self._last_goal.p.z()) marker.pose.orientation = Quaternion(*self._last_goal.M.GetQuaternion()) marker.scale = Vector3(0.2, 0.2, 0.2) marker.color.a = 1.0 marker.color.r = 1.0 marker.color.g = 0.0 marker.color.b = 0.0 self._goal_marker_pub.publish(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 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 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 perform(self, reevaluate=False): pose_msg = self.blackboard.pathfinding.get_ball_goal( self.target, self.distance) self.blackboard.pathfinding.publish(pose_msg) approach_marker = Marker() approach_marker.pose.position.x = self.distance approach_marker.type = Marker.SPHERE approach_marker.action = Marker.MODIFY approach_marker.id = 1 color = ColorRGBA() color.r = 1.0 color.g = 1.0 color.b = 1.0 color.a = 1.0 approach_marker.color = color approach_marker.lifetime = rospy.Duration(nsecs=0.5) scale = Vector3(0.2, 0.2, 0.2) approach_marker.scale = scale approach_marker.header.stamp = rospy.Time.now() approach_marker.header.frame_id = self.blackboard.world_model.base_footprint_frame self.blackboard.pathfinding.approach_marker_pub.publish( approach_marker) if self.blackboard.pathfinding.status in [ GoalStatus.SUCCEEDED, GoalStatus.ABORTED ] or not self.blocking: self.pop()
def output_to_rviz(array, scale, color): print 'Inside output_to_rviz()' global publisher global rviz_id # make MarkerArray message markerArray = MarkerArray() # loop throgh all instances of the array for index in range(len(array)): marker = Marker() marker.id = rviz_id marker.header.frame_id = "/map" marker.type = marker.SPHERE marker.action = marker.ADD marker.lifetime = rospy.Duration(120.0) marker.scale = scale marker.color = color # x and y are inverted due to nature of the map marker.pose.position.x = array[index][1] marker.pose.position.y = array[index][0] markerArray.markers.append(marker) # incremment rviz_id rviz_id = rviz_id + 1 # Publish the MarkerArray publisher.publish(markerArray)
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 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 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_, rot=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 make_sphere_marker(self, xy, radius, color, namespace): # type: ([int,int], float, ColorRGBA, str) -> Marker """ 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 """ marker = Marker() marker.header.stamp = rospy.Time.now() marker.header.frame_id = 'map' marker.ns = namespace marker.id = self.mark_id self.mark_id += 1 marker.type = 2 marker.action = 0 marker.pose = geometry_msgs.msg.Pose() marker.pose.orientation.w = 1 marker.pose.position.x = xy[0] marker.pose.position.y = xy[1] 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 publish_goal(self): # Publish the reference topic msg = PoseStamped() msg.header.frame_id = self._arm_interface.base_link msg.header.stamp = rospy.Time.now() msg.pose.position.x = self._last_goal.p.x() msg.pose.position.y = self._last_goal.p.y() msg.pose.position.z = self._last_goal.p.z() msg.pose.orientation = Quaternion(*self._last_goal.M.GetQuaternion()) self._goal_pub.publish(msg) marker = Marker() marker.header.frame_id = self._arm_interface.base_link marker.header.stamp = rospy.Time.now() marker.ns = self._arm_interface.arm_name marker.id = 0 marker.type = Marker.SPHERE marker.action = Marker.MODIFY marker.pose.position = Vector3(self._last_goal.p.x(), self._last_goal.p.y(), self._last_goal.p.z()) marker.pose.orientation = Quaternion( *self._last_goal.M.GetQuaternion()) marker.scale = Vector3(0.2, 0.2, 0.2) marker.color.a = 1.0 marker.color.r = 1.0 marker.color.g = 0.0 marker.color.b = 0.0 self._goal_marker_pub.publish(marker)
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 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 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 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 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 pub_marker(self, imu, delta): ''' Publishes a new marker based on the current state. Args: imu [Imu]: The ROS message 'sensor_msgs.msg.Imu'. delta [float]: Time delta between the timestamps of the messages. ''' marker = Marker() marker.id = imu.header.seq marker.header.frame_id = self.frame_id marker.header.stamp = imu.header.stamp marker.header.seq = imu.header.seq marker.type = Marker.LINE_STRIP marker.action = marker.ADD marker.color = self.color marker.scale = self.scale marker.pose.position = Point(*self.s) marker.pose.orientation.w = 1.0 marker.points = [Point(*(-self.v * delta)), Point(0.0, 0.0, 0.0)] #rospy.loginfo("{} Marker:\n{}".format(rospy.get_caller_id(), marker)) self.pub.publish(marker)
def output_rviz_message(self): markers = MarkerArray() spheres = Marker() lines = Marker() spheres.id = 1 spheres.header.frame_id = self.frame_id spheres.type = Marker.SPHERE_LIST spheres.action = 0 spheres.scale = Vector3(0.005, 0.005, 0.005) spheres.color.b = 1.0 spheres.color.a = 1.0 for node in self.graph.nodes: spheres.points.append(Point(*node)) lines.id = 2 lines.header.frame_id = self.frame_id lines.type = Marker.LINE_LIST lines.action = 0 lines.scale.x = 0.0025 lines.color.g = 1.0 lines.color.a = 1.0 for node_1, node_2 in self.graph.edges: lines.points.extend([Point(*node_1), Point(*node_2)]) markers.markers = [spheres, lines] return markers
def make_marker(marker_type, scale, r, g, b, a): # make a visualization marker array for the occupancy grid m = Marker() m.action = Marker.ADD m.header.frame_id = '/narko_base_link' m.header.stamp = rospy.Time.now() m.ns = 'marker_test_%d' % marker_type m.id = 0 m.type = marker_type m.pose.position.x = 0 m.pose.position.y = 0 m.pose.position.z = 0 m.pose.orientation.w = 0 m.pose.orientation.x = 0 m.pose.orientation.y = 0 m.pose.orientation.z = 1 m.scale = scale m.color.r = 0 m.color.g = 1 m.color.b = 0 m.color.a = 0.9 m.color.r = r m.color.g = g m.color.b = b m.color.a = a return 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 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 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 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 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 publish_marker(self, marker_pose, colour, ID): """ Publishes a maker at the given maker pose, colors and ID :@param marker_pose: the position to place the marker :@type marker_pose: Pose() :@param colour: Colour of marker. red, green, blue or yellow :@type colour: String. (e.g. "yellow") :@param ID: The ID of the ID of the marker. :@type ID: int """ marker = Marker() marker.header.seq = len(self.marker_array.markers) + 1 marker.id = ID marker.header.frame_id = "odom" marker.header.stamp = rospy.Time.now() marker.pose = marker_pose marker.scale = Vector3(0.2, 0.2, 0.2) marker.color.a = 1.0 # must be non-zero if colour == "red": marker.color.r = 1.0 elif colour == "blue": marker.color.b = 1.0 elif colour == "green": marker.color.g = 1.0 elif colour == "yellow": marker.color.r = 1.0 marker.color.g = 1.0 self.marker_array.markers.append(marker) self.publish_beacon_pos.publish(self.marker_array)
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 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 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 plane(self, frame_id, p, n, r=1.0, key=None, draw_triad=False): """ plane: n . ( X - p ) = 0 This will be drawn as a disc of radius r about p. """ n = np.array(n) p = np.array(p) m = Marker() m.header.frame_id = frame_id m.ns = key or 'planes' m.id = 0 if key else len(self.planes) m.type = Marker.TRIANGLE_LIST m.action = Marker.MODIFY # Compute plane rotation nz = n / np.linalg.norm(n) nx = np.cross(nz, np.array([0,0,1])); nx = nx / np.linalg.norm(nx) ny = np.cross(nz, nx); ny = ny / np.linalg.norm(ny) R = kdl.Rotation( kdl.Vector(*nx), kdl.Vector(*ny), kdl.Vector(*nz)) # define the pose of the plane marker m.pose = Pose(Point(*(p)), Quaternion(*R.GetQuaternion())) m.scale = Vector3(1, 1, 1) # flatten the triangle list for triangle in self.mesh_t: m.points.extend([ Point(r*self.mesh_p[t][0],r*self.mesh_p[t][1],0) for t in triangle]) m.color = ColorRGBA(0.8,0,0.8,0.9) m.colors = [m.color] * len(m.points) if draw_triad: self.line(frame_id, p, nx, t=[0.0,r],key=key+'-x') self.line(frame_id, p, ny, t=[0.0,r],key=key+'-y') self.line(frame_id, p, nz, t=[0.0,r],key=key+'-z') self.point(frame_id, p, 0.005, key=key+'-center-point') key = key or element_key(m) with self.mod_lock: self.planes[key] = m return key
def ScaleMarker(marker_template, control_scale=None, visual_scale=None): """Scale InteractiveMarker and/or a visual Marker associated with the InteractiveMarker. @type marker_template: subclass of MarkerTemplate() @param marker_template: The template object containing InteractiveMarkers. @type control_scale: float @param control_scale: The scale factor for the InteractiveMarker. @type visual_scale: geometry_msgs/Vector3 @param visual_scale: The scale factor for the visualization Marker in the template. """ server = marker_template.server menu_handler = marker_template.menu_handler marker_name = marker_template.key if server: current_marker = server.get(marker_name) if current_marker: # rescale marker marker = Marker() marker = GetVisualMarker(current_marker) if visual_scale is not None: marker.scale = visual_scale # push marker into visual control visual = InteractiveMarkerControl() visual.name = "visual" visual.always_visible = GetVisualControl(current_marker).always_visible visual.interaction_mode = GetVisualControl(current_marker).interaction_mode visual.orientation = GetVisualControl(current_marker).orientation visual.markers.append(marker) new_marker = InteractiveMarker() new_marker.header.frame_id = current_marker.header.frame_id new_marker.name = current_marker.name new_marker.description = current_marker.description new_marker.pose = current_marker.pose new_marker.scale = current_marker.scale if control_scale is not None: new_marker.scale = control_scale new_marker.controls.append(visual) for control in current_marker.controls: if 'Translate' in control.name or 'Rotate' in control.name: # todo rename Plane Translate so we don't need to do this extra check if control.name not in ['TranslateXY', 'TranslateYZ','TranslateXZ']: new_marker.controls.append(CreateTransRotControl(control.name)) # insert the updated marker into the server server.insert(new_marker) menu_handler.apply(server, marker_name)
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 create_arrow_marker(pose, m_id, color=ColorRGBA(1., 0., 0., 1.)): m = Marker() m.header.frame_id = "/base_link" m.header.stamp = rospy.Time.now() m.ns = "ell_pose_viz" m.id = m_id m.type = Marker.ARROW m.action = Marker.ADD m.scale = Vector3(0.19, 0.09, 0.02) m.color = color m.pose = PoseConv.to_pose_msg(pose) return m
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 make_marker(frame_id, id, pose, scale, color, frame_locked): msg = Marker() msg.header.frame_id = frame_id msg.header.stamp = rospy.Time.now() msg.ns = "pouring_visualization" msg.id = id msg.action = Marker.ADD msg.pose = pose msg.scale = scale msg.color = color msg.frame_locked = frame_locked return msg
def publish_vector(self, frame, loc, v, m_id): m = Marker() m.header.frame_id = frame m.header.stamp = rospy.Time.now() m.ns = "netft_zeroing" m.id = m_id m.type = Marker.ARROW m.action = Marker.ADD m.points.append(Point(*loc)) m.points.append(Point(*(loc + v))) m.scale = Vector3(0.01, 0.02, 0.01) m.color = self.colors[m_id] self.vis_pub.publish(m)
def create(**args): m = Marker(**args) # make shure the marker is displayable if not args.has_key('header'): m.header.stamp = rospy.Time.now() m.header.frame_id = '/base_link' if not args.has_key('color'): m.color = ColorRGBA(0.3, 0.3, 0.3, 1) if not args.has_key('orientation'): m.pose.orientation = Quaternion(0, 0, 0, 1) if not args.has_key('scale'): m.scale = Vector3(1, 1, 1) return m
def publish_vector(self, loc, v, m_id): m = Marker() m.header.frame_id = "torso_lift_link" m.header.stamp = rospy.Time() m.ns = "force_torque" m.id = m_id m.type = Marker.ARROW m.action = Marker.ADD m.points.append(Point(*loc)) m.points.append(Point(*(loc + v))) m.scale = Vector3(0.01, 0.02, 0.01) m.color = self.colors[m_id] self.vis_pub.publish(m)
def create_ear_marker(self, pose, m_id, color=ColorRGBA(0., 1., 1., 1.)): m = Marker() #m.header.frame_id = "/base_link" m.header.frame_id = "/ellipse_frame" m.header.stamp = rospy.Time.now() m.ns = "head_markers" m.id = m_id m.type = Marker.CYLINDER m.action = Marker.ADD m.scale = ear_scale m.color = color m.pose = PoseConverter.to_pose_msg(pose) return m
def publish_markers( self ): for i,g in enumerate(self.poses): m = Marker() m.ns = 'sample_positions' m.id = i m.action = m.ADD m.type = m.ARROW m.header.frame_id = self.frame m.header.stamp = rospy.Time.now() m.scale = Vector3( 0.15, 1.0, 1.0 ) m.color = ColorRGBA( 0.2, 1.0, 0.2, 0.7 ) m.pose = g.target_pose.pose self.pub.publish( m )
def create_arrow_marker(pose, m_id, color=ColorRGBA(1.0, 0.0, 0.0, 1.0)): m = Marker() if USE_ELLIPSE_FRAME: m.header.frame_id = "/ellipse_frame" else: m.header.frame_id = "/base_link" m.header.stamp = rospy.Time.now() m.ns = "ell_pose" m.id = m_id m.type = Marker.ARROW m.action = Marker.ADD m.scale = Vector3(0.19, 0.09, 0.02) m.color = color m.pose = PoseConverter.to_pose_msg(pose) return m
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 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 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