MoveIt! の Topic をハンドルする

意外と MoveIt! の Topic ハンドラのサンプルがないので。
なお、最近、ROS Indigo になって、MoveIt!のクラスライブラリも大きく刷新されたのらしく、サンプルがことごとくコンパイルできなくなっちゃってます。それもあってあまり親切な解説はないのですが、ROS の基本的な動作と、モダンな C++ について知識さえあれば、ヘッダファイルから動作を知ることはできます。

#include "ros/ros.h"
#include "moveit_msgs/MoveGroupActionResult.h"
#include "moveit_msgs/MoveGroupActionGoal.h"

#include <iostream>

// /move_group/result TOPIC ハンドラ
void onMoveGroupResult(const moveit_msgs::MoveGroupActionResult& result )
{
    // 軌道計画の result 表示
    std::cout << result << std::endl;
}

// /move_group/goal TOPIC ハンドラ
void onMoveGroupGoal(const moveit_msgs::MoveGroupActionGoal& desired )
{
    // 軌道計画の desired 表示
    std::cout << desired << std::endl;
}

// main 関数
int main(int argc, char **argv)
{
    ros::init(argc, argv, "MoveItTopicSample");
    ros::NodeHandle nodeHandle;
    ros::Subscriber subscribeResult, subscribeGoal;
    ros::Rate rate(10);

    // MoveIt! Topic ハンドラの登録
    subscribeResult = nodeHandle.subscribe("/move_group/result", 1, onMoveGroupResult);
    subscribeGoal   = nodeHandle.subscribe("/move_group/goal", 1, onMoveGroupGoal);

    ros::spin();

    return 0;
}

各 Topic ハンドラで渡される goal や desired の内容はヘッダファイルからメンバ変数を覗くことで確認できますが、基本的に Topic で表示される値そのままです。例えば、result の場合は、こんな感じで扱います。rqt や rostopic /echo などをうまく使って確認します。

// /move_group/result TOPIC ハンドラ
void onMoveGroupResult(const moveit_msgs::MoveGroupActionResult& action )
{
    // 初期角度を取得
    for( int i = 0; i < action.result.trajectory_start.joint_state.name.size(); i++ ){

        // 関節名
        std::cout << action.result.trajectory_start.joint_state.name[i] << std::endl;

        // 関節角度(rad)
        std::cout << action.result.trajectory_start.joint_state.position[i] << std::endl;
    }
}