RosBridgeClientManager.java

/**
 * Created by jimbray on 2018/11/18.
 * Email: jimbray16@gmail.com
 */

public class RosBridgeClientManager {

    private static final String TAG = RosBridgeClientManager.class.getSimpleName();

    private static RosBridgeClientManager instance;

    private ROSBridgeClient mRosBridgeClient = null;

    private String mCurUrl;

    private List<OnRosMessageListener> mROSListenerList;

    private Gson mGson;

    private RosBridgeClientManager() {
        mGson = new Gson();
        EventBus.getDefault().register(this);
    }

    public static RosBridgeClientManager getInstance() {
        if (instance == null) {
            synchronized (RosBridgeClientManager.class) {
                if (instance == null) {
                    instance = new RosBridgeClientManager();
                }
            }
        }
        return instance;
    }


    /**
     * 连接 ROS master
     * @param url ROS master IP
     * @param port ROS master 端口
     * @param listener 连接状态监听器
     */
    public void connect(final String url, int port, final ROSClient.ConnectionStatusListener listener) {
        if (url != null && url.equals(mCurUrl)) {
            // already connected
        } else {
            mRosBridgeClient = new ROSBridgeClient("ws://" + url + ":" + port);
            mRosBridgeClient.connect(new ROSClient.ConnectionStatusListener() {
                @Override
                public void onConnect() {
                    // connected successful
                    App.getInstance().setRosBridgeClient(mRosBridgeClient);
                    mCurUrl = url;
                    if (listener != null) {
                        listener.onConnect();
                    }
                }

                @Override
                public void onDisconnect(boolean normal, String reason, int code) {
                    // client disconnected
                    if (listener != null) {
                        listener.onDisconnect(normal, reason, code);
                    }

                }

                @Override
                public void onError(Exception ex) {
                    // connect error
                    if (listener != null) {
                        listener.onError(ex);
                    }
                }
            });
        }
    }

    public void connect(final String url, final ROSClient.ConnectionStatusListener listener) {
        int port = 9090;
        if (url != null && url.equals(mCurUrl)) {
            // already connected
        } else {
            mRosBridgeClient = new ROSBridgeClient("ws://" + url + ":" + port);
            mRosBridgeClient.connect(new ROSClient.ConnectionStatusListener() {
                @Override
                public void onConnect() {
                    // connected successful
                    App.getInstance().setRosBridgeClient(mRosBridgeClient);
                    mCurUrl = url;
                    if (listener != null) {
                        listener.onConnect();
                    }
                }

                @Override
                public void onDisconnect(boolean normal, String reason, int code) {
                    // client disconnected
                    if (listener != null) {
                        listener.onDisconnect(normal, reason, code);
                    }

                }

                @Override
                public void onError(Exception ex) {
                    // connect error
                    if (listener != null) {
                        listener.onError(ex);
                    }
                }
            });
        }
    }


    /**
     * 发布 topic 消息
     * @param topicName topic名称
     * @param msg 消息
     * @param <T> 消息类型
     */
    public <T> void publishTopic(String topicName, T msg) {
        PublishTopicObject<T> publishTopicObject = new PublishTopicObject<>();
        publishTopicObject.setTopic(topicName);
        publishTopicObject.setMsg(msg);

        String msg_str = mGson.toJson(publishTopicObject);
        if (mRosBridgeClient != null) {
            Log.d(TAG, msg_str);
            mRosBridgeClient.send(msg_str);
            //mRosBridgeClient.send("{\"msg\":\"12121212\",\"op\":\"publish\",\"topic\":\"/chatter\"}");
            //mRosBridgeClient.send("{\"op\":\"publish\",\"topic\":\"/chatter\",\"msg\":{\"data\":\"11111\"}}");

            //mRosBridgeClient.send("{\"op\":\"publish\",\"topic\":\"/turtle1/cmd_vel\",\"msg\":{\"linear\":{\"x\":2.0,\"y\":0.0,\"z\":0.0},\"angular\":{\"x\":0.0,\"y\":0.0,\"z\":1.8}}}");
        }

    }

