編寫節點從ROS的FollowJointTrajectoryGoal主題訂閱路徑點資料

一、首先建一個包

工作空間catkin_ws事先建好了，路徑是/root/catkin_ws

然後運作以下指令在src檔案夾下建立test包

$ cd ~/catkin_ws/src
$ catkin_create_pkg test roscpp

注意在包的名字後加上一些基本的依賴，比如roscpp和rospy這樣就可以直接調用C++和python節點了，這将會在CMakeLists.txt中出現

find_package(catkin REQUIRED COMPONENTS roscpp)

/*test1.cpp
*建立時間：2018.3.1
*   作者：向建平
*/

   #include "ros/ros.h"  
//ros/ros.h是一個實用的頭檔案，它引用了ROS系統中大部分常用的頭檔案  
#include "std_msgs/String.h"  
//std_msgs/String存放在std_msgs package裡，由String.msg檔案自動生成的頭檔案
#include <control_msgs/FollowJointTrajectoryAction.h>  
#include <control_msgs/FollowJointTrajectoryActionGoal.h>
#include <control_msgs/FollowJointTrajectoryGoal.h>
#include <trajectory_msgs/JointTrajectory.h>
#include <trajectory_msgs/JointTrajectoryPoint.h>
  
//回調函數，當message topic消息到達後會調用此函數  control_msgs/FollowJointTrajectoryActionGoal
void execute(const  control_msgs::FollowJointTrajectoryActionGoal::ConstPtr& msg)
{  
    std::cout << "points[0]: "  
             << "[" << msg->goal.trajectory.points[0].positions[0]  
             << "," << msg->goal.trajectory.points[0].positions[1]  
             << "," << msg->goal.trajectory.points[0].positions[2]  
             << "," << msg->goal.trajectory.points[0].positions[3]  
             << "," << msg->goal.trajectory.points[0].positions[4]  
             << "," << msg->goal.trajectory.points[0].positions[5]  
             << "]" << std::endl ;

    std::cout << "points[1]: "  
             << "[" << msg->goal.trajectory.points[1].positions[0]  
             << "," << msg->goal.trajectory.points[1].positions[1]  
             << "," << msg->goal.trajectory.points[1].positions[2]  
             << "," << msg->goal.trajectory.points[1].positions[3]  
             << "," << msg->goal.trajectory.points[1].positions[4]  
             << "," << msg->goal.trajectory.points[1].positions[5]  
             << "]" << std::endl ;

std::cout << "points[2]: "  
             << "[" << msg->goal.trajectory.points[2].positions[0]  
             << "," << msg->goal.trajectory.points[2].positions[1]  
             << "," << msg->goal.trajectory.points[2].positions[2]  
             << "," << msg->goal.trajectory.points[2].positions[3]  
             << "," << msg->goal.trajectory.points[2].positions[4]  
             << "," << msg->goal.trajectory.points[2].positions[5]  
             << "]" << std::endl ;


std::cout << "points[3]: "  
             << "[" << msg->goal.trajectory.points[3].positions[0]  
             << "," << msg->goal.trajectory.points[3].positions[1]  
             << "," << msg->goal.trajectory.points[3].positions[2]  
             << "," << msg->goal.trajectory.points[3].positions[3]  
             << "," << msg->goal.trajectory.points[3].positions[4]  
             << "," << msg->goal.trajectory.points[3].positions[5]  
             << "]" << std::endl ;

std::cout << "points[4]: "  
             << "[" << msg->goal.trajectory.points[4].positions[0]  
             << "," << msg->goal.trajectory.points[4].positions[1]  
             << "," << msg->goal.trajectory.points[4].positions[2]  
             << "," << msg->goal.trajectory.points[4].positions[3]  
             << "," << msg->goal.trajectory.points[4].positions[4]  
             << "," << msg->goal.trajectory.points[4].positions[5]  
             << "]" << std::endl ;

 std::cout << "points[5]: "  
             << "[" << msg->goal.trajectory.points[5].positions[0]  
             << "," << msg->goal.trajectory.points[5].positions[1]  
             << "," << msg->goal.trajectory.points[5].positions[2]  
             << "," << msg->goal.trajectory.points[5].positions[3]  
             << "," << msg->goal.trajectory.points[5].positions[4]  
             << "," << msg->goal.trajectory.points[5].positions[5]  
             << "]" << std::endl ;    

std::cout << "points[6]: "  
             << "[" << msg->goal.trajectory.points[6].positions[0]  
             << "," << msg->goal.trajectory.points[6].positions[1]  
             << "," << msg->goal.trajectory.points[6].positions[2]  
             << "," << msg->goal.trajectory.points[6].positions[3]  
             << "," << msg->goal.trajectory.points[6].positions[4]  
             << "," << msg->goal.trajectory.points[6].positions[5]  
             << "]" << std::endl ;

std::cout << "points[7]: "  
             << "[" << msg->goal.trajectory.points[7].positions[0]  
             << "," << msg->goal.trajectory.points[7].positions[1]  
             << "," << msg->goal.trajectory.points[7].positions[2]  
             << "," << msg->goal.trajectory.points[7].positions[3]  
             << "," << msg->goal.trajectory.points[7].positions[4]  
             << "," << msg->goal.trajectory.points[7].positions[5]  
             << "]" << std::endl ;

std::cout << "points[8]: "  
             << "[" << msg->goal.trajectory.points[8].positions[0]  
             << "," << msg->goal.trajectory.points[8].positions[1]  
             << "," << msg->goal.trajectory.points[8].positions[2]  
             << "," << msg->goal.trajectory.points[8].positions[3]  
             << "," << msg->goal.trajectory.points[8].positions[4]  
             << "," << msg->goal.trajectory.points[8].positions[5]  
             << "]" << std::endl ;

std::cout << "points[9]: "  
             << "[" << msg->goal.trajectory.points[9].positions[0]  
             << "," << msg->goal.trajectory.points[9].positions[1]  
             << "," << msg->goal.trajectory.points[9].positions[2]  
             << "," << msg->goal.trajectory.points[9].positions[3]  
             << "," << msg->goal.trajectory.points[9].positions[4]  
             << "," << msg->goal.trajectory.points[9].positions[5]  
             << "]" << std::endl ;

}  
  
  
int main(int argc, char **argv)  
{  
    
    //初始化ROS，注意：名稱必須唯一  
    ros::init(argc, argv, "test1");  
  //監聽/arm_controller/follow_joint_trajectory/goal話題釋出的消息
    //設定節點程序句柄  
    ros::NodeHandle n;  
  
    //參數一為訂閱的話題名稱（/arm_controller/follow_joint_trajectory/goal）；  
    //參數二是消息緩存隊列，這裡設定為1000，即緩存了1000個消息後，如果由新的消息到達，則開始丢棄先前接收的消息；  
    //參數三為當收到消息後調用的函數名稱（execute）  
    ros::Subscriber sub = n.subscribe("/arm_controller/follow_joint_trajectory/goal", 1000, execute);  
      
    //ros::spin()進入自循環，可以盡可能快的調用消息回調函數  
    ros::spin();    
    return 0;  
}

