ROS自定義msg類型及使用

一、創建msg消息

參考：
CreatingMsgAndSrv

首先創建一個空的package單獨存放msg類型（當然也可以在任意的package中自定義msg類型）
這裏爲便於說明，建立一個名爲test_msgs的包，用於對自定義msg類型的用法舉例

$ cd catkin_ws/src
$ catkin_create_pkg test_msgs

1.新建msg文件

然後在test_msgs中創建msg文件夾，在msg文件夾其中新建一個名爲Test.msg消息類型文件

$ cd test_msgs
$ mkdir msg

Test.msg的內容如下，
基本類型可參考: std_msgs、common_msgs

float32[] data
float32 vel
geometry_msgs/Pose pose
string name

2.修改package.xml

接下來需要message_generation生成C++或Python能使用的代碼，需要message_runtime提供運行時的支持，所以package.xml中添加以下兩句

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

3.修改CMakeLists.txt

CMakeLists.txt要注意四個地方

(1)首先調用find_package查找依賴的包，必備的有roscpp rospy message_generation，其他根據具體類型添加，比如上面的msg文件中用到了geometry_msgs/Pose pose類型，那麼必須查找geometry_msgs

find_package(catkin REQUIRED COMPONENTS roscpp rospy message_generation std_msgs geometry_msgs)

(2)然後是add_message_files，指定msg文件

add_message_files(
  FILES
  Test.msg
  # Message2.msg
)

(3)然後是generate_messages，指定生成消息文件時的依賴項，比如上面嵌套了其他消息類型geometry_msgs，那麼必須註明

#generate_messages必須在catkin_package前面
generate_messages(
 DEPENDENCIES
 geometry_msgs
)

(4)然後是catkin_package設置運行依賴

catkin_package(

CATKIN_DEPENDS message_runtime

)

到這裏新的msg類型test_msgs/Test就可以使用了，下面編譯這個包，然後利用rosmsg show指令查看

$ cd catkin_ws
$ catkin_make
$ rosmsg show test_msgs/Test 
float32[] data
float32 vel
geometry_msgs/Pose pose
  geometry_msgs/Point position
    float64 x
    float64 y
    float64 z
  geometry_msgs/Quaternion orientation
    float64 x
    float64 y
    float64 z
    float64 w
string name

二、msg的使用

要使用自定義的消息類型必須source自定義消息所在的工作空間，否則rosmsg show test_msgs/Test和rostopic echo /test_msg(/test_msg是節點中使用自定義消息類型test_msgs/Test的topic)都會報錯，因爲沒有source的情況下自定義消息類型是不可見的，被認爲是未定義類型

一種典型的錯誤是刪掉工作空間下的devel和build文件夾之後重新編譯代碼，此時由於沒有source自定義消息所在的工作空間，即使使用自定義消息的代碼和自定義消息在同一個包也無法找到，該消息頭文件，此時需要運行source devel/setup.bash之後重新編譯就好

1.其他包調用自定義msg類型

參考：
DefiningCustomMessages
如果是在test_msgs包內的節點中調用test_msgs/Test類型，只需要在.cpp文件中如下調用即可

#include <test_msgs/Test.h>

test_msgs::Test msg;

如果是在其他包調用test_msgs/Test類型則需要修改package.xml和CMakeLists.txt，比如同樣在工作空間catkin_ws內有一個名爲test的包，我們可以在這個包內寫一個節點，使用我們剛纔自定義的消息類型test_msgs/Test，如下：

(1)修改package.xml
養成好習慣，維護軟件包清單的更新，以便於別人使用你的軟件前安裝各種依賴項，當然這個文件不影響程序編譯

<build_depend>roscpp</build_depend>
<run_depend>roscpp</run_depend>

<build_depend>test_msgs</build_depend>
<run_depend>test_msgs</run_depend>

(2)修改CMakeLists.txt
調用自定義消息類型主要修改兩個地方，以下是重點：
一是find_package中需要聲明查找包含該消息類型的包；
二是add_dependencies要註明該消息的依賴，其他地方和普通節點一樣

find_package(catkin REQUIRED COMPONENTS
  roscpp
  geometry_msgs
  test_msgs
)

add_dependencies(test1 test_msgs_gencpp)#調用同一工作空間的自定義消息類型時註明依賴關係，防止發生頭文件找不到的報錯

