Python Marker.mesh_resource方法代码示例

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import mesh_resource [as 别名]
    def publishPositions(self):
        if not self.initialized:
            return
        nr = 0
        markerArray = MarkerArray()
        marker = Marker()
        marker.header.frame_id = "/map"
        marker.type = marker.MESH_RESOURCE
        marker.action = marker.ADD
        marker.scale.x = 0.2
        marker.scale.y = 0.2
        marker.scale.z = 0.2

        marker.mesh_use_embedded_materials = True
        marker.mesh_resource = "package://pacman_controller/meshes/pacman.dae"

        marker.color.a = 1.0
        marker.color.r = 0.0
        marker.color.g = 1.0
        marker.color.b = 0.0
        marker.pose.orientation = self.pacman["orientation"]
        marker.pose.position.x = self.pacman["x"]
        marker.pose.position.y = self.pacman["y"]
        marker.pose.position.z = 0.0
        marker.id = nr
        markerArray.markers.append(marker)

        for ghost in self.ghosts:
            curGhost = self.ghosts[ghost]
            if not curGhost["initialized"]:
                continue
            nr += 1
            marker = Marker()
            marker.header.frame_id = "/map"
            marker.type = marker.MESH_RESOURCE
            marker.action = marker.ADD
            marker.scale.x = 0.3
            marker.scale.y = 0.3
            marker.scale.z = 0.3

            marker.mesh_use_embedded_materials = True
            if curGhost["eaten"]:
                marker.mesh_resource = "package://pacman_controller/meshes/dead.dae"
            elif self.state == State.FLEEING:
                marker.mesh_resource = "package://pacman_controller/meshes/ghost_catchable.dae"
            else:
                marker.mesh_resource = "package://pacman_controller/meshes/%s.dae" % ghost
            marker.color.a = 1.0
            marker.color.r = 0.0
            marker.color.g = 1.0
            marker.color.b = 0.0
            marker.pose.orientation = curGhost["orientation"]
            marker.pose.position.x = curGhost["x"]
            marker.pose.position.y = curGhost["y"]
            marker.pose.position.z = 0.0
            marker.id = nr
            markerArray.markers.append(marker)

        self.pubPositions.publish(markerArray)
        return

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import mesh_resource [as 别名]
def draw_grasps(grasps):
    """
    Draws grasps in RVIZ

    Parameters:
    grasps: a list of grasps
    """
    # TODO: add fingers to gripper
    publisher = rospy.Publisher("visualization_marker", Marker)
    for i, g in enumerate(grasps):
        marker = Marker()
        marker.header.frame_id = "base_link"
        marker.header.stamp = rospy.Time.now()
        marker.ns = "grasps"
        marker.type = Marker.MESH_RESOURCE
        marker.mesh_resource = "package://pr2_description/meshes/gripper_v0/gripper_palm.dae"
        marker.action = marker.ADD
        marker.id = i
        marker.pose.position = g.grasp_pose.position
        marker.pose.orientation = g.grasp_pose.orientation
        marker.scale.x = 1.0
        marker.scale.y = 1.0
        marker.scale.z = 1.0
        marker.mesh_use_embedded_materials = True
        publisher.publish(marker)

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import mesh_resource [as 别名]
  def add_marker(self, mesh_frame, marker_name, mesh_file, use_materials=False, color=None):
    marker_msg = Marker()
    marker_msg.frame_locked = True
    marker_msg.id = 0
    marker_msg.action = Marker.ADD
    marker_msg.mesh_use_embedded_materials = use_materials
    if marker_msg.mesh_use_embedded_materials:
      marker_msg.color.r = 0
      marker_msg.color.g = 0
      marker_msg.color.b = 0
      marker_msg.color.a = 0
    elif color:
      marker_msg.color = color
    else:
      marker_msg.color.r = 0.6
      marker_msg.color.g = 0.6
      marker_msg.color.b = 0.6
      marker_msg.color.a = 1.0
    marker_msg.header.stamp = rospy.get_rostime()
    #marker_msg.lifetime =
    #marker_msg.pose =
    marker_msg.type = Marker.MESH_RESOURCE
    marker_msg.header.frame_id = mesh_frame
    marker_msg.ns = marker_name
    marker_msg.mesh_resource = 'file://%s' % (os.path.abspath(mesh_file))
    scale = 1.0
    marker_msg.scale.x, marker_msg.scale.y, marker_msg.scale.z = scale, scale, scale
    marker_msg.pose.position.x, marker_msg.pose.position.y, marker_msg.pose.position.z = 0, 0, 0
    marker_msg.pose.orientation.x, marker_msg.pose.orientation.y, marker_msg.pose.orientation.z, marker_msg.pose.orientation.w = 0, 0, 0, 1
    self.pub_marker_sync(marker_msg)

    if not self.marker_thread:
      self.marker_thread = thread.start_new_thread(MarkerPublisher.publish_markers, (self,))

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import mesh_resource [as 别名]
    def draw_quad_base(self, quad):
        quad_id = self.hash32(quad)
        if not rospy.is_shutdown():
            marker = Marker()
            marker.header.frame_id = "/my_frame"
            marker.lifetime = rospy.Duration(self.duration)
            marker.type = marker.MESH_RESOURCE
            marker.mesh_resource = "package://hector_quadrotor_description/"\
                + "meshes/quadrotor/quadrotor_base.dae"
            marker.action = marker.ADD
            marker.scale.x = 80
            marker.scale.y = 80
            marker.scale.z = 80
            marker.mesh_use_embedded_materials = True

            marker.pose.orientation.w = math.cos(
                math.radians(quad.get_orientation() / 2)
            )
            marker.pose.orientation.x = 0
            marker.pose.orientation.y = 0
            marker.pose.orientation.z = math.sin(
                math.radians(quad.get_orientation() / 2)
            )

            marker.pose.position.x = quad.get_x()
            marker.pose.position.y = quad.get_y()
            marker.pose.position.z = quad.get_z()
            marker.id = quad_id
            self.markers.append(marker)

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import mesh_resource [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 mesh_resource [as 别名]
    def create_object_marker(self, soma_obj, soma_type, pose):
        # create an interactive marker for our server
        int_marker = InteractiveMarker()
        int_marker.header.frame_id = "/map"
        int_marker.name = soma_obj
        int_marker.description = "id" + soma_obj
        int_marker.pose = pose
        
        mesh_marker = Marker()
        mesh_marker.type = Marker.MESH_RESOURCE
        mesh_marker.scale.x = 1
        mesh_marker.scale.y = 1
        mesh_marker.scale.z = 1

        random.seed(soma_type)
        val = random.random()
        mesh_marker.color.r = r_func(val)
        mesh_marker.color.g = g_func(val)
        mesh_marker.color.b = b_func(val)
        mesh_marker.color.a = 1.0
        #mesh_marker.pose = pose
        mesh_marker.mesh_resource = self.mesh[soma_type]

        control = InteractiveMarkerControl()
        control.markers.append(mesh_marker)
        int_marker.controls.append(control)
        
        return int_marker

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import mesh_resource [as 别名]
def mesh_marker(position, orientation=None, mesh_resource="", rgba=[1.0, 1.0, 1.0, 1.0],
                ns="", id=0, frame_id="", lifetime=None, stamp=None):
    marker = Marker()
    if stamp is None:
        marker.header.stamp = rospy.Time.now()
    else:
        marker.header.stamp = stamp
    marker.header.frame_id = frame_id
    marker.ns = ns
    marker.id = id
    marker.type = Marker.MESH_RESOURCE
    marker.mesh_resource = mesh_resource
    marker.action = Marker.ADD
    marker.pose.position.x = position[0]
    marker.pose.position.y = position[1]
    marker.pose.position.z = position[2]
    if orientation is not None:
        marker.pose.orientation.x = orientation[0] 
        marker.pose.orientation.y = orientation[1] 
        marker.pose.orientation.z = orientation[2] 
        marker.pose.orientation.w = orientation[3] 
    #marker.points = [Point(*tuple(position)), Point(*tuple(position+direction*1.0))]
    marker.scale.x = 1.0
    marker.scale.y = 1.0
    marker.scale.z = 1.0
    marker.color.r = rgba[0]
    marker.color.g = rgba[1]
    marker.color.b = rgba[2]
    marker.color.a = rgba[3]
    if lifetime is None:
        marker.lifetime = rospy.Duration()
    else:
        marker.lifetime = rospy.Duration(lifetime)
    return marker

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import mesh_resource [as 别名]
 def pub_at_position(self, odom_msg):
     """
     Handles necessary information for displaying things at 
                             ACCEPTED (NOVATEL) POSITION SOLUTION:
         -vehicle mesh 
     !!!this should only be invoked when the accepted (novatel) position 
     is updated
     """
     ### G35 Mesh #############################################################
     marker = Marker()
     pub = self.curpos_publisher
     marker.header = odom_msg.header
     marker.id = 0 # enumerate subsequent markers here
     marker.action = Marker.MODIFY # can be ADD, REMOVE, or MODIFY
     marker.pose = odom_msg.pose.pose
     marker.pose.position.z = 1.55
     marker.lifetime = rospy.Duration() # will last forever unless modified
     marker.ns = "vehicle_model"
     marker.type = Marker.MESH_RESOURCE
     marker.scale.x = 0.0254 # artifact of sketchup export
     marker.scale.y = 0.0254 # artifact of sketchup export
     marker.scale.z = 0.0254 # artifact of sketchup export
     marker.color.r = .05
     marker.color.g = .05
     marker.color.b = .05
     marker.color.a = .2
     marker.mesh_resource = self.veh_mesh_resource
     marker.mesh_use_embedded_materials = False
     pub.publish(marker)

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import mesh_resource [as 别名]
def createMeshMarker(resource, offset=(0,0,0), rgba=(1,0,0,1), orientation=(0,0,0,1), scale=1, scales=(1,1,1), frame_id="/map"):
    marker = Marker()
    marker.mesh_resource = resource;
    marker.header.frame_id = frame_id
    marker.type = marker.MESH_RESOURCE
    marker.scale.x = scale*scales[0]
    marker.scale.y = scale*scales[1]
    marker.scale.z = scale*scales[2]
    marker.color.a = rgba[3]
    marker.color.r = rgba[0]
    marker.color.g = rgba[1]
    marker.color.b = rgba[2]
    marker.pose.orientation.x = orientation[0]
    marker.pose.orientation.y = orientation[1]
    marker.pose.orientation.z = orientation[2]
    marker.pose.orientation.w = orientation[3]
    marker.pose.position.x = offset[0]
    marker.pose.position.y = offset[1]
    marker.pose.position.z = offset[2]
    
    obj_control = InteractiveMarkerControl()
    obj_control.always_visible = True
    obj_control.markers.append( marker )
        
    return obj_control

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import mesh_resource [as 别名]
def publish_shelf(publisher, pose_stamped):
    """Publishes a shelf marker at a given pose.

    The pose is assumed to represent the bottom center of the shelf, with the
    +x direction pointing along the depth axis of the bins and +z pointing up.

    Args:
      publisher: A visualization_msgs/Marker publisher
      pose_stamped: A PoseStamped message with the location, orientation, and
        reference frame of the shelf.
    """
    marker = Marker()
    marker.header.frame_id = pose_stamped.header.frame_id
    marker.header.stamp = rospy.Time().now()
    marker.ns = 'shelf'
    marker.id = 0
    marker.type = Marker.MESH_RESOURCE
    marker.mesh_resource = 'package://pr2_pick_perception/models/shelf/shelf.ply'
    marker.mesh_use_embedded_materials = True
    marker.action = Marker.ADD
    marker.pose = pose_stamped.pose
    marker.scale.x = 1
    marker.scale.y = 1
    marker.scale.z = 1
    marker.lifetime = rospy.Duration()
    _publish(publisher, marker, "shelf")

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import mesh_resource [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 mesh_resource [as 别名]
 def publish_subject_marker(self, pos, ori):
     marker = Marker()
     marker.header.frame_id = "/base_link"
     marker.header.stamp = rospy.Time()
     marker.ns = "base_service_subject_model"
     marker.id = 0
     marker.type = Marker.MESH_RESOURCE;
     marker.action = Marker.ADD
     marker.pose.position.x = pos[0]
     marker.pose.position.y = pos[1]
     marker.pose.position.z = 0
     marker.pose.orientation.x = ori[0]
     marker.pose.orientation.y = ori[1]
     marker.pose.orientation.z = ori[2]
     marker.pose.orientation.w = ori[3]
     marker.scale.x = .0254
     marker.scale.y = .0254
     marker.scale.z = .0254
     marker.color.a = 1.
     marker.color.r = 0.0
     marker.color.g = 0.0
     marker.color.b = 1.0
     if self.model=='chair':
         name = 'wc_model'
         marker.mesh_resource = "package://hrl_base_selection/models/ADA_Wheelchair.dae"
         marker.scale.x = .0254
         marker.scale.y = .0254
         marker.scale.z = .0254
     elif self.model=='bed':
         name = 'bed_model'
         marker.mesh_resource = "package://hrl_base_selection/models/head_bed.dae"
         marker.scale.x = 1.0
         marker.scale.y = 1.
         marker.scale.z = 1.0
     elif self.model=='autobed':
         name = 'autobed_model'
         marker.mesh_resource = "package://hrl_base_selection/models/bed_and_body_v3_rviz.dae"
         marker.scale.x = 1.0
         marker.scale.y = 1.0
         marker.scale.z = 1.0
     else:
         print 'I got a bad model. What is going on???'
         return None
     self.vis_pub.publish(marker)

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import mesh_resource [as 别名]
 def publish_sub_marker(self, pos, ori):
     marker = Marker()
     #marker.header.frame_id = "/base_footprint"
     marker.header.frame_id = "/base_link"
     marker.header.stamp = rospy.Time()
     marker.id = 0
     marker.type = Marker.MESH_RESOURCE
     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 = ori[0]
     marker.pose.orientation.y = ori[1]
     marker.pose.orientation.z = ori[2]
     marker.pose.orientation.w = ori[3]
     marker.color.a = 1.
     marker.color.r = 0.0
     marker.color.g = 1.0
     marker.color.b = 0.0
     if self.model == 'chair':
         name = 'subject_model'
         marker.mesh_resource = "package://hrl_base_selection/models/wheelchair_and_body_assembly_rviz.STL"
         marker.scale.x = 1.0
         marker.scale.y = 1.0
         marker.scale.z = 1.0
     elif self.model == 'bed':
         name = 'subject_model'
         marker.mesh_resource = "package://hrl_base_selection/models/head_bed.dae"
         marker.scale.x = 1.0
         marker.scale.y = 1.0
         marker.scale.z = 1.0
     elif self.model == 'autobed':
         name = 'subject_model'
         marker.mesh_resource = "package://hrl_base_selection/models/bed_and_body_v3_rviz.dae"
         marker.scale.x = 1.0
         marker.scale.y = 1.0
         marker.scale.z = 1.0
     else:
         print 'I got a bad model. What is going on???'
         return None
     vis_pub = rospy.Publisher(''.join(['~',name]), Marker, queue_size=1, latch=True)
     marker.ns = ''.join(['base_service_',name])
     vis_pub.publish(marker)
     print 'Published a model of the subject to rviz'

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import mesh_resource [as 别名]
    def create_object_marker(self, soma_obj, soma_type, pose):
        # create an interactive marker for our server
        int_marker = InteractiveMarker()
        int_marker.header.frame_id = "map"
        int_marker.name = soma_obj
        int_marker.description = "id" + soma_obj
        int_marker.pose = pose
        int_marker.pose.position.z = 0.01 
        
        mesh_marker = Marker()
        mesh_marker.type = Marker.MESH_RESOURCE
        mesh_marker.scale.x = 1
        mesh_marker.scale.y = 1
        mesh_marker.scale.z = 1

        random.seed(soma_type)
        val = random.random()
        mesh_marker.color.r = r_func(val)
        mesh_marker.color.g = g_func(val)
        mesh_marker.color.b = b_func(val)
        mesh_marker.color.a = 1.0
        #mesh_marker.pose = pose
        mesh_marker.mesh_resource = self.mesh[soma_type]

        # create a control which will move the box
        # this control does not contain any markers,
        # which will cause RViz to insert two arrows
        control = InteractiveMarkerControl()
        control.orientation.w = 1
        control.orientation.x = 0
        control.orientation.y = 1
        control.orientation.z = 0
        control.interaction_mode = InteractiveMarkerControl.MOVE_ROTATE

        
        if self._interactive:
            int_marker.controls.append(copy.deepcopy(control))
            # add the control to the interactive marker
            if self.marker[soma_type] == '3D':
                control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
                int_marker.controls.append(control)

        # add menu control
        menu_control = InteractiveMarkerControl()

        menu_control.interaction_mode = InteractiveMarkerControl.BUTTON
        menu_control.always_visible = True
        
        menu_control.markers.append( mesh_marker) #makeBox(int_marker) )
        int_marker.controls.append(menu_control)

        return int_marker

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import mesh_resource [as 别名]
def CreateMeshMarker(pose, mesh_path, alpha = 1, scaleFactor=1, id=randint(0,10000)):
    marker = Marker()
    marker.ns = "visual"
    marker.id = id
    marker.scale.x = scaleFactor
    marker.scale.y = scaleFactor
    marker.scale.z = scaleFactor
    marker.pose = pose
    marker.type = Marker.MESH_RESOURCE
    marker.mesh_use_embedded_materials = True
    marker.mesh_resource = mesh_path
    marker.frame_locked = True
    return marker