接着上一篇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();