ニコニコ超会議出ます！

野生の研究者としてニコニコ超学会にでます。

タイトルは「３分間ルンバハッキング」です。

持ち時間は５分なのに、このタイトル、というあからさまに狙った感じです。

ニコニコ超学会というは、

ニコニコ学会βとは、簡単に言うと「ユーザー参加型の研究の場」です。
従来プロの研究者だけがするものだった研究を、「野生の研究者がやってもいんじゃね？」的な
発想で考え出された研究団体・シンポジウムだとのこと。堅苦しくない感じで、みんなで楽しく
研究をして発表し合おうということですね。

ということらしいです。
まあ全然研究じゃないんですけどね。


２９日の14：00　「研究してみたマッドネス ネットの部」
のトリででます。登場は１４：５０くらいかな？


放送はこちらで見れるようです。
http://live.nicovideo.jp/watch/lv89959587

時間はたったの５分。
概論に近いので私のBlogの熱心な読者には面白くないかもしれません。

僕の個人的な知り合いは楽しいでしょうけど。
なんか投票とかもあるっぽい？のでもしあったら投票してくださいね。

実物持っていけばいいとこまで行けると思うんですが、
そこまではできないかなー。

Android + rosjava でRoombaを操作

当初の目標だったAndroidでのルンバ操作アプリが一応できました。

Androidで端末の傾きセンサの値を取得し、それを速度に変換し、rosjavaを使ってtopicとして出力します。

おまけで掃除機能のOn/Off、ドックへの合体指示も出せるようにボタンをつけました。
ただし、この機能はotl_roombaパッケージのroomba_twist_node.pyを想定しています。
システム図は↓のような感じです。
Roomba制御のためにPCを経由しています。本当はAndroidからBluetoothで直接Roombaを操作可能なので、実はこのアプリはかなり冗長です。

でもこのノードは/cmd_vel (geometry_msgs/Twist)を出力するので、制御対象がルンバだろうが、PR2だろうが、動かすことができるというメリットもあります。

シミュレータでの動作画面は↓のようになっています。
スライダで最大動作速度を変えたり、ボタンでクリーナーのOn/Off等ができます。
一番上のでかいロゴに意味はありません。

リポジトリに上げましたが、
ソースもさらしておきます。まずはルンバ操作用クラス

/**
 * @license New BSD License
 * 
 */
package com.ogutti.ros.android.roomba;

import org.ros.message.geometry_msgs.Twist;
import org.ros.message.std_msgs.Bool;

import org.ros.namespace.GraphName;
import org.ros.node.NodeMain;
import org.ros.node.topic.Publisher;
import org.ros.node.Node;

/**
 * class for Controlling a roomba by Twist msg
 * 
 */
public class RoombaControllerNode implements NodeMain {
 private Publisher<Twist> velPublisher;
 private Publisher<Bool> cleanPublisher;
 private Publisher<Bool> dockPublisher;

 @Override
 public void onShutdown(Node arg0) {
 }

 @Override
 public void onShutdownComplete(Node arg0) {
 }

 @Override
 public void onStart(Node node) {
  velPublisher = node.newPublisher("/cmd_vel", "geometry_msgs/Twist");
  cleanPublisher = node.newPublisher("/roomba/clean", "std_msgs/Bool");
  dockPublisher = node.newPublisher("/roomba/dock", "std_msgs/Bool");

 }

 @Override
 public GraphName getDefaultNodeName() {
  return new GraphName("roomba_controller");
 }

 /**
  * start/stop cleaning motor
  * @param isOn if true start cleaning, false stop.
  */
 public void publishClean(boolean isOn) {
  if (cleanPublisher != null) {
   Bool msg = new Bool();
   msg.data = isOn;
   cleanPublisher.publish(msg);
  }
 }

 /**
  * Publish roomba's velocity (Vx, Vtheta)
  * @param linearX twist.linear.x forward speed [m/s]
  * @param angularZ twist.angular.z rotational speed [rad/s]
  */
 public void publishVelocity(double linearX, double angularZ) {
  if (velPublisher != null) {
   Twist vel = new Twist();
   vel.linear.x = linearX;
   vel.angular.z = angularZ;
   velPublisher.publish(vel);
  }
 }

