Python msg.Header类代码示例

def createRandomGrasps(grasp_pose, num_grasps=1):
    """ Returns a list of num_grasps Grasp's around grasp_pose """
    list_grasps = []
    for grasp_num in range(num_grasps):
        my_pre_grasp_approach = createGripperTranslation(Vector3(1.0, 0.0, 0.0)) # rotate over here?
        my_post_grasp_retreat = createGripperTranslation(Vector3(0.0, 0.0, 1.0))
        header = Header()
        header.frame_id = "base_link"
        header.stamp = rospy.Time.now()
        grasp_pose_copy = copy.deepcopy(grasp_pose)
        modified_grasp_pose = PoseStamped(header, grasp_pose_copy)
        #modified_grasp_pose.pose.position.x -= random.random() * 0.25 # randomize entrance to grasp in 25cm in x
        #modified_grasp_pose.pose.position.y -= random.random() * 0.10 # randomize entrance to grasp in 10cm in y
        #modified_grasp_pose.pose.orientation.z = -1.0
        
        grasp = createGrasp(modified_grasp_pose, # change grasp pose?
                    allowed_touch_objects=["part"], 
                    pre_grasp_posture=getPreGraspPosture(),
                    grasp_posture=getPreGraspPosture(),
                    pre_grasp_approach=my_pre_grasp_approach,
                    post_grasp_retreat=my_post_grasp_retreat,
                    id_grasp="random_grasp_" + str(grasp_num)
                    )
        list_grasps.append(grasp)
    return list_grasps

    def _cloud_cb(self, cloud):
        points = np.array(list(read_points(cloud, skip_nans=True)))
        if points.shape[0] == 0:
            return

        # Get 4x4 matrix which transforms point cloud co-ords to odometry frame
        try:
            points_to_map = self.tf.asMatrix('/odom', cloud.header)
        except tf.ExtrapolationException:
            return

        transformed_points = points_to_map.dot(np.vstack((points.T, np.ones((1, points.shape[0])))))
        transformed_points = transformed_points[:3,:].T

        if self.fused is None:
            self.fused = transformed_points
        else:
            self.fused = np.vstack((self.fused, transformed_points))

        self.seq += 1
        header = Header()
        header.seq = self.seq
        header.stamp = rospy.Time.now()
        header.frame_id = '/odom'

        output_cloud = create_cloud(header, cloud.fields, self.fused)

        rospy.loginfo('Publishing new cloud')
        self.cloud_pub.publish(output_cloud)

def fvToMsg(fv, now):

	global seq

	fvMsg = PersonFVMsg()
	
	header = Header()
	header.seq = seq
	header.stamp = now
	header.frame_id = frameID

	rhv = Vector3()
	rev = Vector3()
	lhv = Vector3()
	lev = Vector3()

	setV3(rhv, fv, 0)
	setV3(lhv, fv, 3)
	setV3(rev, fv, 6)
	setV3(lev, fv, 9)

	fvMsg.header = header
	fvMsg.rh = rhv
	fvMsg.re = rev
	fvMsg.lh = lhv
	fvMsg.le = lev

	fvMsg.distance = fv[12]

	return fvMsg

    def execute(self, userdata):
        header = Header()
        header.frame_id = "base_link"
        header.stamp = rospy.Time.now()
        userdata.move_it_joint_goal = MoveGroupGoal()
        goal_c = Constraints()
        for name, value in zip(userdata.manip_joint_names, userdata.manip_goal_joint_pose):
            joint_c = JointConstraint()
            joint_c.joint_name = name
            joint_c.position = value
            joint_c.tolerance_above = 0.01
            joint_c.tolerance_below = 0.01
            joint_c.weight = 1.0
            goal_c.joint_constraints.append(joint_c)

        userdata.move_it_joint_goal.request.goal_constraints.append(goal_c)
        userdata.move_it_joint_goal.request.num_planning_attempts = 5
        userdata.move_it_joint_goal.request.allowed_planning_time = 5.0
        userdata.move_it_joint_goal.planning_options.plan_only = False  # False = Plan + Execute ; True = Plan only
        userdata.move_it_joint_goal.planning_options.planning_scene_diff.is_diff = True
        userdata.move_it_joint_goal.request.group_name = userdata.manip_joint_group
        rospy.loginfo("Group Name: " + userdata.manip_joint_group)
        rospy.loginfo("Joints name: " + str(userdata.manip_joint_names))
        rospy.loginfo("Joints Values: " + str(userdata.manip_goal_joint_pose))

        rospy.loginfo("GOAL TO SEND IS:... " + str(userdata.move_it_joint_goal))
        return "succeeded"

    def test_AG_remote(self):
        self.assertEqual(self.AlarmListener.is_cleared(), True,
                         msg='current state of kill signal is raised')
        # publishing msg to network
        pub = rospy.Publisher('/network', Header, queue_size=10)
        rate = rospy.Rate(10)
        t_end = rospy.Time.now() + rospy.Duration(3)
        while rospy.Time.now() < t_end:
            hello_header = Header()
            hello_header.stamp = rospy.Time.now()
            rospy.loginfo(hello_header)
            pub.publish(hello_header)
            rate.sleep()

        self.assertEqual(
            self.AlarmListener.is_cleared(),
            True,
            msg='REMOTE shutdown not worked. State = {}'.format(
                self._current_alarm_state))

        # stop sending the msg, the remote alarm will raise when stop recieving
        # the msg for 8 secs
        rospy.sleep(8.2)

        self.assertEqual(
            self.AlarmListener.is_raised(),
            True,
            msg='REMOTE raised not worked. State = {}'.format(
                self._current_alarm_state))

 def HeaderGenerater(self):
  self.seq += 1
  header = Header()
  header.seq = self.seq
  header.stamp = rospy.Time.now()
  header.frame_id = self.GoalFrame
  return header

