編寫節點從ROS的FollowJointTrajectoryGoal主題訂閱路徑點數據

原創 xjp13 2018-09-06 06:45

一、首先建一個包

工作空間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總結】Turtlebot ROS 開機自啓動設置

  關於ROS自啓動設置，之前查看了很多相關文章，並不是很適用，尤其是開機啓動時候加載minimal.launch和amcl.launch的時候，目前開啓自啓動的ROS包有upstart，該包是通過創建一個服務來啓動minimal.lau

ros小生 2020-07-08 11:09:33

[CoffeeBot] 運行最終腳本

運行最終腳本 你準備好提供咖啡 在Turtlebot上，打開一個終端然後運行： roslaunch turtlebot_bringup minimal.launch roslaunch turtlebot_navigation amc

ros小生 2020-07-08 11:09:33

【ROS總結】ROS下的百度語音識別應用

前言 今天閒來無事查看下語音識別在ROS中的應用，之前在ROS中玩過一段時間的Pocket Sphinx，關於Pocket Sphinx的學習過程以後會介紹，或者可以去網上搜索一些教程，都是比較不錯的。現在講述下在ROS中百度語音的使用

ros小生 2020-07-08 11:09:33

[CoffeeBot] 配置CoffeeBot的網頁服務器

重要 本文中提供的腳本並不是推薦的配置實例，因爲他們真的沒有安全。世界上任何人都可以在技術上要求咖啡或者更糟。我們離開了安全配置/ API密鑰等，儘可能的保持教程簡單。 我們需要一種方法讓客戶告訴TurtleBot他們想要咖啡，並且如

ros小生 2020-07-08 11:09:33

ROS：：ROS_INFO帶顏色的輸出

ROS_INFO帶顏色的輸出  輸出綠色： ROS_INFO("\033[1;32m----> Feature Extraction Started.\033[0m");  

dieju8330 2020-07-08 10:35:13

ButterWorthFIlter(巴特沃斯濾波器)

ButterWorthFIlter(巴特沃斯濾波器) 一、背景 這種濾波器最先由英國工程師斯蒂芬·巴特沃斯（Stephen Butterworth）在1930年發表在英國《無線電工程》期刊的一篇論文中提出的。 二、特點 巴特

dieju8330 2020-07-08 10:35:13

ROS：：同一個WorkSpace的Package相互調用

同一個WorkSpace的Package相互調用 假設我有 A_package ： 這是一個自己寫的庫 B_package ： 這是另外的可執行package 目標 在B_package中調用A_package中所實現的庫  A_pa

dieju8330 2020-07-08 10:35:13

sudo rosdep init ERROR: cannot download default sources list from:解決辦法

sudo rosdep init ERROR: cannot download default sources list from: 解決方法： 進入/etc/hosts文件目錄，     2、進入到hosts 文件：root@ros-v

嵌入式+ROBOT+ROS 2020-07-08 05:05:59

ROS實踐手冊（四）RoboWare實現話題消息自定義

筆者根據 古月居 · ROS入門21講 學習整理，並參考《ROS機器人開發實踐》一書。 相關課件及源碼可參考 Github/huchunxu/ros_21_tutorials 自定義話題消息 創建 learning_topic/

ME-Msc- 2020-07-08 03:00:13

ROS實踐手冊（三） RoboWare實現learning_topic編程實驗

筆者根據 古月居 · ROS入門21講 學習整理，並參考《ROS機器人開發實踐》一書。 相關課件及源碼可參考 Github/huchunxu/ros_21_tutorials RoboWare Studio IDE 安裝 爲支

ME-Msc- 2020-07-08 03:00:13

ROS實踐手冊（五）RoboWare實現learning_service編程實驗

筆者根據 古月居 · ROS入門21講 學習整理，並參考《ROS機器人開發實踐》一書。 相關課件及源碼可參考 Github/huchunxu/ros_21_tutorials 編程實現"spawn"服務請求 右鍵點擊 ROS 工

ME-Msc- 2020-07-08 03:00:13

ros自定義消息類型create MapMetaData in msg file

在新增自己定義的msg中，出現了錯誤提示： Could not find messages which '../my.msg' depends on. Did you forget to specify generate

夏日辉 2020-07-08 00:59:07

ros 設置 catkin_make -j -l

在實際ROS開發過程中，一個小問題大家有可能都沒注意到，就是在用catkin_make編譯的時候，速度可能會有點慢，但是相關設置的文章比較少，自己在開發的時候也需要設置，因此記錄一下如何設置catkin_make的-j和-l的參數

夏日辉 2020-07-08 00:59:07

rosrun or roslaunch can't find package or ERROR: cannot launch node of type

在編譯自己創建的ros包過程中，往往會出現roslaunch 或 rosrun找不到自己定義的package 原因是ros系統沒有正確定位package的位置。 解決辦法： 1.確定CMakeList中存在catkin_packa

夏日辉 2020-07-08 00:59:07

用bresenham算法求兩點之間連線通過的柵格點

void GridLineTraversal::gridLineCore( IntPoint start, IntPoint end, GridLineTraversalLine *line ) { int dx, dy, i

RobotLife 2020-07-08 00:32:28