#C++でJOYスティックを扱うには？
前々回、JOYパッドで亀を動かしました。
今度はラジコンを動かします。
C++でJOYパッドの入力検出しパブリッシャ経由で送信、Pythonのサブスクライバで受け取ります。
・・・とは言ったものの、中身がよくわかりません。
JOYスティックを動かしたサンプルコードを読んでみます。
##JOYスティックのサンプルコード
ROS講座07 joyプログラム
ROS.org入門演習
ROS.org teleop_twist_joy パッケージのサマリー
この辺りを参考に、JOYSTICKを動かせそうなコードを書いてみることにします。
書いてみるとこんな感じでしょうか？
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/Joy.h>
class TeleopRCCar
{
public:
TeleopRCCar();
private:
void joyCallback(const sensor_msgs::Joy::ConstPtr& joy);
ros::NodeHandle m_nh;
ros::Publisher m_pub;
ros::Subscriber m_sub;
};
TeleopRCCar::TeleopRCCar()
{
m_pub = m_nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);
m_sub = m_nh.subscribe<sensor_msgs::Joy>("joy", 10, &TeleopRCCar::joyCallback, this);
}
void TeleopRCCar::joyCallback(const sensor_msgs::Joy::ConstPtr& joy)
{
double dScale = 1;
geometry_msgs::Twist twist;
twist.angular.x = dScale*joy->axes[0];
twist.linear.x = dScale*joy->axes[1];
m_pub.publish(twist);
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "teleop_rccar");
TeleopRCCar teleop_rccar;
ros::spin();
}
###（寄り道）クラスのメンバーのコールバック関数
サンプルに書いてあるので漫然と書いてましたが、クラスのメンバはそのままではコールバックできません。
メンバ関数のポインタの他に、クラスのインスタンスのポインタも渡しています。
subscribe()の中ではこんな風に変数を保持していると思われます。
void (*callback)() = &TeleopRCCar::joyCallback;
void * pt_inst = this;
呼び出すときは、こんな感じでしょうか。
(pt_inst->*callback)();
あとで実装を見てみます。
関数ポインタについては、このページに詳しく書いてありました。
メンバ関数ポインタ天国
メンバ関数の関数ポインタを扱う方法
##ROSの基本の確認
よくわかっていないので基本の確認。
ROSでは一つのプログラム（実行ファイル）のことを「ノード」と呼ぶ。
複数のノードが役割を分担して、相互に情報交換をして動作する。
ノード間は「トピック」というチャンネルを通じてデータを交換する。トピックでやり取りされるデータは「メッセージ」。
ROS wiki シンプルな配信者(Publisher)と購読者(Subscriber)を書く(C++)
joyトピックの詳細はこちら。ROS wiki/joy
joy_nodeで公開されているトピックjoyを受信できるようにlaunchファイルに設定します。
<node name="joy_node" pkg="joy" type="joy_node" />
次にjoyトピックを受け取って、操作量を処理するプログラムをC++で記述します。
コールバック関数の中にJOYSTICKからの入力を受け取った後の処理を記述します。
m_pub = m_nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);
C++でパプリッシャを定義していますが、ROS初心者の私には見慣れない感じです。
手元にある入門書には説明がなかったので、テンプレートを関数の呼び出しの中に混ぜ込んだこの記述にモヤモヤしたものが残ります。
とりあえず、advertiseの宣言をしているヘッダファイルを探して見てみます。
template <class M>
Publisher advertise(const std::string& topic, uint32_t queue_size, bool latch = false)
{
AdvertiseOptions ops;
ops.template init<M>(topic, queue_size);
ops.latch = latch;
return advertise(ops);
}
template < class M> は関数の中で使用されている< M>の型のテンプレートを指定しています。
（私は引数がテンプレート型だと勘違いしていました。）
内部ではさらに、構造体AdvertiseOptionsのメンバを呼び出しています。
私はこのような記述は見たことがありませんでしたので、かなり混乱しました。
ops.template init<M>(topic, queue_size);
templateは予約語なので、メンバ変数というわけではなさそうです。
テンプレート変数を内部で使うinit()メンバを呼び出すときに、templateを途中に挟み文法の曖昧さを解決する記述方法とのこと。
template <class M>
void init(const std::string& _topic, uint32_t _queue_size,
const SubscriberStatusCallback& _connect_cb = SubscriberStatusCallback(),
const SubscriberStatusCallback& _disconnect_cb = SubscriberStatusCallback())
{
topic = _topic;
queue_size = _queue_size;
connect_cb = _connect_cb;
disconnect_cb = _disconnect_cb;
md5sum = message_traits::md5sum<M>();
datatype = message_traits::datatype<M>();
message_definition = message_traits::definition<M>();
has_header = message_traits::hasHeader<M>();
}
順番に深い階層までテンプレートが引き渡されていきます。
次はサブスクライバ側の実装です。
m_sub = m_nh.subscribe<sensor_msgs::Joy>("joy", 10, &TeleopRCCar::joyCallback, this);
こちらもテンプレート< sensor_msgs::Joy>でsubscribe()関数で使われるテンプレートを決定します。
コールバックしたいメンバ関数と、インスタンスのthisポインタも渡して、クラスのメンバ関数がコールバック関数として使えるようにしています。
template<class M>
Subscriber subscribe(const std::string& topic, uint32_t queue_size,
void(*fp)(const boost::shared_ptr<M const>&),
const TransportHints& transport_hints = TransportHints())
{
SubscribeOptions ops;
ops.template init<M>(topic, queue_size, fp);
ops.transport_hints = transport_hints;
return subscribe(ops);
}
/**
* \brief Subscribe to a topic, version with full range of SubscribeOptions
*
* This method connects to the master to register interest in a given
* topic. The node will automatically be connected with publishers on
* this topic. On each message receipt, fp is invoked and passed a shared pointer
* to the message received. This message should \b not be changed in place, as it
* is shared with any other subscriptions to this topic.
*
* This version of subscribe allows the full range of options, exposed through the SubscribeOptions class
*
* \param ops Subscribe options
* \return On success, a Subscriber that, when all copies of it go out of scope, will unsubscribe from this topic.
* On failure, an empty Subscriber which can be checked with:
\verbatim
SubscribeOptions ops;
...
ros::NodeHandle nodeHandle;
ros::Subscriber sub = nodeHandle.subscribe(ops);
if (sub) // Enter if subscriber is valid
{
...
}
\endverbatim
* \throws InvalidNameException If the topic name begins with a tilde, or is an otherwise invalid graph resource name
* \throws ConflictingSubscriptionException If this node is already subscribed to the same topic with a different datatype
*/
Subscriber subscribe(SubscribeOptions& ops);
template<class M>
void init(const std::string& _topic, uint32_t _queue_size,
const boost::function<void (const boost::shared_ptr<M const>&)>& _callback,
const boost::function<boost::shared_ptr<M>(void)>& factory_fn = DefaultMessageCreator<M>())
{
typedef typename ParameterAdapter<M>::Message MessageType;
topic = _topic;
queue_size = _queue_size;
md5sum = message_traits::md5sum<MessageType>();
datatype = message_traits::datatype<MessageType>();
helper = boost::make_shared<SubscriptionCallbackHelperT<const boost::shared_ptr<MessageType const>&> >(_callback, factory_fn);
}
大事なことは、初期化関数についてるテンプレートは、初期化関数とその先で使うクラスのテンプレートを渡しているということ。初期化関数の引数はテキストとINTでテンプレート型ではないです。
メモ
Cのコールバック関数をC++のメンバ関数にバインディングする方法
初期化部分を自分に納得させるためにかなりの時間を使ってしまいました。
先はまだまだ長そうです。