ROS 發佈和訂閱自定義消息數組

今天學習使用ros發佈和訂閱自定義的消息以及消息數組，記錄，備忘！
說明：
以ubuntu14.04，indigo版本爲例
ros工做空間位置：~/workspace/catkin_ws
定義的消息數組爲：
std_msgs/Header header
geometry_msgs/Pose[] path
其中Header是由uint32 seq，time stamp，string frame_id組成，它的消息定義在/opt/ros/indigo/share/std_msgs/msg 目錄下有詳細說明，而Pose是包含了Point和Quaternion類型組成，Point是3維的座標點，Quaternion是四元數，他們的具體說明在/opt/ros/indigo/share/geometry_msgs/msg目錄下。Pose[] 則表示後面定義的path是一個Pose類型的數組。接下來上代碼：

cd ~/workspace/catkin_ws/src
#建立功能包
catkin_create_pkg mytest std_msgs rospy roscpp
cd mytest/
mkdir msg && cd msg

新建mypath.msg 的文件，將上面我想建立的兩行消息粘貼進來，保存退出。打開mytest目錄下的package.xml文件，在下面添加兩行：

<build_depend>message_generation</build_depend>
  <exec_depend>message_runtime</exec_depend>

保存退出，而後打開mytest目錄下的CMakeLists.txt文件：

1. 在find_package中添加geometry_msgs message_generation，
2. 取消add_message_files和FILES以及下面反括號的註釋，在下面添加一行mypath.msg
3. 取消generate_messages所在部分的註釋，將geometry_msgs添加進去
4. 取消catkin_package所在部分的註釋，在std_msgs後面添加message_runtime

而後從新catkin_make編譯一下，沒有報錯。進入mytest/src目錄下，新建發佈消息節點mytalk.cpp，將下面發佈消息代碼粘貼進去：

#include <ros/ros.h>
#include <geometry_msgs/Pose.h>
#include "mytest/mypath.h"

int main(int argc, char **argv)
{
  ros::init(argc, argv, "emmm");
  ros::NodeHandle n;
  ros::Publisher pub = n.advertise<mytest::mypath>("talking", 1000);
  ros::Rate loop_rate(10);
  while (ros::ok())
  {
    mytest::mypath P;
    geometry_msgs::Pose p1, p2;
    p1.position.x = 1;
    p1.position.y = 2;
    p1.position.z = 3;
    p1.orientation.x = 0;
    p1.orientation.y = 0;
    p1.orientation.z = 0;
    p1.orientation.w = 1;

    p2.position.x = 4;
    p2.position.y = 5;
    p2.position.z = 6;
    p2.orientation.x = 0;
    p2.orientation.y = 0;
    p2.orientation.z = 1;
    p2.orientation.w = 1;

    std::vector<geometry_msgs::Pose> arrp;
    arrp.push_back(p1);
    arrp.push_back(p2);
    arrp.push_back(p1);
    arrp.push_back(p2);
    arrp.push_back(p1);
    arrp.push_back(p2);
    arrp.push_back(p1);

    P.path = arrp;
    pub.publish(P);
    ros::spinOnce();
    loop_rate.sleep();
  }

  return 0;
}

再新建接受消息節點mylisten.cpp，添加以下代碼：

#include <ros/ros.h>
#include <geometry_msgs/Pose.h>
#include "mytest/mypath.h"

void msgCallback(const mytest::mypath::ConstPtr &P)
{
  ROS_INFO("I recevied the topic: ");
  for(std::vector<geometry_msgs::Pose>::const_iterator it = P->path.begin(); it != P->path.end(); ++it)
  {
    std::cout << it->position.x << " " << it->position.y << " " << it->position.z << " " << it->orientation.x << " " << it->orientation.y << " " << it->orientation.z << " " << it->orientation.w << std::endl;
  }
}

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

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

  ros::spin();
  return 0;
}

而後在CMakeLists.txt中添加下面四行，保存退出，而後catkin_make從新編譯，就OK了。（**注意，下面四行必定不要添加太靠前，不然當你運行這個節點的時候系統會提示在xxx目錄下找不到可執行文件…，記得儘可能粘貼到後面，緣由多是前面的find_package，add_message_files，generate_messages，catkin_package等部分會添加依賴，路徑一類的操做，若是把add_executable等操做添加到他們前面，可能會由於找不到某些文件路徑或者依賴庫致使在運行時報錯）而後我並無定義」mytest/mypath.h」的頭文件，爲何就能夠直接include這個頭文件而不報錯呢，由於前面在CMakeLists.txt中的一系列操做會生成名爲「mypath.h」的頭文件，不信的話能夠在catkin_ws目錄下find一下：

find ./ -name mypath.h

結果顯示：./devel/include/mytest/mypath.h,它包含的頭文件實際上是在這個目錄下的。

add_executable(mytalk src/mytalk.cpp)
target_link_libraries(mytalk ${catkin_LIBRARIES})
add_executable(mylisten src/mylisten.cpp)
target_link_libraries(mylisten ${catkin_LIBRARIES})

add_executable(mytalk src/mytalk.cpp)這一行的意思時將src/mytalk.cpp文件生成名爲mytalk的可執行文件，而生成的可執行文件最後放在devel/lib/mytest目錄下，每次rosrun 的時候其實就是運行的這個可執行文件。target_link_libraries(mytalk ${catkin_LIBRARIES})的意思是將可執行文件mytalk連接catkin_ws空間下的庫中，pwd ${catkin_LIBRARIES}一下，終端顯示：/home/sun/workspace/catkin_ws 其實${catkin_LIBRARIES}就指的是catkin_ws目錄。到最後就是驗證消息的接受與發佈了。

roscore
rosrun mytest mytalk
rosrun mytest mylisten

效果，mytalk終端沒有輸出，mylisten終端顯示以下：

參考連接：
http://www.javashuo.com/article/p-glzsnunl-dc.html
http://wiki.ros.org/cn/ROS/Tutorials/CreatingMsgAndSrv