Day9 Topic in Cpp

今天要来讲怎麽用 cpp 写一个 Topic， Topic 是一种异步的通讯方式，一般来说每一个节点间的分工都是很明确的，很少会出现问题，有的负责送，有的负责收，有的负责处理，所以在一些基础的感测器上，是很适合 Topic 的，毕竟他功能简单，不容易出错。

接下来我会以 GPS 定位来当作范例，一个节点模拟发送一个 GPS 讯息，然後另一个节点则是负责接收与处理，已使方式算出他距离一点的距离。

开始建立

首先，我们需要依照这个步骤来完成我们的这个任务，首先我们需要去创立一个 package，然後根据这个我们的需求来定义一个 msg 讯息格式，接下来就可以开始写程序了，在写後就可以开始编译，这样就完成了。

• package
• msg
• talker.cpp
• listener.cpp
• CMakeList.txt & package.xml

package

首先我们需要建立一个套件包

cd ~/catkin_ws/src
catkin_create_pkg topic_demo roscpp rospy std_msgs

msg

首先需要先创建资料夹跟档案

cd topic_demo
mkdir msg
cd msg
nano gps.msg

接下来要来定义一个通讯格式，下面程序码是要放在 gps.msg 里面的，我们要做的是 GPS 所以一定有 X 跟 Y，还有他的状态，所以下面按照我们的想法来做定义

float32 x
float32 y
string state

这边先提一下，在编译以後，会自动生成一个 gps.h ，它的位置在 ~/catkin_ws/devel/include/topic_demo/gps.h 里面，我们以後要用的时候就是去 include 他。

talker.cpp

这边开始呢是创建一个发送讯号的档案

//ROS 的套件
#include <ros/ros.h> 
// 刚刚定义的 gps msg 
#include <topic_demo/gps.h> 

int main(int argc, char **argv)
{
  //初始化参数，第三个参数位置是节点名称
  ros::init(argc, argv, "talker");    

  // 创建一个把手，让我们可以在後面轻松地来处理节点
  ros::NodeHandle nh; 

  // 初始化 gps 讯息
  topic_demo::gps msg;
  msg.x = 1.0;
  msg.y = 1.0;
  msg.state = "working";

  //建立一个publisher ， 他的类型是 topic_demo 这个类型 ， 他的 topic 名称为 gps_info ，後面的1 是指暂存资料的队列的长度
  ros::Publisher pub = nh.advertise<topic_demo::gps>("gps_info", 1);

  //定义发布讯息的频率
  ros::Rate loop_rate(1.0);
  
  //一直发讯息
  while (ros::ok())
  {
    //以指数的方式成长，每个 1 秒发送一次讯息
    msg.x = 1.05 * msg.x ;
    msg.y = 1.05 * msg.y;
    ROS_INFO("Talker: GPS: x = %f, y = %f ",  msg.x ,msg.y);
    
    //以1Hz的频率发 msg
    pub.publish(msg);
    
    //根据前面的定义长度来 sleep 1秒
    loop_rate.sleep();//根据前面设定的时间 loop_rate ,来进行休眠~
  }

  return 0;
} 

其中 ros::ok 指的是，只要 ros 还可以跑，就继续跑

listener.cpp

在写完发送的 cpp 後，我们要开始写接收的 cpp 了~

//ROS 的套件
#include <ros/ros.h>
//刚刚定义的 gps msg 
#include <topic_demo/gps.h>
//ROS 内建的 msg 档案
#include <std_msgs/Float32.h>

void gpsCallback(const topic_demo::gps::ConstPtr &msg)

{  
    //计算它距离 原点  (0,0) 的距离
    std_msgs::Float32 distance; //创建一个变数 叫做距离，他的型态是 std_msgs::Float32 ， 也可以用 c++ 原版的 float
    distance.data = sqrt(pow(msg->x,2)+pow(msg->y,2));
    
     //如果你是用 原版的 float 的话用这句
    // float distance = sqrt(pow(msg->x,2)+pow(msg->y,2));
    
    //把 log 给印出来
    ROS_INFO("Listener: Distance to origin = %f, state: %s",distance.data,msg->state.c_str());
}

int main(int argc, char **argv)
{
  //初始化参数，第三个参数位置是节点名称
  ros::init(argc, argv, "listener");
  
  // 创建一个把手，让我们可以在後面轻松地来处理节点
  ros::NodeHandle n;
  
  //建立一个 Subscriber 订阅者 ， 第一个参数我们要监听的是哪一个 topic ， 後面的1 是指暂存资料的队列的长度 ， 第三个参数是指说，我们的消息接收後要给哪个 function 去处理
  ros::Subscriber sub = n.subscribe("gps_info", 1, gpsCallback);
  
  //ros::spin() 用来调用 所有 可以跑得 function。并进入 loop ，不会返回值
  
  ros::spin(); 
  return 0;
}

其中要注意的是 ConstPtr 指的是常量指标， 他是用来传递一些常量(程序再跑的过程中不会改变的值称为常量)的指标，而使用指标的原因在於有时候数据是很大的，复制来复制去很麻烦，那还不如直接指过去还比较快。

然後接下来是在 distance 的地方，因为我这边用的是 ROS 内建的 float 型态，所以我们的 distance 需要去指向里面的 data 才能放资料的，因为在 ROS 的 float32 他的架构其实是，里面还有一层 data，而 data 才是真正拿来存资料的地方。

接下来来讲里面 pow(msg->x,2) 的 msg->x 指的是说，我们去拿 msg 里面的 x 来用。

然後接下来我们要回来看在 Subscriber 的时候，他并非是来一个做一个，而是需要去 调用 spin，再调用後发现我们的队列里面有人在排队了，就会把开始做事，把队列清空。而 spin 他是会卡住的，也就是说他会一直在那边侦测，值到有讯息才会去把讯息丢给 function 去跑。而有一个叫做 spinOnce 的东西，他跟 spin 刚好相反，他只检查一次，检查发现没有事情，他就走了，直接继续跑後面的程序码。

CMakeList.txt & package.xml

最後我们要来修改这两个档案~ 在他们的身上留下我们的痕迹，这样系统在编译的时候才会去编译我们自己写的东西。

所以要记得修改这两个档案。

CMakeList.txt

首先修改 CMakeList.txt

首先在 find_package 要加上 message_generation，他可以帮我们生成自定义的 msg 文件。接下来是 在 增加这一行，来把我们自定义的 msg 给加上去

 add_message_files(
   FILES
   gps.msg
 )

generate_messages(DEPENDENCIES std_msgs)

再来是加上这一句，有了这一行他才会去真正的生成，一个 msg 的对应 .h 档

最後把 listen 跟 talker 给加上去 ， 让他们可以去找到他们的可执行目标跟所需要的套件

add_executable(talker src/talker.cpp )
add_dependencies(talker topic_demo_generate_messages_cpp) //必须有这个才会生成 msg
target_link_libraries(talker ${catkin_LIBRARIES})

add_executable(listener src/listener.cpp )
add_dependencies(listener topic_demo_generate_messages_cpp)
target_link_libraries(listener ${catkin_LIBRARIES})

package.xml

最後把这个给修改了就完成了~

<build_depend>message_generation</build_depend>
<run_depend>message_runtime</run_depend>

最後

你只要回到你的工作空间，来 编译 跟 执行 rosrun 就完成了~

今天我们讲完了 Ros topic cpp 的部分，接下来我们继续讲其他通讯方式