 /**
  * start docking
  */
 public void publishDock() {
  if (dockPublisher != null) {
   Bool msg = new Bool();
   msg.data = true;
   dockPublisher.publish(msg);
  }
 }
}

つぎはメインのActivityのクラス

/**
 * @license New BSD License
 */

package com.ogutti.ros.android.roomba;

import java.text.DecimalFormat;
import java.util.List;
import android.os.Bundle;
import android.view.View.OnClickListener;
import android.widget.Button;
import android.widget.CompoundButton;
import android.widget.CompoundButton.OnCheckedChangeListener;
import android.widget.SeekBar;
import android.widget.SeekBar.OnSeekBarChangeListener;
import android.widget.TextView;
import android.widget.ToggleButton;

import org.ros.address.InetAddressFactory;
import org.ros.android.MessageCallable;
import org.ros.android.RosActivity;
import org.ros.android.views.RosTextView;
import org.ros.node.NodeConfiguration;
import org.ros.node.NodeMainExecutor;
import android.hardware.Sensor;
import android.hardware.SensorEvent;
import android.hardware.SensorEventListener;
import android.hardware.SensorManager;
import com.ogutti.ros.android.roomba.R;
import org.ros.message.geometry_msgs.Twist;

/**
 * MainActivity of this App (roomba controller)
 * 
 */
public class MainActivity extends RosActivity implements SensorEventListener {

 private RosTextView<Twist> rosTextView;
 private SensorManager sensorManager;
 private RoombaControllerNode controllerNode;
 
 private static final double LINEAR_VELOCITY_RATE  = 0.05;
 private static final double ANGULAR_VELOCITY_RATE = 0.1;

 /**
  * 1.0 means max speed, 0.0 means stop always.
  */
 private double speedRate;

 public MainActivity() {
  super("Roomba Controller", "Roomba Controller");
  speedRate = 0.5;
 }

 @SuppressWarnings("unchecked")
 @Override
 public void onCreate(Bundle savedInstanceState) {
  super.onCreate(savedInstanceState);
  setContentView(R.layout.main);
  sensorManager = (SensorManager) getSystemService(SENSOR_SERVICE);
  rosTextView = (RosTextView<Twist>) findViewById(R.id.text);
  rosTextView.setTopicName("/cmd_vel");
  rosTextView.setMessageType("geometry_msgs/Twist");
  rosTextView
    .setMessageToStringCallable(new MessageCallable<String, Twist>() {
     @Override
     public String call(Twist message) {
      DecimalFormat df = new DecimalFormat();
      df.setMaximumFractionDigits(2);

      return "vel_x:\n" + df.format(message.linear.x) + "\n"
        + "vel_theta:\n" + df.format(message.angular.z);
     }
    });
 }

 @Override
 protected void init(NodeMainExecutor nodeMainExecutor) {
  // create ROS nodes
  controllerNode = new RoombaControllerNode();
  NodeConfiguration nodeConfiguration = NodeConfiguration
    .newPublic(InetAddressFactory.newNonLoopback().getHostName());
  nodeConfiguration.setMasterUri(this.getMasterUri());
  nodeMainExecutor.execute(controllerNode, nodeConfiguration);
  nodeMainExecutor.execute(rosTextView, nodeConfiguration);

  // set callback for accelerometer
  List<Sensor> sensors = sensorManager
    .getSensorList(Sensor.TYPE_ACCELEROMETER);
  if (sensors.size() > 0) {
   Sensor accelerometer = sensors.get(0);
   sensorManager.registerListener(this, accelerometer,
     SensorManager.SENSOR_DELAY_FASTEST);
  } else {
   android.util.Log.v("MainActivity", "NOT found sensor!");
  }
  
  // set toggle button's callback
  ToggleButton tb = (ToggleButton) findViewById(R.id.toggleButton1);
  tb.setOnCheckedChangeListener(new OnCheckedChangeListener() {
   @Override
   public void onCheckedChanged(CompoundButton buttonView,
     boolean isChecked) {
    controllerNode.publishClean(isChecked);
   }
  });

  // set seekbar callback. 
  SeekBar seekBar = (SeekBar) findViewById(R.id.seekBar1);
  final TextView tv1 = (TextView) findViewById(R.id.textView1);
  seekBar.setOnSeekBarChangeListener(new OnSeekBarChangeListener() {
   public void onProgressChanged(SeekBar seekBar, int progress,
     boolean fromUser) {
    tv1.setText("Speed:" + progress + "%");
    speedRate = progress * 0.01f;
   }

   @Override
   public void onStartTrackingTouch(SeekBar seekBar) {
   }

   @Override
   public void onStopTrackingTouch(SeekBar seekBar) {
   }
  });
  Button dockButton = (Button) findViewById(R.id.button1);
  dockButton.setOnClickListener(new OnClickListener() {
   @Override
   public void onClick(android.view.View v) {
    controllerNode.publishDock();
   }
  });

 }

