summaryrefslogtreecommitdiff
path: root/src/rose.erl
blob: c720dd8a9428471c477e111a1c702a3b408a0900 (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
-module(rose).

-export([decode/1, invoke/2, dispatch/1]).

-include("Remote-Operations-Generic-ROS-PDUs.hrl").

decode(Data) ->
	'Remote-Operations-Generic-ROS-PDUs':decode('ROS', Data).

return(Id, Op, {ok, Data}) ->
	'Remote-Operations-Generic-ROS-PDUs':encode('ROS', {returnResult,
	#'ReturnResult'{invokeId = Id,
	result = #'ReturnResult_result'{opcode = Op, result = Data}}}).

invoke(Op, {ok, Data}) ->
	Id = counter:next(),
	'Remote-Operations-Generic-ROS-PDUs':encode('ROS', {invoke,
	#'Invoke'{invokeId = Id, opcode = Op, argument = Data}}).

dispatch({invoke, #'Invoke'{invokeId = Id, opcode = Op, argument = Data}}) ->
	counter:set(Id),
	case Op of
		{local, 21} ->
			ok;
		{local, 51} ->
			{ok, Esc} = csta:decode(Data),
			io:format("Esc ~p~n", [Esc]),
			ok;
		{local, 71} ->
			ok;
		{local, 74} ->
			ok;
		{local, 211} ->
			{ok, Status} = csta:decodeStatus(Data),
			io:format("Status: ~p~n", [Status]),
			return(Id, Op, csta:statusOk());
		_ ->
			error
	end;

dispatch({returnResult, #'ReturnResult'{invokeId = Id, result = Data}}) ->
	counter:set(Id),
	dispatch(Data);

dispatch({returnError, #'ReturnError'{invokeId = Id, errcode = Code, parameter = Par}}) ->
	counter:set(Id),
	io:format("Err: ~p ~p~n", [Code, Par]),
	error;

dispatch({reject, #'Reject'{invokeId = Id, problem = Problem}}) ->
	counter:set(Id),
	io:format("Problem: ~p~n", [Problem]),
	error;

dispatch(#'ReturnResult_result'{opcode = Op, result = Data}) ->
	case Op of
		{local, 74} ->
			{ok, Status} = 'CSTA-snapshot-device':decode(
				'SnapshotDeviceResult', Data),
			io:format("Result: ~p~n", [Status]);
		_ ->
		io:format("Result: ~p~n", [Data])
	end,
	ok.