注意：這個cpp輸出的是弧度制的角度，如果需要角度值需要轉換，用下面的cpp

/*test1.cpp
*建立時間：2018.3.1
*   作者：向建平
*/

   #include "ros/ros.h"  
//ros/ros.h是一個實用的頭檔案，它引用了ROS系統中大部分常用的頭檔案  
#include "std_msgs/String.h" 
#include <angles/angles.h>   //角度轉換的頭檔案
#include <cmath>
//std_msgs/String存放在std_msgs package裡，由String.msg檔案自動生成的頭檔案
#include <control_msgs/FollowJointTrajectoryAction.h>  
#include <control_msgs/FollowJointTrajectoryActionGoal.h>
#include <control_msgs/FollowJointTrajectoryGoal.h>
#include <trajectory_msgs/JointTrajectory.h>
#include <trajectory_msgs/JointTrajectoryPoint.h>
  
//回調函數，當message topic消息到達後會調用此函數  control_msgs/FollowJointTrajectoryActionGoal
void execute(const  control_msgs::FollowJointTrajectoryActionGoal::ConstPtr& msg)
{  
    std::cout << "points[0]: "  
             << "[" << (msg->goal.trajectory.points[0].positions[0]) * 180.0 / M_PI
             << "," << (msg->goal.trajectory.points[0].positions[1]) * 180.0 / M_PI  
             << "," << (msg->goal.trajectory.points[0].positions[2]) * 180.0 / M_PI  
             << "," << (msg->goal.trajectory.points[0].positions[3]) * 180.0 / M_PI  
             << "," << (msg->goal.trajectory.points[0].positions[4]) * 180.0 / M_PI  
             << "," << (msg->goal.trajectory.points[0].positions[5]) * 180.0 / M_PI  
             << "]" << std::endl ;

    std::cout << "points[1]: "  
             << "[" << (msg->goal.trajectory.points[1].positions[0]) * 180.0 / M_PI  
             << "," << (msg->goal.trajectory.points[1].positions[1]) * 180.0 / M_PI  
             << "," << (msg->goal.trajectory.points[1].positions[2]) * 180.0 / M_PI  
             << "," << (msg->goal.trajectory.points[1].positions[3]) * 180.0 / M_PI  
             << "," << (msg->goal.trajectory.points[1].positions[4]) * 180.0 / M_PI  
             << "," << (msg->goal.trajectory.points[1].positions[5]) * 180.0 / M_PI  
             << "]" << std::endl ;

std::cout << "points[2]: "  
             << "[" << (msg->goal.trajectory.points[2].positions[0]) * 180.0 / M_PI  
             << "," << (msg->goal.trajectory.points[2].positions[1]) * 180.0 / M_PI  
             << "," << (msg->goal.trajectory.points[2].positions[2]) * 180.0 / M_PI  
             << "," << (msg->goal.trajectory.points[2].positions[3]) * 180.0 / M_PI  
             << "," << (msg->goal.trajectory.points[2].positions[4]) * 180.0 / M_PI  
             << "," << (msg->goal.trajectory.points[2].positions[5]) * 180.0 / M_PI  
             << "]" << std::endl ;


std::cout << "points[3]: "  
             << "[" << (msg->goal.trajectory.points[3].positions[0]) * 180.0 / M_PI  
             << "," << (msg->goal.trajectory.points[3].positions[1]) * 180.0 / M_PI  
             << "," << (msg->goal.trajectory.points[3].positions[2]) * 180.0 / M_PI  
             << "," << (msg->goal.trajectory.points[3].positions[3]) * 180.0 / M_PI  
             << "," << (msg->goal.trajectory.points[3].positions[4]) * 180.0 / M_PI  
             << "," << (msg->goal.trajectory.points[3].positions[5]) * 180.0 / M_PI  
             << "]" << std::endl ;

std::cout << "points[4]: "  
             << "[" << (msg->goal.trajectory.points[4].positions[0]) * 180.0 / M_PI  
             << "," << (msg->goal.trajectory.points[4].positions[1]) * 180.0 / M_PI  
             << "," << (msg->goal.trajectory.points[4].positions[2]) * 180.0 / M_PI  
             << "," << (msg->goal.trajectory.points[4].positions[3]) * 180.0 / M_PI  
             << "," << (msg->goal.trajectory.points[4].positions[4]) * 180.0 / M_PI  
             << "," << (msg->goal.trajectory.points[4].positions[5]) * 180.0 / M_PI  
             << "]" << std::endl ;

 std::cout << "points[5]: "  
             << "[" << (msg->goal.trajectory.points[5].positions[0]) * 180.0 / M_PI  
             << "," << (msg->goal.trajectory.points[5].positions[1]) * 180.0 / M_PI  
             << "," << (msg->goal.trajectory.points[5].positions[2]) * 180.0 / M_PI  
             << "," << (msg->goal.trajectory.points[5].positions[3]) * 180.0 / M_PI  
             << "," << (msg->goal.trajectory.points[5].positions[4]) * 180.0 / M_PI  
             << "," << (msg->goal.trajectory.points[5].positions[5]) * 180.0 / M_PI  
             << "]" << std::endl ;    

std::cout << "points[6]: "  
             << "[" << (msg->goal.trajectory.points[6].positions[0]) * 180.0 / M_PI  
             << "," << (msg->goal.trajectory.points[6].positions[1]) * 180.0 / M_PI  
             << "," << (msg->goal.trajectory.points[6].positions[2]) * 180.0 / M_PI  
             << "," << (msg->goal.trajectory.points[6].positions[3]) * 180.0 / M_PI  
             << "," << (msg->goal.trajectory.points[6].positions[4]) * 180.0 / M_PI  
             << "," << (msg->goal.trajectory.points[6].positions[5]) * 180.0 / M_PI  
             << "]" << std::endl ;

std::cout << "points[7]: "  
             << "[" << (msg->goal.trajectory.points[7].positions[0]) * 180.0 / M_PI  
             << "," << (msg->goal.trajectory.points[7].positions[1]) * 180.0 / M_PI  
             << "," << (msg->goal.trajectory.points[7].positions[2]) * 180.0 / M_PI  
             << "," << (msg->goal.trajectory.points[7].positions[3]) * 180.0 / M_PI  
             << "," << (msg->goal.trajectory.points[7].positions[4]) * 180.0 / M_PI  
             << "," << (msg->goal.trajectory.points[7].positions[5]) * 180.0 / M_PI 
             << "]" << std::endl ;

std::cout << "points[8]: "  
             << "[" << (msg->goal.trajectory.points[8].positions[0]) * 180.0 / M_PI  
             << "," << (msg->goal.trajectory.points[8].positions[1]) * 180.0 / M_PI  
             << "," << (msg->goal.trajectory.points[8].positions[2]) * 180.0 / M_PI  
             << "," << (msg->goal.trajectory.points[8].positions[3]) * 180.0 / M_PI  
             << "," << (msg->goal.trajectory.points[8].positions[4]) * 180.0 / M_PI  
             << "," << (msg->goal.trajectory.points[8].positions[5]) * 180.0 / M_PI
             << "]" << std::endl ;

std::cout << "points[9]: "  
             << "[" << (msg->goal.trajectory.points[9].positions[0]) * 180.0 / M_PI  
             << "," << (msg->goal.trajectory.points[9].positions[1]) * 180.0 / M_PI  
             << "," << (msg->goal.trajectory.points[9].positions[2]) * 180.0 / M_PI  
             << "," << (msg->goal.trajectory.points[9].positions[3]) * 180.0 / M_PI  
             << "," << (msg->goal.trajectory.points[9].positions[4]) * 180.0 / M_PI  
             << "," << (msg->goal.trajectory.points[9].positions[5]) * 180.0 / M_PI  
             << "]" << std::endl ;

}  
  
