最近在研究ROS 里的导航包,里面的Gmapping 算法要求有2 个 输入,一个是激光数据,通过/scan topic 输入,另一个是里程计信息。
那么如何获得里程计呢?
里程计包含2 方面的信息,一方面是位姿(位置和转角),另一方面是速度(前进速度和转向速度)。
速度的 获取:前进速度和转向速度
前进速度: 左右轮的平均速度, 这个是通过电机的编码器获取到的。编码器能记录一定时间内的转过的弧度数,根据这个看算出每个轮子的速度。
转向速度:根据左右轮的在给定时间内的弧度差计算得到
参考文章:http://blog.csdn.net/heyijia0327/article/details/47021861
位姿的获取:位置和姿态
位置的获取:根据上面的前进速度推算出位置。
姿态的获取:根据上面的转向速度推算出转角。
那么在真实的机器人身上是如何发布这个里程计信息呢?以ros_arduino_bridge 为例来说明。
适用于:ROS 上位机 + arduino 下位机 + 小车轮子(编码器) 这个硬件架构。
在ros_arduino_bridge\ros_arduino_python\nodes 下面实现了一个'arduino' ROS node ,
这个node 使用 self.myBaseController.poll() call 到base_controller.py 这个模块。
使用下面的code 来发布里程计(Odometry)
self.odomPub = rospy.Publisher('odom', Odometry, queue_size=5)
self.odomBroadcaster = TransformBroadcaster()
try:
left_enc, right_enc = self.arduino.get_encoder_counts()
except:
self.bad_encoder_count += 1
rospy.logerr("Encoder exception count: " + str(self.bad_encoder_count))
return
dt = now - self.then
self.then = now
dt = dt.to_sec()
# Calculate odometry
if self.enc_left == None:
dright = 0
dleft = 0
else:
dright = (right_enc - self.enc_right) / self.ticks_per_meter
dleft = (left_enc - self.enc_left) / self.ticks_per_meter
self.enc_right = right_enc
self.enc_left = left_enc
dxy_ave = (dright + dleft) / 2.0
dth = (dright - dleft) / self.wheel_track
vxy = dxy_ave / dt
vth = dth / dt
if (dxy_ave != 0):
dx = cos(dth) * dxy_ave
dy = -sin(dth) * dxy_ave
self.x += (cos(self.th) * dx - sin(self.th) * dy)
self.y += (sin(self.th) * dx + cos(self.th) * dy)
if (dth != 0):
self.th += dth
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(self.th / 2.0)
quaternion.w = cos(self.th / 2.0)
# Create the odometry transform frame broadcaster.
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
rospy.Time.now(),
self.base_frame, // 子坐标系
"odom" // 父坐标系
)
odom = Odometry()
odom.header.frame_id = "odom"
odom.child_frame_id = self.base_frame
odom.header.stamp = now
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.twist.twist.linear.x = vxy
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = vth
self.odomPub.publish(odom)
这里有一个疑问:为什么要不停的广播 odom->base_frame 之间的转换呢?原因是物体(子坐标系,也就是机器人本身 base_link) 相对于父坐标系odom 的位置是不停的变化的, 所以需要把这个位姿信息定时发送给tf.
那么既然base_link(robot 本身)相对于odom (这个fixed 坐标系)的位姿信息都告诉 tf 了, 为什么还要向/odom 个topic 发送Odemetry 信息呢?
关于Odometry 的获取是否可以有其他的方式呢?