2010年1月20日水曜日

PS3のコントローラーを使う

今回はPS3のコントローラーをROSノードにする方法を勉強します。

http://www.ros.org/wiki/ps3joy

必要なもの
・PS3(PlayStation3)コントローラー
・BluetoothアダプタがささったPC

まず、ROSにはjoy_nodeというLinuxのジョイスティック入力を使うためのノードが用意されています。
ROSのps3joyパッケージがやるのは、PS3コントローラーを普通のLinuxジョイスティックとして使うためのラッパーみたいなものです。

ペアリング
http://www.ros.org/wiki/ps3joy/Tutorials/PairingJoystickAndBluetoothDongle

まずはパッケージをmakeします。

$ rosdep install ps3joy
$ rosmake ps3joy
 
次にUSBケーブルでPS3コントローラーをBluetoothを搭載したPCにつなぎます。
そして以下のようにコマンドを打ちます。
$ sudo bash
# rosrun ps3joy sixpair
これでPS3のコントローラーにPCのMACアドレスを教えています。

複数のUSBドングルがある場合は
# hciconfig
してMACアドレスを調べて、

$ rosrun ps3joy sixpair 00:22:B0:D0:5A:09
のようにするといいみたいです。

ひきつづきUSBケーブルを抜いてから、
# rosrun ps3joy ps3joy.py
すると
Waiting for connection. Disconnect your PS3 joystick from USB and press the pairing button.
と表示されるので、
PSボタンを押すと
Connection activated
と表示され接続されます。

Ubuntu8.10で接続できましたが、なぜか左スティックでマウスカーソルが動いてしまいます。
こうしたらちゃんと動きました。
 
では引き続きこれで亀でも動かして見ますか。
http://www.ros.org/wiki/ps3joy/Tutorials/WritingTeleopNode
ではいつものようにsandboxの中でパッケージを作って遊びましょう。
$ roscd sandbox
$ roscreate-pkg learning_ps3joy roscpp turtlesim ps3joy
では以下をsrc/teleop_turtle_ps3joy.cppとして保存してください。


#include <ros/ros.h>
#include <turtlesim/Velocity.h>
#include <joy/Joy.h>

class TeleopTurtle
{
public:
  TeleopTurtle();

private:
  void joyCallback(const joy::Joy::ConstPtr& joy);

  ros::NodeHandle nh_;

  int linear_, angular_;
  double l_scale_, a_scale_;
  ros::Publisher vel_pub_;
  ros::Subscriber joy_sub_;

};

TeleopTurtle::TeleopTurtle():
  linear_(1),
  angular_(2),
  l_scale_(50.0),
  a_scale_(2.0)
{

  nh_.param("axis_linear", linear_, linear_);
  nh_.param("axis_angular", angular_, angular_);
  nh_.param("scale_angular", a_scale_, a_scale_);
  nh_.param("scale_linear", l_scale_, l_scale_);


  vel_pub_ = nh_.advertise<turtlesim::Velocity>("turtle1/command_velocity", 1);
  joy_sub_ = nh_.subscribe<joy::Joy>("joy", 10, &TeleopTurtle::joyCallback, this);
}

void TeleopTurtle::joyCallback(const joy::Joy::ConstPtr& joy)
{
  turtlesim::Velocity vel;
  vel.angular = a_scale_*joy->axes[angular_];
  vel.linear = l_scale_*joy->axes[linear_];
  vel_pub_.publish(vel);
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "teleop_turtle");
  TeleopTurtle teleop_turtle;

  ros::spin();
}
これは
http://www.ros.org/wiki/joystick_drivers/Tutorials/TeleopNode
のサンプルとまったく同じコードです。
つまりPS3だけでなく普通のジョイスティックでも使えます。
 
コードは見れば分かりますね。
#Include<turtlesim/Velocity.h>
で亀を動かすためのメッセージ、
#include<joy/Joy.h>
でジョイスティックのメッセージ
を読み込めるようにしています。

今まであまりやっていないのが、

nh_.param("axis_linear", linear_, linear_);
  nh_.param("axis_angular", angular_, angular_);
  nh_.param("scale_angular", a_scale_, a_scale_);
  nh_.param("scale_linear", l_scale_, l_scale_);

の部分で、 パラメータを定義しています。
このパラメータで、亀の速度とジョイスティックの入力数値との比率を変えられます。 
初期値と変更したい変数を入れています。
ではいつものようにCMakeLists.txtを以下を追加してビルドします。
rosbuild_add_executable(turtle_teleop_ps3joy src/turtle_teleop_ps3joy.cpp)
 
実行前にジョイスティックをユーザーが触れるようにパーミッションをあげちゃいましょう。
$ sudo chmod a+rw /dev/input/js0
 
先に述べたように、上にコピペしたプログラムは普通のジョイスティック用です。
パラメータを調整する必要があるので、以下のようなlaunchファイルを作って、
パラメータを設定して実行しましょう。
 
ソースを変えちゃえばすむんですが、
コンパイル無しでプログラムを変更できるところがROSの柔軟性ですから、
このように実行時にパラメータ調整したほうがいいでしょう。
Don't Repeat Yourselfの原則もありますしね。 
 


<launch>

 <!-- Turtlesim Node-->
  <node pkg="turtlesim" type="turtlesim_node" name="sim"/>


 <!-- PS3 joy node -->
  <node respawn="true" pkg="ps3joy"
        type="ps3_joy_node" name="PS3_turtle_joy" >
    <param name="dev" type="string" value="/dev/input/js0" />
    <param name="deadzone" value="0.12" />
  </node>

 <!-- Axes -->
  <param name="axis_linear" value="1" type="int"/>
  <param name="axis_angular" value="0" type="int"/>
  <param name="scale_linear" value="2" type="double"/>
  <param name="scale_angular" value="2" type="double"/>

  <node pkg="learning_ps3joy" type="turtle_teleop_ps3joy" name="teleop"/>

</launch>
 
1つのターミナルで
$ sudo bash
# rosrun ps3joy ps3joy.py

とした後に
$ roslaunch learning_ps3joy turtle_ps3joy.launch
 として実行します。
今回は少しうまくかけたかな。
もちろんOTLとPS3のコントローラーで書いてみましたよ。
 
これで、おしゃれなロボットへの入力装置ができました!

0 件のコメント:

コメントを投稿