 @Override
 protected void onResume() {
  super.onResume();

 }

 @Override
 protected void onStop() {
  super.onStop();
  sensorManager.unregisterListener(this);
 }

 @Override
 public void onAccuracyChanged(Sensor sensor, int accuracy) {
 }

 /**
  * callback of sensor change(accelerometer)
  */
 @Override
 public void onSensorChanged(SensorEvent event) {
  if (event.sensor.getType() == Sensor.TYPE_ACCELEROMETER) {
   double linearX = event.values[1] * -LINEAR_VELOCITY_RATE * speedRate;
   double angularZ = event.values[0] * ANGULAR_VELOCITY_RATE * speedRate;
   if (Math.abs(linearX) < LINEAR_VELOCITY_RATE) {
    linearX = 0.0d;
   }
   if (Math.abs(angularZ) < ANGULAR_VELOCITY_RATE) {
    angularZ = 0.0d;
   }
   controllerNode.publishVelocity(linearX, angularZ);
  }
 }
}

ちょっと長いですが以上になります。
Androidアプリは作るのが結構簡単なのでチャレンジしてみてはどうでしょうか。

ARDroneとルンバを同時にPS3のコントローラで動かす

AR.Droneとルンバを１つのPS３のジョイパッドから同時にうごかしてみました。
ROSのPub/Subを利用した楽しい例だと思います。

前回紹介したardrone_brownノードと、昔作ったルンバ用のプログラムを組み合わせたら簡単にできました。
ルンバの掃除開始と同期してヘリが飛び立ったら面白いと思い、掃除機のモータ指令メッセージ（Bool型）をardroneのEmpty型へ変換して送信するノードを作りました。
これによりルンバへの速度指令と、掃除開始（離陸）、終了（着陸）が同時にAR.Droneに送られるようになります。

ノードとトピックは↓のような感じ。

メッセージ変換のコードは↓のような感じです。

#!/usr/bin/env python
import roslib
roslib.load_manifest('otl_msgconverter')
import rospy
from std_msgs.msg import Bool
from std_msgs.msg import Empty
class BoolMsg2EmpyMsg():
    """ convert bool msg to empy msg
    """
    def __init__(self, bool_topic, true_topic, false_topic):
        self._bool_topic = bool_topic
        self._true_topic = true_topic
        self._false_topic = false_topic
    def callback(self, bool_value):
        msg = Empty()
        if (bool_value.data):
            self._true_pub.publish(msg)
        else:
            self._false_pub.publish(msg)
    def init(self):
        self._true_pub = rospy.Publisher(self._true_topic, Empty)
        self._false_pub = rospy.Publisher(self._false_topic, Empty)
        self._sub = rospy.Subscriber(self._bool_topic, Bool, self.callback)
if __name__=='__main__':
    rospy.init_node('otl_bool2empty')
    obj = BoolMsg2EmpyMsg('input', 'true', 'false')
    obj.init()
    rospy.spin()

ヘリは迫力があっていいですね。

着陸も成功させたかった！

Kinectを使ったジェスチャ認識でルンバを操縦する

Kinectでルンバを操縦してみました。

ハードウェア接続は

Kinect =USB=> PC =Bluetooth=> Roomba

という感じです。

openni_trackerというパッケージを使ってtfのボーンを作り、

tfのデータから腰と手先位置の差分をとって似非ジェスチャ認識させています。

Kinectの目の前にいないと操縦できないのがいまいちですね。

ソースも適当ですが、

一応otl_niスタックとして上げました。

むちゃくちゃ簡単でした。

