rosコード(C++, Python)から現在存在するトピックのリストを取得する方法
C++
c++ - ROS - get current available topic in code (not command) - Stack Overflow
roscpp: ros::master Namespace Reference
ros::master::V_TopicInfo master_topics; ros::master::getTopics(master_topics); for (ros::master::V_TopicInfo::iterator it = master_topics.begin() ; it != master_topics.end(); it++) { const ros::master::TopicInfo& info = *it; std::cout << "topic_" << it - master_topics.begin() << ": " << info.name << std::endl; }
Python
http://docs.ros.org/api/rospy/html/rospy.client-module.html#get_published_topics
import rospy rospy.get_published_topics('/')
「rostest」 の使い方
rostestを使ううれしさ
- launchファイルと同様の手法で多数のノードを起動したりパラメータを設定した後に、ユニットテストを走らせることができる
- C++ならgtest, Pythonならunittestによって作成したテストコードを実行可能
使い方
rostestを用いるrosパッケージのサンプル構成を一番最後に置いておくので参考にしてください。
テストケースを書く
“.test"ファイルを作る
rosユーザならおなじみのlaunchファイルと同様の形式で記述します。 唯一異なるのは、ノードを立ち上げる際に<test>タグを用いる点です。
<test>タグの詳細はこちらから。
CMakelists.txtの編集
gtest(c++)の場合
以下を追加。
if(CATKIN_ENABLE_TESTING) find_package(rostest REQUIRED) add_rostest_gtest(tests_mynode test/mynode.test test/test_mynode.cpp [more cpp files]) target_link_libraries(tests_mynode ${catkin_LIBRARIES}) endif()
3行目のadd_rostest_gtest(tests_mynode test/mynode.test src/test/test_mynode.cpp [more cpp files])
の部分は、一つ目のパラメータがテスト実行用バイナリの名前、 2つ目が対応する.testファイル、3つ目以降がテストケースが記述されたcppファイルです。
4行目のtarget_link_libraries
で、テスト実行用バイナリの名前を指定するのを忘れないでください。
###### unittest(python)の場合
調査中です。
ビルド & テスト実行
catkin_make run_tests
サンプル
SSL Referee box のマルチキャストアドレス設定
GitHub - RoboCup-SSL/ssl-refbox: RoboCup Small Size League Referee Box
Refboxのディレクトリにある、referee.conf に各種コンフィグが保存されています。
ssl-refbox/referee.conf at master · RoboCup-SSL/ssl-refbox · GitHub
(Verの古いやつだと、refree.confじゃなくてsingle.confになってます)
で、ADDRESSってやつとかPROTOBUF_PORTってやつを任意に書き換えてあげればOK。
【メモ】decision_making パッケージの rqt_decision_graphが動かない問題
plugin.xmlに設定されているパスが間違っている
<library path="src">
これを
<library path="src/rqt_decision_graph">
こう修正
あと、間違ったパスの状態でrqtを起動すると間違ったパスの情報がrqtに残ってしまうらしいので
rm ~/.config/ros.org/rqt_gui.ini rqt
これで削除
navigationスタックで学ぶpluginlibの使い方
navigationスタックはpluginlibによる実装がなされていて、拡張しようと思ったらpluginlibの知識が必要ということなので、勉強しています。
今回は、navigationスタックのソースを追っかけていきながら、pluginlibの基本的な使い方について解説します。 自分も勉強中の身なので、間違っているところがあれば指摘をおねがいします。
pluginlibとは
pluginlibとは、roscppで書いたコードに、プラグインを実装するための機能です。 プラグインが何なのか、自分もまだはっきりとはわかっていませんが、基本的な概念としては、
インタフェースのみを定義したクラスを用意し、そのクラスの中身の実装を外部のライブラリによって行うことによって、 ユーザによってクラスの実装を入れ替えることができる仕組み
のようです。
イメージとしては、こんな感じでしょうか。
で、pluginlibは、roscppで書いたコードのクラスにこのプラグインのしくみを持たせるための機能です。
navigationスタックは、BaseGlobalPlanner、BaseLocalPlannerおよびRecoveryBehaviorがプラグインとして実装されています。
今回は、pluginlibのwikiにかかれている内容を、BaseLocalPlannerのプラグインの一つであるdwa_local_plannerのソースをおっかけながら確認していきます。
解説
プラグイン構成
wikiとソースコードをおっかける前に、プラグインの構成を整理しておきます。
move_baseとBaseLocalPlanner、その実装であるプラグインは下図のような関係になっています。
move_baseに、nav_core::BaseLocalPlannerというインターフェースが用意されており、その実装として dwa_local_planner::DWAPlannerROSと、base_local_planner::TrajectoryPlannerROSが用意されています。
インタフェースを定義しているパッケージは、nav_coreというパッケージで、move_baseパッケージとは別物になっています。 通常はインタフェースを定義するパッケージとそれを使うパッケージは同一だと思います。たぶん。
プラグインの登録
pluginlibのwiki 3.1 Registering/Exporting a Plugin の内容です。
プログラムをプラグインとして登録するには、登録したいプログラムのソースコード中(どこでもいいらしい)に、
PLUGINLIB_EXPORT_CLASS(プラグインとして登録するクラス名, インタフェースクラス名)
というマクロを記述する必要があります。
dwa_local_plannerではどうなっているかというと…
navigation/dwa_planner_ros.cpp at jade-devel · ros-planning/navigation · GitHub
dwa_planner_ros.cppの50行目に、確かにありますね。
PLUGINLIB_EXPORT_CLASS(dwa_local_planner::DWAPlannerROS, nav_core::BaseLocalPlanner)
プラグイン説明ファイル(?)
pluginlibのwiki 3.2 The Plugin Description File の内容です。
内容は、だいたいこんな感じ。
<library path="ライブラリのパス"> <class type="プラグインとして登録するクラス名" base_class_type="インタフェースクラス名"> <description> 説明文 </description> </class> </library>
dwa_local_plannerではどうなっているかというと…
navigation/blp_plugin.xml at jade-devel · ros-planning/navigation · GitHub
blp_plugin.xmlがそのようです。
<library path="lib/libdwa_local_planner"> <class name="dwa_local_planner/DWAPlannerROS" type="dwa_local_planner::DWAPlannerROS" base_class_type="nav_core::BaseLocalPlanner"> <description> A implementation of a local planner using either a DWA approach based on configuration parameters. </description> </class> </library>
<class name=〜〜〜
ってやつが追加されてますが、これは必要なのかな…? よくわかりません。
ROSのパッケージシステムにプラグインを登録する
pluginlibのwiki 3.3 The Plugin Description File の内容です。
ROSのシステムにプラグインの認識をしてもらうために、プラグインパッケージのpackage.xmlファイルに以下の追記が必要です。
<export> <インタフェースが定義されているパッケージ名 plugin="${prefix}/プラグイン説明ファイルの名前" /> </export>
それと、これ。
<build_depend>インタフェースが定義されているパッケージ名</build_depend> <run_depend>インタフェースが定義されているパッケージ名</run_depend>
dwa_local_plannerではどうなっているかというと…
navigation/package.xml at jade-devel · ros-planning/navigation · GitHub
47行目から49行目にかけてありますね。
<export> <nav_core plugin="${prefix}/blp_plugin.xml" /> </export>
build_dependとrun_dependは、それぞれ30行目、41行目に、nav_coreパッケージが記述されています。
定義されているプラグインを検索する
pluginlibのwiki 3.4 Querying ROS Package System For Available Plugins の内容です。
端末上で rospack plugins --attrib=plugin インタフェースが定義されたパッケージ
を実行することで、
定義されているプラグイン一覧を表示できます。
以下の例では、nav_coreパッケージに定義されているプラグイン一覧を表示します。
rospack plugins --attrib=plugin nav_core
プラグインを使う.
pluginlibのwiki 4. Using a Plugin の内容です。
いままでは、プラグイン側の話でしたが、今回はプラグインを使う側の話です。
定義されたプラグインを使うには、以下のようにします。
#include <pluginlib/class_loader.h> #include <インタフェースクラスが定義されたヘッダ> //... some code ... pluginlib::ClassLoader<インタフェースクラス名> poly_loader("インタフェースが定義されたパッケージ名", "インタフェースクラス名"); try { boost::shared_ptr<インタフェースクラス名> poly = poly_loader.createInstance("プラグインクラス名"); //何か処理 } catch(pluginlib::PluginlibException& ex) { //handle the class failing to load ROS_ERROR("The plugin failed to load for some reason. Error: %s", ex.what()); }
流れとしては、pluginlib::ClassLoader
でインタフェースクラスをロードして、それにcreateInstance()
メソッドで実体を定義してやる、
という感じでしょうか。
さて、navigationのソースコードですが、プラグインを使っているのはmove_baseパッケージです。
navigation/move_base.h at jade-devel · ros-planning/navigation · GitHub
navigation/move_base.cpp at jade-devel · ros-planning/navigation · GitHub
move_base.hの205行目に、pluginlib::ClassLoader
の定義がありますね。
pluginlib::ClassLoader<nav_core::BaseLocalPlanner> blp_loader_;
で、move_base.cppの53行目でこれを初期化しています。 この書き方を知らない人は、"c++ メンバイニシャライザ"でググってください。
blp_loader_("nav_core", "nav_core::BaseLocalPlanner"),
129行目では、createInstance()
していますね。
tc_ = blp_loader_.createInstance(local_planner);
おわりに
長い記事になってしまってすいません。
pluginlibに関する最低限のことはこれで理解出来たと思います。
既存のBaseLocalPlannerに満足が行かないので、これからがんばってあたらしいBaseLocalPlannerのプラグインをつくろうと思います。
costmap_2dの障害物情報のクリアについて
勘違いしてました。
costmap_2d/hydro/obstacles - ROS Wiki
costmap_2dは、センサ情報としてPointCloudまたはLaserScan型のメッセージを受け取ります。
RoboCup SSLでは、フィールド上部のカメラからロボット及びボールの絶対座標が常に取得できるので、 その値を使ってロボットの輪郭をなぞったPointCloudを渡してやればいいだろう、と思っていました。
が、うまくいかず。 障害物は確かに認識されるのですが、障害物が動いてPointCloudのデータが変化した後も、 前の場所に障害物の情報が残ってしまう、という現象が起きました。
これでしばらくハマりましたが、おそらく原因がわかりました。
costmap_2dが障害物をクリアする条件は、
以前観測された点が、直近に観測された点とロボット本体の間にある場合
のようです。
ですから、障害物情報を消去したい場合は、ロボット本体と観測の間に障害物が入るような観測を 与えてやる必要があります。
costmap_2dは、RoboCupSSLのようなワールド座標に固定されたセンサ情報は前提としておらず、 ロボット本体に乗っかったセンサのみを扱うように設計されているみたいです。
障害物情報を消去する方法としてもうひとつ、 move_baseのサービスである、"clear_costmap"を呼ぶという方法もあります。
しかしながら、"clear_costmap"サービスは、高速に頻繁に呼ばれることを想定していないみたいです。 あんまり頻繁に呼びすぎるとクラッシュするとか。
move_base crashed when call clear_costmaps service - ROS Answers: Open Source Q&A Forum
参考
publishing "0" points in a point cloud - ROS Answers: Open Source Q&A Forum
C++でのROSのノード開発メモ
Catkin設定方法
How to do common tasks — catkin 0.6.15 documentation
ソースファイルの追加
CMakelist.txtの、add_executable()にファイル名を追加する
add_executable(node_name src/filename1.cpp src/filename2.cpp)
node_nameの部分は、このファイルのノード名にする
ヘッダファイルの追加
CMakelist.txtの、include_directories()にインクルードディレクトリを追記し、そこに入れる
include_directories( "include" ${catkin_INCLUDE_DIRS} )
tf
Quaternion --> Eulerの変換
Transform Quaternion - ROS Answers: Open Source Q&A Forum
#include <tf/transform_datatypes.h> // ... tf::Quaternion q(quat.x, quat.y, quat.z, quat.w); tf::Matrix3x3 m(q); double roll, pitch, yaw; m.getRPY(roll, pitch, yaw); std::cout << "Roll: " << roll << ", Pitch: " << pitch << ", Yaw: " << yaw << std::endl;
geometry_msgs::Quaternionの作り方
createQuaternionMsgFrom***
を使う
Subscribe時のコールバック関数を、クラスのメンバメソッドとする場合
Using subscriber/callback function inside of a class C++ - ROS Answers: Open Source Q&A Forum
roscpp_tutorials/Tutorials/UsingClassMethodsAsCallbacks - ROS Wiki