    /**
     * 注册topic
     * @param topicName topic 名称
     * @param data_type 消息类型
     * @param <T>
     */
    public <T> void advertiseTopic(String topicName, T data_type) {
        AdvertiseTopicObject<T> topic = new AdvertiseTopicObject<>(topicName, data_type, mRosBridgeClient);
        topic.setMessage_type(data_type);
        topic.advertise();

        // 利用 反射获取泛型
//        Class <T> entityClass = (Class <T>) ((ParameterizedType) getClass().getGenericSuperclass()).getActualTypeArguments()[0];
//        Topic topic = new Topic(topicName, entityClass, client);
//        topic.advertise();
    }


    /**
     * 订阅topic
     * @param topicName topic 名称
     * @param listener 消息监听器
     */
    public void subscribeTopic(String topicName, OnRosMessageListener listener) {

        //json方式
        SubscribeTopicObject subscribeTopicObject = new SubscribeTopicObject();
        subscribeTopicObject.setTopic(topicName);

        String msg_str = mGson.toJson(subscribeTopicObject);
        if (mRosBridgeClient != null) {
            mRosBridgeClient.send(msg_str);
            addROSMessageListener(listener);
        }
    }

    /**
     * 取消订阅topic
     * @param topicName
     * @param listener
     */
    public void unSubscribeTopic(String topicName, OnRosMessageListener listener) {
        UnSubscribeTopicObject unSubscribeTopicObject = new UnSubscribeTopicObject();
        unSubscribeTopicObject.setTopic(topicName);

        String msg_str = mGson.toJson(unSubscribeTopicObject);
        if (mRosBridgeClient != null) {
            mRosBridgeClient.send(msg_str);
            removeROSMessageListener(listener);
        }
    }

    public void callAtwoInts() {
        new Thread(new Runnable() {
            @Override
            public void run() {

                Service<AddTwoInstRequest, AddTwoIntsResponse> addTwoIntsResponseService =
                        new Service<>("/add_two_ints", AddTwoInstRequest.class, AddTwoIntsResponse.class, mRosBridgeClient);
                AddTwoInstRequest request = new AddTwoInstRequest();
                request.a = 3;
                request.b = 5;
                addTwoIntsResponseService.callWithHandler(request, new MessageHandler<AddTwoIntsResponse>() {
                    @Override
                    public void onMessage(AddTwoIntsResponse message) {
                        Log.d("add_two_ints_tag", message.sum + "");
                    }
                });

                /*
                Service<Empty, GetTime> getTimeService = new Service<>("/rosapi/get_time", Empty.class, GetTime.class, mRosBridgeClient);
                getTimeService.callWithHandler(new Empty(), new MessageHandler<GetTime>() {
                    @Override
                    public void onMessage(GetTime message) {
                        Log.d("get_time", message.time.toString());
                    }
                });
                */
            }
        }).start();
    }

    public void callGetTime() {
        new Thread(new Runnable() {
            @Override
            public void run() {

                Service<Empty, GetTime> getTimeService = new Service<>("/rosapi/get_time", Empty.class, GetTime.class, mRosBridgeClient);
                getTimeService.callWithHandler(new Empty(), new MessageHandler<GetTime>() {
                    @Override
                    public void onMessage(GetTime message) {
                        Log.d("get_time", message.time.toString());
                    }
                });
            }
        }).start();
    }

