Python msg.Odometry类代码示例

 def publish_odom(self, robot):
     # Transform etc
     x = robot.x
     y = robot.y
     th = robot.th
     br = tf.TransformBroadcaster()
     quat = tf.transformations.quaternion_from_euler(0,0,th)
     odom_trans = TransformStamped()
     odom_trans.header.stamp = rospy.Time.now()
     odom_trans.header.frame_id = "/map"
     odom_trans.child_frame_id = "/base_link"
     odom_trans.transform.translation.x = x
     odom_trans.transform.translation.y = y
     odom_trans.transform.translation.z = 0.0
     odom_trans.transform.rotation = quat;
     #tf.TransformBroadcaster().SendTransform(R,t,time,child,parent)
     br.sendTransform((x,y,0.0), quat, odom_trans.header.stamp, odom_trans.child_frame_id, odom_trans.header.frame_id)
     
     # Publish Odom msg
     odom_msg = Odometry()
     odom_msg.header.stamp = rospy.Time.now()
     odom_msg.header.frame_id = "/map"
     odom_msg.child_frame_id = "/base_link"
     odom_msg.pose.pose.position.x = x
     odom_msg.pose.pose.position.y = y
     odom_msg.pose.pose.position.z = 0.0
     
     odom_msg.pose.pose.orientation.x = quat[0]
     odom_msg.pose.pose.orientation.y = quat[1]
     odom_msg.pose.pose.orientation.z = quat[2]
     odom_msg.pose.pose.orientation.w = quat[3]
     
     # publish
     self.odomPub.publish(odom_msg)

def callback(msg):
    pub = rospy.Publisher("odom_covar", Odometry)

    corrected = Odometry()
    corrected.header.seq = msg.header.seq
    corrected.header.stamp = rospy.Time.now()
    corrected.header.frame_id = msg.header.frame_id
    corrected.child_frame_id = "base_footprint"
    corrected.pose.pose = msg.pose.pose
    corrected.pose.covariance = [.01, 0, 0, 0, 0, 0,
                                 0, .01, 0, 0, 0, 0,
                                 0, 0, .01, 0, 0, 0,
                                 0, 0, 0, .01, 0, 0,
                                 0, 0, 0, 0, .01, 0,
                                 0, 0, 0, 0, 0, .01]

    corrected.twist.twist = msg.twist.twist
    corrected.twist.covariance = [.01, 0, 0, 0, 0, 0,
                                 0, .01, 0, 0, 0, 0,
                                 0, 0, .01, 0, 0, 0,
                                 0, 0, 0, .01, 0, 0,
                                 0, 0, 0, 0, .01, 0,
                                 0, 0, 0, 0, 0, .01]

    pub.publish(corrected)

 def onNavSts(self,data):   
     q = tf.transformations.quaternion_from_euler(math.pi, 0, math.pi/2)
     self.broadcaster.sendTransform((0,0,0), q.conjugate(), rospy.Time.now(), "uwsim_frame", "local");
     
     try:
         (trans,rot) = self.listener.lookupTransform("uwsim_frame", self.base_frame, rospy.Time(0))
         
         odom = Odometry();  
         odom.twist.twist.linear.x = data.body_velocity.x;
         odom.twist.twist.linear.y = data.body_velocity.y;
         odom.twist.twist.linear.z = data.body_velocity.z;
         odom.twist.twist.angular.x = data.orientation_rate.roll;
         odom.twist.twist.angular.y = data.orientation_rate.pitch;
         odom.twist.twist.angular.z = data.orientation_rate.yaw;
         odom.child_frame_id = self.base_frame; 
         odom.pose.pose.orientation.x = rot[0];
         odom.pose.pose.orientation.y = rot[1];
         odom.pose.pose.orientation.z = rot[2];
         odom.pose.pose.orientation.w = rot[3];
         odom.pose.pose.position.x = trans[0];
         odom.pose.pose.position.y = trans[1];
         odom.pose.pose.position.z = trans[2];
         odom.header.stamp = rospy.Time.now();
         odom.header.frame_id = "local";
         
         self.pub.publish(odom);
         
     except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
         None

