gucchiyの日記

仕事や生活の備忘録的に。

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;
    }
}

80年代、エンジニアと漫画イラストレーターと投稿雑誌

www.idea-tech.net
どうしてこうも更新が遅いのか。書くこと(書きたいこと)はたくさんあるのになぁ。

ようやく更新

www.idea-tech.net
いやー、なかなか更新できなかった。がんばろう。

Docker Image 削除時エラーの対処

tsunokawa.hatenablog.com

Error response from daemon: Conflict, cannot delete image 2064fc6b29af because it is tagged in multiple repositories, use -f to force

とかでたら、: で指定すると削除できる。

docker rmi test:001

Docker基本操作あれこれ

Dockerコンテナの作成、起動〜停止まで - Qiita
Docker で コンテナの保存(commit)AICS | AICS
Docker で コンテナの保存(commit)AICS | AICS
【Docker】 ホストとファイルを共有する - 子持ちプログラマーの日記

はてなに移行してみた

なぜかインポート処理しても記事が公開されてないような?

なんだ、はてなダイアリーより、はてなブログの方が本流なのか

知らなかった。どうしてくれるんだ。とっとと移行しなくては。