编写节点从ROS的FollowJointTrajectoryGoal主题订阅路径点数据

最新推荐文章于 2024-07-12 18:37:47 发布

xiang19850801

最新推荐文章于 2024-07-12 18:37:47 发布

阅读量296

点赞数 3

文章标签：机器人

本文链接：https://blog.csdn.net/xiang19850801/article/details/138728949

版权

一、首先建一个包

工作空间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节点就可以截获到数据了。

xiang19850801

关注

3
点赞
踩
4

收藏

觉得还不错? 一键收藏
0
评论
编写节点从ROS的FollowJointTrajectoryGoal主题订阅路径点数据

工作空间catkin_ws事先建好了，路径是/root/catkin_ws然后运行以下命令在src文件夹下建立test包注意在包的名字后加上一些基本的依赖，比如roscpp和rospy这样就可以直接调用C++和python节点了，这将会在CMakeLists.txt中出现注意：这个cpp输出的是弧度制的角度，如果需要角度值需要转换，用下面的cpp。
复制链接

扫一扫