Skip to content

Commit

Permalink
Use rmw types for QoS options class
Browse files Browse the repository at this point in the history
This commit migrates away from the `rclcpp::*Policy` getters and setters
used throughout this package, instead just using the equivalent types
from `rmw/types.h`.

Signed-off-by: Abrar Rahman Protyasha <[email protected]>
  • Loading branch information
aprotyas committed Oct 29, 2021
1 parent 925a5e1 commit 699ab3a
Show file tree
Hide file tree
Showing 7 changed files with 45 additions and 37 deletions.
4 changes: 3 additions & 1 deletion CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -58,6 +58,7 @@ ament_target_dependencies(${PROJECT_NAME}_lib
rcutils
rosbag2_cpp
rosidl_typesupport_cpp
rmw
yaml_cpp_vendor
zstd
)
Expand Down Expand Up @@ -128,8 +129,9 @@ ament_export_targets(export_${PROJECT_NAME})
ament_export_dependencies(
rcl
rclcpp
rosbag2_cpp
rcutils
rosbag2_cpp
rmw
yaml_cpp_vendor
zstd_vendor
zstd
Expand Down
22 changes: 11 additions & 11 deletions include/domain_bridge/qos_options.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,7 @@

#include <optional>

#include "rclcpp/qos.hpp"
#include "rmw/types.h"

#include "domain_bridge/visibility_control.hpp"

Expand All @@ -34,7 +34,7 @@ class QosOptions
*
* - reliability = nullopt_t (detect automatically)
* - durability = nullopt_t (detect automatically)
* - history = rclcpp::HistoryPolicy::KeepLast
* - history = RMW_QOS_POLICY_HISTORY_KEEP_LAST
* - depth = 10
* - deadline = 0 (RMW default)
* - lifespan = 0 (RMW default)
Expand All @@ -44,33 +44,33 @@ class QosOptions

/// Get reliability.
DOMAIN_BRIDGE_PUBLIC
std::optional<rclcpp::ReliabilityPolicy>
std::optional<rmw_qos_reliability_policy_t>
reliability() const;

/// Set reliability.
DOMAIN_BRIDGE_PUBLIC
QosOptions &
reliability(const rclcpp::ReliabilityPolicy & reliability);
reliability(rmw_qos_reliability_policy_t reliability);

/// Get durability.
DOMAIN_BRIDGE_PUBLIC
std::optional<rclcpp::DurabilityPolicy>
std::optional<rmw_qos_durability_policy_t>
durability() const;

/// Set durability.
DOMAIN_BRIDGE_PUBLIC
QosOptions &
durability(const rclcpp::DurabilityPolicy & durability);
durability(rmw_qos_durability_policy_t durability);

/// Get history.
DOMAIN_BRIDGE_PUBLIC
rclcpp::HistoryPolicy
rmw_qos_history_policy_t
history() const;

/// Set history.
DOMAIN_BRIDGE_PUBLIC
QosOptions &
history(const rclcpp::HistoryPolicy & history);
history(rmw_qos_history_policy_t history);

/// Get history depth.
DOMAIN_BRIDGE_PUBLIC
Expand Down Expand Up @@ -129,9 +129,9 @@ class QosOptions
lifespan_auto();

private:
std::optional<rclcpp::ReliabilityPolicy> reliability_;
std::optional<rclcpp::DurabilityPolicy> durability_;
rclcpp::HistoryPolicy history_{rclcpp::HistoryPolicy::KeepLast};
std::optional<rmw_qos_reliability_policy_t> reliability_;
std::optional<rmw_qos_durability_policy_t> durability_;
rmw_qos_history_policy_t history_{RMW_QOS_POLICY_HISTORY_KEEP_LAST};
std::size_t depth_{10};
std::optional<std::int64_t> deadline_{0};
std::optional<std::int64_t> lifespan_{0};
Expand Down
1 change: 1 addition & 0 deletions package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,7 @@
<buildtool_depend>rosidl_default_generators</buildtool_depend>

<build_depend>rcl</build_depend>
<build_depend>rmw</build_depend>

<depend>rclcpp</depend>
<depend>rcutils</depend>
Expand Down
9 changes: 5 additions & 4 deletions src/domain_bridge/domain_bridge.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -334,12 +334,12 @@ class DomainBridgeImpl
if (qos_options.reliability()) {
qos.reliability(qos_options.reliability().value());
} else {
qos.reliability(qos_match.qos.reliability());
qos.reliability(qos_match.qos.get_rmw_qos_profile().reliability);
}
if (qos_options.durability()) {
qos.durability(qos_options.durability().value());
} else {
qos.durability(qos_match.qos.durability());
qos.durability(qos_match.qos.get_rmw_qos_profile().durability);
}
if (qos_options.deadline()) {
const auto deadline_ns = qos_options.deadline().value();
Expand All @@ -363,8 +363,9 @@ class DomainBridgeImpl
qos.lifespan(qos_match.qos.lifespan());
}

qos.liveliness(qos_match.qos.liveliness());
qos.liveliness_lease_duration(qos_match.qos.liveliness_lease_duration());
qos.liveliness(qos_match.qos.get_rmw_qos_profile().liveliness);
qos.liveliness_lease_duration(
qos_match.qos.get_rmw_qos_profile().liveliness_lease_duration);

// Print any match warnings
for (const auto & warning : qos_match.warnings) {
Expand Down
14 changes: 7 additions & 7 deletions src/domain_bridge/parse_domain_bridge_yaml_config.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,7 @@
#include "domain_bridge/topic_bridge_options.hpp"
#include "domain_bridge/qos_options.hpp"

#include "rclcpp/qos.hpp"
#include "rmw/types.h"

#include "domain_bridge/parse_domain_bridge_yaml_config.hpp"

Expand All @@ -49,9 +49,9 @@ static QosOptions parse_qos_options(YAML::Node yaml_node, const std::string & fi
try {
auto reliability_str = qos_node["reliability"].as<std::string>();
if ("reliable" == reliability_str) {
options.reliability(rclcpp::ReliabilityPolicy::Reliable);
options.reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE);
} else if ("best_effort" == reliability_str) {
options.reliability(rclcpp::ReliabilityPolicy::BestEffort);
options.reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT);
} else {
throw YamlParsingError(
file_path, "unsupported reliability policy value '" + reliability_str + "'");
Expand All @@ -65,9 +65,9 @@ static QosOptions parse_qos_options(YAML::Node yaml_node, const std::string & fi
try {
auto durability_str = qos_node["durability"].as<std::string>();
if ("volatile" == durability_str) {
options.durability(rclcpp::DurabilityPolicy::Volatile);
options.durability(RMW_QOS_POLICY_DURABILITY_VOLATILE);
} else if ("transient_local" == durability_str) {
options.durability(rclcpp::DurabilityPolicy::TransientLocal);
options.durability(RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL);
} else {
throw YamlParsingError(
file_path, "unsupported durability policy value '" + durability_str + "'");
Expand All @@ -81,9 +81,9 @@ static QosOptions parse_qos_options(YAML::Node yaml_node, const std::string & fi
try {
auto history_str = qos_node["history"].as<std::string>();
if ("keep_last" == history_str) {
options.history(rclcpp::HistoryPolicy::KeepLast);
options.history(RMW_QOS_POLICY_HISTORY_KEEP_LAST);
} else if ("keep_all" == history_str) {
options.history(rclcpp::HistoryPolicy::KeepAll);
options.history(RMW_QOS_POLICY_HISTORY_KEEP_ALL);
} else {
throw YamlParsingError(file_path, "unsupported history policy value '" + history_str + "'");
}
Expand Down
14 changes: 6 additions & 8 deletions src/domain_bridge/qos_options.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -16,47 +16,45 @@

#include <cstdint>

#include "rclcpp/qos.hpp"

#include "domain_bridge/qos_options.hpp"

namespace domain_bridge
{

std::optional<rclcpp::ReliabilityPolicy>
std::optional<rmw_qos_reliability_policy_t>
QosOptions::reliability() const
{
return reliability_;
}

QosOptions &
QosOptions::reliability(const rclcpp::ReliabilityPolicy & reliability)
QosOptions::reliability(rmw_qos_reliability_policy_t reliability)
{
reliability_.emplace(reliability);
return *this;
}

std::optional<rclcpp::DurabilityPolicy>
std::optional<rmw_qos_durability_policy_t>
QosOptions::durability() const
{
return durability_;
}

QosOptions &
QosOptions::durability(const rclcpp::DurabilityPolicy & durability)
QosOptions::durability(rmw_qos_durability_policy_t durability)
{
durability_.emplace(durability);
return *this;
}

rclcpp::HistoryPolicy
rmw_qos_history_policy_t
QosOptions::history() const
{
return history_;
}

QosOptions &
QosOptions::history(const rclcpp::HistoryPolicy & history)
QosOptions::history(rmw_qos_history_policy_t history)
{
history_ = history;
return *this;
Expand Down
18 changes: 12 additions & 6 deletions src/domain_bridge/wait_for_graph_events.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,8 @@
#include "rclcpp/client.hpp"
#include "rclcpp/node.hpp"
#include "rclcpp/qos.hpp"
#include "rmw/qos_profiles.h"
#include "rmw/types.h"

namespace domain_bridge
{
Expand Down Expand Up @@ -205,10 +207,14 @@ class WaitForGraphEvents
// Initialize QoS
QosMatchInfo result_qos;
// Default reliability and durability to value of first endpoint
result_qos.qos.reliability(endpoint_info_vec[0].qos_profile().reliability());
result_qos.qos.durability(endpoint_info_vec[0].qos_profile().durability());
rmw_qos_reliability_policy_t reliability_policy =
endpoint_info_vec[0].qos_profile().get_rmw_qos_profile().reliability;
rmw_qos_durability_policy_t durability_policy =
endpoint_info_vec[0].qos_profile().get_rmw_qos_profile().durability;
result_qos.qos.reliability(reliability_policy);
result_qos.qos.durability(durability_policy);
// Always use automatic liveliness
result_qos.qos.liveliness(rclcpp::LivelinessPolicy::Automatic);
result_qos.qos.liveliness(RMW_QOS_POLICY_LIVELINESS_AUTOMATIC);

// Reliability and durability policies can cause trouble with enpoint matching
// Count number of "reliable" publishers and number of "transient local" publishers
Expand All @@ -218,11 +224,11 @@ class WaitForGraphEvents
rclcpp::Duration max_deadline(0, 0u);
rclcpp::Duration max_lifespan(0, 0u);
for (const auto & info : endpoint_info_vec) {
const auto & profile = info.qos_profile();
if (profile.reliability() == rclcpp::ReliabilityPolicy::Reliable) {
const auto & profile = info.qos_profile().get_rmw_qos_profile();
if (profile.reliability == RMW_QOS_POLICY_RELIABILITY_RELIABLE) {
reliable_count++;
}
if (profile.durability() == rclcpp::DurabilityPolicy::TransientLocal) {
if (profile.durability == RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL) {
transient_local_count++;
}
if (profile.deadline() > max_deadline) {
Expand Down

0 comments on commit 699ab3a

Please sign in to comment.