本文整理汇总了Python中nav_msgs.msg.Odometry.twist方法的典型用法代码示例。如果您正苦于以下问题：Python Odometry.twist方法的具体用法？Python Odometry.twist怎么用？Python Odometry.twist使用的例子？那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类nav_msgs.msg.Odometry的用法示例。
在下文中一共展示了Odometry.twist方法的2个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的Python代码示例。
示例1: __update_odometry
# 需要导入模块: from nav_msgs.msg import Odometry [as 别名]
# 或者: from nav_msgs.msg.Odometry import twist [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示例2: publish_pose_stamped
# 需要导入模块: from nav_msgs.msg import Odometry [as 别名]
# 或者: from nav_msgs.msg.Odometry import twist [as 别名]
def publish_pose_stamped(self):
if self.base_odom == None:
return
try:
(trans,rot) = self.listener.lookupTransform(self.map_frame, self.base_odom.child_frame_id, rospy.Time(0)) # current map->base transform
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
return
pub_msg = Odometry()
pub_msg.pose.pose.position.x = trans[0]
pub_msg.pose.pose.position.y = trans[1]
pub_msg.pose.pose.position.z = trans[2]
pub_msg.pose.pose.orientation.x = rot[0]
pub_msg.pose.pose.orientation.y = rot[1]
pub_msg.pose.pose.orientation.z = rot[2]
pub_msg.pose.pose.orientation.w = rot[3]
pub_msg.header.stamp = rospy.Time.now()
pub_msg.header.frame_id = self.map_frame
pub_msg.child_frame_id = self.base_odom.child_frame_id
# calculate covariance
# use covariance of base_odom.pose and only transform it to new coords from tf
# TODO: calc covariance in correct way, but what is that ...?
base_odom_homogeneous_matrix = self.make_homogeneous_matrix([self.base_odom.pose.pose.position.x, self.base_odom.pose.pose.position.y, self.base_odom.pose.pose.position.z],
[self.base_odom.pose.pose.orientation.x, self.base_odom.pose.pose.orientation.y, self.base_odom.pose.pose.orientation.z, self.base_odom.pose.pose.orientation.w])
new_odom_homogeneous_matrix = self.make_homogeneous_matrix(trans, rot)
base_to_new_transform = numpy.dot(base_odom_homogeneous_matrix, numpy.linalg.inv(new_odom_homogeneous_matrix))
rotation_matrix = base_to_new_transform[:3, :3]
original_pose_cov_matrix = numpy.matrix(self.base_odom.pose.covariance).reshape(6, 6)
pose_cov_matrix = numpy.zeros((6, 6))
pose_cov_matrix[:3, :3] = (rotation_matrix.T).dot(original_pose_cov_matrix[:3, :3].dot(rotation_matrix))
pose_cov_matrix[3:6, 3:6] = (rotation_matrix.T).dot(original_pose_cov_matrix[3:6, 3:6].dot(rotation_matrix))
pub_msg.pose.covariance = numpy.array(pose_cov_matrix).reshape(-1,).tolist()
# twist is local and transform same as covariance
new_velocity = numpy.dot(rotation_matrix, numpy.array([[self.base_odom.twist.twist.linear.x],
[self.base_odom.twist.twist.linear.y],
[self.base_odom.twist.twist.linear.z]]))
new_omega = numpy.dot(rotation_matrix, numpy.array([[self.base_odom.twist.twist.angular.x],
[self.base_odom.twist.twist.angular.y],
[self.base_odom.twist.twist.angular.z]]))
original_twist_cov_matrix = numpy.matrix(self.base_odom.twist.covariance).reshape(6, 6)
twist_cov_matrix = numpy.zeros((6, 6))
twist_cov_matrix[:3, :3] = (rotation_matrix.T).dot(original_twist_cov_matrix[:3, :3].dot(rotation_matrix))
twist_cov_matrix[3:6, 3:6] = (rotation_matrix.T).dot(original_twist_cov_matrix[3:6, 3:6].dot(rotation_matrix))
pub_msg.twist = TwistWithCovariance(Twist(Vector3(*new_velocity[:, 0]), Vector3(*new_omega[:, 0])), twist_cov_matrix.reshape(-1,).tolist())
self.pub.publish(pub_msg)