ルンバのBluetoothモジュールは@longjie0723 のFRISK Roombaを使っています。
http://orientalrobotics.blogspot.com/2011/01/friskroomba.html
非常に快適です。

FRISK Roomba動作確認編

@longjie0723 さんにFRISK Roombaモジュールをいただきましたので、
早速動作確認させていただきました。

http://orientalrobotics.blogspot.com/2011/01/friskroomba.html

当方ホストPCがUbuntu10.04、Roomba577での動作確認になります。

まず、FRISKをうちのルンバにぶすっとさします。

この状態で以下のコマンドを打ちます。
$ hcitool scan

Scanning ...

00:01:95:09:06:0C Roomba-001

これでモジュールが正常起動していることと、MACアドレスが分かりました。

/etc/bluetooth/rfcomm.conf
を以下のように編集しました。

rfcomm0 {
        # Automatically bind the device at startup
        bind yes;

        # Bluetooth address of the device
        device 00:01:95:09:06:0C;

        # RFCOMM channel for the connection
        channel 1;

        # Description of the connection
        comment "FRISK Roomba";
}

deviceのところにさっき調べたMACアドレスを書きます。

この状態で

$ sudo /etc/init.d/bluetooth stop

$ sudo /etc/init.d/bluetooth start

して、

$ sudo rfcomm bind hci0
とします。
すると、
/dev/rfcomm0
というデバイスファイルができています。

誰でも使えるようにパーミッションをだしておきましょう。
$ sudo chmod 666 /dev/rfcomm0

あとは、これをシリアルデバイスだと思ってプログラムを走らせればOK。

$ sudo rfcomm bind hci0

は起動するたびに実行しないといけません。

/etc/rc.local

などにかきましょう。

ルンバ関係ないですが、PS3のジョイスティックが使えなくなっていたので、

/etc/init/bluetooth/main.conf

に

DisablePlugins = input

としました。

ルンバとの共存は問題ないようです。

使ったプログラムは以前作ったものをそのまんまで動きました。

無線化うれしい！
とりえあず動作確認まで。

自律移動ルンバ＋α完成！

ようやくmove_baseのパラメータ調整（主にbase_local_planner)が終わりました。

で、完成したビデオが↓です。


全く関係ないですが、白くまをフィーチャーしてみました。
我が家のアイドルです。

base_local_planner用の設定ファイルは以下です。

TrajectoryPlannerROS:
max_vel_x: 0.15
min_vel_x: 0.01
max_rotational_vel: 0.4
min_in_place_rotational_vel: 0.4
acc_limit_th: 0.5
acc_limit_x: 0.5
acc_limit_y: 0.0
holonomic_robot: false
path_distance_bias: 0.6
goal_distance_bias: 0.6
heading_scoring: true
occdist_scale: 0.01

経路と大きくずれるとまずいので、経路になるべく追従するように
path_distance_biasを大きくして、heading_scoringをtrueにしてみました。

ちょっと自己位置が脆弱なので、どうにかしたいですね。
でもとりあえずROSでやりたかったことの目標達成できたのでうれしいです。

あとはゆるりとソースでもよみますかねぇ。

AMCL調整成功？

ようやくAMCLが使える状態になってきました。

一番大きな変更点はマップの解像度を50mm/grid -> 15mm/gridに変更したことです。
あとはパラメータは前回紹介したブライアンさんのメーリスへの投稿のものを
そのまま使いました。調整しようとしましたがかなり難しいです。

ちょっとずれますが、なんとか修正できています。
（あと、マップ作成直後なのでマップとの乖離が少ないというのもあるかもしれません。
もしそうなら、次回やるときにはまたずれてるかも。。。。）

マップ作成時のパラメータは以下のような感じにしました。


$ rosrun gmapping slam_gmapping _delta:=0.015 _xmin:=-10 _ymin:=-10 _xmax:=10 _ymax:=10 _lstep:=0.01 _astep:=0.01 _stt:=0.3 _particles:=500

amclのパラメータは以下です。


