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
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159

use std::marker::PhantomData;

#[allow(unused_imports)]
use log::{debug, error, info, warn};
use rustdds::{rpc::*, *};
use serde::{Deserialize, Serialize};

use crate::message::Message;
use super::{
  request_id::{RmwRequestId, SequenceNumber},
  ClientGeneric, ServerGeneric, Service, ServiceMapping,
};

// This is reverse-engineered from
// https://github.com/ros2/rmw_cyclonedds/blob/master/rmw_cyclonedds_cpp/src/rmw_node.cpp
// https://github.com/ros2/rmw_cyclonedds/blob/master/rmw_cyclonedds_cpp/src/serdata.hpp
// This is a header that Cyclone puts in DDS messages. Same header is used for
// Requst and Response.
#[derive(Serialize, Deserialize)]
pub struct CycloneWrapper<R> {
  guid_second_half: [u8; 8], // CycolenDDS RMW only sends last 8 bytes of client GUID
  sequence_number_high: i32,
  sequence_number_low: u32,
  response_or_request: R, // ROS2 payload
}
impl<R: Message> Message for CycloneWrapper<R> {}

pub struct CycloneServiceMapping<S> {
  phantom: PhantomData<S>,
}

pub type CycloneServer<S> = ServerGeneric<S, CycloneServiceMapping<S>>;
pub type CycloneClient<S> = ClientGeneric<S, CycloneServiceMapping<S>>;

pub struct CycloneClientState {
  client_guid: GUID,
  sequence_number_counter: SequenceNumber,
}

impl CycloneClientState {
  pub fn new(client_guid: GUID) -> CycloneClientState {
    CycloneClientState {
      client_guid,
      sequence_number_counter: SequenceNumber::zero(),
    }
  }
}

impl<S> ServiceMapping<S> for CycloneServiceMapping<S>
where
  S: Service,
  S::Request: Clone,
{
  type RequestWrapper = CycloneWrapper<S::Request>;
  type ResponseWrapper = CycloneWrapper<S::Response>;

  fn unwrap_request(
    wrapped: &Self::RequestWrapper,
    sample_info: &SampleInfo,
  ) -> (RmwRequestId, S::Request) {
    let r_id = RmwRequestId {
      writer_guid: sample_info.writer_guid(),
      // Last 8 bytes of writer (client) GUID should be in the wrapper also
      sequence_number: SequenceNumber::from_high_low(
        wrapped.sequence_number_high,
        wrapped.sequence_number_low,
      ),
    };

    (r_id, wrapped.response_or_request.clone())
  }

  fn wrap_response(
    r_id: RmwRequestId,
    response: S::Response,
  ) -> (Self::ResponseWrapper, Option<SampleIdentity>) {
    (cyclone_wrap(r_id, response), None)
  }

  type ClientState = CycloneClientState;

  fn wrap_request(
    state: &mut Self::ClientState,
    request: S::Request,
  ) -> (Self::RequestWrapper, Option<RmwRequestId>) {
    state.sequence_number_counter = state.sequence_number_counter.next();

    // Generate new request id
    let request_id = RmwRequestId {
      writer_guid: state.client_guid,
      sequence_number: state.sequence_number_counter,
    };

    (cyclone_wrap(request_id, request), Some(request_id))
  }

  fn request_id_after_wrap(
    state: &mut Self::ClientState,
    _write_result: SampleIdentity,
  ) -> RmwRequestId {
    // Request id is what we generated into header.
    // write_result is irrelevant, so we discard it.
    RmwRequestId {
      writer_guid: state.client_guid,
      sequence_number: state.sequence_number_counter,
    }
  }

  fn unwrap_response(
    state: &mut Self::ClientState,
    wrapped: Self::ResponseWrapper,
    sample_info: SampleInfo,
  ) -> (RmwRequestId, S::Response) {
    let mut client_guid_bytes = [0; 16];
    {
      let (first_half, second_half) = client_guid_bytes.split_at_mut(8);

      // this seems a bit odd, but source is
      // https://github.com/ros2/rmw_connextdds/blob/master/rmw_connextdds_common/src/common/rmw_impl.cpp
      // function take_response()
      first_half.copy_from_slice(&state.client_guid.to_bytes().as_slice()[0..8]);

      // This is received in the wrapper header
      second_half.copy_from_slice(&sample_info.writer_guid().to_bytes()[8..16]);
    }

    let r_id = RmwRequestId {
      writer_guid: GUID::from_bytes(client_guid_bytes),
      sequence_number: SequenceNumber::from_high_low(
        wrapped.sequence_number_high,
        wrapped.sequence_number_low,
      ),
    };

    (r_id, wrapped.response_or_request)
  }

  fn new_client_state(request_sender: GUID) -> Self::ClientState {
    CycloneClientState {
      client_guid: request_sender,
      sequence_number_counter: SequenceNumber::zero(),
    }
  }
}

fn cyclone_wrap<R>(r_id: RmwRequestId, response_or_request: R) -> CycloneWrapper<R> {
  let sn = r_id.sequence_number;

  let mut guid_second_half = [0; 8];
  // writer_guid means client GUID (i.e. request writer)
  guid_second_half.copy_from_slice(&r_id.writer_guid.to_bytes()[8..16]);

  CycloneWrapper {
    guid_second_half,
    sequence_number_high: sn.high(),
    sequence_number_low: sn.low(),
    response_or_request,
  }
}