128 lines
3.4 KiB
Dart
128 lines
3.4 KiB
Dart
|
import 'dart:async';
|
|||
|
import 'dart:ffi';
|
|||
|
import 'package:flutter/material.dart';
|
|||
|
|
|||
|
import 'ros.dart';
|
|||
|
import 'request.dart';
|
|||
|
import 'dart:math';
|
|||
|
|
|||
|
typedef ActionHandler = void Function(Map<String, dynamic> response);
|
|||
|
typedef UUID = List;
|
|||
|
class GoalHandle {
|
|||
|
ActionHandler onSendGoal;
|
|||
|
ActionHandler onFeedback;
|
|||
|
ActionHandler onResult;
|
|||
|
|
|||
|
GoalHandle({
|
|||
|
required this.onSendGoal,
|
|||
|
required this.onFeedback,
|
|||
|
required this.onResult,
|
|||
|
});
|
|||
|
}
|
|||
|
// 每次发送goal都要构造Action
|
|||
|
class Action {
|
|||
|
Ros ros;
|
|||
|
String name;
|
|||
|
String type;
|
|||
|
// 每次发送goal时,都会生成唯一一个uuid,不能出现重复,用于后续发送cancel_goal,
|
|||
|
Map<String, List<num>> uuids = {};
|
|||
|
|
|||
|
StreamSubscription? goalListener;
|
|||
|
StreamSubscription? feedbackListener;
|
|||
|
StreamSubscription? resultListener;
|
|||
|
|
|||
|
StreamSubscription? cancelListener;
|
|||
|
|
|||
|
GoalHandle goalHandle;
|
|||
|
|
|||
|
Action({
|
|||
|
required this.name,
|
|||
|
required this.ros,
|
|||
|
required this.type,
|
|||
|
required this.goalHandle,
|
|||
|
});
|
|||
|
List<num> generateRandomString(int length) {
|
|||
|
const characters = 'ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789';
|
|||
|
Random random = Random();
|
|||
|
return List.generate(length, (index) {
|
|||
|
return characters[random.nextInt(characters.length)].codeUnitAt(0);
|
|||
|
});
|
|||
|
}
|
|||
|
void _bindFeedbackAndResult(String callId) {
|
|||
|
final feedbackLis = ros.stream;
|
|||
|
feedbackLis.listen((message) {
|
|||
|
goalHandle.onFeedback(message);
|
|||
|
});
|
|||
|
|
|||
|
final resultReceiver = ros.stream.where((message) => message['op'] == 'action_result' && message['id'] == callId)
|
|||
|
.map((Map<String, dynamic> message) => message['result'] == null
|
|||
|
? message['values']!
|
|||
|
: message['values']);
|
|||
|
|
|||
|
resultListener = resultReceiver.listen((d) {
|
|||
|
goalHandle.onResult(d);
|
|||
|
resultListener!.cancel();
|
|||
|
});
|
|||
|
}
|
|||
|
|
|||
|
String sendGoal(dynamic req) {
|
|||
|
final callId = ros.requestServiceCaller(name);
|
|||
|
final completer = Completer<List>();
|
|||
|
final uuid = generateRandomString(16);
|
|||
|
var reqObj = Request(
|
|||
|
op: 'send_action_goal',
|
|||
|
id: callId,
|
|||
|
action: req['action'],
|
|||
|
type: req['type'],
|
|||
|
args: {
|
|||
|
"goal_id" : {
|
|||
|
"uuid": uuid,
|
|||
|
},
|
|||
|
"goal": req['args'],
|
|||
|
},
|
|||
|
);
|
|||
|
if (uuids.containsKey(callId)) {
|
|||
|
uuids[callId] = uuid;
|
|||
|
} else {
|
|||
|
uuids.addAll({callId: uuid});
|
|||
|
}
|
|||
|
ros.send(reqObj);
|
|||
|
|
|||
|
final receiver = ros.stream.where((message) {
|
|||
|
return message['id'] == callId && message['op'] != 'cancel_goal_response';
|
|||
|
} );
|
|||
|
goalListener = receiver.listen((d) {
|
|||
|
if (d["op"] == "action_result") {
|
|||
|
goalHandle.onResult(d);
|
|||
|
goalListener!.cancel();
|
|||
|
} else if (d["op"] == "action_feedback") {
|
|||
|
goalHandle.onFeedback(d);
|
|||
|
} else if (d["op"] == 'send_goal_response') {
|
|||
|
completer.complete(uuids[callId]);
|
|||
|
goalHandle.onSendGoal(d);
|
|||
|
}
|
|||
|
});
|
|||
|
|
|||
|
return callId;
|
|||
|
}
|
|||
|
|
|||
|
Future cancelGoal(dynamic req, String callId, ActionHandler onCancel) {
|
|||
|
final completer = Completer();
|
|||
|
var cancelReq = Request(
|
|||
|
op: 'cancel_action_goal',
|
|||
|
id: callId,
|
|||
|
action: req['action'],
|
|||
|
);
|
|||
|
print("cancelReq: ${cancelReq.toJson()}");
|
|||
|
ros.send(cancelReq);
|
|||
|
final receiver = ros.stream.where(
|
|||
|
(message) => message['id'] == callId && message['op'] == 'cancel_goal_response'
|
|||
|
);
|
|||
|
cancelListener = receiver.listen((d) {
|
|||
|
onCancel(d);
|
|||
|
completer.complete();
|
|||
|
cancelListener!.cancel();
|
|||
|
});
|
|||
|
return completer.future;
|
|||
|
}
|
|||
|
}
|