int main(int argc, char **argv)  
{  
    
    //初始化ROS，注意：名稱必須唯一  
    ros::init(argc, argv, "test1");  
  //監聽/arm_controller/follow_joint_trajectory/goal話題釋出的消息
    //設定節點程序句柄  
    ros::NodeHandle n;  
  
    //參數一為訂閱的話題名稱（/arm_controller/follow_joint_trajectory/goal）；  
    //參數二是消息緩存隊列，這裡設定為1000，即緩存了1000個消息後，如果由新的消息到達，則開始丢棄先前接收的消息；  
    //參數三為當收到消息後調用的函數名稱（execute）  
    ros::Subscriber sub = n.subscribe("/arm_controller/follow_joint_trajectory/goal", 1000, execute);  
      
    //ros::spin()進入自循環，可以盡可能快的調用消息回調函數  
    ros::spin();    
    return 0;  
}

三、編寫CMakeLists.txt檔案

這裡僅僅在自動生成的CMakeLists.txt檔案後面加了以下兩句話，并沒有額外添加什麼依賴

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

四、編譯

$ cd ~/catkin_ws
$ catkin_make --pkg test

執行編譯完成後運作該節點就行了

$ rosrun test test1

當在rviz中用moveit設定一個目标姿态，然後plan and execute的時候，/arm_controller/follow_joint_trajectory/goal這個地方就有路徑資料出來了，我們編寫的這個test1節點就可以截獲到資料了。

