Android 与ROS机器人交互APP开发笔记-连接机器人

前言

随着人工智能和机器人技术的不断发展，智能机器人已经逐渐融入我们的生活和工作中。而在众多技术的融合中，Android系统和ROS（Robot Operating System）相结合，为智能机器人的发展开辟了一条崭新的道路。我也是正在开发Android 与ROS机器人交互的APP，分享一些开发笔记。

Android 与Ros通信

Android 与Ros通信采用的是ROSBridge通信包,也是参照了开源项目：https://github.com/djilk/ROSBridgeClient。

连接机器人

ROSBridge通信包底层其实是基于WebSocket协议来与Ros机器人通信，Android 设备需要和机器人处于同一局域网内。

     /**
     * 连接地址，根据Ros机器人实际ip和端口组成
     */
    private final String uriString = "ws://192.168.0.200:9090";
    private ROSBridgeClient client;

   /**
     * 连接
     * @param uri 地址
     * @param listener 回调事件
     * @param isDebug
     * @return
     */
    public boolean connect(String uri, final ROSClient.ConnectionStatusListener listener, boolean isDebug) {
        boolean isConnect;
        if (TextUtils.isEmpty(uri)) {
            uri = uriString;
        }
        client = new ROSBridgeClient(uri);
        isConnect = client.connect(listener);
        if (isConnect) {
            if (isDebug) {
                client.setDebug(true);
            }
        }
        return isConnect;
    }

public abstract class ROSClient {

    public ROSClient() {}
    
    public static ROSClient create(String uriString) {
        // if we ever implement other ROSClient types, we'll key off the URI protocol (e.g., ws://)
        // we'd also have to abstract out Topic and Service since they depend on the ROSBridge operations
        return new ROSBridgeClient(uriString);
    }
    
    public abstract boolean connect();
    public abstract boolean connect(ConnectionStatusListener listener);
    public abstract void disconnect();
    public abstract void send(Operation operation);
    public abstract void register(Class<? extends Operation> c,
            String s,
            Class<? extends Message> m,
            FullMessageHandler h);
    public abstract void unregister(Class<? extends Operation> c, String s);
    public abstract void setDebug(boolean debug);
    public abstract String[] getNodes() throws InterruptedException;
    public abstract String[] getTopics() throws InterruptedException;
    public abstract String[] getServices() throws InterruptedException;
    public abstract TypeDef getTopicMessageDetails(String topic) throws InterruptedException;
    public abstract TypeDef getServiceRequestDetails(String service) throws InterruptedException;
    public abstract TypeDef getServiceResponseDetails(String service) throws InterruptedException;
    public abstract TypeDef getTypeDetails(String type) throws InterruptedException;
    public abstract void typeMatch(TypeDef t, Class<? extends Message> c) throws InterruptedException; 
    public abstract Object getUnderlyingClient(); // for debugging
    
    public interface ConnectionStatusListener {
        public void onConnect();
        public void onDisconnect(boolean normal, String reason, int code);
        public void onError(Exception ex);
    }

public class ROSBridgeClient extends ROSClient {
    private String uriString;
    private ROSBridgeWebSocketClient client;
    
    public ROSBridgeClient(String uriString) {
        this.uriString = uriString;
    }
    
    @Override
    public boolean connect() {
        return connect(null);
    }
    
    @Override
    public boolean connect(ConnectionStatusListener listener) {
        boolean result = false;
        client = ROSBridgeWebSocketClient.create(uriString);
        if (client != null) {
            client.setListener(listener);
            try {
                result = client.connectBlocking();
            }
            catch (InterruptedException ex) {}
        }
        return result;
    }
    
    @Override
    public void disconnect() {
        try {
            client.closeBlocking();
        }catch (InterruptedException ex) {}
    }
    
    @Override
    public void send(Operation operation) {
        client.send(operation);
    }

    @Override
    public void setDebug(boolean debug) {
        client.setDebug(debug);
    }
}

public class ROSBridgeWebSocketClient extends WebSocketClient {

    private Registry<Class> classes;
    private Registry<FullMessageHandler> handlers;
    private boolean debug;
    private ROSClient.ConnectionStatusListener listener;
    public static final String DEFAULT_GZIP_CHARSET = "UTF-8";

    ROSBridgeWebSocketClient(URI serverURI) {
        super(serverURI);
        classes = new Registry<Class>();
        handlers = new Registry<FullMessageHandler>();
        Operation.initialize(classes);  // note, this ensures that the Message Map is initialized too
        listener = null;
    }

    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;
    }

    public void setListener(ROSClient.ConnectionStatusListener listener) {
        this.listener = listener;
    }

    @Override
    public void onOpen(ServerHandshake handshakedata) {
        if (listener != null) {
            listener.onConnect();
        }
    }

