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等ができます。
一番上のでかいロゴに意味はありません。

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

view plainprint?

1. /** 
2.  * @license New BSD License 
3.  *  
4.  */  
5. package com.ogutti.ros.android.roomba;  
6.   
7. import org.ros.message.geometry_msgs.Twist;  
8. import org.ros.message.std_msgs.Bool;  
9.   
10. import org.ros.namespace.GraphName;  
11. import org.ros.node.NodeMain;  
12. import org.ros.node.topic.Publisher;  
13. import org.ros.node.Node;  
14.   
15. /** 
16.  * class for Controlling a roomba by Twist msg 
17.  *  
18.  */  
19. public class RoombaControllerNode implements NodeMain {  
20.  private Publisher<Twist> velPublisher;  
21.  private Publisher<Bool> cleanPublisher;  
22.  private Publisher<Bool> dockPublisher;  
23.   
24.  @Override  
25.  public void onShutdown(Node arg0) {  
26.  }  
27.   
28.  @Override  
29.  public void onShutdownComplete(Node arg0) {  
30.  }  
31.   
32.  @Override  
33.  public void onStart(Node node) {  
34.   velPublisher = node.newPublisher("/cmd_vel", "geometry_msgs/Twist");  
35.   cleanPublisher = node.newPublisher("/roomba/clean", "std_msgs/Bool");  
36.   dockPublisher = node.newPublisher("/roomba/dock", "std_msgs/Bool");  
37.   
38.  }  
39.   
40.  @Override  
41.  public GraphName getDefaultNodeName() {  
42.   return new GraphName("roomba_controller");  
43.  }  
44.   
45.  /** 
46.   * start/stop cleaning motor 
47.   * @param isOn if true start cleaning, false stop. 
48.   */  
49.  public void publishClean(boolean isOn) {  
50.   if (cleanPublisher != null) {  
51.    Bool msg = new Bool();  
52.    msg.data = isOn;  
53.    cleanPublisher.publish(msg);  
54.   }  
55.  }  
56.   
57.  /** 
58.   * Publish roomba's velocity (Vx, Vtheta) 
59.   * @param linearX twist.linear.x forward speed [m/s] 
60.   * @param angularZ twist.angular.z rotational speed [rad/s] 
61.   */  
62.  public void publishVelocity(double linearX, double angularZ) {  
63.   if (velPublisher != null) {  
64.    Twist vel = new Twist();  
65.    vel.linear.x = linearX;  
66.    vel.angular.z = angularZ;  
67.    velPublisher.publish(vel);  
68.   }  
69.  }  
70.   
71.  /** 
72.   * start docking 
73.   */  
74.  public void publishDock() {  
75.   if (dockPublisher != null) {  
76.    Bool msg = new Bool();  
77.    msg.data = true;  
78.    dockPublisher.publish(msg);  
79.   }  
80.  }  
81. }  

