本文整理汇总了Python中nav_msgs.msg.Odometry类的典型用法代码示例。如果您正苦于以下问题：Python Odometry类的具体用法？Python Odometry怎么用？Python Odometry使用的例子？那么恭喜您, 这里精选的类代码示例或许可以为您提供帮助。
在下文中一共展示了Odometry类的20个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于我们的系统推荐出更棒的Python代码示例。
示例1: publish_odom
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)
开发者ID:Prrrr，项目名称:FroboMind，代码行数:34，代码来源:histogram_map.py
示例2: callback
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)
开发者ID:DanPierce，项目名称:gavlab_atrv，代码行数:25，代码来源:odom_covariance.py
示例3: onNavSts
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
开发者ID:burt1991，项目名称:labust-ros-pkg，代码行数:29，代码来源:NavSts2Odom.py
示例4: update_vel
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
开发者ID:CalPolyRobotics，项目名称:IGVC-ROS，代码行数:34，代码来源:wheel_odometry.py
示例5: Publish_Odom_Tf
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)
开发者ID:BruceLiu1130，项目名称:mastering_ros，代码行数:34，代码来源:launchpad_process_node+(copy).py
示例6: publish_Tfs
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)
开发者ID:BlueWhaleRobot，项目名称:orb_init，代码行数:30，代码来源:temp.py
示例7: publish_odom
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)
开发者ID:code-iai，项目名称:roboclaw_ros，代码行数:34，代码来源:roboclaw_node.py
示例8: tf_and_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)
开发者ID:BOTSlab，项目名称:bupimo_src，代码行数:27，代码来源:zumo_comms_node.py
示例9: poll
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)
开发者ID:BOTSlab，项目名称:bupigo_catkin_src，代码行数:35，代码来源:arbot_controller.py
示例10: _msg
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
开发者ID:tslator，项目名称:arlobot_rpi，代码行数:30，代码来源:arlobot_odom_pub.py
示例11: main
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()
开发者ID:ajayjain，项目名称:husky_pursuit，代码行数:35，代码来源:pose2d_to_odometry.py
示例12: publish_odom
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)
开发者ID:clubcapra，项目名称:Ibex，代码行数:26，代码来源:smart_motor_node.py
示例13: talker
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)
开发者ID:DanPierce，项目名称:pointcloud_shiz，代码行数:26，代码来源:odom_cov.py
示例14: Talk
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)
开发者ID:sebascuri，项目名称:QUACS，代码行数:30，代码来源:ROS_SensorFusion.py
示例15: handle_imu
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)
开发者ID:westpoint-robotics，项目名称:pioneer3at_navigator，代码行数:33，代码来源:imu_to_odom.py
示例16: post_odometry
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)
开发者ID:sylvestre，项目名称:morse，代码行数:27，代码来源:pose.py
示例17: Talk
def Talk(self):
msgs = Odometry()
msgs.header.stamp = rospy.Time.now()
msgs.header.frame_id = '/nav'
msgs.child_frame_id = self.name
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['local_tf'].sendTransform( (msgs.pose.pose.position.x, msgs.pose.pose.position.y, msgs.pose.pose.position.z) ,
(msgs.pose.pose.orientation.x, msgs.pose.pose.orientation.y, msgs.pose.pose.orientation.z, msgs.pose.pose.orientation.w),
msgs.header.stamp,
msgs.child_frame_id,
msgs.header.frame_id)
开发者ID:NachoM，项目名称:QUACS，代码行数:29，代码来源:ROS_Odometry.py
示例18: pubOdometry
def pubOdometry(self, event):
odom = Odometry()
odom.header.stamp = rospy.Time.now()
odom.header.frame_id = str(self.frame_id)
odom.child_frame_id = "real_girona500"
odom.pose.pose.position.x = self.p[0]
odom.pose.pose.position.y = self.p[1]
odom.pose.pose.position.z = self.p[2]
orientation = tf.transformations.quaternion_from_euler(self.p[3], self.p[4], self.p[5], 'sxyz')
odom.pose.pose.orientation.x = orientation[0]
odom.pose.pose.orientation.y = orientation[1]
odom.pose.pose.orientation.z = orientation[2]
odom.pose.pose.orientation.w = orientation[3]
#Haig de comentar les velocitats sino l'UWSim no va
odom.twist.twist.linear.x = 0.0 #v_[0]
odom.twist.twist.linear.y = 0.0 #v_[1]
odom.twist.twist.linear.z = 0.0 #v_[2]
odom.twist.twist.angular.x = 0.0 #v_[3]
odom.twist.twist.angular.y = 0.0 #v_[4]
odom.twist.twist.angular.z = 0.0 #v_[5]
self.pub_odom.publish(odom)
# Broadcast transform
br = tf.TransformBroadcaster()
br.sendTransform((self.p[0], self.p[1], self.p[2]), orientation,
odom.header.stamp, odom.header.frame_id, "world")
开发者ID:snagappa，项目名称:girona500，代码行数:30，代码来源:dynamics.py
示例19: odom_transform_2d
def odom_transform_2d(self, data, new_frame, trans, rot):
# NOTES: only in 2d rotation
# also, it removes covariance, etc information
odom_new = Odometry()
odom_new.header = data.header
odom_new.header.frame_id = new_frame
odom_new.pose.pose.position.x = data.pose.pose.position.x + trans[0]
odom_new.pose.pose.position.y = data.pose.pose.position.y + trans[1]
odom_new.pose.pose.position.z = data.pose.pose.position.z + trans[2]
# rospy.loginfo('td: %s tr: %s' % (str(type(data)), str(type(rot))))
q = data.pose.pose.orientation
q_tuple = (q.x, q.y, q.z, q.w,)
# Quaternion(*(tft quaternion)) converts a tf-style quaternion to a ROS msg quaternion
odom_new.pose.pose.orientation = Quaternion(*tft.quaternion_multiply(q_tuple, rot))
heading_change = quaternion_to_heading(rot)
odom_new.twist.twist.linear.x = data.twist.twist.linear.x*math.cos(heading_change) - data.twist.twist.linear.y*math.sin(heading_change)
odom_new.twist.twist.linear.y = data.twist.twist.linear.y*math.cos(heading_change) + data.twist.twist.linear.x*math.sin(heading_change)
odom_new.twist.twist.linear.z = 0
odom_new.twist.twist.angular.x = 0
odom_new.twist.twist.angular.y = 0
odom_new.twist.twist.angular.z = data.twist.twist.angular.z
return odom_new
开发者ID:cwrucutter，项目名称:drive_stack，代码行数:27，代码来源:leader_nonlinear_force.py
示例20: __update_odometry
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
开发者ID:Knifa，项目名称:HexapodKit，代码行数:27，代码来源:walk_controller.py
注：本文中的nav_msgs.msg.Odometry类示例由纯净天空整理自Github/MSDocs等源码及文档管理平台，相关代码片段筛选自各路编程大神贡献的开源项目，源码版权归原作者所有，传播和使用请参考对应项目的License；未经允许，请勿转载。 |
客服电话
APP下载
官方微信
请发表评论