Python Header.seq方法代码示例

# 需要导入模块: from std_msgs.msg import Header [as 别名]
# 或者: from std_msgs.msg.Header import seq [as 别名]
    def key_callback(self,msg):
        '''
        function to handle keyboard buttons
        '''
        key = str(msg.data)
        
        #rospy.loginfo("Heard keypress %s", key)
                    
        if key == "a":
            self.keyboard_frame_counter = self.keyboard_frame_counter + 1;

            controlMessage = Header()
            #command to add waypoint;
            controlMessage.seq = 1;
            controlMessage.frame_id = 'Frame_' + str(self.keyboard_frame_counter);
            self._command_pub.publish(controlMessage)

        if key == "d":
            
            controlMessage = Header()
            #command to delete waypoint;
            #deleting last waypoint
            controlMessage.seq = 2;
            controlMessage.frame_id = 'Frame_' + str(self.keyboard_frame_counter);
            self._command_pub.publish(controlMessage)
            if (self.keyboard_frame_counter>0):
                self.keyboard_frame_counter -= 1;
            
        if key == "\x13": #CTRL+ S button code           
            controlMessage = Header()
            #command to save map;
            controlMessage.seq = 3;
            self._command_pub.publish(controlMessage)

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

# 需要导入模块: from std_msgs.msg import Header [as 别名]
# 或者: from std_msgs.msg.Header import seq [as 别名]
 def HeaderGenerater(self):
  self.seq += 1
  header = Header()
  header.seq = self.seq
  header.stamp = rospy.Time.now()
  header.frame_id = self.GoalFrame
  return header

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

# 需要导入模块: from std_msgs.msg import Header [as 别名]
# 或者: from std_msgs.msg.Header import seq [as 别名]
    def publish_data(self):
        h = Header()
        h.stamp = rospy.Time.now()
        h.frame_id = self.frame_id
        h.seq = self.seq
        self.seq = self.seq + 1

        self.imu_msg.header = h

        q = self.device.read_quaternion()
        self.imu_msg.orientation.x = q[0]
        self.imu_msg.orientation.y = q[1]
        self.imu_msg.orientation.z = q[2]
        self.imu_msg.orientation.w = q[3]

        g = self.device.read_gyroscope()
        # convert from deg/sec to rad/sec
        self.imu_msg.angular_velocity.x = g[0] * math.pi / 180.0
        self.imu_msg.angular_velocity.y = g[1] * math.pi / 180.0
        self.imu_msg.angular_velocity.z = g[2] * math.pi / 180.0

        a = self.device.read_linear_acceleration()
        self.imu_msg.linear_acceleration.x = a[0]
        self.imu_msg.linear_acceleration.y = a[1]
        self.imu_msg.linear_acceleration.z = a[2]

        self.imu_pub.publish(self.imu_msg)

        self.temp_msg.header = h
        self.temp_msg.temperature = self.device.read_temp()
        self.temp_pub.publish(self.temp_msg)

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

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

# 需要导入模块: from std_msgs.msg import Header [as 别名]
# 或者: from std_msgs.msg.Header import seq [as 别名]
    def app_frame_add_callback(self,msg):
        rospy.loginfo("Heard App button press %s", msg.data)

        controlMessage = Header()
        #command to add waypoint;
        controlMessage.seq = 1;
        controlMessage.frame_id = msg.data;
        self._command_pub.publish(controlMessage)

# 需要导入模块: from std_msgs.msg import Header [as 别名]
# 或者: from std_msgs.msg.Header import seq [as 别名]
def make_header():
    """Returns a ROS header object with time, sequence and frame_id = 0"""
    global seq
    header = Header()
    header.seq = seq
    seq += 1
    time = rospy.get_rostime()
    header.stamp = time
    header.frame_id = "0"
    return header

# 需要导入模块: from std_msgs.msg import Header [as 别名]
# 或者: from std_msgs.msg.Header import seq [as 别名]
 def unpackOdomHeader(self, data):
     # Note this is not modular for frame_id's that are not 4 characters
     # This is written for the frame_id = 'odom'
     head = Header()
     unpk = unpack("!IIxxxxccccI", data)
     now = rospy.get_rostime()
     head.stamp.secs = int(now.secs)
     head.stamp.nsecs = int(now.nsecs)
     head.frame_id = unpk[2] + unpk[3] + unpk[4] + unpk[5]
     head.seq = unpk[6]
     return head

