做機械臂導航時遇到的問題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的資料了