机器人操作系统ROS:从入门到放弃(三)　发布接收不同消息2

上一讲我们讲了如何发布接收int，float和array类型的消息，这些也都算是c++自有的消息，被归纳在ROS中的std_msgs这个命名空间下（命名空间skr啥？）．这讲我们来看看如何发布接收ROS自己独特的message类型:PoseStamped．Pose好理解，就是机器人的位姿(position and orientation)，那么Stamped呢？Stamped表示时间戳(timestamped)，这儿时间戳是指时间戳是指格林威治时间1970年01月01日00时00分00秒起至现在的总纳秒．几乎所有的计算机都可以使用这个时间，方便统一．所以PoseStamped记录的是机器人的位姿加上记录位姿的时间这么一种message．这个message被包含在geometry_msgs这个大类下．所以你可以先搜ROS, geometry message查看．第一个网页应该就是下面这个
http://wiki.ros.org/geometry_msgs
里面又有一大堆不同类型的消息，你以后都可能用到．但这儿我们就用PoseStamped举例了，位姿加时间还是比较有代表性．
找到页面中的PoseStamped并点击进入，会发现下面的内容

poseStampedDef.png
你如果想使用这个消息，那么看到这个页面，根据我们之前讲的内容，你至少可以做下面这几件事
1:　包含头文件#include "geometry_msgs/PoseStamped.h"，这样我们才可以使用这个类型的消息
2: 　通过geometry_msgs::PoseStamped msg定义一个叫msg的对象，该对象可以使用上图中的header,pose两个数据成员．即你可以通过msg.header,msg.pose来调用数据类型为std_msgs/Header,geometry_msgs/Pose的两个数据成员．

但是那个header和pose具体是什么东西我们不清楚．但之前说了，任何复杂数据类型都是由简单的组成的，页面下的浅蓝色字体是链接可以点进去，咱们先点击std_msgs/Header看看他是个什么东西．你会看到下图内容．

headerDef.png
uint32和string就是基础变量了．通过Compact Message Definition可以知道，如果我们通过std_msgs::Header msg_header创建了一个叫msg_header类型的对象的话，那么它可以通过msg_header.seq调用类型为unit32的成员seq．也就是说，代码里你可以用类似于

int a = 1 , b;
msgs_header.seq = 1; 
//or
b = msg_header.seq;

这种代码给msgs_header.seq赋值或者把它赋值给其他整型变量．可能你要问了，seq定义的不是uint32么，我对a,b的定义就是int，会不会出问题？有可能．这时候又要用到下面这个网站了
http://wiki.ros.org/msg
找到uint32对应的c++定义是什么，发现是uint32_t，所以你直接用，uint32_t 定义 a,b是最好的．int是整型，uint32_t是无符号32位整型，两者的范围不一样，这个可以自己上网查，但是a,b在他们交集的范围内就不会改变具体的数值．总之同理你可以给msg_header.frame_id赋值string类型的变量.
但这个time是个什么东西呢？标准数据类型可没有这个玩意儿．不过上面的Raw Message Definition给出解释了，stamp.sec返回从epoch开始的秒为单位的时间，stamp.nsec返回从stamp_sec开始的纳秒时间．
msgs_heder.stamp调用stamp，stamp.sec调用sec得到epoch的时间，那么msgs_header.stamp.sec就可以获取当前的时间，秒为单位
通常我们肯定需要更精确的时间即包含纳秒的，假设我们想把当前的精确时间赋值给变量store_time，那么代码类似于下面这样．

double store_time;
store_time = msg_header.stamp.sec + 1e-9*msg_header.stamp.nsec; //unit s
//or
store_time = msgs_header.stamp.sec * 1e9 + msg_header.stamp.nsec; //unit ns

nsec的单位是纳秒，我们要乘以1e-9才能转换为秒，第二行得到的是时间以秒为单位．第四行得到的时间是以ns为单位．

好了，header说了这么多，目前我们想要这儿的stamp记录一个时间．最开始我们定义了geometry_msgs::PoseStamped msg．
msg包含数据成员header，而heaer作为std_msgs/Header的数据成员包含stamp，那么我们可以通过msg.header.stamp调用数据类型为time的成员stamp，为什么能这么做，我们下一张会讲到．ROS可以很方便获取当前时间ros::Time::now()，返回的是time类型的变量．所以我们可以通过

