利用joint_states、joint_path_command 发布关节值、速度、加速度

ROS中利用API创建listener来读取joint_states、关节轨迹中的关机转动角度、速度、加速度信息

（1）这是ROS官网上的最基础的listener程序

#include "ros/ros.h"
#include "std_msgs/String.h"

void chatterCallback(const std_msgs::String::ConstPtr& msg)
{
  ROS_INFO("I heard: [%s]", msg->data.c_str());
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "listener_wiki_ros");
  ros::NodeHandle n;
  ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);
  ros::spin();
  return 0;
}

（2）从joint_path_command话题中读取一段轨迹的转动角度、速度、加速度信息

• joint_path_command话题需要ros与真实机械臂或仿真软件建立通信，才会被发布出来
• joint_path_command话题是读取关节轨迹中的信息最便捷的话题，其他话题也能实现，但是会有bug
• joint_states信息中只能读取position的值，直接利用rostopic echo /joint_states打印即可

#include "ros/ros.h"
#include "std_msgs/String.h"
#include "trajectory_msgs/JointTrajectory.h"

void Callback(const trajectory_msgs::JointTrajectoryConstPtr& msg)
{
    float pos[6];
    float vel[6];
    float acc[6];
    int len = msg->points.size();  //一段轨迹有多少个路径点
    for (int i = 0; i < len; i++) {  //将每个路径点上的加速度信息打印到屏幕
        for (int j = 0; j < 6; j++) {  
            pos[j] = msg->points[i].positions[j];
            vel[j] = msg->points[i].velocities[j];
            acc[j] = msg->points[i].accelerations[j];
            if (j == 5)
                ROS_INFO("pos,%f,%f,%f,%f,%f,%f", pos[0], pos[1], pos[2], pos[3], pos[4], pos[5]);
                ROS_INFO("vel,%f,%f,%f,%f,%f,%f", vel[0], vel[1], vel[2], vel[3], vel[4], vel[5]);
                ROS_INFO("acc,%f,%f,%f,%f,%f,%f", acc[0], acc[1], acc[2], acc[3], acc[4], acc[5]);
        }
    }
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "listener"); //listener 
    ros::NodeHandle n;
    ros::Subscriber sub = n.subscribe("/joint_path_command", 10000, Callback); //
    ros::spin();
    return 0;
}

• 最后将话题添加到编译环境中，最后catkin_make即可运行了

add_executable(listener src/listener.cpp)  //listener既为话题名称，也为api文件名
target_link_libraries(listener ${catkin_LIBRARIES})
add_dependencies(listener beginner_tutorials_generate_messages_cpp) //beginner_tutorials为笔者的文件夹名称，读者根据自己的编译环境来更改。