iT邦幫忙

2021 iThome 鐵人賽

DAY 9
0
Arm Platforms

ROS with Deep learning 開發與實戰系列 第 9

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 的部分,接下來我們繼續講其他通訊方式


上一篇
Day 8 ROS Client Library 與 Roscpp
下一篇
Day 10 ROS Cpp Service
系列文
ROS with Deep learning 開發與實戰11

尚未有邦友留言

立即登入留言