def update_vel():
    "Main loop"""
    global g_last_loop_time, g_vel_x, g_vel_y, g_vel_dt, x_pos, y_pos

    #while not rospy.is_shutdown():
    current_time = rospy.Time.now()

    linear_velocity_x = g_vel_x
    linear_velocity_y = g_vel_y

    # Calculate current position of the robot
    x_pos += linear_velocity_x * g_vel_dt
    y_pos += linear_velocity_y * g_vel_dt

    # All odometry is 6DOF, so need a quaternion created from yaw
    odom_quat = tf.transformations.quaternion_from_euler(0, 0, 0)

    # next, we'll publish the odometry message over ROS
    odom = Odometry()
    odom.header.stamp = current_time
    odom.header.frame_id = "odom"

    # set the position
    odom.pose.pose = Pose(Point(x_pos, y_pos, 0.), Quaternion(*odom_quat))

    # set the velocity
    odom.child_frame_id = "base_footprint"
    odom.twist.twist = Twist(
        Vector3(linear_velocity_x, linear_velocity_y, 0), Vector3(0, 0, 0))

    # publish the message
    odom_pub.publish(odom)

    g_last_loop_time = current_time

	def Publish_Odom_Tf(self):
		
		
	#quaternion = tf.transformations.quaternion_from_euler(0, 0, theta)
		quaternion = Quaternion()
		quaternion.x = 0 
		quaternion.y = 0
		quaternion.z = math.sin(self.Heading / 2.0)
		quaternion.w = math.cos(self.Heading / 2.0)
			
		rosNow = rospy.Time.now()
		self._tf_broad_caster.sendTransform(
				(self.X_Pos, self.Y_Pos, 0), 
					(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
					rosNow,
					"base_footprint",
					"odom"
					)

		# next, we'll publish the odometry message over ROS
		odometry = Odometry()
		odometry.header.frame_id = "odom"
		odometry.header.stamp = rosNow
		odometry.pose.pose.position.x = self.X_Pos
		odometry.pose.pose.position.y = self.Y_Pos
		odometry.pose.pose.position.z = 0
		odometry.pose.pose.orientation = quaternion


		odometry.child_frame_id = "base_link"
		odometry.twist.twist.linear.x = self.Velocity
		odometry.twist.twist.linear.y = 0
		odometry.twist.twist.angular.z = self.Omega
		self._odom_publisher.publish(odometry)

def publish_Tfs():
    global tf_rot,tf_trans,tf_tda,tf_rda,tf_tbc,tf_rbc,scale
    br = tf.TransformBroadcaster()
    rate = rospy.Rate(50.0)
    while not rospy.is_shutdown():
        rate.sleep()
        camOdom = Odometry()
        camOdom.header.stamp = rospy.Time.now()
        camOdom.header.frame_id = "/odom_combined"
        camOdom.pose.pose.position.x =0
        camOdom.pose.pose.position.y =0
        camOdom.pose.pose.position.z =0. #Tbc[2]
        M=np.identity(4)
        q=tf.transformations.quaternion_from_matrix(M)
        camOdom.pose.pose.orientation.x = q[0]
        camOdom.pose.pose.orientation.y = q[1]
        camOdom.pose.pose.orientation.z = q[2]
        camOdom.pose.pose.orientation.w = q[3]
        camOdom.child_frame_id = "/base_footprint"
        camOdom.twist.twist.linear.x = 0
        camOdom.twist.twist.linear.y = 0
        camOdom.twist.twist.angular.z = 0
        T=np.array([0.,0.,0.])
        M=np.identity(4)
        q=tf.transformations.quaternion_from_matrix(M)
        br.sendTransform(T,q,
            rospy.Time.now(),
            "/base_footprint",
            "/odom_combined")
        camOdomPub.publish(camOdom)

    def publish_odom(self, cur_x, cur_y, cur_theta, vx, vth):
        quat = tf.transformations.quaternion_from_euler(0, 0, cur_theta)
        current_time = rospy.Time.now()

        br = tf.TransformBroadcaster()
        br.sendTransform((cur_x, cur_y, 0),
                         tf.transformations.quaternion_from_euler(0, 0, cur_theta),
                         current_time,
                         "base_footprint",
                         "odom")

        odom = Odometry()
        odom.header.stamp = current_time
        odom.header.frame_id = 'odom'

        odom.pose.pose.position.x = cur_x
        odom.pose.pose.position.y = cur_y
        odom.pose.pose.position.z = 0.0
        odom.pose.pose.orientation = Quaternion(*quat)

        odom.pose.covariance[0] = 0.01
        odom.pose.covariance[7] = 0.01
        odom.pose.covariance[14] = 99999
        odom.pose.covariance[21] = 99999
        odom.pose.covariance[28] = 99999
        odom.pose.covariance[35] = 0.01

        odom.child_frame_id = 'base_footprint'
        odom.twist.twist.linear.x = vx
        odom.twist.twist.linear.y = 0
        odom.twist.twist.angular.z = vth
        odom.twist.covariance = odom.pose.covariance

        self.odom_pub.publish(odom)

        def tf_and_odom(self, zumo_msg):
            """Broadcast tf transform and publish odometry.  It seems a little
            redundant to do both, but both are needed if we want to use the ROS
            navigation stack."""
    
            trans = (zumo_msg.x, zumo_msg.y, 0)
            rot = tf.transformations.quaternion_from_euler(0, 0, zumo_msg.theta)

            self.broadcaster.sendTransform(trans, rot, \
               rospy.Time.now(), \
               "base_link", \
               "odom")

            odom = Odometry()
            odom.header.frame_id = 'odom'
            odom.header.stamp = rospy.Time.now()
            odom.pose.pose.position.x = trans[0]
            odom.pose.pose.position.y = trans[1]
            odom.pose.pose.position.z = trans[2]
            odom.pose.pose.orientation.x = rot[0]
            odom.pose.pose.orientation.y = rot[1]
            odom.pose.pose.orientation.z = rot[2]
            odom.pose.pose.orientation.w = rot[3]
            odom.child_frame_id = 'base_link'
            odom.twist.twist = self.stored_twist

            self.odomPublisher.publish(odom)

    def poll(self):
        (x, y, theta) = self.arduino.arbot_read_odometry()

        quaternion = Quaternion()
        quaternion.x = 0.0 
        quaternion.y = 0.0
        quaternion.z = sin(theta / 2.0)
        quaternion.w = cos(theta / 2.0)
    
        # Create the odometry transform frame broadcaster.
        now = rospy.Time.now()
        self.odomBroadcaster.sendTransform(
            (x, y, 0), 
            (quaternion.x, quaternion.y, quaternion.z, quaternion.w),
            now,
            "base_link",
            "odom"
            )
    
        odom = Odometry()
        odom.header.frame_id = "odom"
        odom.child_frame_id = "base_link"
        odom.header.stamp = now
        odom.pose.pose.position.x = x
        odom.pose.pose.position.y = y
        odom.pose.pose.position.z = 0
        odom.pose.pose.orientation = quaternion
        
        odom.twist.twist.linear.x = self.forwardSpeed
        odom.twist.twist.linear.y = 0
        odom.twist.twist.angular.z = self.angularSpeed

        self.arduino.arbot_set_velocity(self.forwardSpeed, self.angularSpeed)

        self.odomPub.publish(odom)

    def _msg(self, quaternion, x_dist, y_dist, linear_speed, angular_speed, now, use_pose_ekf=False):
        # next, we will publish the odometry message over ROS
        msg = Odometry()
        msg.header.frame_id = "odom"
        msg.header.stamp = now
        msg.pose.pose = Pose(Point(x_dist, y_dist, 0.0), quaternion)

        msg.child_frame_id = "base_link"
        msg.twist.twist = Twist(Vector3(linear_speed, 0, 0), Vector3(0, 0, angular_speed))

        if use_pose_ekf:
            # robot_pose_ekf needs these covariances and we may need to adjust them.
            # From: ~/turtlebot/src/turtlebot_create/create_node/src/create_node/covariances.py
            # However, this is not needed because we are not using robot_pose_ekf
            # odometry.pose.covariance = [1e-3, 0, 0, 0, 0, 0,
            # 0, 1e-3, 0, 0, 0, 0,
            #                         0, 0, 1e6, 0, 0, 0,
            #                         0, 0, 0, 1e6, 0, 0,
            #                         0, 0, 0, 0, 1e6, 0,
            #                         0, 0, 0, 0, 0, 1e3]
            #
            # odometry.twist.covariance = [1e-3, 0, 0, 0, 0, 0,
            #                          0, 1e-3, 0, 0, 0, 0,
            #                          0, 0, 1e6, 0, 0, 0,
            #                          0, 0, 0, 1e6, 0, 0,
            #                          0, 0, 0, 0, 1e6, 0,
            #                          0, 0, 0, 0, 0, 1e3]
            pass

        return msg

def main():
	rospy.init_node("pose2d_to_odometry")

	own_num = rospy.get_param('~own_num', 0)
	
	global odom, head
	head = Header()
	head.stamp = rospy.Time.now()
	head.frame_id = "robot_{}/odom_combined".format(own_num)

	odom = Odometry()
	odom.child_frame_id = "robot_{}/base_footprint".format(own_num)
	o = 10**-9 # zero
	odom.pose.covariance = [1, 0, 0, 0, 0, 0,
							0, 1, 0, 0, 0, 0,
							0, 0, o, 0, 0, 0, # z doesn't change
							0, 0, 0, o, 0, 0, # constant roll
							0, 0, 0, 0, o, 0, # constant pitch
							0, 0, 0, 0, 0, o] # guess - .3 radians rotation cov
	odom.twist.covariance = [100, 0, 0, 0, 0, 0,
							 0, 100, 0, 0, 0, 0,
							 0, 0, 100, 0, 0, 0, # large covariances - no data
							 0, 0, 0, 100, 0, 0,
							 0, 0, 0, 0, 100, 0,
							 0, 0, 0, 0, 0, 100]



	global odom_pub
	odom_topic = rospy.get_param('~odometry_publish_topic', 'vo')
	pose2D_topic = rospy.get_param('~pose2D_topic', 'pose2D')
	odom_pub = rospy.Publisher(odom_topic, Odometry)
	rospy.Subscriber(pose2D_topic, Pose2D, on_pose)

	rospy.spin()

def publish_odom(event):
    global motor_controller, od
    pos = motor_controller.position
    covariance = Config.get_covariance_matrix()

    odom = Odometry()
    odom.header.frame_id = "odom"
    odom.child_frame_id = "base_link"

    if rospy.get_time() - pos.position_time > 50.0/1000:
        odom.header.stamp = rospy.Time.now()
    else:
        odom.header.stamp = rospy.Time.from_sec(pos.position_time)

    odom.pose.pose.position.x = pos.x
    odom.pose.pose.position.y = pos.y

    odom.pose.pose.orientation.z = math.sin(pos.yaw / 2)
    odom.pose.pose.orientation.w = math.cos(pos.yaw / 2)
    odom.pose.covariance = covariance

    odom.twist.twist.linear.x = pos.linear_vel
    odom.twist.twist.angular.z = pos.angular_vel
    odom.twist.covariance = covariance

    odom_publisher.publish(odom)

def talker(message):
    corrected = Odometry()
    corrected.header.seq = message.header.seq
    corrected.header.stamp = rospy.Time.now()
    corrected.header.frame_id = message.header.frame_id #base_footprint #message.header.frame_id
    corrected.child_frame_id = message.child_frame_id #"base_footprint"
    corrected.pose.pose = message.pose.pose

    corrected.pose.covariance = [0.05, 0, 0, 0, 0, 0,
                                 0, 0.05, 0, 0, 0, 0,
                                 0, 0, 0.05, 0, 0, 0,
                                 0, 0, 0, 0.001, 0, 0,
                                 0, 0, 0, 0, 0.001, 0,
                                 0, 0, 0, 0, 0, .5]

    corrected.twist.twist = message.twist.twist

    corrected.twist.covariance = [0.05, 0, 0, 0, 0, 0,
                                 0, 0.05, 0, 0, 0, 0,
                                 0, 0, 0.05, 0, 0, 0,
                                 0, 0, 0, 0.001, 0, 0,
                                 0, 0, 0, 0, 0.001, 0,
                                 0, 0, 0, 0, 0, .5]

    pub = rospy.Publisher('odom_w_cov', Odometry)
    pub.publish(corrected)

    def Talk(self, time):
        #print self.orientation.get_euler()['yaw'], self.position.yaw, self.sensors['gps'].position.yaw
        msgs = Odometry( )
        msgs.header.stamp = rospy.Time.now()
        msgs.header.frame_id = "/nav"

        msgs.child_frame_id = "/drone_local"

        msgs.pose.pose.position.x = self.position.x
        msgs.pose.pose.position.y = self.position.y
        msgs.pose.pose.position.z = self.position.z
        
        msgs.pose.pose.orientation.x = self.orientation.x
        msgs.pose.pose.orientation.y = self.orientation.y
        msgs.pose.pose.orientation.z = self.orientation.z
        msgs.pose.pose.orientation.w = self.orientation.w
        
        msgs.twist.twist.linear.x = self.velocity.x
        msgs.twist.twist.linear.y = self.velocity.y
        msgs.twist.twist.linear.z = self.velocity.z

        #rospy.loginfo(msgs)
        
        self.publisher['state'].publish(msgs)

        self.tf_broadcaster['drone_local_tf'].sendTransform( (msgs.pose.pose.position.x, msgs.pose.pose.position.y, msgs.pose.pose.position.z) , 
            self.orientation.get_quaternion( ), 
            msgs.header.stamp,
            msgs.child_frame_id, 
            msgs.header.frame_id)

 def handle_imu(self, imu_data):
     
     if not self.imu_recv:
         
         rospy.loginfo('IMU data received')
         self.imu_recv = True
 
     if self.pub.get_num_connections() != 0:
 
       msg = Odometry()
       
       #msg.header.frame_id = imu_data.header.frame_id
       msg.header.frame_id = 'imu_frame'
       msg.header.stamp = imu_data.header.stamp
       
       msg.child_frame_id = 'imu_base_link'
       
       msg.pose.pose.position.x = 0.0 # here, we can maybe try to integrate accelerations...
       msg.pose.pose.position.y = 0.0
       msg.pose.pose.position.z = 0.0
       
       msg.pose.pose.orientation = imu_data.orientation # should we transform this orientation into base_footprint frame????
       
       msg.pose.covariance = [99999, 0,     0,     0,     0,     0, # large covariance on x, y, z
                              0,     99999, 0,     0,     0,     0,
                              0,     0,     99999, 0,     0,     0, 
                              0,     0,     0,     1e-06, 0,     0, # we trust in rpy
                              0,     0,     0,     0,     1e-06, 0,
                              0,     0,     0,     0,     0,     1e-06]
       
       # does robot_pose_ekf care about twist?
       
       self.pub.publish(msg)