    public static String unCompress(byte[] bytes, String charset) {
        if (bytes == null || bytes.length == 0) {
            return null;
        }
        ByteArrayOutputStream byteArrayOutputStream = null;
        ByteArrayInputStream byteArrayInputStream = null;
        GZIPInputStream gzipInputStream = null;
        try {
            byteArrayOutputStream = new ByteArrayOutputStream();
            byteArrayInputStream = new ByteArrayInputStream(bytes);
            gzipInputStream = new GZIPInputStream(byteArrayInputStream);
            byte[] buffer = new byte[256];
            int n;
            while ((n = gzipInputStream.read(buffer)) >= 0) {
                byteArrayOutputStream.write(buffer, 0, n);
            }
            //gzipInputStream.transferTo(byteArrayOutputStream);
            closeQuietly(byteArrayInputStream);
            closeQuietly(gzipInputStream);
            closeQuietly(byteArrayOutputStream);
            return byteArrayOutputStream.toString(charset);
        } catch (IOException e) {
            e.printStackTrace();
        }
        closeQuietly(byteArrayInputStream);
        closeQuietly(gzipInputStream);
        closeQuietly(byteArrayOutputStream);
        return null;
    }
    public static String unCompress(byte[] bytes) {
        return unCompress(bytes, DEFAULT_GZIP_CHARSET);
    }
    public static Boolean isCompressed(byte[] bytes) {
        return bytes.length > 2 && (((bytes[0] & 0xFF) == 0x78 &&
                ((bytes[1] & 0xFF) == 0x9C || (bytes[1] & 0xFF) == 0x01 ||
                        (bytes[1] & 0xFF) == 0xDA || (bytes[1] & 0xFF) == 0x5E)) ||
                ((bytes[0] & 0xFF) == 0x1F && (bytes[1] & 0xFF) == 0x8B));
    }
    public static void closeQuietly(Closeable closeable) {
        if (closeable != null) {
            try {
                closeable.close();
            } catch (Exception unused) {
            }
        }
    }

    @Override
    public void onMessage(ByteBuffer bytes) {
        byte[] raw_message_bytes = new byte[bytes.remaining()];;
        bytes.get(raw_message_bytes);
        String decompressed_data = null;
        if(isCompressed(raw_message_bytes)) {
            decompressed_data = unCompress(raw_message_bytes);
            if (decompressed_data == null) {
                return;
            }
        } else {
            decompressed_data = new String(raw_message_bytes);
        }
        onMessage(decompressed_data);
    }


    @Override
    public void onMessage(String message) {
        if (debug) {
            System.out.println("<ROS " + message);
        }
        //System.out.println("ROSBridgeWebSocketClient.onMessage (message): " + message);
        Operation operation = Operation.toOperation(message, classes);
        //System.out.println("ROSBridgeWebSocketClient.onMessage (operation): ");
        //operation.print();

        FullMessageHandler handler = null;
        Message msg = null;
        if (operation instanceof Publish) {
            Publish p = (Publish) operation;
            handler = handlers.lookup(Publish.class, p.topic);
            msg = p.msg;
        }
        else if (operation instanceof ServiceResponse) {
            ServiceResponse r = ((ServiceResponse) operation);
            handler = handlers.lookup(ServiceResponse.class, r.service);
            msg = r.values;
        }
        // later we will add clauses for Fragment, PNG, and Status. When rosbridge has it, we'll have one for service requests.

        // need to handle "result: null" possibility for ROSBridge service responses
        // this is probably some sort of call to the operation for "validation." Do it
        // as part of error handling.


        if (handler != null) {
            handler.onMessage(operation.id, msg);
        } else if (debug) {
            System.out.print("No handler: id# " + operation.id + ", ");
            if (operation instanceof Publish) {
                System.out.println("Publish " + ((Publish) operation).topic);
            } else if (operation instanceof ServiceResponse) {
                System.out.println("Service Response " + ((ServiceResponse) operation).service);
            }
            //operation.print();
        }
    }

    @Override
    public void onClose(int code, String reason, boolean remote) {
        if (listener != null) {
            boolean normal = (remote || (code == CloseFrame.NORMAL));
            listener.onDisconnect(normal, reason, code);
        }
    }

    @Override
    public void onError(Exception ex) {
        if (listener != null) {
            listener.onError(ex);
        } else {
            ex.printStackTrace();
        }
    }

    // There is a bug in V1.2 of java_websockets that seems to appear only in Android, specifically,
    //    it does not shut down the thread and starts using gobs of RAM (symptom is frequent garbage collection).
    //    This method goes into the WebSocketClient object and hard-closes the socket, which causes the thread
    //    to exit (note, just interrupting the thread does not work).
    @Override
    public void closeBlocking() throws InterruptedException {
        super.closeBlocking();
        try {
            Field channelField = this.getClass().getSuperclass().getDeclaredField("channel");
            channelField.setAccessible(true);
            SocketChannel channel = (SocketChannel) channelField.get(this);
            if (channel != null && channel.isOpen()) {
                Socket socket = channel.socket();
                if (socket != null) {
                    socket.close();
                }
            }
        }
        catch (Exception ex) {
            System.out.println("Exception in Websocket close hack.");
            ex.printStackTrace();
        }
    }


    public void send(Operation operation) {
        String json = operation.toJSON();
        if (debug) {
            System.out.println("ROS> " + json);
        }
        send(json);
    }

    public void register(Class<? extends Operation> c,
                         String s,
                         Class<? extends Message> m,
                         FullMessageHandler h) {
        Message.register(m, classes.get(Message.class));
        classes.register(c, s, m);
        if (h != null) {
            handlers.register(c, s, h);
        }
    }

    public void unregister(Class<? extends Operation> c, String s) {
        handlers.unregister(c, s);
        // Note that there is no concept of unregistering a class - it can get replaced is all
    }

    public Class<? extends Message> getRegisteredMessage(String messageString) {
        return classes.lookup(Message.class, messageString);
    }

    public void setDebug(boolean debug) {
        this.debug = debug;
    }
}

下一篇是关于slam地图数据订阅及如何绘制slam地图