r2r/
msg_types.rs

Help
1use crate::error::*;
2use r2r_msg_gen::*;
3use r2r_rcl::{
4    rcl_serialized_message_t, rosidl_action_type_support_t, rosidl_message_type_support_t,
5    rosidl_service_type_support_t,
6};
7use serde::{Deserialize, Serialize};
8use std::{
9    boxed::Box,
10    cell::RefCell,
11    convert::TryInto,
12    fmt::Debug,
13    ops::{Deref, DerefMut},
14};
15
16pub mod generated_msgs {
17    #![allow(clippy::all)]
18    use super::*;
19    include!(concat!(env!("OUT_DIR"), "/_r2r_generated_msgs.rs"));
20    include!(concat!(env!("OUT_DIR"), "/_r2r_generated_untyped_helper.rs"));
21    include!(concat!(env!("OUT_DIR"), "/_r2r_generated_service_helper.rs"));
22    include!(concat!(env!("OUT_DIR"), "/_r2r_generated_action_helper.rs"));
23}
24
25use generated_msgs::{builtin_interfaces, unique_identifier_msgs};
26
27fn vec_to_uuid_bytes<T>(v: Vec<T>) -> [T; 16] {
28    v.try_into().unwrap_or_else(|v: Vec<T>| {
29        panic!("Expected a Vec of length {} but it was {}", 16, v.len())
30    })
31}
32
33/// TODO: maybe expose this somewhere.
34pub(crate) fn uuid_msg_to_uuid(msg: &unique_identifier_msgs::msg::UUID) -> uuid::Uuid {
35    let bytes = vec_to_uuid_bytes(msg.uuid.clone());
36    uuid::Uuid::from_bytes(bytes)
37}
38
39// TODO where is the best place for this?
40thread_local! {
41    pub static SERIALIZED_MESSAGE_CACHE: std::result::Result<RefCell<rcl_serialized_message_t>, i32> = {
42        use r2r_rcl::*;
43
44        let mut msg_buf: rcl_serialized_message_t  = unsafe { rcutils_get_zero_initialized_uint8_array() };
45
46        let ret = unsafe {
47            rcutils_uint8_array_init(
48                &mut msg_buf as *mut rcl_serialized_message_t,
49                0,
50                &rcutils_get_default_allocator(),
51            )
52        };
53
54        if ret != RCL_RET_OK as i32 {
55            Err(ret)
56        } else {
57            Ok(RefCell::new(msg_buf))
58        }
59
60    };
61}
62
63pub trait WrappedTypesupport:
64    Serialize + for<'de> Deserialize<'de> + Default + Debug + Clone
65{
66    type CStruct;
67
68    fn get_ts() -> &'static rosidl_message_type_support_t;
69    fn create_msg() -> *mut Self::CStruct;
70    fn destroy_msg(msg: *mut Self::CStruct);
71    fn from_native(msg: &Self::CStruct) -> Self;
72    fn copy_to_native(&self, msg: &mut Self::CStruct);
73
74    /// This serializes the message using ROS2 methods.
75    fn to_serialized_bytes(&self) -> Result<Vec<u8>> {
76        use r2r_rcl::*;
77
78        SERIALIZED_MESSAGE_CACHE.with(|msg_buf| {
79            let msg = Self::create_msg();
80
81            self.copy_to_native(unsafe { msg.as_mut().expect("not null") });
82
83            let msg_buf: &mut rcl_serialized_message_t = &mut msg_buf
84                .as_ref()
85                .map_err(|err| Error::from_rcl_error(*err))?
86                .borrow_mut();
87
88            let result = unsafe {
89                rmw_serialize(
90                    msg as *const ::std::os::raw::c_void,
91                    Self::get_ts(),
92                    msg_buf as *mut rcl_serialized_message_t,
93                )
94            };
95
96            let data_bytes = if msg_buf.buffer == std::ptr::null_mut() {
97                Vec::new()
98            } else {
99                unsafe {
100                    std::slice::from_raw_parts(msg_buf.buffer, msg_buf.buffer_length).to_vec()
101                }
102            };
103
104            Self::destroy_msg(msg);
105
106            if result == RCL_RET_OK as i32 {
107                Ok(data_bytes)
108            } else {
109                Err(Error::from_rcl_error(result))
110            }
111        })
112    }
113
114    /// This deserializes the message using ROS2 methods.
115    fn from_serialized_bytes(data: &[u8]) -> Result<Self> {
116        use r2r_rcl::*;
117
118        let msg = Self::create_msg();
119
120        let msg_buf = rcl_serialized_message_t {
121            buffer: data.as_ptr() as *mut u8,
122            buffer_length: data.len(),
123            buffer_capacity: data.len(),
124
125            // Since its read only, this should never be used ..
126            allocator: unsafe { rcutils_get_default_allocator() },
127        };
128
129        // Note From the docs of rmw_deserialize, its not clear whether this reuses
130        // any part of msg_buf. However it shouldn't matter since from_native
131        // clones everything again anyway ..
132        let result = unsafe {
133            rmw_deserialize(
134                &msg_buf as *const rcl_serialized_message_t,
135                Self::get_ts(),
136                msg as *mut std::os::raw::c_void,
137            )
138        };
139
140        let ret_val = if result == RCL_RET_OK as i32 {
141            Ok(Self::from_native(unsafe { msg.as_ref().expect("not null") }))
142        } else {
143            Err(Error::from_rcl_error(result))
144        };
145
146        Self::destroy_msg(msg);
147
148        ret_val
149    }
150}
151
152pub trait WrappedServiceTypeSupport: Debug + Clone {
153    type Request: WrappedTypesupport;
154    type Response: WrappedTypesupport;
155
156    fn get_ts() -> &'static rosidl_service_type_support_t;
157}
158
159pub trait WrappedActionTypeSupport: Debug + Clone {
160    type Goal: WrappedTypesupport;
161    type Result: WrappedTypesupport;
162    type Feedback: WrappedTypesupport;
163
164    // internal...
165    type FeedbackMessage: WrappedTypesupport;
166    type SendGoal: WrappedServiceTypeSupport;
167    type GetResult: WrappedServiceTypeSupport;
168
169    fn get_ts() -> &'static rosidl_action_type_support_t;
170
171    fn make_goal_request_msg(
172        goal_id: unique_identifier_msgs::msg::UUID, goal: Self::Goal,
173    ) -> <<Self as WrappedActionTypeSupport>::SendGoal as WrappedServiceTypeSupport>::Request;
174    fn make_goal_response_msg(
175        accepted: bool, stamp: builtin_interfaces::msg::Time,
176    ) -> <<Self as WrappedActionTypeSupport>::SendGoal as WrappedServiceTypeSupport>::Response;
177    fn make_feedback_msg(
178        goal_id: unique_identifier_msgs::msg::UUID, feedback: Self::Feedback,
179    ) -> Self::FeedbackMessage;
180    fn make_result_request_msg(
181        goal_id: unique_identifier_msgs::msg::UUID,
182    ) -> <<Self as WrappedActionTypeSupport>::GetResult as WrappedServiceTypeSupport>::Request;
183    fn make_result_response_msg(
184        status: i8, result: Self::Result,
185    ) -> <<Self as WrappedActionTypeSupport>::GetResult as WrappedServiceTypeSupport>::Response;
186    fn destructure_goal_request_msg(
187        msg: <<Self as WrappedActionTypeSupport>::SendGoal as WrappedServiceTypeSupport>::Request,
188    ) -> (unique_identifier_msgs::msg::UUID, Self::Goal);
189    fn destructure_goal_response_msg(
190        msg: <<Self as WrappedActionTypeSupport>::SendGoal as WrappedServiceTypeSupport>::Response,
191    ) -> (bool, builtin_interfaces::msg::Time);
192    fn destructure_feedback_msg(
193        msg: Self::FeedbackMessage,
194    ) -> (unique_identifier_msgs::msg::UUID, Self::Feedback);
195    fn destructure_result_response_msg(
196        msg: <<Self as WrappedActionTypeSupport>::GetResult as WrappedServiceTypeSupport>::Response,
197    ) -> (i8, Self::Result);
198    fn destructure_result_request_msg(
199        msg: <<Self as WrappedActionTypeSupport>::GetResult as WrappedServiceTypeSupport>::Request,
200    ) -> unique_identifier_msgs::msg::UUID;
201}
202
203/// This struct wraps a RCL message.
204///
205/// It contains a pointer to a C struct.
206pub struct WrappedNativeMsg<T>
207where
208    T: WrappedTypesupport,
209{
210    pub msg: *mut T::CStruct,
211    pub is_loaned: bool,
212    deallocator: Option<Box<dyn FnOnce(*mut T::CStruct)>>,
213}
214
215pub trait VoidPtr {
216    fn void_ptr(&self) -> *const std::os::raw::c_void;
217    fn void_ptr_mut(&mut self) -> *mut std::os::raw::c_void;
218}
219
220#[derive(Debug)]
221pub struct WrappedNativeMsgUntyped {
222    pub ts: &'static rosidl_message_type_support_t,
223    msg: *mut std::os::raw::c_void,
224    destroy: fn(*mut std::os::raw::c_void),
225    msg_to_json: fn(
226        native: *const std::os::raw::c_void,
227    ) -> std::result::Result<serde_json::Value, serde_json::error::Error>,
228    msg_from_json: fn(
229        native: *mut std::os::raw::c_void,
230        json: serde_json::Value,
231    ) -> std::result::Result<(), serde_json::error::Error>,
232}
233
234unsafe impl Send for UntypedServiceSupport {}
235pub struct UntypedServiceSupport {
236    pub ts: &'static rosidl_service_type_support_t,
237    pub make_request_msg: fn() -> WrappedNativeMsgUntyped,
238    pub make_response_msg: fn() -> WrappedNativeMsgUntyped,
239}
240
241impl UntypedServiceSupport {
242    fn new<T>() -> Self
243    where
244        T: WrappedServiceTypeSupport,
245    {
246        let make_request_msg = WrappedNativeMsgUntyped::new::<T::Request>;
247        let make_response_msg = WrappedNativeMsgUntyped::new::<T::Response>;
248
249        UntypedServiceSupport {
250            ts: T::get_ts(),
251            make_request_msg,
252            make_response_msg,
253        }
254    }
255}
256
257// For now only the client side is implemented.
258unsafe impl Send for UntypedActionSupport {}
259pub struct UntypedActionSupport {
260    pub(crate) ts: &'static rosidl_action_type_support_t,
261
262    pub(crate) make_goal_request_msg: Box<
263        dyn Fn(unique_identifier_msgs::msg::UUID, serde_json::Value) -> WrappedNativeMsgUntyped,
264    >,
265    pub(crate) make_goal_response_msg: Box<dyn Fn() -> WrappedNativeMsgUntyped>,
266    pub(crate) destructure_goal_response_msg:
267        Box<dyn Fn(WrappedNativeMsgUntyped) -> (bool, builtin_interfaces::msg::Time)>,
268
269    pub(crate) make_feedback_msg: fn() -> WrappedNativeMsgUntyped,
270    pub(crate) destructure_feedback_msg: Box<
271        dyn Fn(
272            WrappedNativeMsgUntyped,
273        ) -> (unique_identifier_msgs::msg::UUID, Result<serde_json::Value>),
274    >,
275
276    pub(crate) make_result_request_msg:
277        Box<dyn Fn(unique_identifier_msgs::msg::UUID) -> WrappedNativeMsgUntyped>,
278    pub(crate) make_result_response_msg: Box<dyn Fn() -> WrappedNativeMsgUntyped>,
279    pub(crate) destructure_result_response_msg:
280        Box<dyn Fn(WrappedNativeMsgUntyped) -> (i8, Result<serde_json::Value>)>,
281}
282
283impl UntypedActionSupport {
284    fn new<T>() -> Self
285    where
286        T: WrappedActionTypeSupport,
287    {
288        // TODO: this is terrible. These closures perform json (de)serialization just to move the data.
289        // FIX.
290
291        let make_goal_request_msg = Box::new(|goal_id, goal| {
292            let goal_msg: T::Goal =
293                serde_json::from_value(goal).expect("TODO: move this error handling");
294            let request_msg = T::make_goal_request_msg(goal_id, goal_msg);
295            let json = serde_json::to_value(request_msg).expect("TODO: move this error handling");
296            let native_untyped = WrappedNativeMsgUntyped::new::<
297                <<T as WrappedActionTypeSupport>::SendGoal as WrappedServiceTypeSupport>::Request,
298            >();
299            native_untyped
300                .from_json(json)
301                .expect("TODO: move this error handling");
302            native_untyped
303        });
304
305        let make_goal_response_msg = Box::new(|| {
306            WrappedNativeMsgUntyped::new::<
307                <<T as WrappedActionTypeSupport>::SendGoal as WrappedServiceTypeSupport>::Response,
308            >()
309        });
310
311        let destructure_goal_response_msg = Box::new(|msg: WrappedNativeMsgUntyped| {
312            let msg = unsafe {
313                <<<T as WrappedActionTypeSupport>::SendGoal as WrappedServiceTypeSupport>::Response>
314                 ::from_native(&*(msg.msg as *const <<<T as WrappedActionTypeSupport>::SendGoal as
315                                                      WrappedServiceTypeSupport>::Response as WrappedTypesupport>::CStruct))
316            };
317            T::destructure_goal_response_msg(msg)
318        });
319
320        let make_feedback_msg = WrappedNativeMsgUntyped::new::<T::FeedbackMessage>;
321
322        let destructure_feedback_msg = Box::new(|msg: WrappedNativeMsgUntyped| {
323            let msg = unsafe {
324                T::FeedbackMessage::from_native(
325                    &*(msg.msg as *const <T::FeedbackMessage as WrappedTypesupport>::CStruct),
326                )
327            };
328            let (uuid, feedback) = T::destructure_feedback_msg(msg);
329            let json = serde_json::to_value(feedback).map_err(|serde_err| Error::SerdeError {
330                err: serde_err.to_string(),
331            });
332            (uuid, json)
333        });
334
335        let make_result_request_msg = Box::new(|uuid_msg: unique_identifier_msgs::msg::UUID| {
336            let request_msg = T::make_result_request_msg(uuid_msg);
337            let json = serde_json::to_value(request_msg).expect("TODO: move this error handling");
338
339            let native_untyped = WrappedNativeMsgUntyped::new::<
340                <<T as WrappedActionTypeSupport>::GetResult as WrappedServiceTypeSupport>::Request,
341            >();
342
343            native_untyped
344                .from_json(json)
345                .expect("TODO: move this error handling");
346            native_untyped
347        });
348
349        let make_result_response_msg = Box::new(|| {
350            WrappedNativeMsgUntyped::new::<
351                <<T as WrappedActionTypeSupport>::GetResult as WrappedServiceTypeSupport>::Response,
352            >()
353        });
354
355        let destructure_result_response_msg = Box::new(|msg: WrappedNativeMsgUntyped| {
356            let msg = unsafe {
357                <<<T as WrappedActionTypeSupport>::GetResult as WrappedServiceTypeSupport>::Response>
358                 ::from_native(&*(msg.msg as *const <<<T as WrappedActionTypeSupport>::GetResult as
359                                                      WrappedServiceTypeSupport>::Response as WrappedTypesupport>::CStruct))
360            };
361            let (status, result) = T::destructure_result_response_msg(msg);
362            let json = serde_json::to_value(result).map_err(|serde_err| Error::SerdeError {
363                err: serde_err.to_string(),
364            });
365            (status, json)
366        });
367
368        UntypedActionSupport {
369            ts: T::get_ts(),
370            make_goal_request_msg,
371            make_goal_response_msg,
372            destructure_goal_response_msg,
373            make_feedback_msg,
374            destructure_feedback_msg,
375            make_result_request_msg,
376            make_result_response_msg,
377            destructure_result_response_msg,
378            // destructure_goal_response_msg,
379            // make_request_msg,
380            // make_response_msg,
381        }
382    }
383}
384
385impl WrappedNativeMsgUntyped {
386    fn new<T>() -> Self
387    where
388        T: WrappedTypesupport,
389    {
390        let destroy = |native: *mut std::os::raw::c_void| {
391            let native_msg = native as *mut T::CStruct;
392            T::destroy_msg(native_msg);
393        };
394
395        let msg_to_json = |native: *const std::os::raw::c_void| {
396            let msg = unsafe { T::from_native(&*(native as *const T::CStruct)) };
397            serde_json::to_value(&msg)
398        };
399
400        let msg_from_json = |native: *mut std::os::raw::c_void, json: serde_json::Value| {
401            serde_json::from_value(json).map(|msg: T| unsafe {
402                msg.copy_to_native(&mut *(native as *mut T::CStruct));
403            })
404        };
405
406        WrappedNativeMsgUntyped {
407            ts: T::get_ts(),
408            msg: T::create_msg() as *mut std::os::raw::c_void,
409            destroy,
410            msg_to_json,
411            msg_from_json,
412        }
413    }
414
415    pub fn from_serialized_bytes(&mut self, data: &[u8]) -> Result<()> {
416        // TODO: Copy paste from above, should refactor later.
417        use r2r_rcl::*;
418
419        let msg_buf = rcl_serialized_message_t {
420            buffer: data.as_ptr() as *mut u8,
421            buffer_length: data.len(),
422            buffer_capacity: data.len(),
423
424            // Since its read only, this should never be used ..
425            allocator: unsafe { rcutils_get_default_allocator() },
426        };
427
428        // Note From the docs of rmw_deserialize, its not clear whether this reuses
429        // any part of msg_buf. However it shouldn't matter since from_native
430        // clones everything again anyway ..
431        let result = unsafe {
432            rmw_deserialize(&msg_buf as *const rcl_serialized_message_t, self.ts, self.msg)
433        };
434
435        if result == RCL_RET_OK as i32 {
436            Ok(())
437        } else {
438            Err(Error::from_rcl_error(result))
439        }
440    }
441
442    pub fn to_json(&self) -> Result<serde_json::Value> {
443        let json = (self.msg_to_json)(self.msg);
444        json.map_err(|serde_err| Error::SerdeError {
445            err: serde_err.to_string(),
446        })
447    }
448
449    pub fn from_json(&self, json: serde_json::Value) -> Result<()> {
450        (self.msg_from_json)(self.msg, json).map_err(|serde_err| Error::SerdeError {
451            err: serde_err.to_string(),
452        })
453    }
454}
455
456impl VoidPtr for WrappedNativeMsgUntyped {
457    fn void_ptr(&self) -> *const std::os::raw::c_void {
458        self.msg as *const _ as *const std::os::raw::c_void
459    }
460
461    fn void_ptr_mut(&mut self) -> *mut std::os::raw::c_void {
462        self.msg as *mut _ as *mut std::os::raw::c_void
463    }
464}
465
466impl Drop for WrappedNativeMsgUntyped {
467    fn drop(&mut self) {
468        (self.destroy)(self.msg);
469    }
470}
471
472impl<T> WrappedNativeMsg<T>
473where
474    T: WrappedTypesupport + 'static,
475{
476    pub fn new() -> Self {
477        WrappedNativeMsg {
478            msg: T::create_msg(),
479            deallocator: Some(Box::new(T::destroy_msg)),
480            is_loaned: false,
481        }
482    }
483
484    pub fn from(msg: &T) -> Self {
485        let mut native_msg = Self::new();
486        msg.copy_to_native(&mut native_msg);
487        native_msg
488    }
489
490    pub fn from_loaned(
491        msg: *mut T::CStruct, deallocator: Box<dyn FnOnce(*mut <T as WrappedTypesupport>::CStruct)>,
492    ) -> Self {
493        WrappedNativeMsg {
494            msg,
495            deallocator: Some(deallocator),
496            is_loaned: true,
497        }
498    }
499
500    pub fn release(&mut self) {
501        self.deallocator.take();
502    }
503}
504
505impl<T> Default for WrappedNativeMsg<T>
506where
507    T: WrappedTypesupport + 'static,
508{
509    fn default() -> Self {
510        Self::new()
511    }
512}
513
514impl<T: 'static> VoidPtr for WrappedNativeMsg<T>
515where
516    T: WrappedTypesupport,
517{
518    fn void_ptr(&self) -> *const std::os::raw::c_void {
519        self.msg as *const _ as *const std::os::raw::c_void
520    }
521
522    fn void_ptr_mut(&mut self) -> *mut std::os::raw::c_void {
523        self.msg as *mut _ as *mut std::os::raw::c_void
524    }
525}
526
527impl<T> Drop for WrappedNativeMsg<T>
528where
529    T: WrappedTypesupport,
530{
531    fn drop(&mut self) {
532        if let Some(deallocator) = self.deallocator.take() {
533            (deallocator)(self.msg);
534        }
535    }
536}
537
538impl<T> Deref for WrappedNativeMsg<T>
539where
540    T: WrappedTypesupport,
541{
542    type Target = T::CStruct;
543
544    fn deref(&self) -> &Self::Target {
545        unsafe { &(*self.msg) }
546    }
547}
548
549impl<T> DerefMut for WrappedNativeMsg<T>
550where
551    T: WrappedTypesupport,
552{
553    fn deref_mut(&mut self) -> &mut Self::Target {
554        unsafe { &mut (*self.msg) }
555    }
556}
557
558#[cfg(test)]
559mod tests {
560    use super::{generated_msgs::*, *};
561    use r2r_rcl::*;
562
563    #[test]
564    fn test_ros_str() {
565        let hej = "hej hopp";
566        let mut msg = WrappedNativeMsg::<std_msgs::msg::String>::new();
567        msg.data.assign(hej);
568        assert_eq!(msg.data.to_str(), hej);
569    }
570
571    #[test]
572    fn test_copy_fields() {
573        let msg_orig = std_msgs::msg::String { data: "hej".into() };
574        let rosmsg = WrappedNativeMsg::<std_msgs::msg::String>::from(&msg_orig);
575        let msg2 = std_msgs::msg::String::from_native(&rosmsg);
576        assert_eq!(msg_orig, msg2);
577    }
578
579    #[test]
580    fn test_introspection_string() {
581        unsafe {
582            use std::ffi::CStr;
583
584            let x = rosidl_typesupport_introspection_c__get_message_type_support_handle__std_msgs__msg__String();
585            let members = (*x).data as *const rosidl_typesupport_introspection_c__MessageMembers;
586            println!("{:#?}", *members);
587
588            assert_eq!((*members).member_count_, 1);
589
590            let s = CStr::from_ptr((*members).message_namespace_)
591                .to_str()
592                .unwrap();
593            assert_eq!(s, "std_msgs__msg");
594
595            let s = CStr::from_ptr((*members).message_name_).to_str().unwrap();
596            assert_eq!(s, "String");
597
598            let member = (*members).members_;
599            println!("member: {:#?}", *member);
600            let field_name = CStr::from_ptr((*member).name_).to_str().unwrap();
601            let type_id = (*member).type_id_;
602            let is_array = (*member).is_array_;
603            assert_eq!(field_name, "data");
604            assert_eq!(type_id, 16u8); // rosidl_typesupport_introspection_c__ROS_TYPE_STRING as u8
605            assert!(!is_array);
606        }
607    }
608
609    #[test]
610    #[should_panic] // we are testing that we cannot have to many elements in a fixed sized field
611    fn test_fixedsizearray() {
612        unsafe {
613            let x = rosidl_typesupport_introspection_c__get_message_type_support_handle__geometry_msgs__msg__AccelWithCovariance();
614            let members = (*x).data as *const rosidl_typesupport_introspection_c__MessageMembers;
615            println!("{:#?}", *members);
616
617            let memberslice =
618                std::slice::from_raw_parts((*members).members_, (*members).member_count_ as usize);
619            for member in memberslice {
620                println!("member: {:#?}", *member);
621            }
622
623            let msg_native = WrappedNativeMsg::<geometry_msgs::msg::AccelWithCovariance>::new();
624            let mut msg = geometry_msgs::msg::AccelWithCovariance::from_native(&msg_native);
625            println!("{:#?}", msg);
626            msg.covariance[0] = 10.0;
627            msg.covariance[10] = 10.0;
628            msg.covariance[35] = 99.0;
629            msg.covariance.push(4444.0);
630            let msg_native2 =
631                WrappedNativeMsg::<geometry_msgs::msg::AccelWithCovariance>::from(&msg);
632            let msg2 = geometry_msgs::msg::AccelWithCovariance::from_native(&msg_native2);
633            println!("{:#?}", msg2);
634        }
635    }
636
637    #[test]
638    #[should_panic] // we are testing that we cannot have to many elements in a fixed sized field
639    fn test_capped_sequence() {
640        // float64[<=3] dimensions in the .msg translates to a float64 sequence AND an array size field. handle it...
641        unsafe {
642            let x = rosidl_typesupport_introspection_c__get_message_type_support_handle__shape_msgs__msg__SolidPrimitive();
643            let members = (*x).data as *const rosidl_typesupport_introspection_c__MessageMembers;
644            println!("{:#?}", *members);
645
646            let memberslice =
647                std::slice::from_raw_parts((*members).members_, (*members).member_count_ as usize);
648            for member in memberslice {
649                println!("member: {:#?}", *member);
650            }
651
652            let msg_native = WrappedNativeMsg::<shape_msgs::msg::SolidPrimitive>::new();
653            let mut msg = shape_msgs::msg::SolidPrimitive::from_native(&msg_native);
654            println!("{:#?}", msg);
655            msg.dimensions.push(1.0);
656            msg.dimensions.push(1.0);
657            msg.dimensions.push(1.0);
658            msg.dimensions.push(1.0); // only three elements allowed
659            let _msg_native2 = WrappedNativeMsg::<shape_msgs::msg::SolidPrimitive>::from(&msg);
660        }
661    }
662
663    #[test]
664    fn test_generation_string_use() {
665        let msg = std_msgs::msg::String { data: "hej".into() };
666        let msg2 = msg.clone();
667        let msg_native = WrappedNativeMsg::<std_msgs::msg::String>::from(&msg2);
668        let msg2 = std_msgs::msg::String::from_native(&msg_native);
669        assert_eq!(msg, msg2)
670    }
671
672    #[test]
673    fn test_generation_bool_use() {
674        let msg = std_msgs::msg::Bool { data: true };
675        let msg_native = WrappedNativeMsg::<std_msgs::msg::Bool>::from(&msg);
676        let msg2 = std_msgs::msg::Bool::from_native(&msg_native);
677        assert_eq!(msg, msg2);
678    }
679
680    #[test]
681    fn test_float_sequence() {
682        use trajectory_msgs::msg::*;
683        let native = WrappedNativeMsg::<JointTrajectoryPoint>::new();
684        let mut msg = JointTrajectoryPoint::from_native(&native);
685        msg.positions.push(39.0);
686        msg.positions.push(34.0);
687        let new_native = WrappedNativeMsg::<JointTrajectoryPoint>::from(&msg);
688        let new_msg = JointTrajectoryPoint::from_native(&new_native);
689        println!("{:#?}", new_msg);
690        assert_eq!(msg, new_msg);
691    }
692
693    #[test]
694    fn test_deault() {
695        use trajectory_msgs::msg::*;
696        let mut msg: JointTrajectoryPoint = Default::default();
697        msg.positions.push(39.0);
698        msg.positions.push(34.0);
699        let mut new_native = WrappedNativeMsg::<JointTrajectoryPoint>::from(&msg);
700        unsafe { *new_native.positions.data = 88.9 };
701        let new_msg = JointTrajectoryPoint::from_native(&new_native);
702        println!("{:#?}", new_msg);
703        assert_ne!(msg, new_msg);
704    }
705
706    #[test]
707    fn test_untyped_json() {
708        let mut msg = trajectory_msgs::msg::JointTrajectoryPoint::default();
709        msg.positions.push(39.0);
710        msg.positions.push(34.0);
711        let json = serde_json::to_value(msg.clone()).unwrap();
712
713        let native =
714            WrappedNativeMsgUntyped::new_from("trajectory_msgs/msg/JointTrajectoryPoint").unwrap();
715        native.from_json(json.clone()).unwrap();
716        let json2 = native.to_json().unwrap();
717        assert_eq!(json, json2);
718
719        let msg2: trajectory_msgs::msg::JointTrajectoryPoint =
720            serde_json::from_value(json2).unwrap();
721        assert_eq!(msg, msg2);
722    }
723
724    #[test]
725    fn test_from_loaned() {
726        type MsgType = trajectory_msgs::msg::JointTrajectoryPoint;
727        type CMsgType = <MsgType as WrappedTypesupport>::CStruct;
728
729        let borrowed_msg = MsgType::create_msg();
730
731        let native = WrappedNativeMsg::<MsgType>::from_loaned(
732            borrowed_msg as *mut CMsgType,
733            Box::new(|_: *mut CMsgType| {}),
734        );
735
736        assert!(native.void_ptr() == borrowed_msg as *mut core::ffi::c_void);
737    }
738
739    #[test]
740    fn test_serialization_fixed_size() {
741        let message = std_msgs::msg::Int32 { data: 10 };
742
743        let bytes = message.to_serialized_bytes().unwrap();
744
745        let message_2 = std_msgs::msg::Int32::from_serialized_bytes(&bytes).unwrap();
746
747        assert_eq!(message.data, message_2.data);
748
749        let bytes_2 = message_2.to_serialized_bytes().unwrap();
750        let bytes_3 = message_2.to_serialized_bytes().unwrap();
751
752        assert_eq!(bytes, bytes_2);
753        assert_eq!(bytes, bytes_3);
754    }
755
756    #[test]
757    fn test_serialization_dynamic_size() {
758        let message = std_msgs::msg::Int32MultiArray {
759            layout: std_msgs::msg::MultiArrayLayout::default(),
760            data: vec![10, 20, 30],
761        };
762
763        let bytes = message.to_serialized_bytes().unwrap();
764
765        let message_2 = std_msgs::msg::Int32MultiArray::from_serialized_bytes(&bytes).unwrap();
766
767        assert_eq!(message.data, message_2.data);
768
769        let bytes_2 = message_2.to_serialized_bytes().unwrap();
770        let bytes_3 = message_2.to_serialized_bytes().unwrap();
771
772        assert_eq!(bytes, bytes_2);
773        assert_eq!(bytes, bytes_3);
774    }
775
776    #[cfg(r2r__test_msgs__msg__Defaults)]
777    #[test]
778    fn test_untyped_json_default() {
779        // from the msg definition file:
780        // bool bool_value true
781        // byte byte_value 50
782        // char char_value 100
783        // float32 float32_value 1.125
784        // ...
785
786        // let's try to change only a few fields.
787        let json = r#"
788        {
789            "byte_value": 255,
790            "float32_value": 3.14
791        }"#;
792
793        let native = WrappedNativeMsgUntyped::new_from("test_msgs/msg/Defaults").unwrap();
794        let v: serde_json::Value = serde_json::from_str(json).unwrap();
795        native.from_json(v).expect("could make default msg");
796        let json2 = native.to_json().unwrap();
797        let msg2: test_msgs::msg::Defaults = serde_json::from_value(json2).unwrap();
798
799        assert!(msg2.bool_value); // the default
800        assert_eq!(msg2.byte_value, 255); // from our json
801        assert_eq!(msg2.char_value, 100); // the default
802        assert_eq!(msg2.float32_value, 3.14); // from our json
803    }
804
805    #[cfg(r2r__test_msgs__msg__Arrays)]
806    #[test]
807    fn test_test_msgs_array() {
808        let mut msg = test_msgs::msg::Arrays::default();
809        println!("msg: {:?}", msg.string_values);
810        msg.string_values = vec!["hej".to_string(), "hopp".to_string(), "stropp".to_string()];
811
812        let msg_native = WrappedNativeMsg::<test_msgs::msg::Arrays>::from(&msg);
813        let msg2 = test_msgs::msg::Arrays::from_native(&msg_native);
814
815        assert_eq!(msg, msg2);
816    }
817
818    #[cfg(r2r__test_msgs__msg__Arrays)]
819    #[test]
820    #[should_panic]
821    fn test_test_msgs_array_too_few_elems() {
822        let mut msg = test_msgs::msg::Arrays::default();
823        println!("msg: {:?}", msg.string_values);
824        msg.string_values = vec!["hej".to_string(), "hopp".to_string()];
825        let _msg_native = WrappedNativeMsg::<test_msgs::msg::Arrays>::from(&msg);
826    }
827
828    #[cfg(r2r__test_msgs__msg__WStrings)]
829    #[test]
830    fn test_test_msgs_wstring() {
831        let mut msg = test_msgs::msg::WStrings::default();
832        let rust_str = "ハローワールド";
833        msg.wstring_value = rust_str.to_string();
834        let native = WrappedNativeMsg::<test_msgs::msg::WStrings>::from(&msg);
835        println!("msg: {:?}", msg);
836        let msg2 = test_msgs::msg::WStrings::from_native(&native);
837        assert_eq!(msg.wstring_value, msg2.wstring_value);
838    }
839
840    #[cfg(r2r__example_interfaces__srv__AddTwoInts)]
841    #[test]
842    fn test_service_msgs() {
843        use example_interfaces::srv::AddTwoInts;
844        let req = AddTwoInts::Request {
845            a: 5,
846            ..Default::default()
847        };
848        let rn = WrappedNativeMsg::<_>::from(&req);
849        let req2 = AddTwoInts::Request::from_native(&rn);
850        println!("req2 {:?}", req2);
851        assert_eq!(req, req2);
852
853        let resp = AddTwoInts::Response {
854            sum: 5,
855            ..Default::default()
856        };
857        let rn = WrappedNativeMsg::<_>::from(&resp);
858        let resp2 = AddTwoInts::Response::from_native(&rn);
859        println!("resp {:?}", resp2);
860        assert_eq!(resp, resp2);
861    }
862
863    #[cfg(r2r__std_srvs__srv__Empty)]
864    #[test]
865    fn test_empty_msgs() {
866        use std_srvs::srv::Empty;
867        let req = Empty::Request::default();
868        let resp = Empty::Response::default();
869        println!("req {:?}", req);
870        println!("resp {:?}", resp);
871    }
872
873    #[cfg(r2r__example_interfaces__action__Fibonacci)]
874    #[test]
875    fn test_action_msgs() {
876        use example_interfaces::action::Fibonacci;
877        let goal = Fibonacci::Goal { order: 5 };
878        let gn = WrappedNativeMsg::<_>::from(&goal);
879        let goal2 = Fibonacci::Goal::from_native(&gn);
880        println!("goal2 {:?}", goal2);
881        assert_eq!(goal, goal2);
882
883        let res = Fibonacci::Result {
884            sequence: vec![1, 2, 3],
885        };
886        let rn = WrappedNativeMsg::<_>::from(&res);
887        let res2 = Fibonacci::Result::from_native(&rn);
888        println!("res2 {:?}", res2);
889        assert_eq!(res, res2);
890
891        let fb = Fibonacci::Feedback {
892            sequence: vec![4, 3, 6],
893        };
894        let fbn = WrappedNativeMsg::<_>::from(&fb);
895        let fb2 = Fibonacci::Feedback::from_native(&fbn);
896        println!("feedback2 {:?}", fb2);
897        assert_eq!(fb, fb2);
898
899        let fb = WrappedNativeMsg::<Fibonacci::Feedback>::new();
900        let fb1 = Fibonacci::Feedback::default();
901        let fb2 = Fibonacci::Feedback::from_native(&fb);
902        assert_eq!(fb1, fb2);
903    }
904
905    #[cfg(r2r__example_interfaces__srv__AddTwoInts)]
906    #[test]
907    fn test_untyped_service_support() {
908        let ts = UntypedServiceSupport::new_from("example_interfaces/srv/AddTwoInts").unwrap();
909        let msg = (ts.make_request_msg)();
910        let json = msg.to_json();
911        // the message should contain something (default msg)
912        assert!(!json.unwrap().to_string().is_empty());
913    }
914
915    #[cfg(r2r__example_interfaces__action__Fibonacci)]
916    #[test]
917    fn test_untyped_action_support() {
918        use example_interfaces::action::Fibonacci;
919
920        let ts = UntypedActionSupport::new_from("example_interfaces/action/Fibonacci").unwrap();
921        let uuid = unique_identifier_msgs::msg::UUID::default();
922        let goal = Fibonacci::Goal { order: 5 };
923        let json_goal = serde_json::to_value(&goal).unwrap();
924        let json_request = (ts.make_goal_request_msg)(uuid, json_goal)
925            .to_json()
926            .unwrap();
927        // the message should contain something (default msg)
928        assert!(!json_request.to_string().is_empty());
929    }
930
931    #[cfg(r2r__action_msgs__msg__GoalStatus)]
932    #[test]
933    fn test_msg_constants() {
934        use action_msgs::msg::GoalStatus;
935        let gs = GoalStatus::default();
936
937        assert_eq!(gs.status, GoalStatus::STATUS_UNKNOWN as i8);
938        assert_eq!(0, GoalStatus::STATUS_UNKNOWN as i8);
939        assert_eq!(1, GoalStatus::STATUS_ACCEPTED as i8);
940        assert_eq!(2, GoalStatus::STATUS_EXECUTING as i8);
941        assert_eq!(3, GoalStatus::STATUS_CANCELING as i8);
942        assert_eq!(4, GoalStatus::STATUS_SUCCEEDED as i8);
943        assert_eq!(5, GoalStatus::STATUS_CANCELED as i8);
944        assert_eq!(6, GoalStatus::STATUS_ABORTED as i8);
945
946        use action_msgs::srv::CancelGoal;
947        let cgr = CancelGoal::Response::default();
948
949        assert_eq!(cgr.return_code, CancelGoal::Response::ERROR_NONE as i8);
950        assert_eq!(0, CancelGoal::Response::ERROR_NONE as i8);
951        assert_eq!(1, CancelGoal::Response::ERROR_REJECTED as i8);
952        assert_eq!(2, CancelGoal::Response::ERROR_UNKNOWN_GOAL_ID as i8);
953        assert_eq!(3, CancelGoal::Response::ERROR_GOAL_TERMINATED as i8);
954    }
955}
r2r/msg_types.rs

r2r/
msg_types.rs
