2016-10-13

一つのコールバック関数に複数のSubscriberを割り当てる方法

ROS

タイトルの通りです。

別々のTopicをSubscribeしている複数のSubscriberで、同じコールバック関数を呼び出す方法です。

準備

rostopic pub -r 10 chatter_0 std_msgs/String "message of chatter 0" &
rostopic pub -r 15 chatter_1 std_msgs/String "message of chatter 1" &

cpp

コールバック関数の型 ros::MessageEventがミソです。

ros::MessageEventにいくつかメソッドが用意されており、publisherの名前や topic名を取得できます。

#include "ros/ros.h"
#include "std_msgs/String.h"

void chatterCallback(const ros::MessageEvent<std_msgs::String const>& event)
{
    const ros::M_string& header = event.getConnectionHeader();
    std::string topic = header.at("topic");
    const std_msgs::StringConstPtr& msg = event.getMessage();

    ROS_INFO("[%s]:%s", topic.c_str(), msg->data.c_str());
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "listener");
  ros::NodeHandle n;
  ros::Subscriber sub_0 = n.subscribe("/chatter_0", 1000, chatterCallback);
  ros::Subscriber sub_1 = n.subscribe("/chatter_1", 1000, chatterCallback);

  ros::spin();

  return 0;
}

結果

[ INFO] [1476365549.987671394]: [/chatter_1]:message of chatter 1
[ INFO] [1476365550.027815654]: [/chatter_0]:message of chatter 0
[ INFO] [1476365550.054415232]: [/chatter_1]:message of chatter 1
[ INFO] [1476365550.121278541]: [/chatter_1]:message of chatter 1
[ INFO] [1476365550.127638866]: [/chatter_0]:message of chatter 0

Python

rospy.Subscriber()のコンストラクタ引数 "callback_args"がミソです。 "callback_args"に指定した値が、コールバック関数の第二引数として渡されます。

#!/usr/bin/env python
import rospy
import std_msgs.msg

def callback(data, id):
  rospy.loginfo("[ID:"+str(id)+"] : " + data.data)

def listener():
    rospy.init_node('listener', anonymous=True)

    sub_0 = rospy.Subscriber("/chatter_0", std_msgs.msg.String, callback, callback_args=0)
    sub_1 = rospy.Subscriber("/chatter_1", std_msgs.msg.String, callback, callback_args=1)

    rospy.spin()

if __name__ == '__main__':
    listener()

結果

[INFO] [WallTime: 1476365751.387734] [ID:1] : message of chatter 1
[INFO] [WallTime: 1476365751.427946] [ID:0] : message of chatter 0
[INFO] [WallTime: 1476365751.454669] [ID:1] : message of chatter 1
[INFO] [WallTime: 1476365751.521102] [ID:1] : message of chatter 1
[INFO] [WallTime: 1476365751.527823] [ID:0] : message of chatter 0

2016-09-20

第49回　情報科学若手の会　に参加してきた

情報科学若手の会

情報科学若手の会という、情報系の若手（？）が集う勉強会に参加してきました。結構歴史のある勉強会で、バックボーンに情報処理学会がいる、割とちゃんとした勉強会です。

とはいえ、想像していたよりもフランクな雰囲気で、あの空気感であの発表内容が聞ける会って本当に貴重だなと思います。

自分はもともとずっとロボットの人間だったので、正直情報系というクラスタには片足くるぶしぐらいまでしか入ってないのですが、せっかくなのでたくさん刺激を貰いに行こうと思い参加しました。 ~~（参加費は結構お財布に響きますが）~~

f:id:tilt_silvie:20160920000405j:plain:w300

会場の山喜旅館(静岡県伊東市)

発表

初参加だったのですが、せっかくなので発表をさせて頂きました。参加者の皆さんのレベルが高くて（後述しますが、本当に）僕なんかが同じ舞台で発表していいのかと思いましたが…　なかなかこういうチャンスもないので頑張りました。

発表では、RoboCup SSLについての概要と、趣味で立ち上げているSSLチームの紹介をしました。発表スライドは、下のリンクからアクセスできます。

docs.google.com

大変嬉しいことに、皆さん興味を持って聞いて頂けたみたいでした。自分の発表を聞いて、「RoboCup面白そうなので僕もやります！」と言ってくださった方もいらっしゃいまして、発表者冥利に尽きる思いでした。

あと、SSLロボットを持って行って操縦体験もしてもらったのですが、みなさん楽しんで頂けたみたいです。嬉しい。

少しでもRoboCup界隈が賑わうといいなぁ…と思います。 RoboCup本当に楽しいので、興味ある方は是非参加してみてください。

感想

率直な感想としては、本当にたくさんの刺激を貰えて、とても楽しかったですし有意義でした。

