5. ROS编程入门--航迹推算（dead reckoning）

TASK

这篇教程主要是测量机器人的里程计误差的，让机器人重复的走固定的路线，在这里我让机器人走正方形，比较机器人的起始位置（理想状态是重合的） 以及 局部里程计（机器人自身测算得）和全局里程计的的位置（定位系统）。

仿真用的机器人里程计是准确的，因此这次实验要在自己的机器上进行。

Algorithm

DeadReckoning（航迹推算） 算法在 NodeDeadReckoningI 类中实现，在你发送指令之前，你需要知道机器人在局部里程计和全局里程计的位置。两个位置都知道后才能运动。在 NodeDeadReckoning 类里面需要需要两个变量 （globalOdomReceived and localOdomReceived）来判断是否接收到位置信息。
通过PID控制机器人直走多少米，转弯多少度，PID控制的反馈数据来源私有程计（local odometry：机器人自带).
机器人静止后，再次等待local和global的里程计并计算与起始点的误差。

输入处理

输入的是odometry消息。你只需要位置和航向角。坐标x，y存放在pose.pose.position.x,pose.pose.position.y，航向角（φ）和四元数z元素之间的关系如下：

φ=2⋅arcsin(Qz)(quaternion-angle)

方形路径跟随

当收到local和global的里程计数据后，调用commander()函数控制机器人。让机器人跑几圈后，等待接收里程计数据。‘

评估结果

如果global和local的数据都受到了调用evaluateResults()函数，起始点的误差需要计算x、y、φ的偏差。local里程计会相对于global里程计飘移。将local的初始位置和global的设定为一致。
θ记作local和global里程计的初始位置角度偏差

θ=φlocStart−φglobStart(odom-angle)

将系统1（local 里程计）坐标变换到系统0（global 里程计）下：

(x0 y0)=(cosθsinθ−sinθcosθ)(x1y1)

展开：

x0=x1cosθ−y1sinθ(x0)

y0=x1sinθ+y1cosθ(y0)

Program

创建包：

catkin_create_pkg dem_dead_reckoning roscpp rospy std_msgs
跳转到新产生的目录dem_dead_reckoning 下面，创建文件：

• include/node_deadReckoningI.h
• src/node_deadReckoningI.cpp

添加主程序：

• src/deadReckoningI.cpp

为了独立使用PID控制器，把上一节的内容拷贝到合理的文件夹下面：

• src/myPoint.cpp
• src/node_pid.cpp
• src/pid.cpp
• include/myPoint.h
• include/node_pid.h

修改CMakeLists.txt，添加下面内容 :

set (SRCS1 ${SRCS1} src/myPoint.cpp)set (SRCS1 ${SRCS1} src/node_pid.cpp)set (SRCS1 ${SRCS1} src/pid.cpp)set (SRCS2 ${SRCS2} src/node_deadReckoningI.cpp)set (SRCS2 ${SRCS2} src/deadReckoningI.cpp)include_directories(include ${catkin_INCLUDE_DIRS})add_executable(cmd ${SRCS1})target_link_libraries(cmd ${catkin_LIBRARIES})add_executable(deadReckoningReg ${SRCS2})target_link_libraries(deadReckoningReg ${catkin_LIBRARIES})

Code: node_deadReckoningI.h

#include "ros/ros.h"#include "nav_msgs/Odometry.h"#include <string>using namespace std;class NodeDeadReckoningI{public:  /* Constructor   * pub Publisher, which can send commands to robot.   * fw  Variable, which will be stored in forward.   * an  Variable, which will be stored in angle.   * num Variable, which will be stored in numberOfTrips.   */  NodeDeadReckoningI(int num, double fw, double an);  ~NodeDeadReckoningI();  /* This method receives global odometry data.   * If #evaluate = false, than this method processes   * start position.   * If #evaluate = true, than this method processes   * end position and start method evaluateResults()   *    * msg   Message with global odometry data   */  void messageCallback(const nav_msgs::Odometry::ConstPtr& msg);  /* This method is similar to messageCallback,   * but receives local odometry data.   *   * msg   Message with local odometry data.   */  void messageCallback2(const nav_msgs::Odometry::ConstPtr& msg);  /* This method controls movement of robot (Trips forward   * and turn around by using PID controller).   */  void commander();  /* Evaluate difference between start and end   * position in global odometry and compare   * global with local odometry.   */  void evaluateResults();  //variables  double forward;     // Distance to go forward  double angle;       // Angle to turn around  int numberOfTrips;  // How many times will robot go forward and turn.  //Initial position (Loc - local odometry)  geometry_msgs::Point start;  double startAngle;  geometry_msgs::Point startLoc;  double startLocAngle;  //Final position (Loc - local odometry)  geometry_msgs::Point end;  double endAngle;  geometry_msgs::Point endLoc;   double endLocAngle;  bool evaluate; // True if robot finished its trajectory  bool localOdomReceived;  // True, if local odometry is received  bool globalOdomReceived; // True, if global odometry is received};

Code: node_deadReckoningI.cpp

