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 削除時エラーの対処
はてなに移行してみた
なぜかインポート処理しても記事が公開されてないような?
なんだ、はてなダイアリーより、はてなブログの方が本流なのか
知らなかった。どうしてくれるんだ。とっとと移行しなくては。