編寫節點從ROS的FollowJointTrajectoryGoal主題訂閱路徑點資料

編寫節點從ROS的FollowJointTrajectoryGoal主題訂閱路徑點資料

三、編寫CMakeLists.txt檔案

四、編譯

繼續閱讀

ROS學習——Client Library——rospy

ROS----rospy編寫turtlesim程式

ROS基礎四之roscpp/rospy節點編寫

ROS學習（十四）：ROS Spinning

ROS命名空間

關于ros的參數伺服器ros param和命名空間

ROS中工作空間的建立

【ROS】移動機器人導航仿真（3）——定位（amcl）和路徑規劃（move_base）

ROS Navigation-----amcl簡介

C++實作RVIZ 2D POSE ESTIMATE 功能設定機器人初始坐标1. 首先檢視設定初始坐标的話題為 /INTIALPOSE ，檢視消息類型和格式進而決定怎麼給它發資料2.監聽RVIZ發出的資料格式3.仿照RVIZ消息格式4.編寫釋出節點程式

ROS2(二) - nav2_amcl節點概況

amcl ROS1.算法2.舉例3.節點3.1.6Transforms

ROS Navigation之amcl源碼解析（完全詳解）0. 寫在最前面1. amcl是幹什麼的2. 總體情況3. amcl_node.cpp參考

or frame [laser]: No transform to fixed frame [map]

ros安裝-Ubuntu14.04

ROS Stage學習