<launch>
<node pkg="amcl" type="amcl" name="amcl">
  <!-- Publish scans from best pose at a max of 10 Hz -->
  <param name="odom_model_type" value="diff"/>
  <param name="transform_tolerance" value="0.2" />
  <param name="gui_publish_rate" value="10.0"/>
  <param name="laser_max_beams" value="30"/>
  <param name="min_particles" value="500"/>
  <param name="max_particles" value="5000"/>
  <param name="kld_err" value="0.05"/>
  <param name="kld_z" value="0.99"/>
  <param name="odom_alpha1" value="0.8"/>
  <param name="odom_alpha2" value="0.8"/>
  <!-- translation std dev, m -->
  <param name="odom_alpha3" value="0.8"/>
  <param name="odom_alpha4" value="0.2"/>
  <param name="laser_z_hit" value="0.5"/>
  <param name="laser_z_short" value="0.05"/>
  <param name="laser_z_max" value="0.05"/>
  <param name="laser_z_rand" value="0.5"/>
  <param name="laser_sigma_hit" value="0.2"/>

  <param name="laser_lambda_short" value="0.1"/>

  <param name="laser_lambda_short" value="0.1"/>

  <param name="laser_model_type" value="likelihood_field"/>

  <!-- <param name="laser_model_type" value="beam"/> -->

  <param name="laser_likelihood_max_dist" value="2.0"/>

  <param name="update_min_d" value="0.2"/>

  <param name="update_min_a" value="0.2"/>

  <param name="odom_frame_id" value="odom"/>

  <param name="resample_interval" value="1"/>

  <param name="transform_tolerance" value="0.1"/>

  <param name="recovery_alpha_slow" value="0.0"/>

  <param name="recovery_alpha_fast" value="0.0"/>

</node>

</launch>

なんとか使えそうなので、次回これにプランニングのほうも足してみます。
はぁ、むつかしかった。
パラメータの調整はやはり職人芸が必要ですね。

今回はマップのグリッドサイズでしたが。
ロボットのサイズによってグリッドサイズは大きく変える必要がありますね。

続 amclの調整

ひさびさにロボットを触るチャンスなのでやってみましたが
うまくいきません。
が、かなりいい情報を見つけました。


ros-users MLのログをみたら、
やっぱりcreate(ルンバの開発用)でうまくamclが動かない、という人がいました。
で、ながーいやりとりを見ていくと、


1. mapの灰色があやしい、
という話と、
パラメータ調整だ。
という二つの話が書いてありました。


パラメータ調整の話は以下。


Some notes on your robot's odometry:
 * The yaw estimate is pretty bad, exhibiting a very high rate of drift.
 * It feels like there's a non-zero bias in the yaw integration
(either under- or over-reporting of rotation), but I was unable to
compensate for it, so I'm not sure of this.
 * Jumps in the robot's pose suggest instances of non-trivial wheel
slip; do you see the wheels slip against the floor?

To get the behavior shown in the video, I adjusted three parameters in
amcl (with respect to amcl_diff.launch):
 * increase odom_alpha1 to 0.8, which is used to compute uncertainty
in rotation and translation, as a function of rotation.
 * increase odom_alpha2 to 0.8, which is used to compute uncertainty
in rotation, as a function of rotation
 * decrease update_min_a to 0.2, which is the change in angle
required to cause a filter update




つまり、
odom_alpha1, odom_alpha2を0.8に、update_min_aを0.2にするとよいそうです。


そのへんも十分いじったつもりなんですが、
いろいろ同時にいじってるんで、この３つだけを変えてやってみようと思います。


次回へ続きます。


やっぱルンバの回転方向のオドメトリは精度がでないみたいですね。。。。

続AMCL悩み

うーん、
全然自己位置がさだまりません。
パラメータもいろいろそれっぽいのいじったりしましたがだめです。

オドメトリは全然むちゃくちゃなんですが、それでもレーザーでなんとか
修正してほしいところです。

症状はこんな↓感じです。
ちなみにrvizのキャプチャはxvidcapでやりました。
recordmydesktopだとうまくいきませんでした。

ルンバ自律移動途中経過

rvizの使い方もだんだん分かってきました。

なんとかルンバが動き始めました。
速度をはやくすると位置がずれまくります。
もう少しパラメータを調整する必要がありそうです。

ルンバ改造

ルンバを改造しました。

以前はしたの写真のように、ルンバの上にPC、レーザ、回路をのせただけの構成でした。

これだとよけいなケーブルをPCの下に隠すため、PCが非常に不安定でした。

