Python Marker.frame_locked方法代码示例

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import frame_locked [as 别名]
    def publish_debug_info(self, footprint_point, distance, scan_point):
        array = MarkerArray()

        fp = Marker()
        fp.id = 1
        fp.type = fp.SPHERE
        scale = 0.1
        fp.pose.position.x,fp.pose.position.y, fp.pose.position.z = footprint_point[0], footprint_point[1], 0
        fp.scale.x, fp.scale.y, fp.scale.z = scale, scale, scale
        fp.color.r, fp.color.g, fp.color.b, fp.color.a = (0, 0, 1, 1)
        fp.header.frame_id = "{}/base_link".format(self.robot.robot_name)
        fp.frame_locked = True
        fp.action = fp.ADD
        fp.ns = "door_opening"
        array.markers += [fp]

        sp = Marker()
        sp.id = 2
        sp.type = sp.SPHERE
        scale = 0.1
        sp.pose.position.x,sp.pose.position.y, sp.pose.position.z = scan_point[0], scan_point[1], 0
        sp.scale.x, sp.scale.y, sp.scale.z = scale, scale, scale
        sp.color.r, sp.color.g, sp.color.b, sp.color.a = (1, 0, 0, 1)
        sp.header.frame_id = "{}/base_link".format(self.robot.robot_name)
        sp.frame_locked = False
        sp.action = sp.ADD
        sp.ns = "door_opening"
        array.markers += [sp]

        self.debug_vizualizer.publish(array)

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import frame_locked [as 别名]
	def add_rviz_disks(self, disk, rod, act, move):
		marker = Marker()
		if(move == True):
			marker.header.frame_id = "/pl_6"
			marker.frame_locked = True
			marker.pose.position.x = 0.
			marker.pose.position.y = 0.
			marker.pose.position.z = 0.27965
		else:
			marker.header.frame_id = "/pl_base"
			marker.frame_locked = False
			marker.pose.position.x = act.position.x
			marker.pose.position.y = act.position.y
			marker.pose.position.z = act.position.z - 0.143 + 0.012 + len(self.rod_content[rod]) * 0.01
			if(self.carrying_disk):
				marker.pose.position.z = marker.pose.position.z + 0.03 
					
		marker.ns = "basic_shapes"
		marker.id = disk
		marker.type = marker.CYLINDER
		marker.action = marker.ADD
		marker.pose.orientation.w = 1.0
		marker.scale.z = 0.01

		if(disk == 1):
			marker.color.r = 1.0
			marker.color.a = 1.0
			marker.scale.x = 0.027
		elif(disk == 2):
			marker.color.r = 1.0
			marker.color.g = 1.0
			marker.color.a = 1.0
			marker.scale.x = 0.031
		elif(disk == 3):
			marker.color.g = 1.0
			marker.color.a = 1.0
			marker.scale.x = 0.036
		elif(disk == 4):
			marker.color.b = 1.0
			marker.color.a = 1.0
			marker.scale.x = 0.04
		elif(disk == 5):
			marker.color.r = 1.0
			marker.color.g = 1.0			
			marker.color.b = 1.0
			marker.color.a = 1.0
			marker.scale.x = 0.027

		marker.scale.y = marker.scale.x

		self.rviz_pub.publish(marker)

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import frame_locked [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 frame_locked [as 别名]
	def move_disk(self, disk, rod):
		marker = Marker()

		marker.header.frame_id = "/pl_6"
		marker.frame_locked = True
		marker.pose.position.x = 0.0
		marker.pose.position.y = 0.0
		marker.pose.position.z = 0.27965
					
		marker.ns = "basic_shapes"
		marker.id = disk
		marker.type = marker.CYLINDER
		marker.action = marker.ADD
		marker.pose.orientation.w = 1.0
		marker.scale.x = DISK_SIZE[disk - 1]
		marker.scale.y = DISK_SIZE[disk - 1]
		marker.scale.z = 0.01
		marker.color.r = DISK_COLOR[disk - 1][0]
		marker.color.g = DISK_COLOR[disk - 1][1]
		marker.color.b = DISK_COLOR[disk - 1][2]
		marker.color.a = 1.0
			
		self.carrying_disk = disk

		self.rviz_pub.publish(marker)

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import frame_locked [as 别名]
	def put_disk(self, disk, rod):
		marker = Marker()
		
		marker.header.frame_id = "/pl_base"
		marker.frame_locked = False
		marker.pose.position.x = self.rod_position[rod][0]
		marker.pose.position.y = self.rod_position[rod][1]
		marker.pose.position.z = self.position_z + DIFF_DISTANCE_BASE + 0.012 + len(self.rod_content[rod]) * 0.01
					
		marker.ns = "basic_shapes"
		marker.id = disk
		marker.type = marker.CYLINDER
		marker.action = marker.ADD
		marker.pose.orientation.w = 1.0
		marker.scale.x = DISK_SIZE[disk - 1]
		marker.scale.y = DISK_SIZE[disk - 1]
		marker.scale.z = 0.01
		marker.color.r = DISK_COLOR[disk - 1][0]
		marker.color.g = DISK_COLOR[disk - 1][1]
		marker.color.b = DISK_COLOR[disk - 1][2]
		marker.color.a = 1.0

		self.carrying_disk = -1
	
		self.rviz_pub.publish(marker)

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import frame_locked [as 别名]
 def create_marker(self, type, dims, frame, ns, id, duration = 60., color = [1,0,0], opaque = 0.5, pos = [0.,0.,0.], quat = [0.,0.,0.,1.], frame_locked = False):
     marker = Marker()
     marker.header.frame_id = frame
     marker.header.stamp = rospy.Time.now()
     marker.ns = ns
     marker.type = type
     marker.action = Marker.ADD
     marker.scale.x = dims[0]
     marker.scale.y = dims[1]
     marker.scale.z = dims[2]
     marker.color.a = opaque
     marker.color.r = color[0]
     marker.color.g = color[1]
     marker.color.b = color[2]
     marker.lifetime = rospy.Duration(duration)
     marker.id = id
     marker.pose.position.x = pos[0]
     marker.pose.position.y = pos[1]
     marker.pose.position.z = pos[2]
     marker.pose.orientation.x = quat[0]
     marker.pose.orientation.y = quat[1]
     marker.pose.orientation.z = quat[2]
     marker.pose.orientation.w = quat[3]       
     marker.frame_locked = frame_locked
     return marker

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import frame_locked [as 别名]
 def _default_marker(self, index=0):
     m = Marker()
     m.ns = self._ns
     m.id = self._ids[index]
     m.header.frame_id = self._frame_id
     m.action = Marker.ADD
     m.frame_locked = True
     return m

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import frame_locked [as 别名]
def test():
    rospy.init_node('intersect_plane_test')
    marker_pub = rospy.Publisher('table_marker', Marker)
    pose_pub = rospy.Publisher('pose', PoseStamped)
    int_pub = rospy.Publisher('intersected_points', PointCloud2)
    tfl = tf.TransformListener()
    
    # Test table
    table = Table()
    table.pose.header.frame_id = 'base_link'
    table.pose.pose.orientation.x, table.pose.pose.orientation.y, table.pose.pose.orientation.z, table.pose.pose.orientation.w = (0,0,0,1)
    table.x_min = -0.5
    table.x_max =  0.5
    table.y_min = -0.5
    table.y_max =  0.5

    # A marker for that table
    marker = Marker()
    marker.header.frame_id = table.pose.header.frame_id
    marker.id = 1
    marker.type = Marker.LINE_STRIP
    marker.action = 0
    marker.pose = table.pose.pose
    marker.scale.x, marker.scale.y, marker.scale.z = (0.005,0.005,0.005)
    marker.color.r, marker.color.g, marker.color.b, marker.color.a = (0.0,1.0,1.0,1.0) 
    marker.frame_locked = False
    marker.ns = 'table'
    marker.points = [
        Point(table.x_min,table.y_min, table.pose.pose.position.z),
        Point(table.x_min,table.y_max, table.pose.pose.position.z),
        Point(table.x_max,table.y_max, table.pose.pose.position.z),
        Point(table.x_max,table.y_min, table.pose.pose.position.z),
        Point(table.x_min,table.y_min, table.pose.pose.position.z),
    ]
    marker.colors = []
    marker.text = ''
    # marker.mesh_resource = ''
    marker.mesh_use_embedded_materials = False
    marker.header.stamp = rospy.Time.now()

    # A test pose
    pose = PoseStamped()
    pose.header = table.pose.header
    pose.pose.position.x, pose.pose.position.y, pose.pose.position.z = (0,0,0.5)
    pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w = quaternion_from_euler(0,-pi/5,pi/5)
    
    intersection = cast_ray(pose, table, tfl)
    cloud = xyz_array_to_pointcloud2(np.array([[intersection.point.x, intersection.point.y, intersection.point.z]]))
    cloud.header = pose.header
    
    while not rospy.is_shutdown():
        marker_pub.publish(marker)
        pose_pub.publish(pose)
        int_pub.publish(cloud)
        rospy.loginfo('published')
        rospy.sleep(0.1)

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

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

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import frame_locked [as 别名]
	def create_status_marker(self, text):
		marker = Marker()
		marker.header.frame_id = 'base_footprint'
		marker.header.stamp = rospy.Time.now()
		marker.pose.position.z += 1
		marker.pose.orientation.w = 1
		marker.type = Marker.TEXT_VIEW_FACING
		marker.text = text
		marker.frame_locked = True
		marker.ns = 'status'
		marker.scale.z = 0.3
		marker.color.r = 0.8
		marker.color.g = 0.5
		marker.color.b = 0.1
		marker.color.a = 1.0
		return marker

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import frame_locked [as 别名]
def main():

    rospy.init_node('field_marker_publisher_node', anonymous=True)
    pub = rospy.Publisher('agent_markers', MarkerArray, queue_size=10)

    agent_number = rospy.get_param('~agent_number')   
    print "agent_number = " + str(agent_number)

    height = rospy.get_param('/agent/height')   
    print "height = " + str(height)

    radius = rospy.get_param('/agent/radius')   
    print "radius = " + str(radius)


    ma = MarkerArray()

    #Green rectangle
    m = Marker()
    m.header.stamp = rospy.Time.now()
    m.header.frame_id = "base_footprint_agent" + str(agent_number)
    m.ns = "agent"
    m.id = 0
    m.type = 3
    m.frame_locked = True

    m.pose.orientation.w = 1
    m.pose.position.z = 0
    m.scale.x = radius
    m.scale.y = radius
    m.scale.z = height
    m.color.r = 0.2
    m.color.g = 0.2
    m.color.b = 0.2
    m.color.a = 1
    ma.markers.append(copy.deepcopy(m))

          
    rate = rospy.Rate(1) 
    while not rospy.is_shutdown():

        print "in cycle"
        pub.publish(ma)
        
        ##rospy.loginfo(t)
        rate.sleep()

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import frame_locked [as 别名]
    def draw_grasps(self, grasps, frame, ns = 'grasps', pause = 0, frame_locked = False):

        marker = Marker()
        marker.header.frame_id = frame
        marker.header.stamp = rospy.Time.now()
        marker.ns = ns
        marker.type = Marker.ARROW
        marker.action = Marker.ADD
        marker.color.a = 1.0
        marker.lifetime = rospy.Duration(0)
        marker.frame_locked = frame_locked

        for (grasp_num, grasp) in enumerate(grasps):
            if grasp_num == 0:
                marker.scale.x = 0.015
                marker.scale.y = 0.025
                length_fact = 1.5

            else:
                marker.scale.x = 0.01
                marker.scale.y = 0.015
                length_fact = 1.0

            orientation = grasp.orientation
            quat = [orientation.x, orientation.y, orientation.z, orientation.w]
            mat = tf.transformations.quaternion_matrix(quat)
            start = [grasp.position.x, grasp.position.y, grasp.position.z]
            x_end = list(mat[:,0][0:3]*.05*length_fact + scipy.array(start))    
            y_end = list(mat[:,1][0:3]*.02*length_fact + scipy.array(start))
            marker.id = grasp_num*3
            marker.points = [Point(*start), Point(*x_end)]
            marker.color.r = 1.0
            marker.color.g = 0.0
            marker.color.b = 0.0
            self.marker_pub.publish(marker)
            marker.id = grasp_num*3+1
            marker.points = [Point(*start), Point(*y_end)]
            marker.color.r = 0.0
            marker.color.g = 1.0
            marker.color.b = 0.0
            self.marker_pub.publish(marker)
            marker.id = grasp_num*3+2
            if pause:
                print "press enter to continue"
                raw_input()
        time.sleep(.5)

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import frame_locked [as 别名]
 def _init_markers(self):
     self.marker_idx = 0
     for i in range(self.max_markers):
         marker = Marker()
         marker.header.frame_id = self.global_frame
         marker.id = self.marker_idx
         marker.type = 2
         marker.action = 2
         marker.pose = Pose()
         marker.color.r = 0.0
         marker.color.g = 0.0
         marker.color.b = 0.0
         marker.color.a = 0.0
         marker.scale.x = 0.1
         marker.scale.y = 0.1
         marker.scale.z = 0.1
         marker.frame_locked = False
         marker.ns = "Goal-%u"%i
         self.marker_array_msg.markers.append(marker)            

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import frame_locked [as 别名]
def initMarker(baseFrameId="world", stamp=None, markerNamespace=None, markerId=0, markerType=None, markerAction=Marker.ADD, markerDuration=rospy.Duration()):
    marker = Marker()
    # 	cframes.header.frame_id = baseFrameId;
    marker.header.frame_id = baseFrameId
    # 	marker.header.stamp = timestamp; // TODO: check on this...not sure this is right
    if stamp is None:
        stamp = rospy.Time.now()
    marker.header.stamp = stamp
    # 	marker.ns = "/vertices";
    if not markerNamespace is None:
        marker.ns = markerNamespace
    # 	marker.id = 0;
    marker.id = markerId
    # 	marker.type = visualization_msgs::Marker::LINE_LIST;
    if not markerType is None:
        marker.type = markerType
    # 	marker.action = visualization_msgs::Marker::ADD;
    marker.action = markerAction
    # 	marker.lifetime = ros::Duration();
    marker.lifetime = markerDuration
    # 	marker.pose.position.x = marker.pose.position.y = marker.pose.position.z = 0.0;
    marker.pose.position.x = 0.0
    marker.pose.position.y = 0.0
    marker.pose.position.z = 0.0
    # 	marker.pose.orientation.x = marker.pose.orientation.y = marker.pose.orientation.z = 0.0;
    # 	marker.pose.orientation.w = 1.0;
    marker.pose.orientation.x = 0.0
    marker.pose.orientation.y = 0.0
    marker.pose.orientation.z = 0.0
    marker.pose.orientation.w = 1.0
    # 	marker.scale.x = marker.scale.y = marker.scale.z = 0.01;
    marker.scale.x = 1.0;
    marker.scale.y = 1.0;
    marker.scale.z = 1.0;
    # 	marker.color.r = marker.color.g = marker.color.b = marker.color.a = 1.f;
    marker.color.r = 1.0
    marker.color.g = 1.0
    marker.color.b = 1.0
    marker.color.a = 1.0
    # 	marker.frame_locked = false;
    marker.frame_locked = False
    return marker