Python Odometry.child_frame_id方法代码示例

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

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

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

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

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

# 需要导入模块: from nav_msgs.msg import Odometry [as 别名]
# 或者: from nav_msgs.msg.Odometry import child_frame_id [as 别名]
def getOdometry(odometry_line):
    # Timestamp [seconds.microseconds]
    # Rolling Counter [signed 16bit integer]
    # TicksRight [ticks]
    # TicksLeft [ticks]
    # X [m]*
    # Y [m]*
    # Theta [rad]*
    # => 1235603339.32339, 4103, 2464, 2559, 1.063, 0.008, -0.028
    str_x, str_y, str_th = odometry_line.split(', ')[4:7]
    current_time = rospy.Time.now()
    th = float(str_th)
    x = float(str_x)
    y = float(str_y)
    z = 0.0
    roll = 0
    pitch = 0
    yaw = th
    qx, qy, qz, qw = tf.transformations.quaternion_from_euler(roll, pitch, yaw)

    # BUILDING ODOMETRY
    msg = Odometry()
    msg.header.stamp = current_time
    msg.header.frame_id = 'odom'
    msg.child_frame_id = 'base_link'
    msg.pose.pose.position = Point(x, y, z)
    msg.pose.pose.orientation = Quaternion(qx, qy, qz, qw)

    return msg

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

# 需要导入模块: from nav_msgs.msg import Odometry [as 别名]
# 或者: from nav_msgs.msg.Odometry import child_frame_id [as 别名]
	def __update_odometry(self, linear_offset, angular_offset, tf_delay):
		self.__heading = (self.__heading + angular_offset) % 360
		
		q = tf.transformations.quaternion_from_euler(0, 0, math.radians(self.__heading))
		self.__pose.position.x += math.cos(math.radians(self.__heading)) * self.__distance_per_cycle * self.__twist.linear.x
		self.__pose.position.y += math.sin(math.radians(self.__heading)) * self.__distance_per_cycle * self.__twist.linear.x
		self.__pose.position.z = 0.33
		self.__pose.orientation = Quaternion(q[0], q[1], q[2], q[3])

		now = rospy.Time.now() + rospy.Duration(tf_delay)

		self.__tfb.sendTransform(
			(self.__pose.position.x, self.__pose.position.y, self.__pose.position.z),
			q,
			now,
			'base_link',
			'odom')

		o = Odometry()
		o.header.stamp = now
		o.header.frame_id = 'odom'
		o.child_frame_id = 'base_link'
		o.pose = PoseWithCovariance(self.__pose, None)

		o.twist = TwistWithCovariance()
		o.twist.twist.linear.x = self.__distance_per_second * self.__twist.linear.x
		o.twist.twist.angular.z = math.radians(self.__rotation_per_second) * self.__twist.angular.z

# 需要导入模块: from nav_msgs.msg import Odometry [as 别名]
# 或者: from nav_msgs.msg.Odometry import child_frame_id [as 别名]
def post_odometry(self, component_instance):
    """ Publish the data of the Pose as a Odometry message for fake localization 
    """
    parent_name = component_instance.robot_parent.blender_obj.name
    component_name = component_instance.blender_obj.name
    frame_id = self._properties[component_name]['frame_id']
    child_frame_id = self._properties[component_name]['child_frame_id']

    odometry = Odometry()
    odometry.header.stamp = rospy.Time.now()
    odometry.header.frame_id = frame_id
    odometry.child_frame_id = child_frame_id

    odometry.pose.pose.position.x = component_instance.local_data['x']
    odometry.pose.pose.position.y = component_instance.local_data['y']
    odometry.pose.pose.position.z = component_instance.local_data['z']

    euler = mathutils.Euler((component_instance.local_data['roll'],
                             component_instance.local_data['pitch'],
                             component_instance.local_data['yaw']))
    quaternion = euler.to_quaternion()
    odometry.pose.pose.orientation = quaternion

    for topic in self._topics:
        # publish the message on the correct topic    
        if str(topic.name) == str("/" + parent_name + "/" + component_name):
            topic.publish(odometry)

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

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

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

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

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

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