#![allow(non_camel_case_types)]
#![allow(clippy::not_unsafe_ptr_arg_deref)]
#![allow(clippy::needless_borrow)]
use std::os::raw::c_char;
use std::ptr;
use std::slice;
use crate::builtin_interfaces::{Duration, Time};
use crate::cdr::{self, CdrFixed};
use crate::edgefirst_msgs;
use crate::foxglove_msgs;
use crate::geometry_msgs::{self, *};
use crate::sensor_msgs::{self, NavSatStatus};
use crate::std_msgs;
const EINVAL: i32 = libc::EINVAL;
const EBADMSG: i32 = libc::EBADMSG;
const ENOBUFS: i32 = libc::ENOBUFS;
fn set_errno(code: i32) {
errno::set_errno(errno::Errno(code));
}
fn str_as_c(s: &str) -> *const c_char {
if s.is_empty() {
c"".as_ptr()
} else {
s.as_ptr() as *const c_char
}
}
unsafe fn c_to_str<'a>(s: *const c_char) -> &'a str {
if s.is_null() {
""
} else {
std::ffi::CStr::from_ptr(s).to_str().unwrap_or("")
}
}
unsafe fn erase_lifetime(s: &[u8]) -> &'static [u8] {
unsafe { slice::from_raw_parts(s.as_ptr(), s.len()) }
}
macro_rules! check_null_ret_null {
($ptr:expr) => {
if $ptr.is_null() {
set_errno(EINVAL);
return ptr::null_mut();
}
};
}
#[no_mangle]
pub extern "C" fn ros_bytes_free(bytes: *mut u8, len: usize) {
if !bytes.is_null() && len > 0 {
unsafe {
drop(Vec::from_raw_parts(bytes, len, len));
}
}
}
fn encode_fixed_to_buf<T: CdrFixed>(val: &T, buf: *mut u8, cap: usize, written: *mut usize) -> i32 {
let bytes = match cdr::encode_fixed(val) {
Ok(b) => b,
Err(_) => {
set_errno(EBADMSG);
return -1;
}
};
let size = bytes.len();
unsafe {
if !written.is_null() {
*written = size;
}
}
if buf.is_null() {
return 0; }
if cap < size {
set_errno(ENOBUFS);
return -1;
}
unsafe {
ptr::copy_nonoverlapping(bytes.as_ptr(), buf, size);
}
0
}
fn decode_fixed_from_buf<T: CdrFixed>(data: *const u8, len: usize) -> Result<T, ()> {
if data.is_null() {
set_errno(EINVAL);
return Err(());
}
if len < cdr::CDR_HEADER_SIZE {
set_errno(EBADMSG);
return Err(());
}
let slice = unsafe { slice::from_raw_parts(data, len) };
cdr::decode_fixed(slice).map_err(|_| set_errno(EBADMSG))
}
#[no_mangle]
pub extern "C" fn ros_time_encode(
buf: *mut u8,
cap: usize,
written: *mut usize,
sec: i32,
nanosec: u32,
) -> i32 {
let t = Time { sec, nanosec };
encode_fixed_to_buf(&t, buf, cap, written)
}
#[no_mangle]
pub extern "C" fn ros_time_decode(
data: *const u8,
len: usize,
sec: *mut i32,
nanosec: *mut u32,
) -> i32 {
match decode_fixed_from_buf::<Time>(data, len) {
Ok(t) => unsafe {
if !sec.is_null() {
*sec = t.sec;
}
if !nanosec.is_null() {
*nanosec = t.nanosec;
}
0
},
Err(()) => -1,
}
}
#[no_mangle]
pub extern "C" fn ros_duration_encode(
buf: *mut u8,
cap: usize,
written: *mut usize,
sec: i32,
nanosec: u32,
) -> i32 {
let d = Duration { sec, nanosec };
encode_fixed_to_buf(&d, buf, cap, written)
}
#[no_mangle]
pub extern "C" fn ros_duration_decode(
data: *const u8,
len: usize,
sec: *mut i32,
nanosec: *mut u32,
) -> i32 {
match decode_fixed_from_buf::<Duration>(data, len) {
Ok(d) => unsafe {
if !sec.is_null() {
*sec = d.sec;
}
if !nanosec.is_null() {
*nanosec = d.nanosec;
}
0
},
Err(()) => -1,
}
}
#[no_mangle]
pub extern "C" fn ros_vector3_encode(
buf: *mut u8,
cap: usize,
written: *mut usize,
x: f64,
y: f64,
z: f64,
) -> i32 {
encode_fixed_to_buf(&Vector3 { x, y, z }, buf, cap, written)
}
#[no_mangle]
pub extern "C" fn ros_vector3_decode(
data: *const u8,
len: usize,
x: *mut f64,
y: *mut f64,
z: *mut f64,
) -> i32 {
match decode_fixed_from_buf::<Vector3>(data, len) {
Ok(v) => unsafe {
if !x.is_null() {
*x = v.x;
}
if !y.is_null() {
*y = v.y;
}
if !z.is_null() {
*z = v.z;
}
0
},
Err(()) => -1,
}
}
#[no_mangle]
pub extern "C" fn ros_point_encode(
buf: *mut u8,
cap: usize,
written: *mut usize,
x: f64,
y: f64,
z: f64,
) -> i32 {
encode_fixed_to_buf(&Point { x, y, z }, buf, cap, written)
}
#[no_mangle]
pub extern "C" fn ros_point_decode(
data: *const u8,
len: usize,
x: *mut f64,
y: *mut f64,
z: *mut f64,
) -> i32 {
match decode_fixed_from_buf::<Point>(data, len) {
Ok(v) => unsafe {
if !x.is_null() {
*x = v.x;
}
if !y.is_null() {
*y = v.y;
}
if !z.is_null() {
*z = v.z;
}
0
},
Err(()) => -1,
}
}
#[no_mangle]
pub extern "C" fn ros_quaternion_encode(
buf: *mut u8,
cap: usize,
written: *mut usize,
x: f64,
y: f64,
z: f64,
w: f64,
) -> i32 {
encode_fixed_to_buf(&Quaternion { x, y, z, w }, buf, cap, written)
}
#[no_mangle]
pub extern "C" fn ros_quaternion_decode(
data: *const u8,
len: usize,
x: *mut f64,
y: *mut f64,
z: *mut f64,
w: *mut f64,
) -> i32 {
match decode_fixed_from_buf::<Quaternion>(data, len) {
Ok(v) => unsafe {
if !x.is_null() {
*x = v.x;
}
if !y.is_null() {
*y = v.y;
}
if !z.is_null() {
*z = v.z;
}
if !w.is_null() {
*w = v.w;
}
0
},
Err(()) => -1,
}
}
#[no_mangle]
pub extern "C" fn ros_pose_encode(
buf: *mut u8,
cap: usize,
written: *mut usize,
px: f64,
py: f64,
pz: f64,
ox: f64,
oy: f64,
oz: f64,
ow: f64,
) -> i32 {
let val = Pose {
position: Point {
x: px,
y: py,
z: pz,
},
orientation: Quaternion {
x: ox,
y: oy,
z: oz,
w: ow,
},
};
encode_fixed_to_buf(&val, buf, cap, written)
}
#[no_mangle]
pub extern "C" fn ros_pose_decode(
data: *const u8,
len: usize,
px: *mut f64,
py: *mut f64,
pz: *mut f64,
ox: *mut f64,
oy: *mut f64,
oz: *mut f64,
ow: *mut f64,
) -> i32 {
match decode_fixed_from_buf::<Pose>(data, len) {
Ok(v) => unsafe {
if !px.is_null() {
*px = v.position.x;
}
if !py.is_null() {
*py = v.position.y;
}
if !pz.is_null() {
*pz = v.position.z;
}
if !ox.is_null() {
*ox = v.orientation.x;
}
if !oy.is_null() {
*oy = v.orientation.y;
}
if !oz.is_null() {
*oz = v.orientation.z;
}
if !ow.is_null() {
*ow = v.orientation.w;
}
0
},
Err(()) => -1,
}
}
#[no_mangle]
pub extern "C" fn ros_transform_encode(
buf: *mut u8,
cap: usize,
written: *mut usize,
tx: f64,
ty: f64,
tz: f64,
rx: f64,
ry: f64,
rz: f64,
rw: f64,
) -> i32 {
let val = Transform {
translation: Vector3 {
x: tx,
y: ty,
z: tz,
},
rotation: Quaternion {
x: rx,
y: ry,
z: rz,
w: rw,
},
};
encode_fixed_to_buf(&val, buf, cap, written)
}
#[no_mangle]
pub extern "C" fn ros_transform_decode(
data: *const u8,
len: usize,
tx: *mut f64,
ty: *mut f64,
tz: *mut f64,
rx: *mut f64,
ry: *mut f64,
rz: *mut f64,
rw: *mut f64,
) -> i32 {
match decode_fixed_from_buf::<Transform>(data, len) {
Ok(v) => unsafe {
if !tx.is_null() {
*tx = v.translation.x;
}
if !ty.is_null() {
*ty = v.translation.y;
}
if !tz.is_null() {
*tz = v.translation.z;
}
if !rx.is_null() {
*rx = v.rotation.x;
}
if !ry.is_null() {
*ry = v.rotation.y;
}
if !rz.is_null() {
*rz = v.rotation.z;
}
if !rw.is_null() {
*rw = v.rotation.w;
}
0
},
Err(()) => -1,
}
}
#[no_mangle]
pub extern "C" fn ros_twist_encode(
buf: *mut u8,
cap: usize,
written: *mut usize,
lx: f64,
ly: f64,
lz: f64,
ax: f64,
ay: f64,
az: f64,
) -> i32 {
let val = Twist {
linear: Vector3 {
x: lx,
y: ly,
z: lz,
},
angular: Vector3 {
x: ax,
y: ay,
z: az,
},
};
encode_fixed_to_buf(&val, buf, cap, written)
}
#[no_mangle]
pub extern "C" fn ros_twist_decode(
data: *const u8,
len: usize,
lx: *mut f64,
ly: *mut f64,
lz: *mut f64,
ax: *mut f64,
ay: *mut f64,
az: *mut f64,
) -> i32 {
match decode_fixed_from_buf::<Twist>(data, len) {
Ok(v) => unsafe {
if !lx.is_null() {
*lx = v.linear.x;
}
if !ly.is_null() {
*ly = v.linear.y;
}
if !lz.is_null() {
*lz = v.linear.z;
}
if !ax.is_null() {
*ax = v.angular.x;
}
if !ay.is_null() {
*ay = v.angular.y;
}
if !az.is_null() {
*az = v.angular.z;
}
0
},
Err(()) => -1,
}
}
#[no_mangle]
pub extern "C" fn ros_accel_encode(
buf: *mut u8,
cap: usize,
written: *mut usize,
lx: f64,
ly: f64,
lz: f64,
ax: f64,
ay: f64,
az: f64,
) -> i32 {
let val = Accel {
linear: Vector3 {
x: lx,
y: ly,
z: lz,
},
angular: Vector3 {
x: ax,
y: ay,
z: az,
},
};
encode_fixed_to_buf(&val, buf, cap, written)
}
#[no_mangle]
pub extern "C" fn ros_accel_decode(
data: *const u8,
len: usize,
lx: *mut f64,
ly: *mut f64,
lz: *mut f64,
ax: *mut f64,
ay: *mut f64,
az: *mut f64,
) -> i32 {
match decode_fixed_from_buf::<Accel>(data, len) {
Ok(v) => unsafe {
if !lx.is_null() {
*lx = v.linear.x;
}
if !ly.is_null() {
*ly = v.linear.y;
}
if !lz.is_null() {
*lz = v.linear.z;
}
if !ax.is_null() {
*ax = v.angular.x;
}
if !ay.is_null() {
*ay = v.angular.y;
}
if !az.is_null() {
*az = v.angular.z;
}
0
},
Err(()) => -1,
}
}
#[no_mangle]
pub extern "C" fn ros_nav_sat_status_encode(
buf: *mut u8,
cap: usize,
written: *mut usize,
status: i8,
service: u16,
) -> i32 {
encode_fixed_to_buf(&NavSatStatus { status, service }, buf, cap, written)
}
#[no_mangle]
pub extern "C" fn ros_nav_sat_status_decode(
data: *const u8,
len: usize,
status: *mut i8,
service: *mut u16,
) -> i32 {
match decode_fixed_from_buf::<NavSatStatus>(data, len) {
Ok(v) => unsafe {
if !status.is_null() {
*status = v.status;
}
if !service.is_null() {
*service = v.service;
}
0
},
Err(()) => -1,
}
}
fn return_cdr_bytes(cdr: Vec<u8>, out_bytes: *mut *mut u8, out_len: *mut usize) -> i32 {
let len = cdr.len();
let ptr = Box::into_raw(cdr.into_boxed_slice()) as *mut u8;
unsafe {
if !out_bytes.is_null() {
*out_bytes = ptr;
}
if !out_len.is_null() {
*out_len = len;
}
}
0
}
pub struct ros_header_t(std_msgs::Header<&'static [u8]>);
#[no_mangle]
pub extern "C" fn ros_header_from_cdr(data: *const u8, len: usize) -> *mut ros_header_t {
check_null_ret_null!(data);
let slice = unsafe { slice::from_raw_parts(data, len) };
match std_msgs::Header::from_cdr(unsafe { erase_lifetime(slice) }) {
Ok(v) => Box::into_raw(Box::new(ros_header_t(v))),
Err(_) => {
set_errno(EBADMSG);
ptr::null_mut()
}
}
}
#[no_mangle]
pub extern "C" fn ros_header_free(view: *mut ros_header_t) {
if !view.is_null() {
unsafe {
drop(Box::from_raw(view));
}
}
}
#[no_mangle]
pub extern "C" fn ros_header_get_stamp_sec(view: *const ros_header_t) -> i32 {
if view.is_null() {
return 0;
}
unsafe { (*view).0.stamp().sec }
}
#[no_mangle]
pub extern "C" fn ros_header_get_stamp_nanosec(view: *const ros_header_t) -> u32 {
if view.is_null() {
return 0;
}
unsafe { (*view).0.stamp().nanosec }
}
#[no_mangle]
pub extern "C" fn ros_header_get_frame_id(view: *const ros_header_t) -> *const c_char {
if view.is_null() {
return ptr::null();
}
str_as_c(unsafe { (*view).0.frame_id() })
}
#[no_mangle]
pub extern "C" fn ros_header_encode(
out_bytes: *mut *mut u8,
out_len: *mut usize,
stamp_sec: i32,
stamp_nanosec: u32,
frame_id: *const c_char,
) -> i32 {
let fid = unsafe { c_to_str(frame_id) };
let v = match std_msgs::Header::new(Time::new(stamp_sec, stamp_nanosec), fid) {
Ok(v) => v,
Err(_) => {
set_errno(EBADMSG);
return -1;
}
};
return_cdr_bytes(v.into_cdr(), out_bytes, out_len)
}
pub struct ros_image_t(sensor_msgs::Image<&'static [u8]>);
#[no_mangle]
pub extern "C" fn ros_image_from_cdr(data: *const u8, len: usize) -> *mut ros_image_t {
check_null_ret_null!(data);
let slice = unsafe { slice::from_raw_parts(data, len) };
match sensor_msgs::Image::from_cdr(unsafe { erase_lifetime(slice) }) {
Ok(v) => Box::into_raw(Box::new(ros_image_t(v))),
Err(_) => {
set_errno(EBADMSG);
ptr::null_mut()
}
}
}
#[no_mangle]
pub extern "C" fn ros_image_free(view: *mut ros_image_t) {
if !view.is_null() {
unsafe {
drop(Box::from_raw(view));
}
}
}
#[no_mangle]
pub extern "C" fn ros_image_get_stamp_sec(view: *const ros_image_t) -> i32 {
if view.is_null() {
return 0;
}
unsafe { (*view).0.stamp().sec }
}
#[no_mangle]
pub extern "C" fn ros_image_get_stamp_nanosec(view: *const ros_image_t) -> u32 {
if view.is_null() {
return 0;
}
unsafe { (*view).0.stamp().nanosec }
}
#[no_mangle]
pub extern "C" fn ros_image_get_frame_id(view: *const ros_image_t) -> *const c_char {
if view.is_null() {
return ptr::null();
}
str_as_c(unsafe { (*view).0.frame_id() })
}
#[no_mangle]
pub extern "C" fn ros_image_get_height(view: *const ros_image_t) -> u32 {
if view.is_null() {
return 0;
}
unsafe { (*view).0.height() }
}
#[no_mangle]
pub extern "C" fn ros_image_get_width(view: *const ros_image_t) -> u32 {
if view.is_null() {
return 0;
}
unsafe { (*view).0.width() }
}
#[no_mangle]
pub extern "C" fn ros_image_get_encoding(view: *const ros_image_t) -> *const c_char {
if view.is_null() {
return ptr::null();
}
str_as_c(unsafe { (*view).0.encoding() })
}
#[no_mangle]
pub extern "C" fn ros_image_get_is_bigendian(view: *const ros_image_t) -> u8 {
if view.is_null() {
return 0;
}
unsafe { (*view).0.is_bigendian() }
}
#[no_mangle]
pub extern "C" fn ros_image_get_step(view: *const ros_image_t) -> u32 {
if view.is_null() {
return 0;
}
unsafe { (*view).0.step() }
}
#[no_mangle]
pub extern "C" fn ros_image_get_data(view: *const ros_image_t, out_len: *mut usize) -> *const u8 {
if view.is_null() {
return ptr::null();
}
let data = unsafe { (*view).0.data() };
unsafe {
if !out_len.is_null() {
*out_len = data.len();
}
}
data.as_ptr()
}
#[no_mangle]
pub extern "C" fn ros_image_encode(
out_bytes: *mut *mut u8,
out_len: *mut usize,
stamp_sec: i32,
stamp_nanosec: u32,
frame_id: *const c_char,
height: u32,
width: u32,
encoding: *const c_char,
is_bigendian: u8,
step: u32,
data: *const u8,
data_len: usize,
) -> i32 {
let fid = unsafe { c_to_str(frame_id) };
let enc = unsafe { c_to_str(encoding) };
let d = if data.is_null() {
&[]
} else {
unsafe { slice::from_raw_parts(data, data_len) }
};
let v = match sensor_msgs::Image::new(
Time::new(stamp_sec, stamp_nanosec),
fid,
height,
width,
enc,
is_bigendian,
step,
d,
) {
Ok(v) => v,
Err(_) => {
set_errno(EBADMSG);
return -1;
}
};
return_cdr_bytes(v.into_cdr(), out_bytes, out_len)
}
pub struct ros_compressed_image_t(sensor_msgs::CompressedImage<&'static [u8]>);
#[no_mangle]
pub extern "C" fn ros_compressed_image_from_cdr(
data: *const u8,
len: usize,
) -> *mut ros_compressed_image_t {
check_null_ret_null!(data);
let slice = unsafe { slice::from_raw_parts(data, len) };
match sensor_msgs::CompressedImage::from_cdr(unsafe { erase_lifetime(slice) }) {
Ok(v) => Box::into_raw(Box::new(ros_compressed_image_t(v))),
Err(_) => {
set_errno(EBADMSG);
ptr::null_mut()
}
}
}
#[no_mangle]
pub extern "C" fn ros_compressed_image_free(view: *mut ros_compressed_image_t) {
if !view.is_null() {
unsafe {
drop(Box::from_raw(view));
}
}
}
#[no_mangle]
pub extern "C" fn ros_compressed_image_get_stamp_sec(view: *const ros_compressed_image_t) -> i32 {
if view.is_null() {
return 0;
}
unsafe { (*view).0.stamp().sec }
}
#[no_mangle]
pub extern "C" fn ros_compressed_image_get_stamp_nanosec(
view: *const ros_compressed_image_t,
) -> u32 {
if view.is_null() {
return 0;
}
unsafe { (*view).0.stamp().nanosec }
}
#[no_mangle]
pub extern "C" fn ros_compressed_image_get_frame_id(
view: *const ros_compressed_image_t,
) -> *const c_char {
if view.is_null() {
return ptr::null();
}
str_as_c(unsafe { (*view).0.frame_id() })
}
#[no_mangle]
pub extern "C" fn ros_compressed_image_get_format(
view: *const ros_compressed_image_t,
) -> *const c_char {
if view.is_null() {
return ptr::null();
}
str_as_c(unsafe { (*view).0.format() })
}
#[no_mangle]
pub extern "C" fn ros_compressed_image_get_data(
view: *const ros_compressed_image_t,
out_len: *mut usize,
) -> *const u8 {
if view.is_null() {
return ptr::null();
}
let data = unsafe { (*view).0.data() };
unsafe {
if !out_len.is_null() {
*out_len = data.len();
}
}
data.as_ptr()
}
#[no_mangle]
pub extern "C" fn ros_compressed_image_encode(
out_bytes: *mut *mut u8,
out_len: *mut usize,
stamp_sec: i32,
stamp_nanosec: u32,
frame_id: *const c_char,
format: *const c_char,
data: *const u8,
data_len: usize,
) -> i32 {
let fid = unsafe { c_to_str(frame_id) };
let fmt = unsafe { c_to_str(format) };
let d = if data.is_null() {
&[]
} else {
unsafe { slice::from_raw_parts(data, data_len) }
};
let v =
match sensor_msgs::CompressedImage::new(Time::new(stamp_sec, stamp_nanosec), fid, fmt, d) {
Ok(v) => v,
Err(_) => {
set_errno(EBADMSG);
return -1;
}
};
return_cdr_bytes(v.into_cdr(), out_bytes, out_len)
}
pub struct ros_compressed_video_t(foxglove_msgs::FoxgloveCompressedVideo<&'static [u8]>);
#[no_mangle]
pub extern "C" fn ros_compressed_video_from_cdr(
data: *const u8,
len: usize,
) -> *mut ros_compressed_video_t {
check_null_ret_null!(data);
let slice = unsafe { slice::from_raw_parts(data, len) };
match foxglove_msgs::FoxgloveCompressedVideo::from_cdr(unsafe { erase_lifetime(slice) }) {
Ok(v) => Box::into_raw(Box::new(ros_compressed_video_t(v))),
Err(_) => {
set_errno(EBADMSG);
ptr::null_mut()
}
}
}
#[no_mangle]
pub extern "C" fn ros_compressed_video_free(view: *mut ros_compressed_video_t) {
if !view.is_null() {
unsafe {
drop(Box::from_raw(view));
}
}
}
#[no_mangle]
pub extern "C" fn ros_compressed_video_get_stamp_sec(view: *const ros_compressed_video_t) -> i32 {
if view.is_null() {
return 0;
}
unsafe { (*view).0.stamp().sec }
}
#[no_mangle]
pub extern "C" fn ros_compressed_video_get_stamp_nanosec(
view: *const ros_compressed_video_t,
) -> u32 {
if view.is_null() {
return 0;
}
unsafe { (*view).0.stamp().nanosec }
}
#[no_mangle]
pub extern "C" fn ros_compressed_video_get_frame_id(
view: *const ros_compressed_video_t,
) -> *const c_char {
if view.is_null() {
return ptr::null();
}
str_as_c(unsafe { (*view).0.frame_id() })
}
#[no_mangle]
pub extern "C" fn ros_compressed_video_get_data(
view: *const ros_compressed_video_t,
out_len: *mut usize,
) -> *const u8 {
if view.is_null() {
return ptr::null();
}
let data = unsafe { (*view).0.data() };
unsafe {
if !out_len.is_null() {
*out_len = data.len();
}
}
data.as_ptr()
}
#[no_mangle]
pub extern "C" fn ros_compressed_video_get_format(
view: *const ros_compressed_video_t,
) -> *const c_char {
if view.is_null() {
return ptr::null();
}
str_as_c(unsafe { (*view).0.format() })
}
#[no_mangle]
pub extern "C" fn ros_compressed_video_encode(
out_bytes: *mut *mut u8,
out_len: *mut usize,
stamp_sec: i32,
stamp_nanosec: u32,
frame_id: *const c_char,
data: *const u8,
data_len: usize,
format: *const c_char,
) -> i32 {
let fid = unsafe { c_to_str(frame_id) };
let fmt = unsafe { c_to_str(format) };
let d = if data.is_null() {
&[]
} else {
unsafe { slice::from_raw_parts(data, data_len) }
};
let v = match foxglove_msgs::FoxgloveCompressedVideo::new(
Time::new(stamp_sec, stamp_nanosec),
fid,
d,
fmt,
) {
Ok(v) => v,
Err(_) => {
set_errno(EBADMSG);
return -1;
}
};
return_cdr_bytes(v.into_cdr(), out_bytes, out_len)
}
pub struct ros_mask_t(edgefirst_msgs::Mask<&'static [u8]>);
#[no_mangle]
pub extern "C" fn ros_mask_from_cdr(data: *const u8, len: usize) -> *mut ros_mask_t {
check_null_ret_null!(data);
let slice = unsafe { slice::from_raw_parts(data, len) };
match edgefirst_msgs::Mask::from_cdr(unsafe { erase_lifetime(slice) }) {
Ok(v) => Box::into_raw(Box::new(ros_mask_t(v))),
Err(_) => {
set_errno(EBADMSG);
ptr::null_mut()
}
}
}
#[no_mangle]
pub extern "C" fn ros_mask_free(view: *mut ros_mask_t) {
if !view.is_null() {
unsafe {
drop(Box::from_raw(view));
}
}
}
#[no_mangle]
pub extern "C" fn ros_mask_get_height(view: *const ros_mask_t) -> u32 {
if view.is_null() {
return 0;
}
unsafe { (*view).0.height() }
}
#[no_mangle]
pub extern "C" fn ros_mask_get_width(view: *const ros_mask_t) -> u32 {
if view.is_null() {
return 0;
}
unsafe { (*view).0.width() }
}
#[no_mangle]
pub extern "C" fn ros_mask_get_length(view: *const ros_mask_t) -> u32 {
if view.is_null() {
return 0;
}
unsafe { (*view).0.length() }
}
#[no_mangle]
pub extern "C" fn ros_mask_get_encoding(view: *const ros_mask_t) -> *const c_char {
if view.is_null() {
return ptr::null();
}
str_as_c(unsafe { (*view).0.encoding() })
}
#[no_mangle]
pub extern "C" fn ros_mask_get_data(view: *const ros_mask_t, out_len: *mut usize) -> *const u8 {
if view.is_null() {
return ptr::null();
}
let data = unsafe { (*view).0.mask_data() };
unsafe {
if !out_len.is_null() {
*out_len = data.len();
}
}
data.as_ptr()
}
#[no_mangle]
pub extern "C" fn ros_mask_get_boxed(view: *const ros_mask_t) -> bool {
if view.is_null() {
return false;
}
unsafe { (*view).0.boxed() }
}
#[no_mangle]
pub extern "C" fn ros_mask_encode(
out_bytes: *mut *mut u8,
out_len: *mut usize,
height: u32,
width: u32,
length: u32,
encoding: *const c_char,
data: *const u8,
data_len: usize,
boxed: bool,
) -> i32 {
let enc = unsafe { c_to_str(encoding) };
let d = if data.is_null() {
&[]
} else {
unsafe { slice::from_raw_parts(data, data_len) }
};
let v = match edgefirst_msgs::Mask::new(height, width, length, enc, d, boxed) {
Ok(v) => v,
Err(_) => {
set_errno(EBADMSG);
return -1;
}
};
return_cdr_bytes(v.into_cdr(), out_bytes, out_len)
}
pub struct ros_dmabuffer_t(edgefirst_msgs::DmaBuffer<&'static [u8]>);
#[no_mangle]
pub extern "C" fn ros_dmabuffer_from_cdr(data: *const u8, len: usize) -> *mut ros_dmabuffer_t {
check_null_ret_null!(data);
let slice = unsafe { slice::from_raw_parts(data, len) };
match edgefirst_msgs::DmaBuffer::from_cdr(unsafe { erase_lifetime(slice) }) {
Ok(v) => Box::into_raw(Box::new(ros_dmabuffer_t(v))),
Err(_) => {
set_errno(EBADMSG);
ptr::null_mut()
}
}
}
#[no_mangle]
pub extern "C" fn ros_dmabuffer_free(view: *mut ros_dmabuffer_t) {
if !view.is_null() {
unsafe {
drop(Box::from_raw(view));
}
}
}
#[no_mangle]
pub extern "C" fn ros_dmabuffer_get_stamp_sec(view: *const ros_dmabuffer_t) -> i32 {
if view.is_null() {
return 0;
}
unsafe { (*view).0.stamp().sec }
}
#[no_mangle]
pub extern "C" fn ros_dmabuffer_get_stamp_nanosec(view: *const ros_dmabuffer_t) -> u32 {
if view.is_null() {
return 0;
}
unsafe { (*view).0.stamp().nanosec }
}
#[no_mangle]
pub extern "C" fn ros_dmabuffer_get_frame_id(view: *const ros_dmabuffer_t) -> *const c_char {
if view.is_null() {
return ptr::null();
}
str_as_c(unsafe { (*view).0.frame_id() })
}
#[no_mangle]
pub extern "C" fn ros_dmabuffer_get_pid(view: *const ros_dmabuffer_t) -> u32 {
if view.is_null() {
return 0;
}
unsafe { (*view).0.pid() }
}
#[no_mangle]
pub extern "C" fn ros_dmabuffer_get_fd(view: *const ros_dmabuffer_t) -> i32 {
if view.is_null() {
return 0;
}
unsafe { (*view).0.fd() }
}
#[no_mangle]
pub extern "C" fn ros_dmabuffer_get_width(view: *const ros_dmabuffer_t) -> u32 {
if view.is_null() {
return 0;
}
unsafe { (*view).0.width() }
}
#[no_mangle]
pub extern "C" fn ros_dmabuffer_get_height(view: *const ros_dmabuffer_t) -> u32 {
if view.is_null() {
return 0;
}
unsafe { (*view).0.height() }
}
#[no_mangle]
pub extern "C" fn ros_dmabuffer_get_stride(view: *const ros_dmabuffer_t) -> u32 {
if view.is_null() {
return 0;
}
unsafe { (*view).0.stride() }
}
#[no_mangle]
pub extern "C" fn ros_dmabuffer_get_fourcc(view: *const ros_dmabuffer_t) -> u32 {
if view.is_null() {
return 0;
}
unsafe { (*view).0.fourcc() }
}
#[no_mangle]
pub extern "C" fn ros_dmabuffer_get_length(view: *const ros_dmabuffer_t) -> u32 {
if view.is_null() {
return 0;
}
unsafe { (*view).0.length() }
}
#[no_mangle]
pub extern "C" fn ros_dmabuffer_encode(
out_bytes: *mut *mut u8,
out_len: *mut usize,
stamp_sec: i32,
stamp_nanosec: u32,
frame_id: *const c_char,
pid: u32,
fd: i32,
width: u32,
height: u32,
stride: u32,
fourcc: u32,
length: u32,
) -> i32 {
let fid = unsafe { c_to_str(frame_id) };
let v = match edgefirst_msgs::DmaBuffer::new(
Time::new(stamp_sec, stamp_nanosec),
fid,
pid,
fd,
width,
height,
stride,
fourcc,
length,
) {
Ok(v) => v,
Err(_) => {
set_errno(EBADMSG);
return -1;
}
};
return_cdr_bytes(v.into_cdr(), out_bytes, out_len)
}
pub struct ros_imu_t(sensor_msgs::Imu<&'static [u8]>);
#[no_mangle]
pub extern "C" fn ros_imu_from_cdr(data: *const u8, len: usize) -> *mut ros_imu_t {
check_null_ret_null!(data);
let slice = unsafe { slice::from_raw_parts(data, len) };
match sensor_msgs::Imu::from_cdr(unsafe { erase_lifetime(slice) }) {
Ok(v) => Box::into_raw(Box::new(ros_imu_t(v))),
Err(_) => {
set_errno(EBADMSG);
ptr::null_mut()
}
}
}
#[no_mangle]
pub extern "C" fn ros_imu_free(view: *mut ros_imu_t) {
if !view.is_null() {
unsafe {
drop(Box::from_raw(view));
}
}
}
#[no_mangle]
pub extern "C" fn ros_imu_get_stamp_sec(view: *const ros_imu_t) -> i32 {
if view.is_null() {
return 0;
}
unsafe { (*view).0.stamp().sec }
}
#[no_mangle]
pub extern "C" fn ros_imu_get_stamp_nanosec(view: *const ros_imu_t) -> u32 {
if view.is_null() {
return 0;
}
unsafe { (*view).0.stamp().nanosec }
}
#[no_mangle]
pub extern "C" fn ros_imu_get_frame_id(view: *const ros_imu_t) -> *const c_char {
if view.is_null() {
return ptr::null();
}
str_as_c(unsafe { (*view).0.frame_id() })
}
#[no_mangle]
pub extern "C" fn ros_imu_get_orientation(
view: *const ros_imu_t,
x: *mut f64,
y: *mut f64,
z: *mut f64,
w: *mut f64,
) {
if view.is_null() {
return;
}
let q = unsafe { (*view).0.orientation() };
unsafe {
if !x.is_null() {
*x = q.x;
}
if !y.is_null() {
*y = q.y;
}
if !z.is_null() {
*z = q.z;
}
if !w.is_null() {
*w = q.w;
}
}
}
#[no_mangle]
pub extern "C" fn ros_imu_get_orientation_covariance(view: *const ros_imu_t, out: *mut f64) {
if view.is_null() || out.is_null() {
return;
}
let cov = unsafe { (*view).0.orientation_covariance() };
unsafe {
ptr::copy_nonoverlapping(cov.as_ptr(), out, 9);
}
}
#[no_mangle]
pub extern "C" fn ros_imu_get_angular_velocity(
view: *const ros_imu_t,
x: *mut f64,
y: *mut f64,
z: *mut f64,
) {
if view.is_null() {
return;
}
let v = unsafe { (*view).0.angular_velocity() };
unsafe {
if !x.is_null() {
*x = v.x;
}
if !y.is_null() {
*y = v.y;
}
if !z.is_null() {
*z = v.z;
}
}
}
#[no_mangle]
pub extern "C" fn ros_imu_get_angular_velocity_covariance(view: *const ros_imu_t, out: *mut f64) {
if view.is_null() || out.is_null() {
return;
}
let cov = unsafe { (*view).0.angular_velocity_covariance() };
unsafe {
ptr::copy_nonoverlapping(cov.as_ptr(), out, 9);
}
}
#[no_mangle]
pub extern "C" fn ros_imu_get_linear_acceleration(
view: *const ros_imu_t,
x: *mut f64,
y: *mut f64,
z: *mut f64,
) {
if view.is_null() {
return;
}
let v = unsafe { (*view).0.linear_acceleration() };
unsafe {
if !x.is_null() {
*x = v.x;
}
if !y.is_null() {
*y = v.y;
}
if !z.is_null() {
*z = v.z;
}
}
}
#[no_mangle]
pub extern "C" fn ros_imu_get_linear_acceleration_covariance(
view: *const ros_imu_t,
out: *mut f64,
) {
if view.is_null() || out.is_null() {
return;
}
let cov = unsafe { (*view).0.linear_acceleration_covariance() };
unsafe {
ptr::copy_nonoverlapping(cov.as_ptr(), out, 9);
}
}
pub struct ros_nav_sat_fix_t(sensor_msgs::NavSatFix<&'static [u8]>);
#[no_mangle]
pub extern "C" fn ros_nav_sat_fix_from_cdr(data: *const u8, len: usize) -> *mut ros_nav_sat_fix_t {
check_null_ret_null!(data);
let slice = unsafe { slice::from_raw_parts(data, len) };
match sensor_msgs::NavSatFix::from_cdr(unsafe { erase_lifetime(slice) }) {
Ok(v) => Box::into_raw(Box::new(ros_nav_sat_fix_t(v))),
Err(_) => {
set_errno(EBADMSG);
ptr::null_mut()
}
}
}
#[no_mangle]
pub extern "C" fn ros_nav_sat_fix_free(view: *mut ros_nav_sat_fix_t) {
if !view.is_null() {
unsafe {
drop(Box::from_raw(view));
}
}
}
#[no_mangle]
pub extern "C" fn ros_nav_sat_fix_get_stamp_sec(view: *const ros_nav_sat_fix_t) -> i32 {
if view.is_null() {
return 0;
}
unsafe { (*view).0.stamp().sec }
}
#[no_mangle]
pub extern "C" fn ros_nav_sat_fix_get_stamp_nanosec(view: *const ros_nav_sat_fix_t) -> u32 {
if view.is_null() {
return 0;
}
unsafe { (*view).0.stamp().nanosec }
}
#[no_mangle]
pub extern "C" fn ros_nav_sat_fix_get_frame_id(view: *const ros_nav_sat_fix_t) -> *const c_char {
if view.is_null() {
return ptr::null();
}
str_as_c(unsafe { (*view).0.frame_id() })
}
#[no_mangle]
pub extern "C" fn ros_nav_sat_fix_get_latitude(view: *const ros_nav_sat_fix_t) -> f64 {
if view.is_null() {
return 0.0;
}
unsafe { (*view).0.latitude() }
}
#[no_mangle]
pub extern "C" fn ros_nav_sat_fix_get_longitude(view: *const ros_nav_sat_fix_t) -> f64 {
if view.is_null() {
return 0.0;
}
unsafe { (*view).0.longitude() }
}
#[no_mangle]
pub extern "C" fn ros_nav_sat_fix_get_altitude(view: *const ros_nav_sat_fix_t) -> f64 {
if view.is_null() {
return 0.0;
}
unsafe { (*view).0.altitude() }
}
pub struct ros_transform_stamped_t(geometry_msgs::TransformStamped<&'static [u8]>);
#[no_mangle]
pub extern "C" fn ros_transform_stamped_from_cdr(
data: *const u8,
len: usize,
) -> *mut ros_transform_stamped_t {
check_null_ret_null!(data);
let slice = unsafe { slice::from_raw_parts(data, len) };
match geometry_msgs::TransformStamped::from_cdr(unsafe { erase_lifetime(slice) }) {
Ok(v) => Box::into_raw(Box::new(ros_transform_stamped_t(v))),
Err(_) => {
set_errno(EBADMSG);
ptr::null_mut()
}
}
}
#[no_mangle]
pub extern "C" fn ros_transform_stamped_free(view: *mut ros_transform_stamped_t) {
if !view.is_null() {
unsafe {
drop(Box::from_raw(view));
}
}
}
#[no_mangle]
pub extern "C" fn ros_transform_stamped_get_stamp_sec(view: *const ros_transform_stamped_t) -> i32 {
if view.is_null() {
return 0;
}
unsafe { (*view).0.stamp().sec }
}
#[no_mangle]
pub extern "C" fn ros_transform_stamped_get_stamp_nanosec(
view: *const ros_transform_stamped_t,
) -> u32 {
if view.is_null() {
return 0;
}
unsafe { (*view).0.stamp().nanosec }
}
#[no_mangle]
pub extern "C" fn ros_transform_stamped_get_frame_id(
view: *const ros_transform_stamped_t,
) -> *const c_char {
if view.is_null() {
return ptr::null();
}
str_as_c(unsafe { (*view).0.frame_id() })
}
#[no_mangle]
pub extern "C" fn ros_transform_stamped_get_child_frame_id(
view: *const ros_transform_stamped_t,
) -> *const c_char {
if view.is_null() {
return ptr::null();
}
str_as_c(unsafe { (*view).0.child_frame_id() })
}
pub struct ros_radar_cube_t(edgefirst_msgs::RadarCube<&'static [u8]>);
#[no_mangle]
pub extern "C" fn ros_radar_cube_from_cdr(data: *const u8, len: usize) -> *mut ros_radar_cube_t {
check_null_ret_null!(data);
let slice = unsafe { slice::from_raw_parts(data, len) };
match edgefirst_msgs::RadarCube::from_cdr(unsafe { erase_lifetime(slice) }) {
Ok(v) => Box::into_raw(Box::new(ros_radar_cube_t(v))),
Err(_) => {
set_errno(EBADMSG);
ptr::null_mut()
}
}
}
#[no_mangle]
pub extern "C" fn ros_radar_cube_free(view: *mut ros_radar_cube_t) {
if !view.is_null() {
unsafe {
drop(Box::from_raw(view));
}
}
}
#[no_mangle]
pub extern "C" fn ros_radar_cube_get_stamp_sec(view: *const ros_radar_cube_t) -> i32 {
if view.is_null() {
return 0;
}
unsafe { (*view).0.stamp().sec }
}
#[no_mangle]
pub extern "C" fn ros_radar_cube_get_stamp_nanosec(view: *const ros_radar_cube_t) -> u32 {
if view.is_null() {
return 0;
}
unsafe { (*view).0.stamp().nanosec }
}
#[no_mangle]
pub extern "C" fn ros_radar_cube_get_frame_id(view: *const ros_radar_cube_t) -> *const c_char {
if view.is_null() {
return ptr::null();
}
str_as_c(unsafe { (*view).0.frame_id() })
}
#[no_mangle]
pub extern "C" fn ros_radar_cube_get_timestamp(view: *const ros_radar_cube_t) -> u64 {
if view.is_null() {
return 0;
}
unsafe { (*view).0.timestamp() }
}
#[no_mangle]
pub extern "C" fn ros_radar_cube_get_layout(
view: *const ros_radar_cube_t,
out_len: *mut usize,
) -> *const u8 {
if view.is_null() {
return ptr::null();
}
let data = unsafe { (*view).0.layout() };
unsafe {
if !out_len.is_null() {
*out_len = data.len();
}
}
data.as_ptr()
}
#[no_mangle]
pub extern "C" fn ros_radar_cube_get_cube_raw(
view: *const ros_radar_cube_t,
out_len: *mut usize,
) -> *const u8 {
if view.is_null() {
return ptr::null();
}
let data = unsafe { (*view).0.cube_raw() };
unsafe {
if !out_len.is_null() {
*out_len = data.len();
}
}
data.as_ptr()
}
#[no_mangle]
pub extern "C" fn ros_radar_cube_get_cube_len(view: *const ros_radar_cube_t) -> u32 {
if view.is_null() {
return 0;
}
unsafe { (*view).0.cube_len() }
}
#[no_mangle]
pub extern "C" fn ros_radar_cube_get_is_complex(view: *const ros_radar_cube_t) -> bool {
if view.is_null() {
return false;
}
unsafe { (*view).0.is_complex() }
}
pub struct ros_radar_info_t(edgefirst_msgs::RadarInfo<&'static [u8]>);
#[no_mangle]
pub extern "C" fn ros_radar_info_from_cdr(data: *const u8, len: usize) -> *mut ros_radar_info_t {
check_null_ret_null!(data);
let slice = unsafe { slice::from_raw_parts(data, len) };
match edgefirst_msgs::RadarInfo::from_cdr(unsafe { erase_lifetime(slice) }) {
Ok(v) => Box::into_raw(Box::new(ros_radar_info_t(v))),
Err(_) => {
set_errno(EBADMSG);
ptr::null_mut()
}
}
}
#[no_mangle]
pub extern "C" fn ros_radar_info_free(view: *mut ros_radar_info_t) {
if !view.is_null() {
unsafe {
drop(Box::from_raw(view));
}
}
}
#[no_mangle]
pub extern "C" fn ros_radar_info_get_stamp_sec(view: *const ros_radar_info_t) -> i32 {
if view.is_null() {
return 0;
}
unsafe { (*view).0.stamp().sec }
}
#[no_mangle]
pub extern "C" fn ros_radar_info_get_stamp_nanosec(view: *const ros_radar_info_t) -> u32 {
if view.is_null() {
return 0;
}
unsafe { (*view).0.stamp().nanosec }
}
#[no_mangle]
pub extern "C" fn ros_radar_info_get_frame_id(view: *const ros_radar_info_t) -> *const c_char {
if view.is_null() {
return ptr::null();
}
str_as_c(unsafe { (*view).0.frame_id() })
}
#[no_mangle]
pub extern "C" fn ros_radar_info_get_center_frequency(
view: *const ros_radar_info_t,
) -> *const c_char {
if view.is_null() {
return ptr::null();
}
str_as_c(unsafe { (*view).0.center_frequency() })
}
#[no_mangle]
pub extern "C" fn ros_radar_info_get_frequency_sweep(
view: *const ros_radar_info_t,
) -> *const c_char {
if view.is_null() {
return ptr::null();
}
str_as_c(unsafe { (*view).0.frequency_sweep() })
}
#[no_mangle]
pub extern "C" fn ros_radar_info_get_range_toggle(view: *const ros_radar_info_t) -> *const c_char {
if view.is_null() {
return ptr::null();
}
str_as_c(unsafe { (*view).0.range_toggle() })
}
#[no_mangle]
pub extern "C" fn ros_radar_info_get_detection_sensitivity(
view: *const ros_radar_info_t,
) -> *const c_char {
if view.is_null() {
return ptr::null();
}
str_as_c(unsafe { (*view).0.detection_sensitivity() })
}
#[no_mangle]
pub extern "C" fn ros_radar_info_get_cube(view: *const ros_radar_info_t) -> bool {
if view.is_null() {
return false;
}
unsafe { (*view).0.cube() }
}
pub struct ros_detect_t(edgefirst_msgs::Detect<&'static [u8]>);
#[no_mangle]
pub extern "C" fn ros_detect_from_cdr(data: *const u8, len: usize) -> *mut ros_detect_t {
check_null_ret_null!(data);
let slice = unsafe { slice::from_raw_parts(data, len) };
match edgefirst_msgs::Detect::from_cdr(unsafe { erase_lifetime(slice) }) {
Ok(v) => Box::into_raw(Box::new(ros_detect_t(v))),
Err(_) => {
set_errno(EBADMSG);
ptr::null_mut()
}
}
}
#[no_mangle]
pub extern "C" fn ros_detect_free(view: *mut ros_detect_t) {
if !view.is_null() {
unsafe {
drop(Box::from_raw(view));
}
}
}
#[no_mangle]
pub extern "C" fn ros_detect_get_stamp_sec(view: *const ros_detect_t) -> i32 {
if view.is_null() {
return 0;
}
unsafe { (*view).0.stamp().sec }
}
#[no_mangle]
pub extern "C" fn ros_detect_get_stamp_nanosec(view: *const ros_detect_t) -> u32 {
if view.is_null() {
return 0;
}
unsafe { (*view).0.stamp().nanosec }
}
#[no_mangle]
pub extern "C" fn ros_detect_get_frame_id(view: *const ros_detect_t) -> *const c_char {
if view.is_null() {
return ptr::null();
}
str_as_c(unsafe { (*view).0.frame_id() })
}
#[no_mangle]
pub extern "C" fn ros_detect_get_boxes_len(view: *const ros_detect_t) -> u32 {
if view.is_null() {
return 0;
}
unsafe { (*view).0.boxes_len() }
}
pub struct ros_model_t(edgefirst_msgs::Model<&'static [u8]>);
#[no_mangle]
pub extern "C" fn ros_model_from_cdr(data: *const u8, len: usize) -> *mut ros_model_t {
check_null_ret_null!(data);
let slice = unsafe { slice::from_raw_parts(data, len) };
match edgefirst_msgs::Model::from_cdr(unsafe { erase_lifetime(slice) }) {
Ok(v) => Box::into_raw(Box::new(ros_model_t(v))),
Err(_) => {
set_errno(EBADMSG);
ptr::null_mut()
}
}
}
#[no_mangle]
pub extern "C" fn ros_model_free(view: *mut ros_model_t) {
if !view.is_null() {
unsafe {
drop(Box::from_raw(view));
}
}
}
#[no_mangle]
pub extern "C" fn ros_model_get_stamp_sec(view: *const ros_model_t) -> i32 {
if view.is_null() {
return 0;
}
unsafe { (*view).0.stamp().sec }
}
#[no_mangle]
pub extern "C" fn ros_model_get_stamp_nanosec(view: *const ros_model_t) -> u32 {
if view.is_null() {
return 0;
}
unsafe { (*view).0.stamp().nanosec }
}
#[no_mangle]
pub extern "C" fn ros_model_get_frame_id(view: *const ros_model_t) -> *const c_char {
if view.is_null() {
return ptr::null();
}
str_as_c(unsafe { (*view).0.frame_id() })
}
#[no_mangle]
pub extern "C" fn ros_model_get_boxes_len(view: *const ros_model_t) -> u32 {
if view.is_null() {
return 0;
}
unsafe { (*view).0.boxes_len() }
}
#[no_mangle]
pub extern "C" fn ros_model_get_masks_len(view: *const ros_model_t) -> u32 {
if view.is_null() {
return 0;
}
unsafe { (*view).0.masks_len() }
}
pub struct ros_model_info_t(edgefirst_msgs::ModelInfo<&'static [u8]>);
#[no_mangle]
pub extern "C" fn ros_model_info_from_cdr(data: *const u8, len: usize) -> *mut ros_model_info_t {
check_null_ret_null!(data);
let slice = unsafe { slice::from_raw_parts(data, len) };
match edgefirst_msgs::ModelInfo::from_cdr(unsafe { erase_lifetime(slice) }) {
Ok(v) => Box::into_raw(Box::new(ros_model_info_t(v))),
Err(_) => {
set_errno(EBADMSG);
ptr::null_mut()
}
}
}
#[no_mangle]
pub extern "C" fn ros_model_info_free(view: *mut ros_model_info_t) {
if !view.is_null() {
unsafe {
drop(Box::from_raw(view));
}
}
}
#[no_mangle]
pub extern "C" fn ros_model_info_get_stamp_sec(view: *const ros_model_info_t) -> i32 {
if view.is_null() {
return 0;
}
unsafe { (*view).0.stamp().sec }
}
#[no_mangle]
pub extern "C" fn ros_model_info_get_stamp_nanosec(view: *const ros_model_info_t) -> u32 {
if view.is_null() {
return 0;
}
unsafe { (*view).0.stamp().nanosec }
}
#[no_mangle]
pub extern "C" fn ros_model_info_get_frame_id(view: *const ros_model_info_t) -> *const c_char {
if view.is_null() {
return ptr::null();
}
str_as_c(unsafe { (*view).0.frame_id() })
}
#[no_mangle]
pub extern "C" fn ros_model_info_get_model_type(view: *const ros_model_info_t) -> *const c_char {
if view.is_null() {
return ptr::null();
}
str_as_c(unsafe { (*view).0.model_type() })
}
#[no_mangle]
pub extern "C" fn ros_model_info_get_model_format(view: *const ros_model_info_t) -> *const c_char {
if view.is_null() {
return ptr::null();
}
str_as_c(unsafe { (*view).0.model_format() })
}
#[no_mangle]
pub extern "C" fn ros_model_info_get_model_name(view: *const ros_model_info_t) -> *const c_char {
if view.is_null() {
return ptr::null();
}
str_as_c(unsafe { (*view).0.model_name() })
}
#[no_mangle]
pub extern "C" fn ros_model_info_get_input_type(view: *const ros_model_info_t) -> u8 {
if view.is_null() {
return 0;
}
unsafe { (*view).0.input_type() }
}
#[no_mangle]
pub extern "C" fn ros_model_info_get_output_type(view: *const ros_model_info_t) -> u8 {
if view.is_null() {
return 0;
}
unsafe { (*view).0.output_type() }
}
pub struct ros_point_cloud2_t(sensor_msgs::PointCloud2<&'static [u8]>);
#[no_mangle]
pub extern "C" fn ros_point_cloud2_from_cdr(
data: *const u8,
len: usize,
) -> *mut ros_point_cloud2_t {
check_null_ret_null!(data);
let slice = unsafe { slice::from_raw_parts(data, len) };
match sensor_msgs::PointCloud2::from_cdr(unsafe { erase_lifetime(slice) }) {
Ok(v) => Box::into_raw(Box::new(ros_point_cloud2_t(v))),
Err(_) => {
set_errno(EBADMSG);
ptr::null_mut()
}
}
}
#[no_mangle]
pub extern "C" fn ros_point_cloud2_free(view: *mut ros_point_cloud2_t) {
if !view.is_null() {
unsafe {
drop(Box::from_raw(view));
}
}
}
#[no_mangle]
pub extern "C" fn ros_point_cloud2_get_stamp_sec(view: *const ros_point_cloud2_t) -> i32 {
if view.is_null() {
return 0;
}
unsafe { (*view).0.stamp().sec }
}
#[no_mangle]
pub extern "C" fn ros_point_cloud2_get_stamp_nanosec(view: *const ros_point_cloud2_t) -> u32 {
if view.is_null() {
return 0;
}
unsafe { (*view).0.stamp().nanosec }
}
#[no_mangle]
pub extern "C" fn ros_point_cloud2_get_frame_id(view: *const ros_point_cloud2_t) -> *const c_char {
if view.is_null() {
return ptr::null();
}
str_as_c(unsafe { (*view).0.frame_id() })
}
#[no_mangle]
pub extern "C" fn ros_point_cloud2_get_height(view: *const ros_point_cloud2_t) -> u32 {
if view.is_null() {
return 0;
}
unsafe { (*view).0.height() }
}
#[no_mangle]
pub extern "C" fn ros_point_cloud2_get_width(view: *const ros_point_cloud2_t) -> u32 {
if view.is_null() {
return 0;
}
unsafe { (*view).0.width() }
}
#[no_mangle]
pub extern "C" fn ros_point_cloud2_get_point_step(view: *const ros_point_cloud2_t) -> u32 {
if view.is_null() {
return 0;
}
unsafe { (*view).0.point_step() }
}
#[no_mangle]
pub extern "C" fn ros_point_cloud2_get_row_step(view: *const ros_point_cloud2_t) -> u32 {
if view.is_null() {
return 0;
}
unsafe { (*view).0.row_step() }
}
#[no_mangle]
pub extern "C" fn ros_point_cloud2_get_data(
view: *const ros_point_cloud2_t,
out_len: *mut usize,
) -> *const u8 {
if view.is_null() {
return ptr::null();
}
let data = unsafe { (*view).0.data() };
unsafe {
if !out_len.is_null() {
*out_len = data.len();
}
}
data.as_ptr()
}
#[no_mangle]
pub extern "C" fn ros_point_cloud2_get_is_dense(view: *const ros_point_cloud2_t) -> bool {
if view.is_null() {
return false;
}
unsafe { (*view).0.is_dense() }
}
#[no_mangle]
pub extern "C" fn ros_point_cloud2_get_is_bigendian(view: *const ros_point_cloud2_t) -> bool {
if view.is_null() {
return false;
}
unsafe { (*view).0.is_bigendian() }
}
#[no_mangle]
pub extern "C" fn ros_point_cloud2_get_fields_len(view: *const ros_point_cloud2_t) -> u32 {
if view.is_null() {
return 0;
}
unsafe { (*view).0.fields_len() }
}
pub struct ros_camera_info_t(sensor_msgs::CameraInfo<&'static [u8]>);
#[no_mangle]
pub extern "C" fn ros_camera_info_from_cdr(data: *const u8, len: usize) -> *mut ros_camera_info_t {
check_null_ret_null!(data);
let slice = unsafe { slice::from_raw_parts(data, len) };
match sensor_msgs::CameraInfo::from_cdr(unsafe { erase_lifetime(slice) }) {
Ok(v) => Box::into_raw(Box::new(ros_camera_info_t(v))),
Err(_) => {
set_errno(EBADMSG);
ptr::null_mut()
}
}
}
#[no_mangle]
pub extern "C" fn ros_camera_info_free(view: *mut ros_camera_info_t) {
if !view.is_null() {
unsafe {
drop(Box::from_raw(view));
}
}
}
#[no_mangle]
pub extern "C" fn ros_camera_info_get_stamp_sec(view: *const ros_camera_info_t) -> i32 {
if view.is_null() {
return 0;
}
unsafe { (*view).0.stamp().sec }
}
#[no_mangle]
pub extern "C" fn ros_camera_info_get_stamp_nanosec(view: *const ros_camera_info_t) -> u32 {
if view.is_null() {
return 0;
}
unsafe { (*view).0.stamp().nanosec }
}
#[no_mangle]
pub extern "C" fn ros_camera_info_get_frame_id(view: *const ros_camera_info_t) -> *const c_char {
if view.is_null() {
return ptr::null();
}
str_as_c(unsafe { (*view).0.frame_id() })
}
#[no_mangle]
pub extern "C" fn ros_camera_info_get_height(view: *const ros_camera_info_t) -> u32 {
if view.is_null() {
return 0;
}
unsafe { (*view).0.height() }
}
#[no_mangle]
pub extern "C" fn ros_camera_info_get_width(view: *const ros_camera_info_t) -> u32 {
if view.is_null() {
return 0;
}
unsafe { (*view).0.width() }
}
#[no_mangle]
pub extern "C" fn ros_camera_info_get_distortion_model(
view: *const ros_camera_info_t,
) -> *const c_char {
if view.is_null() {
return ptr::null();
}
str_as_c(unsafe { (*view).0.distortion_model() })
}
#[no_mangle]
pub extern "C" fn ros_camera_info_get_binning_x(view: *const ros_camera_info_t) -> u32 {
if view.is_null() {
return 0;
}
unsafe { (*view).0.binning_x() }
}
#[no_mangle]
pub extern "C" fn ros_camera_info_get_binning_y(view: *const ros_camera_info_t) -> u32 {
if view.is_null() {
return 0;
}
unsafe { (*view).0.binning_y() }
}
pub struct ros_track_t(edgefirst_msgs::Track<&'static [u8]>);
#[no_mangle]
pub extern "C" fn ros_track_from_cdr(data: *const u8, len: usize) -> *mut ros_track_t {
check_null_ret_null!(data);
let slice = unsafe { slice::from_raw_parts(data, len) };
match edgefirst_msgs::Track::from_cdr(unsafe { erase_lifetime(slice) }) {
Ok(v) => Box::into_raw(Box::new(ros_track_t(v))),
Err(_) => {
set_errno(EBADMSG);
ptr::null_mut()
}
}
}
#[no_mangle]
pub extern "C" fn ros_track_free(view: *mut ros_track_t) {
if !view.is_null() {
unsafe {
drop(Box::from_raw(view));
}
}
}
#[no_mangle]
pub extern "C" fn ros_track_get_id(view: *const ros_track_t) -> *const c_char {
if view.is_null() {
return ptr::null();
}
str_as_c(unsafe { (*view).0.id() })
}
#[no_mangle]
pub extern "C" fn ros_track_get_lifetime(view: *const ros_track_t) -> i32 {
if view.is_null() {
return 0;
}
unsafe { (*view).0.lifetime() }
}
pub struct ros_box_t(edgefirst_msgs::DetectBox<&'static [u8]>);
#[no_mangle]
pub extern "C" fn ros_box_from_cdr(data: *const u8, len: usize) -> *mut ros_box_t {
check_null_ret_null!(data);
let slice = unsafe { slice::from_raw_parts(data, len) };
match edgefirst_msgs::DetectBox::from_cdr(unsafe { erase_lifetime(slice) }) {
Ok(v) => Box::into_raw(Box::new(ros_box_t(v))),
Err(_) => {
set_errno(EBADMSG);
ptr::null_mut()
}
}
}
#[no_mangle]
pub extern "C" fn ros_box_free(view: *mut ros_box_t) {
if !view.is_null() {
unsafe {
drop(Box::from_raw(view));
}
}
}
#[no_mangle]
pub extern "C" fn ros_box_get_center_x(view: *const ros_box_t) -> f32 {
if view.is_null() {
return 0.0;
}
unsafe { (*view).0.center_x() }
}
#[no_mangle]
pub extern "C" fn ros_box_get_center_y(view: *const ros_box_t) -> f32 {
if view.is_null() {
return 0.0;
}
unsafe { (*view).0.center_y() }
}
#[no_mangle]
pub extern "C" fn ros_box_get_width(view: *const ros_box_t) -> f32 {
if view.is_null() {
return 0.0;
}
unsafe { (*view).0.width() }
}
#[no_mangle]
pub extern "C" fn ros_box_get_height(view: *const ros_box_t) -> f32 {
if view.is_null() {
return 0.0;
}
unsafe { (*view).0.height() }
}
#[no_mangle]
pub extern "C" fn ros_box_get_label(view: *const ros_box_t) -> *const c_char {
if view.is_null() {
return ptr::null();
}
str_as_c(unsafe { (*view).0.label() })
}
#[no_mangle]
pub extern "C" fn ros_box_get_score(view: *const ros_box_t) -> f32 {
if view.is_null() {
return 0.0;
}
unsafe { (*view).0.score() }
}
#[no_mangle]
pub extern "C" fn ros_box_get_distance(view: *const ros_box_t) -> f32 {
if view.is_null() {
return 0.0;
}
unsafe { (*view).0.distance() }
}
#[no_mangle]
pub extern "C" fn ros_box_get_speed(view: *const ros_box_t) -> f32 {
if view.is_null() {
return 0.0;
}
unsafe { (*view).0.speed() }
}
#[no_mangle]
pub extern "C" fn ros_box_get_track_id(view: *const ros_box_t) -> *const c_char {
if view.is_null() {
return ptr::null();
}
str_as_c(unsafe { (*view).0.track_id() })
}
#[no_mangle]
pub extern "C" fn ros_box_get_track_lifetime(view: *const ros_box_t) -> i32 {
if view.is_null() {
return 0;
}
unsafe { (*view).0.track_lifetime() }
}
pub struct ros_local_time_t(edgefirst_msgs::LocalTime<&'static [u8]>);
#[no_mangle]
pub extern "C" fn ros_local_time_from_cdr(data: *const u8, len: usize) -> *mut ros_local_time_t {
check_null_ret_null!(data);
let slice = unsafe { slice::from_raw_parts(data, len) };
match edgefirst_msgs::LocalTime::from_cdr(unsafe { erase_lifetime(slice) }) {
Ok(v) => Box::into_raw(Box::new(ros_local_time_t(v))),
Err(_) => {
set_errno(EBADMSG);
ptr::null_mut()
}
}
}
#[no_mangle]
pub extern "C" fn ros_local_time_free(view: *mut ros_local_time_t) {
if !view.is_null() {
unsafe {
drop(Box::from_raw(view));
}
}
}
#[no_mangle]
pub extern "C" fn ros_local_time_get_stamp_sec(view: *const ros_local_time_t) -> i32 {
if view.is_null() {
return 0;
}
unsafe { (*view).0.stamp().sec }
}
#[no_mangle]
pub extern "C" fn ros_local_time_get_stamp_nanosec(view: *const ros_local_time_t) -> u32 {
if view.is_null() {
return 0;
}
unsafe { (*view).0.stamp().nanosec }
}
#[no_mangle]
pub extern "C" fn ros_local_time_get_frame_id(view: *const ros_local_time_t) -> *const c_char {
if view.is_null() {
return ptr::null();
}
str_as_c(unsafe { (*view).0.frame_id() })
}
#[no_mangle]
pub extern "C" fn ros_local_time_get_timezone(view: *const ros_local_time_t) -> i16 {
if view.is_null() {
return 0;
}
unsafe { (*view).0.timezone() }
}
macro_rules! impl_as_cdr {
($fn_name:ident, $view_type:ty) => {
#[no_mangle]
pub extern "C" fn $fn_name(view: *const $view_type, out_len: *mut usize) -> *const u8 {
if view.is_null() {
return ptr::null();
}
let cdr = unsafe { (*view).0.as_cdr() };
unsafe {
if !out_len.is_null() {
*out_len = cdr.len();
}
}
cdr.as_ptr()
}
};
}
impl_as_cdr!(ros_header_as_cdr, ros_header_t);
impl_as_cdr!(ros_image_as_cdr, ros_image_t);
impl_as_cdr!(ros_compressed_image_as_cdr, ros_compressed_image_t);
impl_as_cdr!(ros_compressed_video_as_cdr, ros_compressed_video_t);
impl_as_cdr!(ros_mask_as_cdr, ros_mask_t);
impl_as_cdr!(ros_dmabuffer_as_cdr, ros_dmabuffer_t);
impl_as_cdr!(ros_imu_as_cdr, ros_imu_t);
impl_as_cdr!(ros_nav_sat_fix_as_cdr, ros_nav_sat_fix_t);
impl_as_cdr!(ros_transform_stamped_as_cdr, ros_transform_stamped_t);
impl_as_cdr!(ros_radar_cube_as_cdr, ros_radar_cube_t);
impl_as_cdr!(ros_radar_info_as_cdr, ros_radar_info_t);
impl_as_cdr!(ros_detect_as_cdr, ros_detect_t);
impl_as_cdr!(ros_model_as_cdr, ros_model_t);
impl_as_cdr!(ros_model_info_as_cdr, ros_model_info_t);
impl_as_cdr!(ros_point_cloud2_as_cdr, ros_point_cloud2_t);
impl_as_cdr!(ros_camera_info_as_cdr, ros_camera_info_t);
impl_as_cdr!(ros_track_as_cdr, ros_track_t);
impl_as_cdr!(ros_box_as_cdr, ros_box_t);
impl_as_cdr!(ros_local_time_as_cdr, ros_local_time_t);