Python Marker.pose方法代码示例

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

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

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import pose [as 别名]
    def publish(self, grasps, obj=None):
        msg = MarkerArray()

        self.marker_id = 0  # reset marker counter
        if obj and len(obj.primitives) > 0 and len(obj.primitive_poses) > 0:
            m = Marker()
            m.header = obj.header
            m.ns = "object"
            m.id = self.marker_id
            self.marker_id += 1
            m.type = m.CUBE
            m.action = m.ADD
            m.pose = obj.primitive_poses[0]
            m.scale.x = obj.primitives[0].dimensions[0]
            m.scale.y = obj.primitives[0].dimensions[1]
            m.scale.z = obj.primitives[0].dimensions[2]
            m.color.r = 0
            m.color.g = 0
            m.color.b = 1
            m.color.a = 0.8
            msg.markers.append(m)

        for grasp in grasps:
            msg.markers.append(self.get_gripper_marker(grasp.grasp_pose, grasp.grasp_quality))

        self.pub.publish(msg)

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import pose [as 别名]
 def publish_prob(self, waypoints, objects, probs):
     prob_msg = MarkerArray() 
     i = 0
     n_waypoints = len(waypoints)
     n_objects = len(objects)        
     scaling_factor = max(probs)      
     for node in self._topo_map:
         if node.name in waypoints:
             marker = Marker()
             marker.header.frame_id = 'map'
             marker.id = i
             marker.type = Marker.CYLINDER
             marker.action = Marker.ADD
             marker.pose = node.pose
             prob = 1
             for j in range(0, n_objects):
                 prob = prob*probs[n_objects*i + j]
             prob = prob/(scaling_factor**2)
             print prob
             marker.scale.x = 1*prob
             marker.scale.y = 1*prob                
             marker.scale.z = 1
             marker.color.a = 1.0
             marker.color.r = 0.0
             marker.color.g = 1.0
             marker.color.b = 0.0
             prob_msg.markers.append(marker)
             i = i + 1
     self._prob_marker_pub.publish(prob_msg)

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import pose [as 别名]
    def pose_cb(self, pose):
        pose = self.listener.transformPose(self.reference_frame,pose)
        q = pose.pose.orientation
        qvec = [q.x,q.y,q.z,q.w]
        euler = euler_from_quaternion(qvec)
        self.goal_x = pose.pose.position.x
        self.goal_y = pose.pose.position.y
        self.goal_theta = euler[2]
        (ex,ey,etheta) = self.compute_error()
        if ex < -self.dist_threshold:
            self.goal_theta = norm_angle(etheta + pi)
        print "New goal: %.2f %.2f %.2f" % (self.goal_x, self.goal_y, self.goal_theta)

        marker = Marker()
        marker.header = pose.header
        marker.ns = "goal_marker"
        marker.id = 10
        marker.type = Marker.ARROW
        marker.action = Marker.ADD
        marker.pose = pose.pose
        marker.scale.x = 0.5
        marker.scale.y = 0.5
        marker.scale.z = 1.0;
        marker.color.a = 1.0;
        marker.color.r = 1.0;
        marker.color.g = 1.0;
        marker.color.b = 0.0;
        marker.lifetime.secs=-1.0
        self.marker_pub.publish(marker)

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import pose [as 别名]
    def draw_bounding_box(self, id, box_msg, color=(1.0, 1.0, 0.0, 0.0), duration=0.0):
        """
        Draws a bounding box as detectd by detect_bounding_box.
        
        Parameters: 
        box_msg is a FindClusterBoundingBoxResponse msg.
        color: a quadruple with alpha, r,g,b
        duration: how long should the bounding box last. 0 means forever.
        """

        marker = Marker()
        marker.header.stamp = rospy.Time.now()
        marker.ns = "object_detector"
        marker.type = Marker.CUBE
        marker.action = marker.ADD
        marker.id = id
        marker.header.frame_id = box_msg.pose.header.frame_id

        marker.pose = box_msg.pose.pose
        marker.scale.x = box_msg.box_dims.x
        marker.scale.y = box_msg.box_dims.y
        marker.scale.z = box_msg.box_dims.z

        marker.color.a = color[0]
        marker.color.r = color[1]
        marker.color.g = color[2]
        marker.color.b = color[3]
        marker.lifetime = rospy.Duration(duration)

        self.box_drawer.publish(marker)

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

    m = Marker()
    m.header.frame_id = data.header.frame_id
