Skip to content

Commit

Permalink
clang format
Browse files Browse the repository at this point in the history
  • Loading branch information
@Martin-Oehler
Martin-Oehler committed Feb 5, 2025
1 parent 6b1cbb1 commit 9c29326
Show file tree
Hide file tree
Showing 4 changed files with 218 additions and 136 deletions.
71 changes: 71 additions & 0 deletions .clang-format
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,71 @@
# Base Style
BasedOnStyle: LLVM

# Indentation
IndentWidth: 2
TabWidth: 2
UseTab: Never
IndentCaseLabels: true
IndentPPDirectives: AfterHash
NamespaceIndentation: None

# Brace Wrapping
BraceWrapping:
AfterClass: true
AfterControlStatement: false
AfterEnum: true
AfterFunction: true
AfterNamespace: false
AfterStruct: true
BeforeCatch: true
BeforeElse: true
IndentBraces: false
SplitEmptyFunction: false
BreakBeforeBraces: Custom

# Line Breaking
ColumnLimit: 120
BreakTemplateDeclarations: Yes
AlwaysBreakBeforeMultilineStrings: false
BreakBeforeBinaryOperators: None
BreakBeforeTernaryOperators: false

# Penalties
PenaltyExcessCharacter: 50
PenaltyBreakBeforeFirstCallParameter: 30
PenaltyBreakComment: 1000
PenaltyBreakFirstLessLess: 10
PenaltyBreakString: 100
PenaltyReturnTypeOnItsOwnLine: 50

# Spacing
SpacesInSquareBrackets: false
SpaceBeforeParens: ControlStatements
PointerAlignment: Left
SpacesBeforeTrailingComments: 2
SpacesInParentheses: false
SpacesInAngles: false
SpaceInEmptyParentheses: false
SpacesInCStyleCastParentheses: false
SpaceAfterCStyleCast: true
SpaceBeforeAssignmentOperators: true

# Short Constructs Formatting
AllowShortBlocksOnASingleLine: Always
AllowShortIfStatementsOnASingleLine: false
AllowShortLoopsOnASingleLine: false
AllowShortFunctionsOnASingleLine: Empty
AllowShortCaseLabelsOnASingleLine: false

# Constructor Initializers
PackConstructorInitializers: NextLineOnly

# Include Sorting
SortIncludes: Never

# Comment Formatting
AlignTrailingComments: true
ReflowComments: true

# Miscellaneous
MaxEmptyLinesToKeep: 1
26 changes: 14 additions & 12 deletions include/hector_multi_robot_tools/prefix_tf_republisher_node.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,25 +6,27 @@

namespace hector_multi_robot_tools {

class PrefixTfRepublisherNode : public rclcpp::Node {
class PrefixTfRepublisherNode : public rclcpp::Node
{
public:
PrefixTfRepublisherNode();
explicit PrefixTfRepublisherNode(const rclcpp::NodeOptions& options);
PrefixTfRepublisherNode();
explicit PrefixTfRepublisherNode(const rclcpp::NodeOptions& options);

private:
void tfMessageCallback(const tf2_msgs::msg::TFMessage& msg) const;
void tfMessageCallback(const tf2_msgs::msg::TFMessage& msg) const;

void prependFramePrefixToMsg(tf2_msgs::msg::TFMessage &tf_message, const std::string& prefix) const;
std::string preprendFramePrefix(const std::string& frame_id, const std::string& prefix) const;
void prependFramePrefixToMsg(tf2_msgs::msg::TFMessage& tf_message, const std::string& prefix) const;
std::string preprendFramePrefix(const std::string& frame_id, const std::string& prefix) const;

std::optional<rclcpp::QoS> tryDiscoverQoSProfile(const std::string& topic) const;
std::optional<rclcpp::QoS> tryDiscoverQoSProfile(const std::string& topic) const;

rclcpp::Subscription<tf2_msgs::msg::TFMessage>::SharedPtr sub_;
rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr pub_;
rclcpp::Subscription<tf2_msgs::msg::TFMessage>::SharedPtr sub_;
rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr pub_;

std::string frame_prefix_;
std::vector<std::string> global_frames_;
std::string frame_prefix_;
std::vector<std::string> global_frames_;
};

}
} // namespace hector_multi_robot_tools

