地图导航功能的实现 之 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.setNodeName("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