ROS 位置反馈控制机器人

ROS 位置反馈控制机器人

上一篇中我们学习了如何发布Twist消息控制机器人运动特定距离，但是上一篇用的是基于延时的开环控制，并没有引入反馈，在实际情况中几乎不可能准确到达目标点。所以这一篇我们学习如何用位置反馈闭环控制机器人。

• 启动机器人
• 从终端发布Twist消息控制机器人
• 下一步

---

1.启动机器人

ROS中提供了了一个Odometry消息类型可以得到机器人的位置信息。先看一下该消息的具体内容

 rosmsg show nav_msgs/Odometry

可以看出Odometry中包含pose：三轴位置信息，三轴欧拉角（用四元数表示），Twist：三轴线速度，三轴角速度。这两个消息中都包含了与测量不确定度有关的方差信息，可以很方便地用来卡尔曼滤波。
先启动机器人：

roslaunch rbx1_bringup fake_turtlebot.launch

rosrun rviz rviz -d `rospack find rbx1_nav`/sim.rviz

然后启动odom_out_and_back.py：

rosrun rbx1_nav odom_out_and_back.py

结果如下：

和开环效果是一样的，因为这里是模拟的理想情况。

2.源码分析

import rospy
from geometry_msgs.msg import Twist, Point, Quaternion
import tf
from rbx1_nav.transform_utils import quat_to_angle, normalize_angle
from math import radians, copysign, sqrt, pow, pi

class