# 需要导入模块: from std_msgs.msg import Header [as 别名]
# 或者: from std_msgs.msg.Header import seq [as 别名]
def pubTaskSwitch(task_name, switch_seq, time_pub):
    pubTask = rospy.Publisher(
        '/task_switch', Header, queue_size=10)
    taskPubMsg = Header()
    taskPubMsg.frame_id = task_name
    taskPubMsg.seq = switch_seq

    count_idx = 0
    while count_idx < time_pub/0.2:
        pubTask.publish(taskPubMsg)
        time.sleep(0.2)
        count_idx = count_idx+1

# 需要导入模块: from std_msgs.msg import Header [as 别名]
# 或者: from std_msgs.msg.Header import seq [as 别名]
        def __init__(self):
            super(TestWindow, self).__init__()

            rospy.init_node('camLatencyPublisher')
            self.pubTime = rospy.Publisher("/camLatency", HeaderMSG)

            self.widget = GLPlotWidget()

            geo = QtGui.QDesktopWidget().screenGeometry(1)
            self.setGeometry(30, 30, geo.width()-100, geo.height()-100)
            self.setCentralWidget(self.widget)
            self.widget.repaint()
            self.showFullScreen()
            #self.show()

            r = rospy.Rate(4)
            for x in range(25*5):
                msg = HeaderMSG()
                msg.seq = x+1
                self.widget.repaint()
                t = rospy.Time.now()
                msg.stamp = t
                self.pubTime.publish(msg)
                r.sleep()
                if x%5:
                    rospy.sleep(1./25)
                if x==0:
                    self.widget.repaint()
                    rospy.sleep(3.)
                self.widget.toggle()

            msg = HeaderMSG()
            msg.seq = 0 # reset
            self.widget.repaint()
            t = rospy.Time.now()
            msg.stamp = t
            self.pubTime.publish(msg)
            sys.exit()

# 需要导入模块: from std_msgs.msg import Header [as 别名]
# 或者: from std_msgs.msg.Header import seq [as 别名]
 def to_twist_stamped_msg(*args):
     header, homo_mat, _, euler_rot = PoseConverter._make_generic(args)
     if homo_mat is None:
         rospy.logwarn("[pose_converter] Unknown pose type.")
         return None, None, None, None
     twist_stamped = TwistStamped()
     header_msg = Header()
     if header is None:
         header_msg.stamp = rospy.Time.now()
     else:
         header_msg.seq = header[0]
         header_msg.stamp = header[1]
         header_msg.frame_id = header[2]
     return TwistStamped(header_msg, Twist(Vector3(*homo_mat[:3,3].T.A[0]), Vector3(*euler_rot)))

# 需要导入模块: from std_msgs.msg import Header [as 别名]
# 或者: from std_msgs.msg.Header import seq [as 别名]
	def command_drive(data):
		# initialize the message components
		header = Header()
		foo  = TwistWithCovarianceStamped()
		bar = TwistWithCovariance()
		baz = Twist()
		linear = Vector3()
		angular = Vector3()

		# get some data to publish
		# fake covariance until we know better
		covariance = [1,0,0,0,0,0, 0,1,0,0,0,0, 0,0,1,0,0,0, 0,0,0,1,0,0, 0,0,0,0,1,0, 0,0,0,0,0,1]
		# to be overwritten when we decide what we want to go where
		linear.x = data.axes[1] * 15
		linear.y = 0
		linear.z = 0
		angular.x = 0
		angular.y = 0
		angular.z = data.axes[0] * 10
		
		# put it all together
		# Twist
		baz.linear = linear
		baz.angular = angular
		
		# TwistWithCovariance
		bar.covariance = covariance
		bar.twist = baz

		# Header
		header.seq = data.header.seq
		header.frame_id = frame_id
		header.stamp = stopwatch.now()
		# seq = seq + 1
		# TwistWithCovarianceStamped
		foo.header = header
		foo.twist = bar

		# publish and log
		rospy.loginfo(foo)
		pub.publish(foo)

# 需要导入模块: from std_msgs.msg import Header [as 别名]
# 或者: from std_msgs.msg.Header import seq [as 别名]
#!/usr/bin/env python

import rospy
from std_msgs.msg import Header
import sys

if __name__ == '__main__':
    if len(sys.argv) > 1:
        rate = float(sys.argv[1])
    else:
        rate = 10
    rospy.init_node('asfasdfa')
    r = rospy.Rate(rate)
    pub = rospy.Publisher('/test', Header, queue_size=1)
    seq = 0
    while not rospy.is_shutdown():
        h = Header()
        h.seq = seq
        seq += 1
        h.stamp = rospy.Time.now()
        pub.publish(h)
        r.sleep()