仕事や趣味では全く知らない分野の動向が知れたり、某企業のヒミツの裏話が聞けたりとか、参加してなければ知り得なかった情報が本当にたくさん手に入りました。

あと、参加されている方々が本当にすごくて、何かのコンテストで一位になりましたーとか、世界中がお世話になってる某企業で勤めてますーとか、日本で指折りのブログ運営してますーとか、そんな方々ばっかり。そういう方々とお酒を飲みながら色んな話を聞けたのは本当に楽しかったです。

（お知り合いになった方々、ぜひ今後共よろしくお願いします）

なかなかそういう方々とこんなにフランクな形で知り合える場は滅多に無いと思いますので、今後もこの会がずっと続いていけばいいなぁ…と思います。

来年もいきます！

2016-09-04

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('/')

2016-06-15

「rostest」の使い方

rostest - ROS Wiki

rostestを使ううれしさ

launchファイルと同様の手法で多数のノードを起動したりパラメータを設定した後に、ユニットテストを走らせることができる
C++ならgtest, Pythonならunittestによって作成したテストコードを実行可能

使い方

rostest/Writing - ROS Wiki

rostestを用いるrosパッケージのサンプル構成を一番最後に置いておくので参考にしてください。

テストケースを書く

C++ならgtest、Pythonならunittestでテストケースを書きます。
“.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()

３行目のadd_rostest_gtest(tests_mynode test/mynode.test src/test/test_mynode.cpp [more cpp files]) の部分は、一つ目のパラメータがテスト実行用バイナリの名前、２つ目が対応する.testファイル、3つ目以降がテストケースが記述されたcppファイルです。

4行目のtarget_link_librariesで、テスト実行用バイナリの名前を指定するのを忘れないでください。

###### unittest(python)の場合

調査中です。

ビルド & テスト実行

catkin_make run_tests

サンプル

GitHub - tilt-silvie/rostest_sample

2016-03-23

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。

2016-03-21

【メモ】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

これで削除

2015-09-29

navigationスタックで学ぶpluginlibの使い方

ROS pluginlib

navigationスタックはpluginlibによる実装がなされていて、拡張しようと思ったらpluginlibの知識が必要ということなので、勉強しています。

今回は、navigationスタックのソースを追っかけていきながら、pluginlibの基本的な使い方について解説します。自分も勉強中の身なので、間違っているところがあれば指摘をおねがいします。

pluginlibとは

pluginlib - ROS Wiki

pluginlibとは、roscppで書いたコードに、プラグインを実装するための機能です。プラグインが何なのか、自分もまだはっきりとはわかっていませんが、基本的な概念としては、

インタフェースのみを定義したクラスを用意し、そのクラスの中身の実装を外部のライブラリによって行うことによって、ユーザによってクラスの実装を入れ替えることができる仕組み

のようです。

プラグイン - Wikipedia

イメージとしては、こんな感じでしょうか。

f:id:tilt_silvie:20150928231636p:plain

で、pluginlibは、roscppで書いたコードのクラスにこのプラグインのしくみを持たせるための機能です。

navigationスタックは、BaseGlobalPlanner、BaseLocalPlannerおよびRecoveryBehaviorがプラグインとして実装されています。

今回は、pluginlibのwikiにかかれている内容を、BaseLocalPlannerのプラグインの一つであるdwa_local_plannerのソースをおっかけながら確認していきます。

解説

プラグイン構成

wikiとソースコードをおっかける前に、プラグインの構成を整理しておきます。

move_baseとBaseLocalPlanner、その実装であるプラグインは下図のような関係になっています。

f:id:tilt_silvie:20150928233033p:plain

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 の内容です。

プラグインの説明ファイルをxmlで作る必要があります。

内容は、だいたいこんな感じ。

<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のプラグインをつくろうと思います。

うごくものづくりのために

技術的な備忘録がメインです。

一つのコールバック関数に複数のSubscriberを割り当てる方法

準備

cpp

結果

Python

結果

第49回　情報科学若手の会　に参加してきた

情報科学若手の会

発表

感想

rosコード(C++, Python)から現在存在するトピックのリストを取得する方法

C++

Python

「rostest」の使い方

rostestを使ううれしさ

使い方

gtest(c++)の場合

サンプル

SSL Referee box のマルチキャストアドレス設定

【メモ】decision_making パッケージの rqt_decision_graphが動かない問題

navigationスタックで学ぶpluginlibの使い方

pluginlibとは

解説

プラグイン構成

プラグインの登録

プラグイン説明ファイル（？）

ROSのパッケージシステムにプラグインを登録する

定義されているプラグインを検索する

プラグインを使う.

おわりに