如果缺少add_dependencies中對test_msgs_gencpp的依賴聲明，在編譯的時候如果先編譯test包再編譯test_msgs包則會出現如下報錯（ROS工作空間各個軟件包的編譯順序是隨機的），因爲頭文件test_msgs/Test.h還未生成

fatal error: test_msgs/Test.h: 沒有那個文件或目錄
 #include "test_msgs/Test.h"

2.msg類型數組的使用

需要說明的是std_msgs中有些數組用法和C++標準庫中不一樣
比如Float32MultiArray，其定義如下：

std_msgs/MultiArrayLayout layout
float32[] data

其中data是一個浮點數組，但是方括號只是一個用來表明它是數組的符號，我們不能在定義的時候在方括號中給定數組長度，實際上ROS中類似float32[]，int8[]這樣的數組類型都是std::vector，使用方法也和std::vector一樣

測試代碼可以從這裏下載
下面是一個消息訂閱節點，用於測試

/************************  
 * @Author: Jinglin Zhang  
 * @DateTime: 2017-06-07 19:57:30  
 * @Description: 節點test1,訂閱了test_msgs包下talker節點發布的"test_msg"話題，用於測試test_msgs::Test消息類型  
************************/
#include <ros/ros.h>
#include <test_msgs/Test.h>

void msgCallback(const test_msgs::Test::ConstPtr &msg)
{
  //test_msgs::Test類型裏的float32[]數據傳到vector
  std::vector<float> array = msg->data;

  std::cout << "msg->data[0]=" << msg->data[0] << std::endl;
  std::cout << "msg->data.size=" << msg->data.size() << std::endl;
  std::cout << "msg->data=" << msg->data[0] << ", " << msg->data[1] <<  ", " << msg->data[2] << ", " <<  msg->data[3] << ", " <<  msg->data[4] << ", " <<  msg->data[5] << std::endl;

}

int main(int argc,char ** argv)
{
  ros::init(argc,argv,"test1");
  ros::NodeHandle n;

  ros::Subscriber msg_sub = n.subscribe("test_msg", 100, msgCallback);

  ros::spin();
  return 0;
}

下面是一個消息發佈節點，用於測試

/************************  
 * @Author: Jinglin Zhang  
 * @DateTime: 2017-06-07 20:03:35  
 * @Description: 節點talker，發佈"test_msg"話題，用於測試test_msgs::Test消息類型  
************************/ 
#include <ros/ros.h>
#include <test_msgs/Test.h>

int main(int argc, char **argv)
{
  ros::init(argc, argv, "msg_talker");
  ros::NodeHandle n;
  ros::Publisher msg_pub = n.advertise<test_msgs::Test>("test_msg", 1000);
  ros::Rate loop_rate(10);
  int count = 0;

  while (ros::ok())
  {
    test_msgs::Test msg;
    std::cout << "msg.data.size=" << msg.data.size() << std::endl;

    //用vector給float32[]數組賦值
    float array[3] = {1.1,1.2,0.3};
    std::vector<float> array1(array,array+3);
    msg.data = array1;
    std::cout << "msg.data3[0]=" << msg.data[0] << std::endl;
    std::cout << "msg.data3.size=" << msg.data.size() << std::endl;

    //下標訪問float32[]數組
    msg.data[0] = 0.1;
    std::cout << "msg.data3[0]=" << msg.data[0] << std::endl;


    float array4[4] = {1.0,2.0,0.3,6.6};
    std::vector<float> array41(array4,array4+4);
    msg.data = array41;
    std::cout << "msg.data4.size=" << msg.data.size() << std::endl;
    std::cout << "msg.data4=" << msg.data[0] << " " << msg.data[1] <<  " " << msg.data[2] << " " <<  msg.data[3] << std::endl;

    msg.data.push_back(5.5);
    std::cout << "msg.data[5]=" << msg.data[4] << std::endl;

    //使用迭代器
    msg.data.resize(6);
    std::cout << "msg.data6.size=" << msg.data.size() << std::endl;
    std::cout << "msg.data6=" ;
    for(std::vector<float>::iterator it = msg.data.begin(); it != msg.data.end(); ++it)
    {
      *it = 0.6;
      std::cout << *it << " ";
    }
    std::cout << std::endl;

    msg_pub.publish(msg);
    ros::spinOnce();
    loop_rate.sleep();
    ++count;
  }

  return 0;
}