def publish_poses_grasps(grasps):
    rospy.loginfo("Publishing PoseArray on /grasp_pose_from_block_bla for grasp_pose")
    grasp_publisher = rospy.Publisher("grasp_pose_from_block_bla", PoseArray)
    graspmsg = Grasp()
    grasp_PA = PoseArray()
    header = Header()
    header.frame_id = "base_link"
    header.stamp = rospy.Time.now()
    grasp_PA.header = header
    for graspmsg in grasps:
        print graspmsg
        print type(graspmsg)
        p = Pose(graspmsg.grasp_pose.pose.position, graspmsg.grasp_pose.pose.orientation)
        grasp_PA.poses.append(p)
    grasp_publisher.publish(grasp_PA)
    rospy.loginfo("Press a to continue...")
    while True:
        choice = raw_input("> ")
    
        if choice == 'a' :
            print "Continuing, a pressed"
            break
        else:
            grasp_publisher.publish(grasp_PA)
            rospy.sleep(0.1)

def talker():
  #Initial Pose Set Up
  pub_init = rospy.Publisher('initialpose', PoseWithCovarianceStamped)
  rospy.init_node('Initial_Pose_Publish')
  initial=PoseWithCovarianceStamped()
  initial_pose=PoseWithCovariance()
  initial_pose.pose=Pose(Point(0.100,0.100,0.100),Quaternion(0.000,0.000,-0.0149,0.999))
  initial_pose.covariance=\
  [1.0,0.0,0.0,0.0,0.0,0.0,\
  0.0,1.0,0.0,0.0,0.0,0.0,\
  0.0,0.0,1.0,0.0,0.0,0.0,\
  0.0,0.0,0.0,1.0,0.0,0.0,\
  0.0,0.0,0.0,0.0,1.0,0.0,\
  0.0,0.0,0.0,0.0,0.0,1.0]
  initial.pose=initial_pose
  initial_info=Header()
  initial_info.frame_id="map"
  initial_info.stamp=rospy.Time.now()
  initial.header=initial_info
  pub_init.publish(initial)
  r=rospy.Rate(1)
  ans=str("y")

  #publishing the goal
  
  while not rospy.is_shutdown():
    rospy.loginfo("Setting Initial Pose")
    pub_init.publish(initial)
    ans=raw_input("Enter to continue") 
    rospy.loginfo("Setting Goal")

	def __init__(self):
		#Setup subscriber for combined searched image
		frontierMarkerSub = rospy.Subscriber('frontierMarkers',Marker,self.markerCallback,queue_size = 1) 
		
		#Get Number of Robots
		self.numRobots = rospy.get_param('/num_robots')
		
		#Initialize Goal Publishers
		self.goalPub = []
		for i in range(0, self.numRobots):
			#Setup Subscribers to individual robot searched grids
			topicName = 'robot_' + `i` +'/move_base_simple/goal'
			self.goalPub.append(rospy.Publisher(topicName,PoseStamped))
			
		#Setup start/stop time publishers
		self.startPub = rospy.Publisher('startTime', Header)
		self.stopPub = rospy.Publisher('stopTime', Header)
		
		#Get simulation start time
		rospy.sleep(0.01)						#Without this line, get_rostime always returns 0
		self.start_time = rospy.get_rostime()
		rospy.loginfo("SIMULATION STARTED AT: %i %i", self.start_time.secs, self.start_time.nsecs)
		
		#Publish Start TIme
		startMsg = Header()
		startMsg.stamp = self.start_time
		startMsg.frame_id = '0'
		self.startPub.publish(startMsg)
		
		#Wait for subscribed messages to start arriving
		rospy.spin()

    def create_move_group_pose_goal(self, goal_pose=Pose(), group="manipulator", end_link_name=None, plan_only=True):

        header = Header()
        header.frame_id = 'torso_base_link'
        header.stamp = rospy.Time.now()

        moveit_goal = MoveGroupGoal()
        goal_c = Constraints()
        position_c = PositionConstraint()
        position_c.header = header
        if end_link_name != None: 
            position_c.link_name = end_link_name
        position_c.constraint_region.primitives.append(SolidPrimitive(type=SolidPrimitive.SPHERE, dimensions=[0.01])) 
        position_c.constraint_region.primitive_poses.append(goal_pose)
        position_c.weight = 1.0
        goal_c.position_constraints.append(position_c)
        orientation_c = OrientationConstraint()
        orientation_c.header = header
        if end_link_name != None:
            orientation_c.link_name = end_link_name
        orientation_c.orientation = goal_pose.orientation
        orientation_c.absolute_x_axis_tolerance = 0.01 
        orientation_c.absolute_y_axis_tolerance = 0.01
        orientation_c.absolute_z_axis_tolerance = 0.01
        orientation_c.weight = 1.0
        goal_c.orientation_constraints.append(orientation_c)
        moveit_goal.request.goal_constraints.append(goal_c)
        moveit_goal.request.num_planning_attempts = 5 
        moveit_goal.request.allowed_planning_time = 5.0
        moveit_goal.planning_options.plan_only = plan_only
        moveit_goal.planning_options.planning_scene_diff.is_diff = True # Necessary
        moveit_goal.request.group_name = group
    
        return moveit_goal

    def retreat_move(self, height, velocity, forced=False):
        if forced:
            self.is_forced_retreat = True
        cart_pos, cart_quat = self.current_ee_pose(self.ee_frame, '/ellipse_frame')
        ell_pos, ell_quat = self.ellipsoid.pose_to_ellipsoidal((cart_pos, cart_quat))
        #print "Retreat EP:", ell_pos
        latitude = ell_pos[0]
        if self.trim_retreat:
            latitude = min(latitude, TRIM_RETREAT_LATITUDE)
        #rospy.loginfo("[face_adls_manager] Retreating from current location.")

        retreat_pos = [latitude, ell_pos[1], height]
        retreat_pos = self.ellipsoid.enforce_bounds(retreat_pos)
        retreat_quat = [0,0,0,1]
        if forced:
            cart_path = self.ellipsoid.ellipsoidal_to_pose((retreat_pos, retreat_quat))
            cart_msg = [PoseConv.to_pose_msg(cart_path)]
        else:
            ell_path = self.ellipsoid.create_ellipsoidal_path(ell_pos,
                                                              ell_quat,
                                                              retreat_pos,
                                                              retreat_quat,
                                                              velocity=0.01,
                                                              min_jerk=True)
            cart_path = [self.ellipsoid.ellipsoidal_to_pose(ell_pose) for ell_pose in ell_path]
            cart_msg = [PoseConv.to_pose_msg(pose) for pose in cart_path]
            
        head = Header()
        head.frame_id = '/ellipse_frame'
        head.stamp = rospy.Time.now()
        pose_array = PoseArray(head, cart_msg)
        self.pose_traj_pub.publish(pose_array)

        self.is_forced_retreat = False

 def get_ros_header(self):
     header = Header()
     header.stamp = rospy.Time.now()
     header.seq = self.sequence
     # http://www.ros.org/wiki/geometry/CoordinateFrameConventions#Multi_Robot_Support
     header.frame_id = self.frame_id
     return header

    def test_4_remote(self):
        '''
        Tests remote kill by publishing hearbeat, stopping and checking alarm is raised, then
        publishing hearbeat again to ensure alarm gets cleared.
        '''
        # publishing msg to network
        pub = rospy.Publisher('/network', Header, queue_size=10)
        rate = rospy.Rate(10)
        t_end = rospy.Time.now() + rospy.Duration(1)
        while rospy.Time.now() < t_end:
            h = Header()
            h.stamp = rospy.Time.now()
            pub.publish(h)
            rate.sleep()

        self.reset_update()
        rospy.sleep(8.5)  # Wait slighly longer then the timeout on killboard
        self.assert_raised()

        self.reset_update()
        t_end = rospy.Time.now() + rospy.Duration(1)
        while rospy.Time.now() < t_end:
            h = Header()
            h.stamp = rospy.Time.now()
            pub.publish(h)
            rate.sleep()
        self.AlarmBroadcaster.clear_alarm()
        self.assert_cleared()

    def _cloud_cb(self, cloud):
        points = np.array(list(read_points(cloud)))
        if points.shape[0] == 0:
            return
        
        pos = points[:,0:3]
        cor = np.reshape(points[:,-1], (points.shape[0], 1))

        # Get 4x4 matrix which transforms point cloud co-ords to odometry frame
        try:
            points_to_map = self.tf.asMatrix('/lasths', cloud.header)
        except tf.ExtrapolationException:
            return

        transformed_points = points_to_map.dot(np.vstack((pos.T, np.ones((1, pos.shape[0])))))
        transformed_points = transformed_points[:3,:].T

        self.seq += 1
        header = Header()
        header.seq = self.seq
        header.stamp = rospy.Time.now()
        header.frame_id = '/lasths'

        self.cloud = np.vstack((self.cloud, np.hstack((transformed_points, cor))))
        if self.seq % 30 == 0:
            print "plup!"
            self.cloud = np.zeros((0, 4))

        output_cloud = create_cloud(header, cloud.fields, self.cloud)
        self.cloud_pub.publish(output_cloud)

def make_header(frame_id, stamp=None):
    if stamp == None:
        stamp = rospy.Time.now()
    header = Header()
    header.stamp = stamp
    header.frame_id = frame_id
    return header