/**
 * @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のクラス

view plainprint?

1. /** 
2.  * @license New BSD License 
3.  */  
4.   
5. package com.ogutti.ros.android.roomba;  
6.   
7. import java.text.DecimalFormat;  
8. import java.util.List;  
9. import android.os.Bundle;  
10. import android.view.View.OnClickListener;  
11. import android.widget.Button;  
12. import android.widget.CompoundButton;  
13. import android.widget.CompoundButton.OnCheckedChangeListener;  
14. import android.widget.SeekBar;  
15. import android.widget.SeekBar.OnSeekBarChangeListener;  
16. import android.widget.TextView;  
17. import android.widget.ToggleButton;  
18.   
19. import org.ros.address.InetAddressFactory;  
20. import org.ros.android.MessageCallable;  
21. import org.ros.android.RosActivity;  
22. import org.ros.android.views.RosTextView;  
23. import org.ros.node.NodeConfiguration;  
24. import org.ros.node.NodeMainExecutor;  
25. import android.hardware.Sensor;  
26. import android.hardware.SensorEvent;  
27. import android.hardware.SensorEventListener;  
28. import android.hardware.SensorManager;  
29. import com.ogutti.ros.android.roomba.R;  
30. import org.ros.message.geometry_msgs.Twist;  
31.   
32. /** 
33.  * MainActivity of this App (roomba controller) 
34.  *  
35.  */  
36. public class MainActivity extends RosActivity implements SensorEventListener {  
37.   
38.  private RosTextView<Twist> rosTextView;  
39.  private SensorManager sensorManager;  
40.  private RoombaControllerNode controllerNode;  
41.    
42.  private static final double LINEAR_VELOCITY_RATE  = 0.05;  
43.  private static final double ANGULAR_VELOCITY_RATE = 0.1;  
44.   
45.  /** 
46.   * 1.0 means max speed, 0.0 means stop always. 
47.   */  
48.  private double speedRate;  
49.   
50.  public MainActivity() {  
51.   super("Roomba Controller", "Roomba Controller");  
52.   speedRate = 0.5;  
53.  }  
54.   
55.  @SuppressWarnings("unchecked")  
56.  @Override  
57.  public void onCreate(Bundle savedInstanceState) {  
58.   super.onCreate(savedInstanceState);  
59.   setContentView(R.layout.main);  
60.   sensorManager = (SensorManager) getSystemService(SENSOR_SERVICE);  
61.   rosTextView = (RosTextView<Twist>) findViewById(R.id.text);  
62.   rosTextView.setTopicName("/cmd_vel");  
63.   rosTextView.setMessageType("geometry_msgs/Twist");  
64.   rosTextView  
65.     .setMessageToStringCallable(new MessageCallable<String, Twist>() {  
66.      @Override  
67.      public String call(Twist message) {  
68.       DecimalFormat df = new DecimalFormat();  
69.       df.setMaximumFractionDigits(2);  
70.   
71.       return "vel_x:\n" + df.format(message.linear.x) + "\n"  
72.         + "vel_theta:\n" + df.format(message.angular.z);  
73.      }  
74.     });  
75.  }  
76.   
77.  @Override  
78.  protected void init(NodeMainExecutor nodeMainExecutor) {  
79.   // create ROS nodes  
80.   controllerNode = new RoombaControllerNode();  
81.   NodeConfiguration nodeConfiguration = NodeConfiguration  
82.     .newPublic(InetAddressFactory.newNonLoopback().getHostName());  
83.   nodeConfiguration.setMasterUri(this.getMasterUri());  
84.   nodeMainExecutor.execute(controllerNode, nodeConfiguration);  
85.   nodeMainExecutor.execute(rosTextView, nodeConfiguration);  
86.   
87.   // set callback for accelerometer  
88.   List<Sensor> sensors = sensorManager  
89.     .getSensorList(Sensor.TYPE_ACCELEROMETER);  
90.   if (sensors.size() > 0) {  
91.    Sensor accelerometer = sensors.get(0);  
92.    sensorManager.registerListener(this, accelerometer,  
93.      SensorManager.SENSOR_DELAY_FASTEST);  
94.   } else {  
95.    android.util.Log.v("MainActivity", "NOT found sensor!");  
96.   }  
97.     
98.   // set toggle button's callback  
99.   ToggleButton tb = (ToggleButton) findViewById(R.id.toggleButton1);  
100.   tb.setOnCheckedChangeListener(new OnCheckedChangeListener() {  
101.    @Override  
102.    public void onCheckedChanged(CompoundButton buttonView,  
103.      boolean isChecked) {  
104.     controllerNode.publishClean(isChecked);  
105.    }  
106.   });  
107.   
108.   // set seekbar callback.   
109.   SeekBar seekBar = (SeekBar) findViewById(R.id.seekBar1);  
110.   final TextView tv1 = (TextView) findViewById(R.id.textView1);  
111.   seekBar.setOnSeekBarChangeListener(new OnSeekBarChangeListener() {  
112.    public void onProgressChanged(SeekBar seekBar, int progress,  
113.      boolean fromUser) {  
114.     tv1.setText("Speed:" + progress + "%");  
115.     speedRate = progress * 0.01f;  
116.    }  
117.   
118.    @Override  
119.    public void onStartTrackingTouch(SeekBar seekBar) {  
120.    }  
121.   
122.    @Override  
123.    public void onStopTrackingTouch(SeekBar seekBar) {  
124.    }  
125.   });  
126.   Button dockButton = (Button) findViewById(R.id.button1);  
127.   dockButton.setOnClickListener(new OnClickListener() {  
128.    @Override  
129.    public void onClick(android.view.View v) {  
130.     controllerNode.publishDock();  
131.    }  
132.   });  
133.   
134.  }  
135.   
136.  @Override  
137.  protected void onResume() {  
138.   super.onResume();  
139.   
140.  }  
141.   
142.  @Override  
143.  protected void onStop() {  
144.   super.onStop();  
145.   sensorManager.unregisterListener(this);  
146.  }  
147.   
148.  @Override  
149.  public void onAccuracyChanged(Sensor sensor, int accuracy) {  
150.  }  
151.   
152.  /** 
153.   * callback of sensor change(accelerometer) 
154.   */  
155.  @Override  
156.  public void onSensorChanged(SensorEvent event) {  
157.   if (event.sensor.getType() == Sensor.TYPE_ACCELEROMETER) {  
158.    double linearX = event.values[1] * -LINEAR_VELOCITY_RATE * speedRate;  
159.    double angularZ = event.values[0] * ANGULAR_VELOCITY_RATE * speedRate;  
160.    if (Math.abs(linearX) < LINEAR_VELOCITY_RATE) {  
161.     linearX = 0.0d;  
162.    }  
163.    if (Math.abs(angularZ) < ANGULAR_VELOCITY_RATE) {  
164.     angularZ = 0.0d;  
165.    }  
166.    controllerNode.publishVelocity(linearX, angularZ);  
167.   }  
168.  }  
169. }  

/**
 * @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アプリは作るのが結構簡単なのでチャレンジしてみてはどうでしょうか。