#include "node_deadReckoningI.h"#include <cstdlib>#define PI 3.141592654//Constructor and destructorNodeDeadReckoningI::NodeDeadReckoningI(int num, double fw, double an){  numberOfTrips = num;  forward = fw;  angle = an;}NodeDeadReckoningI::~NodeDeadReckoningI(){}//Subscribervoid NodeDeadReckoningI::messageCallback(const nav_msgs::Odometry::ConstPtr& msg){  if (globalOdomReceived == false && evaluate == false)  {    globalOdomReceived = true;    start = msg->pose.pose.position;    startAngle = 2.0*asin(msg->pose.pose.orientation.z);    if (localOdomReceived == true)    {      commander();    }  }  else if (evaluate)  {    globalOdomReceived = true;    end = msg->pose.pose.position;    endAngle = 2.0*asin(msg->pose.pose.orientation.z);    if (localOdomReceived == true)    {      evaluateResults();    }  }}//Subscribervoid NodeDeadReckoningI::messageCallback2(const nav_msgs::Odometry::ConstPtr& msg){  if (localOdomReceived == false && evaluate == false)  {    localOdomReceived = true;    startLoc = msg->pose.pose.position;    startLocAngle = 2.0*asin(msg->pose.pose.orientation.z);    if (globalOdomReceived == true)    {      commander();    }  }  else if (evaluate)  {    localOdomReceived = true;    endLoc = msg->pose.pose.position;    endLocAngle = 2.0*asin(msg->pose.pose.orientation.z);    if (globalOdomReceived == true)    {      evaluateResults();    }  }}void NodeDeadReckoningI::commander(){    stringstream sf;    sf << "./cmd -f " << forward;    stringstream sr;    sr << "./cmd -r " << angle;    for (int tripsDone = 0; tripsDone < numberOfTrips ; tripsDone++)    {      std::system(sf.str().c_str());      std::system(sr.str().c_str());    }    evaluate = true;    localOdomReceived = false;    globalOdomReceived = false;}void NodeDeadReckoningI::evaluateResults(){  //evaluating results    double diffx = end.x - start.x;    double diffy = end.y - start.y;    double diffAngle = endAngle - startAngle;    //TRANSFORMATION    double angleTransStart = startAngle - startLocAngle;    //How much is local angle different from global    double xStartLocNew = startLoc.x*cos(angleTransStart)-startLoc.y*sin(angleTransStart);    double yStartLocNew = startLoc.x*sin(angleTransStart)+startLoc.y*cos(angleTransStart);    double xEndLocNew = endLoc.x*cos(angleTransStart)-endLoc.y*sin(angleTransStart);    double yEndLocNew = endLoc.x*sin(angleTransStart)+endLoc.y*cos(angleTransStart);    double diffxLoc = xEndLocNew  - xStartLocNew ;    double diffyLoc = yEndLocNew - yStartLocNew ;    double diffAngleLoc = endLocAngle - startLocAngle;    while (fabs(diffAngle) > PI)    {      diffAngle -= copysign(2*PI,diffAngle);    }    //sending message    ROS_INFO("DIFFERENCE BETWEEN LOCAL AND GLOBAL ODOMETRY:\nDIFFERENCES IN AXIS:\nDIFF_X = %f\nDIFF_Y = %f\nDIFF_ANGLE = %f",diffxLoc-diffx,diffyLoc-diffy,diffAngleLoc-diffAngle);    ROS_INFO("DIFFERENCE FROM START POSITION:\nDIFERENCES IN AXIS:\nDIFF_X = %f\nDIFF_Y = %f\nDIFF_ANGLE = %f",diffx, diffy, diffAngle);    exit(0);}

Code: deadReckoningI.cpp

#include "node_deadReckoningI.h"#define SUBSCRIBER_BUFFER_SIZE 1  // Size of buffer for subscriber.#define NUMBER_OF_TRIPS 8 // How many times will robot go forward and turn#define FORWARD 0.6  // Distance to go forward.#define ANGLE 1.570796 // Robot will turn around by this angle#define SUBSCRIBER_TOPIC "/syros/global_odom"#define SUBSCRIBER2_TOPIC "/syros/base_odom"int main(int argc, char **argv){  //Initialization of node  ros::init(argc, argv, "deadReckoningI");  ros::NodeHandle n;  //Creating object, which stores data from sensors and has methods for subscribing  NodeDeadReckoningI *nodeDeadReckoningI = new NodeDeadReckoningI(NUMBER_OF_TRIPS, FORWARD, ANGLE);  //Creating subscribers  ros::Subscriber sub = n.subscribe(SUBSCRIBER_TOPIC, SUBSCRIBER_BUFFER_SIZE, &NodeDeadReckoningI::messageCallback, nodeDeadReckoningI);  ros::Subscriber sub2 = n.subscribe(SUBSCRIBER2_TOPIC, SUBSCRIBER_BUFFER_SIZE, &NodeDeadReckoningI::messageCallback2, nodeDeadReckoningI);  ros::spin();  return 0;}

编译

catkin_make

运行程序：

rosrun dem_dead_reckoning deadReckoningReg

译者注：粘贴代码时候一定注意在 .h文件里面添加防止重编译的宏定义
代码github地址: https://github.com/will1991/rosintrodution