Python Marker.scale方法代码示例

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import scale [as 别名]
    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)

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import scale [as 别名]
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

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import scale [as 别名]
 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)

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import scale [as 别名]
 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)

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import scale [as 别名]
    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

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import scale [as 别名]
  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)     

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import scale [as 别名]
    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

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import scale [as 别名]
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    

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import scale [as 别名]
    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)

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import scale [as 别名]
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

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import scale [as 别名]
	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

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import scale [as 别名]
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

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import scale [as 别名]
 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()

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import scale [as 别名]
 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

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import scale [as 别名]
 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