msg.header.stamp = ros::Time::now()

来获取当前时间并储存在这个message中．在赋值之后，如果想要取出我记录那个位姿时的时间并且是double类型呢？根据上面的内容，很容易可以得到．

double store_time;
store_time = msg.header.stamp.sec + 1e-9*msg.header.stamp.nsec; //unit s 
//or
store_time =  msg.header.stamp.sec * 1e9 +  msg.header.stamp.nsec; //unit ns

注意不要以为msg.header 等于msg_header哈...只是上面的例子我恰巧给std_msgs::Header对象取名叫msg_header罢了．张三李四都可以．msg.header是geometry_msgs::PoseStamped的对象msg调用数据成员header，而msg_header是std_msgs::Header的对象．
说完header，我们可以看msg的另一个数据成员pose了．同样，我们点击第一张图片中的浅蓝色字体geometry_msgs/Pose看一看它具体是由什么基本变量构成的．点击进去之后我们进入了Pose的定义的界面．Pose的数据成员是position和orientation，非常好理解，位置和方向．他们俩的数据类型分别是geometry_msgs/Point和geometry_msgs/Quaternion．这俩也不是基本数据类型，你可以继续点进去直到变量只有基本数据类型．这儿为了简洁，我直接画了个图，把PoseStamped包含的成员一一列了出来．

geometry_msgs.png
看这个图就一目了然了．（免费用了人家的东西得打个广告，这个图是用一个叫ProcessOn的在线软件画出来的，该软件用来画流程图，思维图，结构图什么的非常方便）．
Header部分已经讲过．咱们继续看geometry/msgs/Pose类型的数据pose所包含的成员
１是geometry_msgs/Point类型的position，而position包含三个float64的变量x,y,z，这个很好理解了，怎么定义机器人的位置？三维坐标x,y,z就可以了．
2是geometry_msgs/Quaternion类型的'oreintation'，而oreintation包含四个float64的变量x,y,z,w，quaterion中文四元数，是一个用来表示方向的东西．四元数的缺点是不很形象，不熟的人很难直接通过四元数在脑海里构想出机器人目前到底是个什么朝向．我们一般用欧拉角表示方向时，一共有三个数roll,pitch,yaw，比较直观，但是欧拉角表示方向时会遇到一个叫Gimbal Lock（万向锁）的尴尬问题，所以ROS里统一用四元数表示方向．ROS当然提供了一些API把四元数转换成欧拉角或者旋转矩阵之类的，但是在储存pose的时候，都统一用四元数储存．如果不清楚四元数的同学可以网上查一下，很多资料．
数据的结构说完了，怎么使用呢？和msg.header.stamp.sec调用int32类型的成员sec一样，咱们可以用msg.pose.position.x调用或者赋值float64的成员x．直接用代码来说明．接下来我们要建立一个叫pub_poseStamped.cpp的文件和sub_poseStamped.cpp的文件．第一个文件咱们来人工产生一些pose，恩，，，我相信很多同学手上没有直接可以和ROS沟通的传感器用的．产生了这些pose后我们把他们发布出去用接收器接收．
和之前一样，我们先在pub_sub_test/src中创建一个名字叫pub_poseStamped.cpp的文件．在文件中写入下面的内容．

#include "ros/ros.h"
#include "geometry_msgs/PoseStamped.h" //include posestamp head file

#include <cmath>//for sqrt() function

