diff --git a/turtlesim/include/turtlesim/qos.hpp b/turtlesim/include/turtlesim/qos.hpp new file mode 100644 index 00000000..6fde3224 --- /dev/null +++ b/turtlesim/include/turtlesim/qos.hpp @@ -0,0 +1,43 @@ +// Copyright (c) 2024, Open Source Robotics Foundation, Inc. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Willow Garage nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef TURTLESIM__QOS_HPP_ +#define TURTLESIM__QOS_HPP_ + +#include + +namespace turtlesim +{ +// Return the QoS used for all publishers/subscriptions. +inline rclcpp::QoS topic_qos() +{ + return rclcpp::QoS(rclcpp::KeepLast(7)).reliable(); +} +} // namespace turtlesim + +#endif // TURTLESIM__QOS_HPP_ diff --git a/turtlesim/src/turtle.cpp b/turtlesim/src/turtle.cpp index b535df5f..d8cc4d5f 100644 --- a/turtlesim/src/turtle.cpp +++ b/turtlesim/src/turtle.cpp @@ -44,6 +44,7 @@ #include "turtlesim/srv/set_pen.hpp" #include "turtlesim/srv/teleport_absolute.hpp" #include "turtlesim/srv/teleport_relative.hpp" +#include "turtlesim/qos.hpp" #define DEFAULT_PEN_R 0xb3 #define DEFAULT_PEN_G 0xb8 @@ -72,7 +73,7 @@ Turtle::Turtle( { pen_.setWidth(3); - rclcpp::QoS qos(rclcpp::KeepLast(7)); + const rclcpp::QoS qos = topic_qos(); velocity_sub_ = nh_->create_subscription( real_name + "/cmd_vel", qos, std::bind( &Turtle::velocityCallback, this, diff --git a/turtlesim/tutorials/draw_square.cpp b/turtlesim/tutorials/draw_square.cpp index 6ffbf4b6..871a7881 100644 --- a/turtlesim/tutorials/draw_square.cpp +++ b/turtlesim/tutorials/draw_square.cpp @@ -36,6 +36,8 @@ #include #include +#include "turtlesim/qos.hpp" + #define PI 3.141592f class DrawSquare final : public rclcpp::Node @@ -44,11 +46,12 @@ class DrawSquare final : public rclcpp::Node explicit DrawSquare(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) : rclcpp::Node("draw_square", options) { - twist_pub_ = this->create_publisher("turtle1/cmd_vel", 1); + const rclcpp::QoS qos = turtlesim::topic_qos(); + twist_pub_ = this->create_publisher("turtle1/cmd_vel", qos); pose_sub_ = this->create_subscription( - "turtle1/pose", 1, std::bind(&DrawSquare::poseCallback, this, std::placeholders::_1)); + "turtle1/pose", qos, std::bind(&DrawSquare::poseCallback, this, std::placeholders::_1)); reset_client_ = this->create_client("reset"); diff --git a/turtlesim/tutorials/mimic.cpp b/turtlesim/tutorials/mimic.cpp index 6aada785..ec9bc894 100644 --- a/turtlesim/tutorials/mimic.cpp +++ b/turtlesim/tutorials/mimic.cpp @@ -30,15 +30,18 @@ #include #include +#include "turtlesim/qos.hpp" + class MimicNode : public rclcpp::Node { public: MimicNode() : rclcpp::Node("turtle_mimic") { - twist_pub_ = this->create_publisher("output/cmd_vel", 1); + const rclcpp::QoS qos = turtlesim::topic_qos(); + twist_pub_ = this->create_publisher("output/cmd_vel", qos); pose_sub_ = this->create_subscription( - "input/pose", 1, std::bind(&MimicNode::poseCallback, this, std::placeholders::_1)); + "input/pose", qos, std::bind(&MimicNode::poseCallback, this, std::placeholders::_1)); } private: diff --git a/turtlesim/tutorials/teleop_turtle_key.cpp b/turtlesim/tutorials/teleop_turtle_key.cpp index 7a3510ec..9e6ac557 100644 --- a/turtlesim/tutorials/teleop_turtle_key.cpp +++ b/turtlesim/tutorials/teleop_turtle_key.cpp @@ -44,6 +44,8 @@ # include // NO LINT #endif +#include "turtlesim/qos.hpp" + static constexpr char KEYCODE_RIGHT = 0x43; static constexpr char KEYCODE_LEFT = 0x44; static constexpr char KEYCODE_UP = 0x41; @@ -186,7 +188,9 @@ class TeleopTurtle final nh_->declare_parameter("scale_angular", 2.0); nh_->declare_parameter("scale_linear", 2.0); - twist_pub_ = nh_->create_publisher("turtle1/cmd_vel", 1); + twist_pub_ = nh_->create_publisher( + "turtle1/cmd_vel", + turtlesim::topic_qos()); rotate_absolute_client_ = rclcpp_action::create_client( nh_, "turtle1/rotate_absolute");