なので、取り外し可能で、かつ、きれいにケーブル類を収納できるように
改造します。
まずは一階部分。
台をおいて、その上に回路、ケーブル類をいれます。
木の柱をたてました。
材料はすべて１００円ショップです。

そして二階の台を取り付け。
この上にPCをおきます。

これで完成

斜めからみるとこんな感じ。

ちょっとかっこよくなりました。

gmapping + Roombaでマップができた

おひさしぶりです。

ようやくマップができました。

（自宅の地図大公開です。）
上側がキッチン、下がリビングダイニングです。
左側には広大な部屋がたくさんありますが、あまり遠くにいかせてもあれなので、
今回は自宅のごく一部の地図としました。

（本当は段差があるので、段差なしの領域だけとしてみました。）
（本当は部屋を片付けるのが面倒なので、ここだけです。）



ルンバから取得するオドメトリの値は全然実世界と合わないので、
距離は８倍、角度は３倍しました。
するとちょうどよさげです。
謎ですが気にしない。気楽にやります。

なぜ今まで、オドメトリとレーザーをやってきたか、というとgmappingを使ってマップを作るためです。

この二つがあればマップを自動作成することができます。


http://www.ros.org/wiki/navigation/Tutorials/RobotSetup/Odom

をよく読んで自分のロボットのオドメトリをpublishします。
TFの形と、nav_msgs/Odometryの形の２つでpublishします。

Odometryのデバッグはrvizを使いました。

そしてgmappingを使います。
http://www.ros.org/wiki/slam_gmapping/Tutorials/MappingFromLoggedData

基本的にはロボットを動かす前に、

$ rosbag record -a -O map
などとして、トピックをすべて記録。


そして、その状態でロボットを動かします。

で、できたmap.bagファイルをslam_gmappingに食わせます。

$ rosparam set use_sim_time true
して、rosbagでの再生にgmappingを対応させて、
$ rosrun gmapping slam_gmapping
でgmappingを起動。

$ rosbag play map.bag
でトピックを再生します。

rosbagは非常に便利です。テストにも必須な気がします。

gmappinはパラメータがいろいろありますが、デフォルトの設定で一応できたのでよしとします。

再生が終わってから
$ rosrun map_server map_saver
とすると安定版マップがmap.pgm/map.yamlとして取ってこれます。
$ rosrun map_server map_saver static_map:=dynamic_map
とすると、現状の最新マップが取ってこれます。

やっとマップができましたので、次回はこれを使っていよいよ自律移動です！

Roombaのオドメトリを取る

ルンバにはオドメトリを取るコマンドがありますが、
どうもこれが変です。

で、ちょっと調べてみると以下のような記事が見つかりました。

http://createforums.irobot.com/irobotcreate/board/message?board.id=Create_Support&view=by_date_ascending&message.id=492

ようするに、車輪の回る方向はルンバは分からないので、どっちの方向に
指令を受けたか、で車輪の方向を判断している。
なので、手でルンバを押してしまうと正確にでません。特に回転は判断できない。

たしかに、方向が分かるエンコーダは高いですからついてないんですね。

というわけで、車輪をモータを使って動かして調べましょう。

Roomba(ルンバ)からURGの電源を取る

いよいよルンバにURGを取り付けます。

ルンバのバッテリーは14.4V、URGは12V。
直接はつなげられないので、３端子レギュレータを入れて、
ノイズを取り除くためのコンデンサを入れました。
（回路図は簡単なので省略）

OIインタフェース（シリアル線の隣のピン）からもバッテリー電源がきていますが、
実験の結果、こちらからは電流があまりとれないということが分かりました。
URGブートまではいきますが、安定して立ち上がりません。
1Aくらいでしょうか？
URGは瞬間的に3Aくらい流れるようです。
この線がバッテリ直だと思っていたので残念です。

そのため、ルンバの底をあけて、バッテリから直接電源を取ることにしました。
バッテリのコネクタに直にはんだづけしてしまいました。

あられもないルンバの姿。

回路がむき出しなのでカバーしました。

で、URGとPCとを乗っけて完成です。

ごちゃっとしてますが、PCを小さくして、配線をその下におさまるようにすれば
結構かっこよくなるのではないでしょうか？

もしこいつがちゃんと動けば、次回はこのニュールンバを使って家のマップ作りをしたいと思います。

