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>

１つのターミナルで

$ sudo bash

# rosrun ps3joy ps3joy.py

とした後に

$ roslaunch learning_ps3joy turtle_ps3joy.launch

 として実行します。

今回は少しうまくかけたかな。

もちろんOTLとPS3のコントローラーで書いてみましたよ。

これで、おしゃれなロボットへの入力装置ができました！