Skip to content

Commit

Permalink
Add ignore_local QoS to services/actions (#6)
Browse files Browse the repository at this point in the history
  • Loading branch information
@JEnoch
JEnoch authored Oct 9, 2023
1 parent 9b33dd5 commit fb6f5bd
Show file tree
Hide file tree
Showing 8 changed files with 1,142 additions and 42 deletions.
4 changes: 0 additions & 4 deletions zenoh-plugin-ros2dds/src/dds_utils.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -73,10 +73,6 @@ pub fn serialize_atomic_entity_guid<S>(entity: &AtomicDDSEntity, s: S) -> Result
where
S: Serializer,
{
println!(
"--- serialize_atomic_entity_guid: {}",
entity.load(std::sync::atomic::Ordering::Relaxed)
);
match entity.load(std::sync::atomic::Ordering::Relaxed) {
DDS_ENTITY_NULL => s.serialize_str(""),
entity => serialize_entity_guid(&entity, s),
Expand Down
40 changes: 35 additions & 5 deletions zenoh-plugin-ros2dds/src/ros2_utils.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,8 +17,9 @@ use std::sync::atomic::{AtomicU32, Ordering};
use cyclors::{
dds_entity_t,
qos::{
Durability, DurabilityKind, History, HistoryKind, Qos, Reliability, ReliabilityKind,
TypeConsistency, TypeConsistencyKind, WriterDataLifecycle, DDS_INFINITE_TIME,
Durability, DurabilityKind, History, HistoryKind, IgnoreLocal, IgnoreLocalKind, Qos,
Reliability, ReliabilityKind, TypeConsistency, TypeConsistencyKind, WriterDataLifecycle,
DDS_INFINITE_TIME,
},
};
use zenoh::prelude::{keyexpr, KeyExpr};
Expand All @@ -35,9 +36,9 @@ lazy_static::lazy_static!(
pub static ref KE_SUFFIX_ACTION_FEEDBACK: &'static keyexpr = ke_for_sure!("_action/feedback");
pub static ref KE_SUFFIX_ACTION_STATUS: &'static keyexpr = ke_for_sure!("_action/status");

pub static ref QOS_ACTION_FEEDBACK: Qos = ros2_action_feedback_default_qos();
pub static ref QOS_ACTION_STATUS: Qos = ros2_action_status_default_qos();

pub static ref QOS_DEFAULT_SERVICE: Qos = ros2_service_default_qos();
pub static ref QOS_DEFAULT_ACTION_FEEDBACK: Qos = ros2_action_feedback_default_qos();
pub static ref QOS_DEFAULT_ACTION_STATUS: Qos = ros2_action_status_default_qos();
);

/// Convert DDS Topic type to ROS2 Message type
Expand Down Expand Up @@ -95,6 +96,25 @@ pub fn dds_type_to_ros2_action_type(dds_topic: &str) -> String {
)
}

fn ros2_service_default_qos() -> Qos {
// Default Service QoS copied from:
// https://github.com/ros2/rmw/blob/83445be486deae8c78d275e092eafb4bf380bd49/rmw/include/rmw/qos_profiles.h#L64C44-L64C44
let mut qos = Qos::default();
qos.history = Some(History {
kind: HistoryKind::KEEP_LAST,
depth: 10,
});
qos.reliability = Some(Reliability {
kind: ReliabilityKind::RELIABLE,
max_blocking_time: DDS_INFINITE_TIME,
});
// Add ignore_local to avoid loops
qos.ignore_local = Some(IgnoreLocal {
kind: IgnoreLocalKind::PARTICIPANT,
});
qos
}

fn ros2_action_feedback_default_qos() -> Qos {
let mut qos = Qos::default();
qos.history = Some(History {
Expand All @@ -117,10 +137,16 @@ fn ros2_action_feedback_default_qos() -> Qos {
prevent_type_widening: false,
force_type_validation: false,
});
// Add ignore_local to avoid loops
qos.ignore_local = Some(IgnoreLocal {
kind: IgnoreLocalKind::PARTICIPANT,
});
qos
}

fn ros2_action_status_default_qos() -> Qos {
// Default Status topic QoS copied from:
// https://github.com/ros2/rcl/blob/8f7f4f0804a34ee9d9ecd2d7e75a57ce2b7ced5d/rcl_action/include/rcl_action/default_qos.h#L30
let mut qos = Qos::default();
qos.durability = Some(Durability {
kind: DurabilityKind::TRANSIENT_LOCAL,
Expand All @@ -141,6 +167,10 @@ fn ros2_action_status_default_qos() -> Qos {
prevent_type_widening: false,
force_type_validation: false,
});
// Add ignore_local to avoid loops
qos.ignore_local = Some(IgnoreLocal {
kind: IgnoreLocalKind::PARTICIPANT,
});
qos
}

Expand Down
8 changes: 4 additions & 4 deletions zenoh-plugin-ros2dds/src/route_action_cli.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -107,7 +107,7 @@ impl RouteActionCli<'_> {
format!("{ros2_type}_FeedbackMessage"),
&zenoh_key_expr_prefix / *KE_SUFFIX_ACTION_FEEDBACK,
true,
QOS_ACTION_FEEDBACK.clone(),
QOS_DEFAULT_ACTION_FEEDBACK.clone(),
context,
)
.await?;
Expand All @@ -117,7 +117,7 @@ impl RouteActionCli<'_> {
ROS2_ACTION_STATUS_MSG_TYPE.to_string(),
&zenoh_key_expr_prefix / *KE_SUFFIX_ACTION_STATUS,
true,
QOS_ACTION_STATUS.clone(),
QOS_DEFAULT_ACTION_STATUS.clone(),
context,
)
.await?;
Expand Down Expand Up @@ -232,9 +232,9 @@ impl RouteActionCli<'_> {
self.route_cancel_goal.add_local_node(node.clone()),
self.route_get_result.add_local_node(node.clone()),
self.route_feedback
.add_local_node(node.clone(), &QOS_ACTION_FEEDBACK),
.add_local_node(node.clone(), &QOS_DEFAULT_ACTION_FEEDBACK),
self.route_status
.add_local_node(node.clone(), &QOS_ACTION_STATUS),
.add_local_node(node.clone(), &QOS_DEFAULT_ACTION_STATUS),
);

self.local_nodes.insert(node);
Expand Down
8 changes: 4 additions & 4 deletions zenoh-plugin-ros2dds/src/route_action_srv.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -107,7 +107,7 @@ impl RouteActionSrv<'_> {
&zenoh_key_expr_prefix / *KE_SUFFIX_ACTION_FEEDBACK,
&None,
true,
QOS_ACTION_FEEDBACK.clone(),
QOS_DEFAULT_ACTION_FEEDBACK.clone(),
context,
)
.await?;
Expand All @@ -118,7 +118,7 @@ impl RouteActionSrv<'_> {
&zenoh_key_expr_prefix / *KE_SUFFIX_ACTION_STATUS,
&None,
true,
QOS_ACTION_STATUS.clone(),
QOS_DEFAULT_ACTION_STATUS.clone(),
context,
)
.await?;
Expand Down Expand Up @@ -233,9 +233,9 @@ impl RouteActionSrv<'_> {
self.route_cancel_goal.add_local_node(node.clone()),
self.route_get_result.add_local_node(node.clone()),
self.route_feedback
.add_local_node(node.clone(), &QOS_ACTION_FEEDBACK),
.add_local_node(node.clone(), &QOS_DEFAULT_ACTION_FEEDBACK),
self.route_status
.add_local_node(node.clone(), &QOS_ACTION_STATUS),
.add_local_node(node.clone(), &QOS_DEFAULT_ACTION_STATUS),
);

self.local_nodes.insert(node);
Expand Down
Loading

0 comments on commit fb6f5bd

Please sign in to comment.