int main(int argc, char **argv)
{
    ros::init(argc, argv, "talker");

    ros::NodeHandle n;

    ros::Publisher chatter_pub = n.advertise<geometry_msgs::PoseStamped>("chatter", 10); //initialize chatter

    ros::Rate loop_rate(10);

    //generate pose by ourselves.
    double positionX, positionY, positionZ;
    double orientationX, orientationY, orientationZ, orientationW;

    //We just make the robot has fixed orientation. Normally quaternion needs to be normalized, which means x^2 + y^2 + z^2 +w^2 = 1
    double fixedOrientation = 0.1;
    orientationX = fixedOrientation ;
    orientationY = fixedOrientation ;
    orientationZ = fixedOrientation ;
    orientationW = sqrt(1.0 - 3.0*fixedOrientation*fixedOrientation); 

    double count = 0.0;
    while (ros::ok())
    {
        //We just make the position x,y,z all the same. The X,Y,Z increase linearly
        positionX = count;
        positionY = count;
        positionZ = count;

        geometry_msgs::PoseStamped msg; 

        //assign value to poseStamped

            //First assign value to "header".
        ros::Time currentTime = ros::Time::now();
        msg.header.stamp = currentTime;

            //Then assign value to "pose", which has member position and orientation
        msg.pose.position.x = positionX;
        msg.pose.position.y = positionY;
        msg.pose.position.z = positionY;

        msg.pose.orientation.x = orientationX;
        msg.pose.orientation.y = orientationY;
        msg.pose.orientation.z = orientationZ;
        msg.pose.orientation.w = orientationW;

        ROS_INFO("we publish the robot's position and orientaion"); 
        ROS_INFO("the position(x,y,z) is %f , %f, %f", msg.pose.position.x, msg.pose.position.y, msg.pose.position.z);
        ROS_INFO("the orientation(x,y,z,w) is %f , %f, %f, %f", msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z, msg.pose.orientation.w);
        ROS_INFO("the time we get the pose is %f",  msg.header.stamp.sec + 1e-9*msg.header.stamp.nsec);

        std::cout<<"\n \n"<<std::endl; //add two more blank row so that we can see the message more clearly

        chatter_pub.publish(msg);

        ros::spinOnce();

        loop_rate.sleep();

        ++count;
    }


  return 0;
}

咱们在发布器的代码中fixedOrientation的变量，赋值0.1，然后分别赋值给创建的double类型变量orientationX,Y,Z,W．在循环中，orientationX,Y,Z,W在分别赋值给我们创建的msg的成员变量msg.pose.orientation.x y z w．为什么能这么赋值？通过上面数据结构那张图我们了解到的msg.pose.orientation.x y z w都是float64类型的变量，赋值了他们几个就可以定义pose的orientation了．orientation是相同的数，那么机器人就没有旋转．
那么pose的position的x y z我们直接赋值了count，count在循环中递增，那么XYZ都同时递增且相同．如果我们画一个三维坐标轴XYZ的话，那么咱么这儿模拟的机器人的运动状态，相当于机器人沿着坐标轴对角线匀速直线行驶．
同样位置再创建一个sub_poseStamped.cpp．写入下面内容．

#include "ros/ros.h"
#include "geometry_msgs/PoseStamped.h" 

void chatterCallback(const geometry_msgs::PoseStamped::ConstPtr& msg) //Note it is geometry_msgs::PoseStamped, not std_msgs::PoseStamped
{
    ROS_INFO("I heard the pose from the robot"); 
    ROS_INFO("the position(x,y,z) is %f , %f, %f", msg->pose.position.x, msg->pose.position.y, msg->pose.position.z);
    ROS_INFO("the orientation(x,y,z,w) is %f , %f, %f, %f", msg->pose.orientation.x, msg->pose.orientation.y, msg->pose.orientation.z, msg->pose.orientation.w);
    ROS_INFO("the time we get the pose is %f",  msg->header.stamp.sec + 1e-9*msg->header.stamp.nsec);

    std::cout<<"\n \n"<<std::endl; //add two more blank row so that we can see the message more clearly
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "listener");

    ros::NodeHandle n;

    ros::Subscriber sub = n.subscribe("chatter", 10, chatterCallback);

    ros::spin();

    return 0;
}

写完之后，我们同样打开位于pub_sub_test目录下的CMakeLists.txt添加编译两个文件的内容．保存退出后使用catkin_make编译（注意catkin_make这个命令要在catkin_ws这个目录下使用的）．

结果编译出错了！！错误大概长下面的样子

error_poseStamped.png

读不懂啥意思啊．仔细对比看看前面的内容和你之前编译pub_int8, pub_string等文件时有没有什么出入？经过仔细的检查，发现　木有．所有步骤都是一毛一样的．那么问题出在哪儿呢？
记得我们当初创建pub_sub_test这个package的时候，我们是这么创建的