Roombaのインタフェースを変える

前回見せたビデオでは内部的には
ルンバへの指令は
速度と回転半径で与えていました。

ROSは全方位移動台車を持つPR2のために、速度をTwistというメッセージ(geometry_msgsパッケージ）で与えるようになっていますので、
ルンバもこのインタフェースで動かせるようにしたいです。


そもそもなぜ、速度と回転半径にしたかというと、
Roombaは基本的に速度を

速度と回転半径

で与えるようになっています(DRIVEコマンド）。
なのでシンプルにそのまま使いました。


しかし、実はもう一つ操作方法があって、

左車輪速度と右車輪速度

というインタフェースのコマンド（DIRECT　DRIVEコマンド）もあります。

今回は直接的にうごかすDIRECT　DRIVEコマンドを利用して、
Twistメッセージを受け取ることができるようにしました。
といっても、
Twist.linear.xとTwist.angular.zしか使っていないですし、
うーん、挙動がちょっと間違っていますね。

今後自律移動プログラムを作っていくことになるので、
そっちが見通しついてからまた考えます。

Pythonコードをunittestでテストする

たまにはまじめにコードを書こうと思い、テストの勉強です。

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

ROSではPythonのテストはそのパッケージであるunittestを使ってテストするのが標準のようです。

C++はgtestというgoogleが作ったテスト環境を使っています。

pythonのunittestもよく分かっていないので、その勉強からです。

↓のへんを読みました。

http://docs.python.org/library/unittest.html

http://www.python.jp/doc/2.5/lib/testcase-objects.html

ようするに

unittest.TestCaseクラスにメソッドを定義しておくと、それを順に呼んでくれる。

assert_, assertRaises, assertEqualsなどを使って結果が想定どおりかどうかを確かめる。

テスト前に呼ばれるsetUp(), テスト後に呼ばれるtearDown()というメソッドが使える。

という感じですかね。（他にも機能はいっぱいあります）

普通テストドライバで関数呼び出しをずら〜っと手書きすると思いますが、

その辺の自動化と結果のレポートをしてくれるようです。

さっそくルンバのインタフェースプログラムに使いました。

TestRoombaOpen, TestRoombaCommand

という二つのテストケースクラスを作ったとすると、

テストプログラムから

test_support.run_unittest(TestRoombaOpen, TestRoombaCommand)

として呼び出すと思いますが、

rostestの場合は、

import rostest

rostest.unitrun('test_roombaif', sys.argv[0], TestRoombaOpen)

rostest.unitrun('test_roombaif', sys.argv[0], TestRoombaCommand)

とするみたいです。

そして、

CMakeList.txtに

rosbuild_add_pyunit(test/test_roombaif.py)

を追加します。

この状態で

$ make test

するとテストが実行され、結果が

~/.ros/test_results/otl_roomba/test_roombaif.py.xml

として保存されます。

中身は↓みたいな感じです。

<testsuite errors="0" failures="0" name="unittest.TestSuite" tests="35" time="17.229">

  <testcase classname="__main__.TestRoombaCommand" name="test_ascii" time="0.6085"></testcase>

......

  <testcase classname="__main__.TestRoombaCommand" name="test_vel9" time="0.5080"></testcase>

  <system-out><![CDATA[speed limit over -501 < -500

rad limit over -2001 < -2000

speed limit over 501 > 500

rad limit over 2001 > 2000

speed limit over -501 < -500

rad limit over -2001 < -2000

]]></system-out>

  <system-err><![CDATA[]]></system-err>

</testsuite>

これは単体テスト（ROSノード無関係）用です。

いっぽうでROSノードレベルのテストをする場合は以下のようにして呼び出します。

rostest.rosrun(PKG, 'test_bare_bones', TestBareBones)

あとはrostestを使ったテストですがこれはまた次回にしましょう。

PS3コントローラでルンバを動かす（今回はROS使ったよ）

やっとRoombaとROSとがつながりました。
やったのは前回のi-SOBOTの時と同じでPS3コントローラとつないだだけ、なんですけど。

これでROSがRoombaにのりました
（正確にはPCから操作しているだけですが、そのPCもRoombaに乗っているのでよしとしましょう）。



ノードの関係は↓のようになっています。

roombaノードがルンバへの指令を受け付けます。
ps3_joyからの指令をroombaps3ノードが仲介して
roombaノードに渡しています。
roombaノードが実際にルンバを動かします。

万が一ソースを見たい人がいれば↓をどうぞ。

http://code.google.com/p/otl-ros-pkg/

Roomba動いた！

なんか、ルンバが動くんですが、挙動がおかしくて
なんか変だな、と思っていたら、
今は
Roomba OI (Roomba Open Interface)と名前が変わっていることに気づきました。

見る資料も間違っていました。
http://www.irobot.lv/uploaded_files/File/iRobot_Roomba_500_Open_Interface_Spec.pdf

通信速度が57600から115200に変更になっていますので注意が必要です。
（過去記事は修正しました）

なんと。。。ぐぐり力が足りないですね。反省。

ということでやっとルンバが動きました！

LEDや７セグメント（４桁）などが使え、音楽作曲もできますので、
ロボットとしての表現力もばっちりありますね。

とりあえず動いたので動画にしておきました。

回転時にLEDが光ったりします。
７セグメントLEDを使って「OTL」を表示していますが
分かりますかね？



構成：
Roomba 577 <==> FT232RL <==> Thinkpad X61 (Ubuntu) + i-SOBOT(意味なし)
プログラム環境：python (pyserial)

pyserialは
$ sudo apt-get install python-serial
としてインストールしました。
こういう低レベルな操作はCとかのほうが簡単ですね。
Pythonで書くの結構苦労しました。適材適所ですな。
逆に面白かったですけど。

Roomba OI (Open Interface) (旧SCI (Serial Control Interface))の勉強

やっと家にネットが開通しました！
そろそろ勉強を始めますか。
RO(omba)S(ystem)の勉強を！

前回紹介した公式コマンドPDFは間違っていました。
500シリーズ用のドキュメントがありましたので、こちらを読んでいきます。
http://www.irobot.lv/uploaded_files/File/iRobot_Roomba_500_Open_Interface_Spec.pdf

まず、通信スピードですが、ボーレート15520です。
設定すればもっと遅くできますが、今回はこのままにします。

ルンバには４つのSCIモードがあるようです。
off, passive, safe, full
の４つです。

充電中や電源がやばいときはoffになります。

スターとコマンドを受け取るとpassiveモードになります。
passiveモードでできるのは

• センサデータの取得
• 掃除の開始・終了
• 歌の設定
• 充電開始

だけです。

なのでロボットのように操作はできません。

で、コマンドを送るとsafeモードにうつります。

セーフモードでは以下の３つの安全機能が有効な状態で自由にルンバを操作できます。

• 落下防止機能
• 脱輪検知
• 充電器合体による充電

これが発生すると自然にpassiveモードに移行してしまいます。

コマンドを送ればこれらの制限のないfullモードへ移行できます。

今回はsafeモードでいいかな。

コマンドは基本１バイト。データがあとにつづきます。

主なコマンド（[]の中は指令の数字。）

Start [128] .... スタート指令。passiveモードへ遷移。最初にかならずこれをやる。

Safe[131] ...  safeモードに遷移

Full[132] ....  safe -> fullに遷移

Drive[137][vh][vl][rh][rl] ... 並進速度V[mm/s], 回転半径R[mm]で動く。

どちらも正にすると左前に進む。

-500 < V < 500

-2000 < R < 2000

32768 = 0x8000hのとき直進

-1で時計回り、1で反時計回りにその場回転。

V, Rはsignedの２バイト。

あとはLEDとか、センサとか、音楽とか楽しいのが残っていますが、

まあ、とりあえずこれくらいでためしてみますかねぇ。

ルンバ用ケーブル作成＆リンク集

ルンバに接続するケーブルを作っています。

こんな感じ。

参考にしたサイトなど。

• 便利アイテムを売っている。作るのめんどいんでBluetoothのアダプタ買っちゃおうかなぁ。 http://www.protechrobotics.com/proddetail.php?prod=DEV-00684
• 公式コマンド集。公式ってのがすごい。結線情報もここにある。 http://www.irobot.com/images/consumer/hacker/Roomba_SCI_Spec_Manual.pdf
• Cradloomba。偉大なる先輩の作品。 http://www.youtube.com/watch?v=PllYYGakjEU