地图导航功能的实现 之 ROS通信

地图导航分为三个部分，第一个部分为雷达部分的ROS系统，第二部分为通信部分，一般有http协议，ros协议（ROSjava和Rosbridge协议），zmq协议等，第三部分为安卓端和用户交互部分。这里介绍一下通信部分。

一. RosJava协议

ROS官方支持的绑定只有C++和Python，所以目前安卓想与ROS通信，必须借助官方的ROSjava包，通过ROSjava实现与ROS端的通信并进行地图绘制。使用ROSjava开发，需要熟悉gradle脚本，对rosjava进行编译，当然也可以直接使用官方提供的demo，在相应的基础上进行开发。

RosAndroid初始化map地图显示：

protected void init(NodeMainExecutor nodeMainExecutor) {

super.init(nodeMainExecutor);

this.nodeMainExecutor = nodeMainExecutor;
nodeConfiguration = NodeConfiguration.newPublic(InetAddressFactory
.newNonLoopback().getHostAddress(), getMasterUri());
String joyTopic = remaps.get(getString(R.string.joystick_topic));
String camTopic = remaps.get(getString(R.string.camera_topic));

NameResolver appNameSpace = getMasterNameSpace();
joyTopic = appNameSpace.resolve(joyTopic).toString();
camTopic = appNameSpace.resolve(camTopic).toString();
cameraView.setTopicName(camTopic);
virtualJoystickView.setTopicName(joyTopic);

nodeMainExecutor.execute(cameraView,
nodeConfiguration.setNodeName("android/camera_view"));
nodeMainExecutor.execute(virtualJoystickView,
nodeConfiguration.setNod
4000
eName("android/virtual_joystick"));
ViewControlLayer viewControlLayer = new ViewControlLayer(this,
nodeMainExecutor.getScheduledExecutorService(), cameraView,
mapView, mainLayout, sideLayout, params);

String mapTopic   = remaps.get(getString(R.string.map_topic));
String scanTopic  = remaps.get(getString(R.string.scan_topic));
String robotFrame = (String) params.get("robot_frame", getString(R.string.robot_frame));

occupancyGridLayer = new OccupancyGridLayer(appNameSpace.resolve(mapTopic).toString());
laserScanLayer = new LaserScanLayer(appNameSpace.resolve(scanTopic).toString());
robotLayer = new RobotLayer(robotFrame);

mapView.addLayer(viewControlLayer);
mapView.addLayer(occupancyGridLayer);
mapView.addLayer(laserScanLayer);
mapView.addLayer(robotLayer);

mapView.init(nodeMainExecutor);
viewControlLayer.addListener(new CameraControlListener() {
@Override
public void onZoom(float focusX, float focusY, float factor) {
Logger.v("focusX1: " + focusX + "focusY1:" + focusY + "factor1:" + factor);
}
@Override
public void onDoubleTap(float x, float y) {
Logger.v("x: " + x + "y:" + y );
}
@Override
public void onTranslate(float distanceX, float distanceY) {
Logger.v("distanceX: " + distanceX + "distanceY:" + distanceY );
}
@Override
public void onRotate(float focusX, float focusY, double deltaAngle) {
Logger.v("focusX2: " + focusX + "focusY2:" + focusY + "factor2:" + deltaAngle);
}
});
nodeMainExecutor.execute(mapView, nodeConfiguration.setNodeName("android/map_view"));

}

保存地图：

try {
final MapManager mapManager = new MapManager(MakeMapActivity.this, remaps);
if (name != null) {
mapManager.setMapName(name);
}
mapManager.setNameResolver(getMasterNameSpace());
mapManager.registerCallback(new MapManager.StatusCallback() {
@Override
public void timeoutCallback() {
safeDismissWaitingDialog();
safeShowNotiDialog("Error", "Timeout");
}
@Override
public void onSuccessCallback(SaveMapResponse arg0) {
safeDismissWaitingDialog();
safeShowNotiDialog("Success", "Map saving success!");
}
@Override
public void onFailureCallback(Exception e) {
safeDismissWaitingDialog();
safeShowNotiDialog("Error", e.getMessage());
}
});

nodeMainExecutor.execute(mapManager,
nodeConfiguration.setNodeName("android/save_map"));

} catch (Exception e) {
e.printStackTrace();
safeShowNotiDialog("Error", "Error during saving: " + e.toString());
}

二.rosbridge协议
对websocket的一直封装协议。rosbridge相对于rosjava更方便与简洁，可以自定义地图数据节点json协议。

通过：广播，话题，订阅，发布来与ros系统进行通信。

通过WebSocketClient连接ros服务端：

boolean conneSucc = client.connect(new ROSClient.ConnectionStatusListener() {
@Override
public void onConnect() {
client.setDebug(true);
((RCApplication)getApplication()).setRosClient(client);
showTip("Connect ROS success");
Log.d(TAG,"Connect ROS success");
startActivity(new Intent(MainActivity.this,NodesActivity.class));
}

@Override
public void onDisconnect(boolean normal, String reason, int code) {
showTip("ROS disconnect");
Log.d(TAG,"ROS disconnect");
}

@Override
public void onError(Exception ex) {
ex.printStackTrace();
showTip("ROS communication error");
Log.d(TAG,"ROS communication error");
}
});
public boolean connect(ROSClient.ConnectionStatusListener listener) {
boolean result = false;
client = ROSBridgeWebSocketClient.create(uriString);
if (client != null) {
client.setListener(listener);
try {
result = client.connectBlocking();
}
catch (InterruptedException ex) {}
}
return result;
}
public static ROSBridgeWebSocketClient create(String URIString) {
ROSBridgeWebSocketClient client = null;
try {
URI uri = new URI(URIString);
client = new ROSBridgeWebSocketClient(uri);
}
catch (URISyntaxException ex) {
ex.printStackTrace();
}
return client;
}

通信广播/话题/订阅/发布消息：

@MessageType(string = "rosapi/Services")
public class Services extends Message {
public String[] services;
}

@MessageType(string = "rosapi/Topic")
public class Topic extends Message {
public String topic;

public Topic() {}

public Topic(String topic) {
this.topic = topic;
}
}

public class PublishEvent {
public String msg;
public String id;
public String name;
public String op;

public PublishEvent(Operation operation,String name, String content) {
if(operation != null) {
id = operation.id;
op = operation.op;
}
this.name = name;
msg = content;
}
}
@MessageType(string = "call_service")
public class CallService extends Operation {
@Indicator public String service;
@Indicated @AsArray public Message args;
public Integer fragment_size; // use Integer for optional items
public String compression;

public CallService() {}

public CallService(String service, Message args) {
this.service = service;
this.args = args;
}
}

通过获取/发送的json进行通信：

{\"op\":\"call_service\",\"service\":\"/add_two_ints\",\"args\":{\"a\":2,\"b\":8}}
{\"op\":\"call_service\",\"service\":\"scanmap\",\"args\":{\"type\":\"save\"},\"info\":{\"mapName\":\"tianyicesuo\",\"type\":8}}");//"{\"op\":\"call_service\",\"service\":\"scanmap\",\"args\":{\"a\":\"start\"}}
{\"op\":\"call_service\",\"service\":\"scanmap\",\"args\":{\"type\":\"save\"},\"info\":{\"mapName\":\"tianyicesuo\",\"type\":8}}

RosAndroid DEMO