catkin_create_pkg pub_sub_test  std_msgs  roscpp rospy

我们说三个依赖项std_msgs roscpp rospy就好像C++的头文件一样，如果我们要使用ROS自带的std_msgs这个包，我们就需要添加这个依赖项．那么现在问题很明显了，我们缺少添加geometry_msgs这个依赖项．可以想象，如果当初我们创建package时用的下面的内容，就不会有什么问题了

catkin_create_pkg pub_sub_test  std_msgs geometry_msgs  roscpp rospy

可是我们package已经创建了，又不能重新创建一个相同名字的package把依赖项加进去．那该怎么办呢？
当我们需要添加新的依赖项时，我们需要修改两个文件，一个是package目录下的CMakeLists.txt，另一个是位于同一位置的package.xml
打开CMakeLists.txt，发现就在最前面几行，有下面的内容．

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
)

括号中的内容正好一一对应我们创建package时添加的依赖项，那么想都不用想啦，肯定要在后面添加geometry_msgs，变成下面的样子，保存退出．

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  geometry_msgs
)

添加完成后，保存退出．之后打开位于同一目录下的package.xml．（直接双击打开可能不能修改其中内容，还是用gedit什么的打开）．打开之后，在文档下方，你可以看到一下内容

  <build_depend>roscpp</build_depend>
  <build_depend>rospy</build_depend>
  <build_depend>std_msgs</build_depend>
  <build_export_depend>roscpp</build_export_depend>
  <build_export_depend>rospy</build_export_depend>
  <build_export_depend>std_msgs</build_export_depend>
  <exec_depend>roscpp</exec_depend>
  <exec_depend>rospy</exec_depend>
  <exec_depend>std_msgs</exec_depend>

啊哈，又有std_msgs, rospy, roscpp，只不过每个出现了三次，那么同样，不用管什么意思，我们只需要按照这个文档里相同的语法让geometry_msgs出现三次就行了．更改之后该文件同样位置变成下面的内容

  <build_depend>roscpp</build_depend>
  <build_depend>rospy</build_depend>
  <build_depend>std_msgs</build_depend>
  <build_depend>geometry_msgs</build_depend>
  <build_export_depend>roscpp</build_export_depend>
  <build_export_depend>rospy</build_export_depend>
  <build_export_depend>std_msgs</build_export_depend>
  <build_export_depend>geometry_msgs</build_export_depend>
  <exec_depend>roscpp</exec_depend>
  <exec_depend>rospy</exec_depend>
  <exec_depend>std_msgs</exec_depend>
  <exec_depend>geometry_msgs</exec_depend>

保存退出．这时候再用catkin_make编译，就成功了．改变上面两个文档的内容就相当于我们在创建package时添加了依赖项geometry_msgs．编译成功之后，跑程序的方法和以前的例子一样．

总结

这一讲我们讲述了一个稍微复杂一点的message类型:poseStamped．我们需要通过ROS wiki的帮助知道它包含什么数据成员，了解它包含的数据成员之后，利用类对象引用数据成员的方法(就像msg.pose.orientation)的方法，就可以调用或者赋值相应类型的成员了．因为我们手上没有传感器，我们自己产生了double类型的变量赋值，从最底层float64 x y z之类的开始赋值，模拟机器人的运动．但其实如果一个好的定位传感器自己有ROS的接口的话，很可能直接产生geometry_msgs/Point类型的变量，假设名字叫currentPosition，那么我们通过msg.pose.position = currentPosition给pose的位置赋值就可以了，因为pose.position也是geometry_msgs/Point类型的变量．这个大家接触具体的传感器就知道了．

另外这一讲讲了如何添加新的ROS依赖项．

那么目前我们总共聊了怎么发布string,int8,array和poseStamped．关于不同类型的消息，我们就讲到这儿了．希望大家以后面对自己想要发布的不同类型的消息的时候，知道在ROS wiki中怎么找到并看懂相应的资料．

下下讲我们会介绍如何在类中发布和接收消息．在这之前呢，为了照顾到语言不那么熟的同学，我们可能下讲就先回顾一下C++类的内容．还会提到命名空间和模版，并且和ROS中出现的语法对应起来．语言熟悉的同学就可以跳过下一讲了．