use std::time::Duration;
use crate::{log_error, rcl_bindings::*};
#[derive(Clone, Copy, Debug, Eq, Ord, PartialEq, PartialOrd)]
pub enum QoSHistoryPolicy {
SystemDefault {
depth: u32,
},
KeepLast {
depth: u32,
},
KeepAll,
}
#[derive(Clone, Copy, Debug, Eq, Ord, PartialEq, PartialOrd)]
pub enum QoSReliabilityPolicy {
SystemDefault = 0,
Reliable = 1,
BestEffort = 2,
#[cfg(not(ros_distro = "humble"))]
BestAvailable = 4,
}
#[derive(Clone, Copy, Debug, Eq, Ord, PartialEq, PartialOrd)]
pub enum QoSDurabilityPolicy {
SystemDefault = 0,
TransientLocal = 1,
Volatile = 2,
#[cfg(not(ros_distro = "humble"))]
BestAvailable = 4,
}
#[derive(Clone, Copy, Debug, Eq, Ord, PartialEq, PartialOrd)]
pub enum QoSLivelinessPolicy {
SystemDefault = 0,
Automatic = 1,
ManualByTopic = 3,
#[cfg(not(ros_distro = "humble"))]
BestAvailable = 5,
}
#[derive(Clone, Copy, Debug, Eq, Ord, PartialEq, PartialOrd)]
pub enum QoSDuration {
SystemDefault,
Infinite,
Custom(Duration),
}
#[derive(Clone, Copy, Debug, Eq, Ord, PartialEq, PartialOrd)]
pub struct QoSProfile {
pub history: QoSHistoryPolicy,
pub reliability: QoSReliabilityPolicy,
pub durability: QoSDurabilityPolicy,
pub deadline: QoSDuration,
pub lifespan: QoSDuration,
pub liveliness: QoSLivelinessPolicy,
pub liveliness_lease: QoSDuration,
pub avoid_ros_namespace_conventions: bool,
}
impl Default for QoSProfile {
fn default() -> Self {
QOS_PROFILE_DEFAULT
}
}
impl From<QoSProfile> for rmw_qos_profile_t {
fn from(qos: QoSProfile) -> Self {
Self {
history: qos.history.into(),
depth: match qos.history {
QoSHistoryPolicy::SystemDefault { depth } => depth as usize,
QoSHistoryPolicy::KeepLast { depth } => depth as usize,
QoSHistoryPolicy::KeepAll => 0,
},
reliability: qos.reliability.into(),
durability: qos.durability.into(),
deadline: qos.deadline.into(),
lifespan: qos.lifespan.into(),
liveliness: qos.liveliness.into(),
liveliness_lease_duration: qos.liveliness_lease.into(),
avoid_ros_namespace_conventions: qos.avoid_ros_namespace_conventions,
}
}
}
impl From<&rmw_qos_profile_t> for QoSProfile {
fn from(profile: &rmw_qos_profile_t) -> Self {
let history = match profile.history {
rmw_qos_history_policy_e::RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT => {
QoSHistoryPolicy::SystemDefault {
depth: profile.depth as u32,
}
}
rmw_qos_history_policy_e::RMW_QOS_POLICY_HISTORY_KEEP_LAST => {
QoSHistoryPolicy::KeepLast {
depth: profile.depth as u32,
}
}
rmw_qos_history_policy_e::RMW_QOS_POLICY_HISTORY_KEEP_ALL => QoSHistoryPolicy::KeepAll,
rmw_qos_history_policy_e::RMW_QOS_POLICY_HISTORY_UNKNOWN => {
log_error!(
"QoSProfile.conversion",
"History policy is unknown. Converting as KeepLast.",
);
QoSHistoryPolicy::KeepLast {
depth: profile.depth as u32,
}
}
};
QoSProfile {
history,
reliability: (&profile.reliability).into(),
durability: (&profile.durability).into(),
deadline: (&profile.deadline).into(),
lifespan: (&profile.lifespan).into(),
liveliness: (&profile.liveliness).into(),
liveliness_lease: (&profile.liveliness_lease_duration).into(),
avoid_ros_namespace_conventions: profile.avoid_ros_namespace_conventions,
}
}
}
impl QoSProfile {
pub fn keep_last(mut self, depth: u32) -> Self {
self.history = QoSHistoryPolicy::KeepLast { depth };
self
}
pub fn keep_all(mut self) -> Self {
self.history = QoSHistoryPolicy::KeepAll;
self
}
pub fn reliable(mut self) -> Self {
self.reliability = QoSReliabilityPolicy::Reliable;
self
}
pub fn best_effort(mut self) -> Self {
self.reliability = QoSReliabilityPolicy::BestEffort;
self
}
pub fn volatile(mut self) -> Self {
self.durability = QoSDurabilityPolicy::Volatile;
self
}
pub fn transient_local(mut self) -> Self {
self.durability = QoSDurabilityPolicy::TransientLocal;
self
}
pub fn deadline_duration(mut self, deadline: Duration) -> Self {
self.deadline = QoSDuration::Custom(deadline);
self
}
pub fn liveliness_lease_duration(mut self, lease_duration: Duration) -> Self {
self.liveliness_lease = QoSDuration::Custom(lease_duration);
self
}
pub fn lifespan_duration(mut self, lifespan: Duration) -> Self {
self.lifespan = QoSDuration::Custom(lifespan);
self
}
pub fn topics_default() -> Self {
QOS_PROFILE_DEFAULT
}
pub fn sensor_data_default() -> Self {
QOS_PROFILE_SENSOR_DATA
}
pub fn services_default() -> Self {
QOS_PROFILE_SERVICES_DEFAULT
}
pub fn parameter_services_default() -> Self {
QOS_PROFILE_PARAMETERS
}
pub fn parameter_events_default() -> Self {
QOS_PROFILE_PARAMETER_EVENTS
}
pub fn system_default() -> Self {
QOS_PROFILE_SYSTEM_DEFAULT
}
pub fn action_status_default() -> Self {
QOS_PROFILE_ACTION_STATUS_DEFAULT
}
#[cfg(not(ros_distro = "humble"))]
pub fn best_available() -> Self {
unsafe {
(&rmw_qos_profile_best_available).into()
}
}
}
impl From<QoSHistoryPolicy> for rmw_qos_history_policy_t {
fn from(policy: QoSHistoryPolicy) -> Self {
match policy {
QoSHistoryPolicy::SystemDefault { .. } => {
rmw_qos_history_policy_t::RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT
}
QoSHistoryPolicy::KeepLast { .. } => {
rmw_qos_history_policy_t::RMW_QOS_POLICY_HISTORY_KEEP_LAST
}
QoSHistoryPolicy::KeepAll => rmw_qos_history_policy_t::RMW_QOS_POLICY_HISTORY_KEEP_ALL,
}
}
}
impl From<QoSReliabilityPolicy> for rmw_qos_reliability_policy_t {
fn from(policy: QoSReliabilityPolicy) -> Self {
match policy {
QoSReliabilityPolicy::SystemDefault => {
rmw_qos_reliability_policy_t::RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT
}
QoSReliabilityPolicy::Reliable => {
rmw_qos_reliability_policy_t::RMW_QOS_POLICY_RELIABILITY_RELIABLE
}
QoSReliabilityPolicy::BestEffort => {
rmw_qos_reliability_policy_t::RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT
}
#[cfg(not(ros_distro = "humble"))]
QoSReliabilityPolicy::BestAvailable => {
rmw_qos_reliability_policy_t::RMW_QOS_POLICY_RELIABILITY_BEST_AVAILABLE
}
}
}
}
impl From<&rmw_qos_reliability_policy_t> for QoSReliabilityPolicy {
fn from(policy: &rmw_qos_reliability_policy_t) -> Self {
match policy {
rmw_qos_reliability_policy_e::RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT => {
QoSReliabilityPolicy::SystemDefault
}
rmw_qos_reliability_policy_e::RMW_QOS_POLICY_RELIABILITY_RELIABLE => {
QoSReliabilityPolicy::Reliable
}
rmw_qos_reliability_policy_e::RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT => {
QoSReliabilityPolicy::BestEffort
}
#[cfg(not(ros_distro = "humble"))]
rmw_qos_reliability_policy_e::RMW_QOS_POLICY_RELIABILITY_BEST_AVAILABLE => {
QoSReliabilityPolicy::BestAvailable
}
rmw_qos_reliability_policy_e::RMW_QOS_POLICY_RELIABILITY_UNKNOWN => {
log_error!(
"QoSReliabilityPolicy.conversion",
"Reliability policy is unknown. Converting as SystemDefault",
);
QoSReliabilityPolicy::SystemDefault
}
}
}
}
impl From<QoSDurabilityPolicy> for rmw_qos_durability_policy_t {
fn from(policy: QoSDurabilityPolicy) -> Self {
match policy {
QoSDurabilityPolicy::SystemDefault => {
rmw_qos_durability_policy_t::RMW_QOS_POLICY_DURABILITY_SYSTEM_DEFAULT
}
QoSDurabilityPolicy::TransientLocal => {
rmw_qos_durability_policy_t::RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL
}
QoSDurabilityPolicy::Volatile => {
rmw_qos_durability_policy_t::RMW_QOS_POLICY_DURABILITY_VOLATILE
}
#[cfg(not(ros_distro = "humble"))]
QoSDurabilityPolicy::BestAvailable => {
rmw_qos_durability_policy_t::RMW_QOS_POLICY_DURABILITY_BEST_AVAILABLE
}
}
}
}
impl From<&rmw_qos_durability_policy_t> for QoSDurabilityPolicy {
fn from(policy: &rmw_qos_durability_policy_t) -> Self {
match policy {
rmw_qos_durability_policy_e::RMW_QOS_POLICY_DURABILITY_SYSTEM_DEFAULT => {
QoSDurabilityPolicy::SystemDefault
}
rmw_qos_durability_policy_e::RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL => {
QoSDurabilityPolicy::TransientLocal
}
rmw_qos_durability_policy_e::RMW_QOS_POLICY_DURABILITY_VOLATILE => {
QoSDurabilityPolicy::Volatile
}
#[cfg(not(ros_distro = "humble"))]
rmw_qos_durability_policy_e::RMW_QOS_POLICY_DURABILITY_BEST_AVAILABLE => {
QoSDurabilityPolicy::BestAvailable
}
rmw_qos_durability_policy_e::RMW_QOS_POLICY_DURABILITY_UNKNOWN => {
log_error!(
"QoSDurabilityPolicy.conversion",
"Durability policy is unknown. Converting as SystemDefault.",
);
QoSDurabilityPolicy::SystemDefault
}
}
}
}
impl From<QoSLivelinessPolicy> for rmw_qos_liveliness_policy_t {
fn from(policy: QoSLivelinessPolicy) -> Self {
match policy {
QoSLivelinessPolicy::SystemDefault => {
rmw_qos_liveliness_policy_t::RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT
}
QoSLivelinessPolicy::Automatic => {
rmw_qos_liveliness_policy_t::RMW_QOS_POLICY_LIVELINESS_AUTOMATIC
}
QoSLivelinessPolicy::ManualByTopic => {
rmw_qos_liveliness_policy_t::RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC
}
#[cfg(not(ros_distro = "humble"))]
QoSLivelinessPolicy::BestAvailable => {
rmw_qos_liveliness_policy_t::RMW_QOS_POLICY_LIVELINESS_BEST_AVAILABLE
}
}
}
}
impl From<&rmw_qos_liveliness_policy_t> for QoSLivelinessPolicy {
fn from(value: &rmw_qos_liveliness_policy_t) -> Self {
match value {
rmw_qos_liveliness_policy_e::RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT => {
QoSLivelinessPolicy::SystemDefault
}
rmw_qos_liveliness_policy_e::RMW_QOS_POLICY_LIVELINESS_AUTOMATIC => {
QoSLivelinessPolicy::Automatic
}
rmw_qos_liveliness_policy_e::RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC => {
QoSLivelinessPolicy::ManualByTopic
}
#[cfg(not(ros_distro = "humble"))]
rmw_qos_liveliness_policy_e::RMW_QOS_POLICY_LIVELINESS_BEST_AVAILABLE => {
QoSLivelinessPolicy::BestAvailable
}
rmw_qos_liveliness_policy_e::RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_NODE => {
log_error!(
"QoSLivelinessPolicy.conversion",
"Using deprecated liveliness mode. Converting as ManualByTopic.",
);
QoSLivelinessPolicy::ManualByTopic
}
rmw_qos_liveliness_policy_e::RMW_QOS_POLICY_LIVELINESS_UNKNOWN => {
log_error!(
"QoSLivelinessPolicy.conversion",
"Liveliness policy is unknown. Converting as SystemDefault.",
);
QoSLivelinessPolicy::SystemDefault
}
}
}
}
impl From<QoSDuration> for rmw_time_t {
fn from(duration: QoSDuration) -> Self {
match duration {
QoSDuration::Custom(dt) => {
let nonzero_dt = dt.max(Duration::from_nanos(1));
Self {
sec: nonzero_dt.as_secs(),
nsec: u64::from(nonzero_dt.subsec_nanos()),
}
}
QoSDuration::SystemDefault => Self { sec: 0, nsec: 0 },
QoSDuration::Infinite => Self {
sec: 9223372036,
nsec: 854775807,
},
}
}
}
impl From<&rmw_time_t> for Duration {
fn from(value: &rmw_time_t) -> Self {
Duration::from_secs(value.sec) + Duration::from_nanos(value.nsec)
}
}
impl From<&rmw_time_t> for QoSDuration {
fn from(value: &rmw_time_t) -> Self {
let value: Duration = value.into();
let time_eq = |a: Duration, b: QoSDuration| {
let b: rmw_time_t = b.into();
let b: Duration = (&b).into();
a == b
};
if time_eq(value, QoSDuration::SystemDefault) {
return QoSDuration::SystemDefault;
}
if time_eq(value, QoSDuration::Infinite) {
return QoSDuration::Infinite;
}
QoSDuration::Custom(value)
}
}
pub const QOS_PROFILE_SENSOR_DATA: QoSProfile = QoSProfile {
history: QoSHistoryPolicy::KeepLast { depth: 5 },
reliability: QoSReliabilityPolicy::BestEffort,
durability: QoSDurabilityPolicy::Volatile,
deadline: QoSDuration::SystemDefault,
lifespan: QoSDuration::SystemDefault,
liveliness: QoSLivelinessPolicy::SystemDefault,
liveliness_lease: QoSDuration::SystemDefault,
avoid_ros_namespace_conventions: false,
};
pub const QOS_PROFILE_CLOCK: QoSProfile = QoSProfile {
history: QoSHistoryPolicy::KeepLast { depth: 1 },
reliability: QoSReliabilityPolicy::BestEffort,
durability: QoSDurabilityPolicy::Volatile,
deadline: QoSDuration::SystemDefault,
lifespan: QoSDuration::SystemDefault,
liveliness: QoSLivelinessPolicy::SystemDefault,
liveliness_lease: QoSDuration::SystemDefault,
avoid_ros_namespace_conventions: false,
};
pub const QOS_PROFILE_PARAMETERS: QoSProfile = QoSProfile {
history: QoSHistoryPolicy::KeepLast { depth: 1000 },
reliability: QoSReliabilityPolicy::Reliable,
durability: QoSDurabilityPolicy::Volatile,
deadline: QoSDuration::SystemDefault,
lifespan: QoSDuration::SystemDefault,
liveliness: QoSLivelinessPolicy::SystemDefault,
liveliness_lease: QoSDuration::SystemDefault,
avoid_ros_namespace_conventions: false,
};
pub const QOS_PROFILE_DEFAULT: QoSProfile = QoSProfile {
history: QoSHistoryPolicy::KeepLast { depth: 10 },
reliability: QoSReliabilityPolicy::Reliable,
durability: QoSDurabilityPolicy::Volatile,
deadline: QoSDuration::SystemDefault,
lifespan: QoSDuration::SystemDefault,
liveliness: QoSLivelinessPolicy::SystemDefault,
liveliness_lease: QoSDuration::SystemDefault,
avoid_ros_namespace_conventions: false,
};
pub const QOS_PROFILE_SERVICES_DEFAULT: QoSProfile = QoSProfile {
history: QoSHistoryPolicy::KeepLast { depth: 10 },
reliability: QoSReliabilityPolicy::Reliable,
durability: QoSDurabilityPolicy::Volatile,
deadline: QoSDuration::SystemDefault,
lifespan: QoSDuration::SystemDefault,
liveliness: QoSLivelinessPolicy::SystemDefault,
liveliness_lease: QoSDuration::SystemDefault,
avoid_ros_namespace_conventions: false,
};
pub const QOS_PROFILE_PARAMETER_EVENTS: QoSProfile = QoSProfile {
history: QoSHistoryPolicy::KeepAll,
reliability: QoSReliabilityPolicy::Reliable,
durability: QoSDurabilityPolicy::Volatile,
deadline: QoSDuration::SystemDefault,
lifespan: QoSDuration::SystemDefault,
liveliness: QoSLivelinessPolicy::SystemDefault,
liveliness_lease: QoSDuration::SystemDefault,
avoid_ros_namespace_conventions: false,
};
pub const QOS_PROFILE_SYSTEM_DEFAULT: QoSProfile = QoSProfile {
history: QoSHistoryPolicy::SystemDefault { depth: 0 },
reliability: QoSReliabilityPolicy::SystemDefault,
durability: QoSDurabilityPolicy::SystemDefault,
deadline: QoSDuration::SystemDefault,
lifespan: QoSDuration::SystemDefault,
liveliness: QoSLivelinessPolicy::SystemDefault,
liveliness_lease: QoSDuration::SystemDefault,
avoid_ros_namespace_conventions: false,
};
pub const QOS_PROFILE_ACTION_STATUS_DEFAULT: QoSProfile = QoSProfile {
history: QoSHistoryPolicy::KeepLast { depth: 1 },
reliability: QoSReliabilityPolicy::Reliable,
durability: QoSDurabilityPolicy::TransientLocal,
deadline: QoSDuration::SystemDefault,
lifespan: QoSDuration::SystemDefault,
liveliness: QoSLivelinessPolicy::SystemDefault,
liveliness_lease: QoSDuration::SystemDefault,
avoid_ros_namespace_conventions: false,
};