做机械臂导航时遇到的问题4：如何订阅joint_states话题（输出关节状态）

    在完成用arbotix测试机械臂后，我们想控制真实的机械臂伺服电机。由于arbotix是针对舵机的，我们要想控制伺服电机，要将ROS里机械臂的关节信息（角度、角速度、加速度、力等）发给下位机，首先就是要获取关节信息。

    关于让机械臂动起来，这里参考了一些大神的博客，在这里表示感谢

让模型动起来：     http://blog.csdn.net/yaked/article/details/51412781

moveit驱动：       http://blog.csdn.net/xu1129005165/article/details/70037698

如何在ROS下编写自己的节点来订阅话题（C++）  http://blog.csdn.net/u013453604/article/details/49102957

一、查看joint_states话题内容

在我的上一篇博客“做机械臂导航遇到的问题3：如何用arbotix接口控制机械臂”的基础上，

1、我们用  $ rostopic list   查看话题列表

2、查看joint_states话题的详细信息      $ rostopic echo /joint_states

可以看到joint_states话题里包含我们需要的关节信息

二、编写节点，订阅joint_states话题

此部分参考http://wiki.ros.org/cn/ROS/Tutorials中的教程中3、

1、建立一个包

$ cd ~/catkin_ws/src

$ catkin_create_pkg beginner_tutorials std_msgs rospy roscpp

2、编写订阅器节点（参考http://wiki.ros.org/cn/ROS/Tutorials/WritingPublisherSubscriber%28c%2B%2B%29）
在 beginner_tutorials目录下创建 src/listener.cpp 文件，并粘贴如下代码：

（我们对wiki中代码作了一些修改，添加了#include "sensor_msgs/JointState.h"，定义了回调函数jointstatesCallback，监听/joint_states话题发布的消息，修改话题发布的消息类型：const sensor_msgs::JointStateConstPtr& msg，获取position、velocity中的数据，并显示出来）

#include "ros/ros.h"
#include "std_msgs/String.h"
#include "sensor_msgs/JointState.h"
/**
 * This tutorial demonstrates simple receipt of messages over the ROS system.
 */

void jointstatesCallback(const sensor_msgs::JointStateConstPtr& msg)
{
  float pos[3],vel[3];
 // pos=msg.position;
  pos[0]=msg->position[0];
  pos[1]=msg->position[1];
  pos[2]=msg->position[2];
  vel[0]=msg->velocity[0];
  vel[1]=msg->velocity[1];
  vel[2]=msg->velocity[2];
  ROS_INFO("I heard: [%f] [%f] [%f] [%f] [%f] [%f]",pos[0],pos[1],pos[2],vel[0],vel[1],vel[2]);
 
}

int main(int argc, char **argv)
{
 
  ros::init(argc, argv, "listener");


  ros::NodeHandle n;


  ros::Subscriber sub = n.subscribe("/joint_states", 1000, jointstatesCallback);


  ros::spin();


  return 0;
}

3、编写CMakeList.txt文件

文件内容如下：

cmake_minimum_required(VERSION 2.8.3)
project(beginner_tutorials)

## Find catkin and any catkin packages
find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs genmsg)

## Declare ROS messages and services
#add_message_files(FILES Num.msg)
#add_service_files(FILES AddTwoInts.srv)

## Generate added messages and services
generate_messages(DEPENDENCIES std_msgs)

## Declare a catkin package
catkin_package()

## Build talker and listener
include_directories(include ${catkin_INCLUDE_DIRS} include )

add_executable(talker src/talker.cpp)
target_link_libraries(talker ${catkin_LIBRARIES})
add_dependencies(talker beginner_tutorials_generate_messages_cpp)

add_executable(listener src/listener.cpp src/uart.c)
target_link_libraries(listener ${catkin_LIBRARIES})
add_dependencies(listener beginner_tutorials_generate_messages_cpp)

4、编译，在工作空间目录下 $ catkin_make

编译成功后，运行节点rosrun beginner_tutorials listenner,可以发现已经接收到position和velocity的数据了