简单的代码让turtlebot动起来II（使用kobuki底座）走三角形

接着上一篇turtlebot的直行和旋转。

ROS中，节点之间通过消息沟通，主题相当于是对消息进行路由和管理的总线。节点向主题发布消息即表示一个节点在发送数据；节点订阅某个主题即可以接受其他节点的消息。在上一篇的直行和旋转当中，我们只写一个publisher，仅仅有话题的发布并没有话题的订阅。于是就有了尝试写一个subsrciber的想法，实现turtlebot稍微复杂一点的运动。

想要让turtlebot走一个三角形，那么同上一篇，首先我们需要向底座发布速度的信息，publisher有了。那么subscriber是什么呢？三角形有自己的边长，turtlebot要知道自己走了多远，就需要知道自己的里程计信息（转载的一篇里程计相关的文章有介绍），即订阅/odom主题。写一个订阅者的步骤：（1）初始化ROS系统（2）订阅话题（3）Spin，等待消息的到来（4）当一个消息到达时，相应的Callback()函数被调用。

有了思路就可以实现了，附上代码：

 // 头文件velcontrol.h  
  
#ifndef VELCONTROL_H  
#define VELCONTROL_H  
  
#include <ros/ros.h>  
#include <geometry_msgs/Twist.h>  
#include <geometry_msgs/TwistStamped.h>  
  
#include <nav_msgs/Odometry.h>  
#include "tf/LinearMath/Matrix3x3.h"  
#include "geometry_msgs/Quaternion.h"  
#define CV_PI   3.1415926535897932384626433832795  
  
#include <iostream>  
#include <stdlib.h>  
  
using namespace std ;  
  
typedef struct roPos  
{  
   double  x;  
   double  y;  
   double  theta;  
   void init(double x1, double x2, double x3)  
   {  
     x = x1;  
     y = x2;  
     theta = x3;  
   }  
} roPos;  
  
class velControl  
{  
public:  
    velControl();  
    roPos  start_pos_;  
    roPos  curr_pos_;  
  
  
protected:  
  
    void odomCallback(const nav_msgs::Odometry::ConstPtr& msg);  
  
private:  
    double rect_width_;  
    double rect_height_;  
    double offset_xy_;  
    double offset_theta_;  
  
    double vel_line_;  
    double vel_angle_;  
  
    int state_;  
    bool Is_Fininsh_;  
  
    ros::NodeHandle handle_;  
    ros::Publisher  vel_pub_ ;  
    ros::Subscriber sub_ ;  
    geometry_msgs::Twist  controlVel_;  
  
private:  
    double angleWrap(double angle);  
    bool init();