    public void callService(String serviceName) {
        /*
        if (mRosBridgeClient != null) {
            String msg = "{\"op\":\"call_service\",\"service\":\"" + "beginner_tutorials/Add_Two_Ints" +"\",\"args\":["+ "\"a\":9, \"b\":6" +"]}";
            mRosBridgeClient.send(msg);
        }
        */

        //Service<Empty, GetTime> getTimeService = new Service<>("/rosapi/get_time", Empty.class, GetTime.class, mRosBridgeClient);
        //getTimeService.call(new Empty());

        /*
        Observable.create(new ObservableOnSubscribe<Integer>() {
            @Override
            public void subscribe(ObservableEmitter<Integer> emitter) throws Exception {
                Service<AddTwoInstRequest, AddTwoIntsResponse> addTwoIntsResponseService =
                        new Service<>("/add_two_ints", AddTwoInstRequest.class, AddTwoIntsResponse.class, mRosBridgeClient);
                AddTwoInstRequest request = new AddTwoInstRequest();
                request.setA(3);
                request.setB(5);
                addTwoIntsResponseService.call(request);
            }
        }).subscribeOn(Schedulers.newThread())
                .observeOn(AndroidSchedulers.mainThread())
                .subscribe(new Consumer<Integer>() {
                    @Override
                    public void accept(Integer integer) throws Exception {

                    }
                });
           */

    }

    public void addROSMessageListener(OnRosMessageListener listener) {
        if (mROSListenerList == null) {
            mROSListenerList = new ArrayList<>();
        }

        if (listener != null) {
            mROSListenerList.add(listener);
        }
    }

    public void removeROSMessageListener(OnRosMessageListener listener) {
        if (listener != null && mROSListenerList != null) {
            mROSListenerList.remove(listener);
        }
    }


    //Receive data from ROS server, send from ROSBridgeWebSocketClient onMessage()
    // using eventbus ?!
    public void onEvent(final PublishEvent event) {
        Log.d(TAG, "receive rosdata -> " + event.msg);

        if (mROSListenerList != null) {
            for (int index = 0 ; index < mROSListenerList.size(); index++) {
                final int curIndex = index;
                if (event.name.equals(ITopicNames.USB_CAM_IMAGE_COMPRESSED)) {
                    Disposable disposable = Observable.create(new ObservableOnSubscribe<RosImageData>() {
                        @Override
                        public void subscribe(ObservableEmitter<RosImageData> emitter) throws Exception {
                            JSONParser jsonParser = new JSONParser();
                            JSONObject jsonObject = (JSONObject)jsonParser.parse(event.msg);
                            String image_base64_data = (String) jsonObject.get("data");
                            RosImageData imageData = new RosImageData();
                            imageData.data = image_base64_data;

                            emitter.onNext(imageData);
                        }
                    }).subscribeOn(Schedulers.io())
                            .observeOn(AndroidSchedulers.mainThread())
                            .subscribe(new Consumer<RosImageData>() {
                                @Override
                                public void accept(RosImageData imageData) {
                                    if (curIndex < mROSListenerList.size() && curIndex >= 0) {
                                        mROSListenerList.get(curIndex).onImageMessageReceive(event.name, imageData);
                                    }
                                }
                            });

                } else {
                    Disposable disposable = Observable.create(new ObservableOnSubscribe<RosStringData>() {
                        @Override
                        public void subscribe(ObservableEmitter<RosStringData> emitter) throws Exception {
                            JSONParser jsonParser = new JSONParser();
                            JSONObject jsonObject = (JSONObject)jsonParser.parse(event.msg);
                            String string_data = (String) jsonObject.get("data");
                            RosStringData stringData = new RosStringData(string_data);
                            emitter.onNext(stringData);
                        }
                    }).subscribeOn(Schedulers.io())
                            .observeOn(AndroidSchedulers.mainThread())
                            .subscribe(new Consumer<RosStringData>() {
                                @Override
                                public void accept(RosStringData stringData) {
                                    if (curIndex < mROSListenerList.size() && curIndex >= 0) {
                                        mROSListenerList.get(curIndex).onStringMessageReceive(event.name, stringData);
                                    }
                                }
                            });


                }
            }
        }
    }

    public interface OnRosMessageListener {
        void onStringMessageReceive(String topicName, RosStringData stringData);
        void onImageMessageReceive(String topicName, RosImageData imageData);
    }

}

RosStringData 等很多类找不到啊