Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
/**
* 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);
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.