#endif
12 changes: 6 additions & 6 deletions src/prefix_tf_republisher.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,11 +1,11 @@
#include <hector_multi_robot_tools/prefix_tf_republisher_node.hpp>

int main(const int argc, char **argv)
int main(const int argc, char** argv)
{
rclcpp::init(argc, argv);
rclcpp::init(argc, argv);

const auto node = std::make_shared<hector_multi_robot_tools::PrefixTfRepublisherNode>();
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
const auto node = std::make_shared<hector_multi_robot_tools::PrefixTfRepublisherNode>();
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}
245 changes: 127 additions & 118 deletions src/prefix_tf_republisher_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,140 +2,149 @@

namespace {

std::string stripLeadingSlash(const std::string& frame_id) {
if (!frame_id.empty() && frame_id.front() == '/') {
return frame_id.substr(1, frame_id.size() - 1);
}
return frame_id;
}
std::string stripLeadingSlash(const std::string& frame_id)
{
if (!frame_id.empty() && frame_id.front() == '/') {
return frame_id.substr(1, frame_id.size() - 1);
}
return frame_id;
}

} // namespace

namespace hector_multi_robot_tools {
PrefixTfRepublisherNode::PrefixTfRepublisherNode() : PrefixTfRepublisherNode(rclcpp::NodeOptions()) {
}

PrefixTfRepublisherNode::PrefixTfRepublisherNode(const rclcpp::NodeOptions &options)
PrefixTfRepublisherNode::PrefixTfRepublisherNode()
: PrefixTfRepublisherNode(rclcpp::NodeOptions())
{}

PrefixTfRepublisherNode::PrefixTfRepublisherNode(const rclcpp::NodeOptions& options)
: Node("prefix_tf_republisher_node", options)
{
if (get_effective_namespace() == "/") {
RCLCPP_FATAL_STREAM(get_logger(), "Node is started in global namespace. This will lead to a topic loop.");
rclcpp::shutdown();
}

rcl_interfaces::msg::ParameterDescriptor frame_prefix_descriptor;
frame_prefix_descriptor.description = "Frame prefix to prepend on frame_id (default: node namespace)";
frame_prefix_descriptor.read_only = true;
std::string default_frame_prefix = stripLeadingSlash(get_effective_namespace());
frame_prefix_ = declare_parameter("frame_prefix", default_frame_prefix, frame_prefix_descriptor);
RCLCPP_INFO_STREAM(get_logger(), "Republishing tf frame ids with prefix '" << frame_prefix_ << "'");

rcl_interfaces::msg::ParameterDescriptor global_frames_descriptor;
global_frames_descriptor.description = "Global frames are republished without attaching the robot-specific prefix.";
global_frames_descriptor.read_only = true;
global_frames_ = declare_parameter("global_frames", std::vector<std::string>(), global_frames_descriptor);

const std::string input_topic = "input_tf_topic";
std::optional<rclcpp::QoS> qos;
rclcpp::Rate rate(10);
while (!qos) {
rate.sleep();
RCLCPP_INFO_STREAM_THROTTLE(get_logger(), *get_clock(), 1000, "Waiting for input topic QoS profile.");
qos = tryDiscoverQoSProfile(input_topic);
}

pub_ = create_publisher<tf2_msgs::msg::TFMessage>("output_tf_topic", qos.value());
sub_ = create_subscription<tf2_msgs::msg::TFMessage>(input_topic, qos.value(),
std::bind(&PrefixTfRepublisherNode::tfMessageCallback, this, std::placeholders::_1));
if (get_effective_namespace() == "/") {
RCLCPP_FATAL_STREAM(get_logger(), "Node is started in global namespace. This will lead to a topic loop.");
rclcpp::shutdown();
}

rcl_interfaces::msg::ParameterDescriptor frame_prefix_descriptor;
frame_prefix_descriptor.description = "Frame prefix to prepend on frame_id (default: node namespace)";
frame_prefix_descriptor.read_only = true;
std::string default_frame_prefix = stripLeadingSlash(get_effective_namespace());
frame_prefix_ = declare_parameter("frame_prefix", default_frame_prefix, frame_prefix_descriptor);
RCLCPP_INFO_STREAM(get_logger(), "Republishing tf frame ids with prefix '" << frame_prefix_ << "'");

rcl_interfaces::msg::ParameterDescriptor global_frames_descriptor;
global_frames_descriptor.description = "Global frames are republished without attaching the robot-specific prefix.";
global_frames_descriptor.read_only = true;
global_frames_ = declare_parameter("global_frames", std::vector<std::string>(), global_frames_descriptor);

const std::string input_topic = "input_tf_topic";
std::optional<rclcpp::QoS> qos;
rclcpp::Rate rate(10);
while (!qos) {
rate.sleep();
RCLCPP_INFO_STREAM_THROTTLE(get_logger(), *get_clock(), 1000, "Waiting for input topic QoS profile.");
qos = tryDiscoverQoSProfile(input_topic);
}

pub_ = create_publisher<tf2_msgs::msg::TFMessage>("output_tf_topic", qos.value());
sub_ = create_subscription<tf2_msgs::msg::TFMessage>(
input_topic, qos.value(), std::bind(&PrefixTfRepublisherNode::tfMessageCallback, this, std::placeholders::_1));
}

void PrefixTfRepublisherNode::tfMessageCallback(const tf2_msgs::msg::TFMessage &msg) const {
tf2_msgs::msg::TFMessage msg_copy = msg;
prependFramePrefixToMsg(msg_copy, frame_prefix_);
pub_->publish(msg_copy);
void PrefixTfRepublisherNode::tfMessageCallback(const tf2_msgs::msg::TFMessage& msg) const
{
tf2_msgs::msg::TFMessage msg_copy = msg;
prependFramePrefixToMsg(msg_copy, frame_prefix_);
pub_->publish(msg_copy);
}

void PrefixTfRepublisherNode::prependFramePrefixToMsg(tf2_msgs::msg::TFMessage &tf_message, const std::string &prefix) const {
for (auto& msg: tf_message.transforms) {
msg.child_frame_id = preprendFramePrefix(msg.child_frame_id, prefix);
msg.header.frame_id = preprendFramePrefix(msg.header.frame_id, prefix);
}
void PrefixTfRepublisherNode::prependFramePrefixToMsg(tf2_msgs::msg::TFMessage& tf_message,
const std::string& prefix) const
{
for (auto& msg : tf_message.transforms) {
msg.child_frame_id = preprendFramePrefix(msg.child_frame_id, prefix);
msg.header.frame_id = preprendFramePrefix(msg.header.frame_id, prefix);
}
}

std::string PrefixTfRepublisherNode::preprendFramePrefix(const std::string &frame_id, const std::string &prefix) const {
auto it = std::find(global_frames_.begin(), global_frames_.end(), frame_id);
if (it != global_frames_.end()) {
// Frame is a global frame
return frame_id;
}
// Frame is a local frame, attach prefix
return prefix + "/" + frame_id;
std::string PrefixTfRepublisherNode::preprendFramePrefix(const std::string& frame_id, const std::string& prefix) const
{
auto it = std::find(global_frames_.begin(), global_frames_.end(), frame_id);
if (it != global_frames_.end()) {
// Frame is a global frame
return frame_id;
}
// Frame is a local frame, attach prefix
return prefix + "/" + frame_id;
}

std::optional<rclcpp::QoS> PrefixTfRepublisherNode::tryDiscoverQoSProfile(const std::string& topic) const {
// taken from https://github.com/ros-tooling/topic_tools/blob/rolling/topic_tools/src/tool_base_node.cpp#L81
// Query QoS info for publishers
std::vector<rclcpp::TopicEndpointInfo> endpoint_info_vec = this->get_publishers_info_by_topic(topic);
std::size_t num_endpoints = endpoint_info_vec.size();

// If there are no publishers, return an empty optional
if (endpoint_info_vec.empty()) {
return {};
std::optional<rclcpp::QoS> PrefixTfRepublisherNode::tryDiscoverQoSProfile(const std::string& topic) const
{
// taken from https://github.com/ros-tooling/topic_tools/blob/rolling/topic_tools/src/tool_base_node.cpp#L81
// Query QoS info for publishers
std::vector<rclcpp::TopicEndpointInfo> endpoint_info_vec = this->get_publishers_info_by_topic(topic);
std::size_t num_endpoints = endpoint_info_vec.size();

// If there are no publishers, return an empty optional
if (endpoint_info_vec.empty()) {
return {};
}

// Initialize QoS
rclcpp::QoS qos{10};
// Default reliability and durability to value of first endpoint
qos.reliability(endpoint_info_vec[0].qos_profile().reliability());
qos.durability(endpoint_info_vec[0].qos_profile().durability());
// Always use automatic liveliness
qos.liveliness(rclcpp::LivelinessPolicy::Automatic);

// Reliability and durability policies can cause trouble with endpoint matching
// Count number of "reliable" publishers and number of "transient local" publishers
std::size_t reliable_count = 0u;
std::size_t transient_local_count = 0u;
// For duration-based policies, note the largest value to ensure matching all publishers
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) {
reliable_count++;
}

// Initialize QoS
rclcpp::QoS qos{10};
// Default reliability and durability to value of first endpoint
qos.reliability(endpoint_info_vec[0].qos_profile().reliability());
qos.durability(endpoint_info_vec[0].qos_profile().durability());
// Always use automatic liveliness
qos.liveliness(rclcpp::LivelinessPolicy::Automatic);

// Reliability and durability policies can cause trouble with endpoint matching
// Count number of "reliable" publishers and number of "transient local" publishers
std::size_t reliable_count = 0u;
std::size_t transient_local_count = 0u;
// For duration-based policies, note the largest value to ensure matching all publishers
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) {
reliable_count++;
}
if (profile.durability() == rclcpp::DurabilityPolicy::TransientLocal) {
transient_local_count++;
}
if (profile.deadline() > max_deadline) {
max_deadline = profile.deadline();
}
if (profile.lifespan() > max_lifespan) {
max_lifespan = profile.lifespan();
}
if (profile.durability() == rclcpp::DurabilityPolicy::TransientLocal) {
transient_local_count++;
}

// If not all publishers have a "reliable" policy, then use a "best effort" policy
// and print a warning
if (reliable_count > 0u && reliable_count != num_endpoints) {
qos.best_effort();
RCLCPP_WARN_STREAM(
this->get_logger(), "Some, but not all, publishers on topic '" << topic << "' "
"offer 'reliable' reliability. Falling back to 'best effort' reliability in order"
"to connect to all publishers.");
if (profile.deadline() > max_deadline) {
max_deadline = profile.deadline();
}

// If not all publishers have a "transient local" policy, then use a "volatile" policy
// and print a warning
if (transient_local_count > 0u && transient_local_count != num_endpoints) {
qos.durability_volatile();
RCLCPP_WARN_STREAM(
this->get_logger(), "Some, but not all, publishers on topic '" << topic << "' "
"offer 'transient local' durability. Falling back to 'volatile' durability in order"
"to connect to all publishers.");
if (profile.lifespan() > max_lifespan) {
max_lifespan = profile.lifespan();
}

qos.deadline(max_deadline);
qos.lifespan(max_lifespan);

return qos;
}
}

// If not all publishers have a "reliable" policy, then use a "best effort" policy
// and print a warning
if (reliable_count > 0u && reliable_count != num_endpoints) {
qos.best_effort();
RCLCPP_WARN_STREAM(this->get_logger(), "Some, but not all, publishers on topic '"
<< topic
<< "' offer 'reliable' reliability. Falling back to 'best effort' "
"reliability in order to connect to all publishers.");
}

// If not all publishers have a "transient local" policy, then use a "volatile" policy
// and print a warning
if (transient_local_count > 0u && transient_local_count != num_endpoints) {
qos.durability_volatile();
RCLCPP_WARN_STREAM(this->get_logger(), "Some, but not all, publishers on topic '"
<< topic
<< "' offer 'transient local' durability. Falling back to 'volatile' "
"durability in order to connect to all publishers.");
}

qos.deadline(max_deadline);
qos.lifespan(max_lifespan);

return qos;
}
} // namespace hector_multi_robot_tools

0 comments on commit 9c29326

Please sign in to comment.