#    m.header.stamp = rospy.get_time()
    m.ns = 'ncvrl'
    m.id = 0
    m.type = 2
#    m.pose.position.x = 0
#    m.pose.position.y = 0
#    m.pose.position.z = 0
#    m.pose.orientation.x = 0
#    m.pose.orientation.y = 0
#    m.pose.orientation.z = 0
#    m.pose.orientation.w = 1.0

    m.pose = data.pose

    m.scale.x = 0.2         
    m.scale.y = 0.2         
    m.scale.z = 0.2         
    m.color.a = 0.5
    m.color.r = 0.0
    m.color.g = 1.0
    m.color.b = 0.0



    pub.publish(m);

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import pose [as 别名]
	def create_banner_markers(self, pose_percept=None, approach_pose=None, id=0):
		if pose_percept:
			banner = Marker()
			banner.header.frame_id = pose_percept.header.frame_id
			banner.header.stamp = rospy.Time.now()
			banner.pose = copy.deepcopy(pose_percept.pose.pose)
			banner.ns = 'number_banners'
			banner.id = id
			i = id + 4
			banner.color.r = i/9*0.4
			banner.color.g = i%9/3*0.4+0.1
			banner.color.b = i%3*0.4+0.2
			banner.color.a = 1.0
			#banner.type = Marker.CUBE
			banner.type = Marker.ARROW
			banner.scale.x = 0.4
			#banner.scale.x = 0.02
			banner.scale.y = 0.2
			banner.scale.z = 0.2
			label = copy.deepcopy(banner)
			label.type = Marker.TEXT_VIEW_FACING
			label.text = '{} ({:3.1f})'.format(id, pose_percept.info.support)
			label.ns = 'banner_label'
			label.pose.position.z += 0.4
			label.scale.z = 0.4
			if approach_pose:
				approach = copy.deepcopy(banner)
				approach.ns = 'banner_approach'
				approach.type = Marker.ARROW
				approach.pose = copy.deepcopy(approach_pose.pose)
				approach.scale = Point(0.2, 0.1, 0.1)
			else:
				approach = self.create_delete_marker('banner_approach', id)
			return banner, label, approach
		return self.create_delete_marker('number_banners', id), self.create_delete_marker('banner_label', id), self.create_delete_marker('banner_approach', id)

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import pose [as 别名]
	def create_loading_station_markers(self, pose_percept=None, approach_pose=None, id=0):
		if pose_percept:
			station = Marker()
			station.header.frame_id = pose_percept.header.frame_id
			station.header.stamp = rospy.Time.now()
			station.pose = copy.deepcopy(pose_percept.pose.pose)
			station.ns = 'loading_stations'
			station.id = id
			station.color.r = 0.4
			station.color.g = 0.4
			station.color.b = 0.8
			station.color.a = 1.0
			station.type = Marker.CUBE
			station.scale.x = 0.02
			station.scale.y = 0.2
			station.scale.z = 0.2
			if approach_pose:
				approach = copy.deepcopy(station)
				approach.ns = 'loading_approach'
				approach.type = Marker.ARROW
				approach.pose = copy.deepcopy(approach_pose.pose)
				approach.scale = Point(0.2, 0.1, 0.1)
			else:
				approach = self.create_delete_marker('loading_approach', id)
			return station, approach
		return self.create_delete_marker('loading_stations', id), self.create_delete_marker('loading_approach', id)

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

        ma = MarkerArray()
        self._path = MoveBaseActionPath()

        for i in range(len(userdata.path.poses)):

            marker = Marker(type=Marker.ARROW)
            marker.header = userdata.path.header
            marker.pose = userdata.path.poses[i].pose
            marker.scale.x = 0.2
            marker.scale.y = 0.02
            marker.scale.z = 0.02
            marker.color.b = 1.0
            marker.color.r = 0.9 - 0.7 * i / len(userdata.path.poses)
            marker.color.g = 0.9 - 0.7 * i / len(userdata.path.poses)
            marker.color.a = 0.8 - 0.5 * i / len(userdata.path.poses)
            marker.id = i
            ma.markers.append(marker)

        self._failed = False

        self._path.goal.target_path.poses = userdata.path.poses
        self._path.goal.target_path.header.frame_id = "map"

        self._pub.publish(self._pathTopic, self._path)
        self._pub.publish(self._marker_topic, ma)
        self._reached = True

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