diff --git a/apriltag_detector/CMakeLists.txt b/apriltag_detector/CMakeLists.txt index 317889d..20c1a06 100644 --- a/apriltag_detector/CMakeLists.txt +++ b/apriltag_detector/CMakeLists.txt @@ -39,6 +39,10 @@ if(${rclcpp_VERSION} VERSION_GREATER_EQUAL "28.0.0") add_definitions(-DUSE_MATCHED_EVENTS) endif() +if(${image_transport_VERSION} VERSION_GREATER_EQUAL "6.3.0") + add_definitions(-DIMAGE_TRANSPORT_USE_QOS) +endif() + # # --------- header-only library # diff --git a/apriltag_detector/src/detector_component.cpp b/apriltag_detector/src/detector_component.cpp index 8a65979..fb0488b 100644 --- a/apriltag_detector/src/detector_component.cpp +++ b/apriltag_detector/src/detector_component.cpp @@ -92,14 +92,26 @@ rmw_qos_profile_t string_to_profile(const std::string & s) return (rmw_qos_profile_default); } +#ifdef IMAGE_TRANSPORT_USE_QOS +static rclcpp::QoS convert_profile(const rmw_qos_profile_t & p) +{ + return (rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(p), p)); +} +#else +static const rmw_qos_profile_t & convert_profile(const rmw_qos_profile_t & p) +{ + return (p); +} +#endif + void DetectorComponent::subscribe() { + const auto profile = string_to_profile(image_qos_profile_); image_sub_ = std::make_shared( image_transport::create_subscription( this, "image", std::bind(&DetectorComponent::callback, this, std::placeholders::_1), - in_transport_, - string_to_profile(image_qos_profile_))); // rmw_qos_profile_default);// + in_transport_, convert_profile(profile))); is_subscribed_ = true; } diff --git a/apriltag_draw/CMakeLists.txt b/apriltag_draw/CMakeLists.txt index b163dbc..4ababd2 100644 --- a/apriltag_draw/CMakeLists.txt +++ b/apriltag_draw/CMakeLists.txt @@ -46,6 +46,10 @@ endif() if(${rclcpp_VERSION} VERSION_GREATER_EQUAL "28.0.0") add_definitions(-DUSE_MATCHED_EVENTS) endif() + +if(${image_transport_VERSION} VERSION_GREATER_EQUAL "6.3.0") + add_definitions(-DIMAGE_TRANSPORT_USE_QOS) +endif() # # --------- composable node library (public) # diff --git a/apriltag_draw/src/apriltag_draw.cpp b/apriltag_draw/src/apriltag_draw.cpp index e20f35a..ac20305 100644 --- a/apriltag_draw/src/apriltag_draw.cpp +++ b/apriltag_draw/src/apriltag_draw.cpp @@ -86,6 +86,18 @@ static rmw_qos_profile_t string_to_profile(const std::string & s) return (rmw_qos_profile_default); } +#ifdef IMAGE_TRANSPORT_USE_QOS +static rclcpp::QoS convert_profile(const rmw_qos_profile_t & p) +{ + return (rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(p), p)); +} +#else +static const rmw_qos_profile_t & convert_profile(const rmw_qos_profile_t & p) +{ + return (p); +} +#endif + ApriltagDraw::ApriltagDraw(const rclcpp::NodeOptions & options) : Node("apriltag_draw", options) { @@ -109,10 +121,10 @@ ApriltagDraw::ApriltagDraw(const rclcpp::NodeOptions & options) } }; image_pub_ = image_transport::create_publisher( - this, "image_tags", rmw_qos_profile_default, pub_options); + this, "image_tags", convert_profile(rmw_qos_profile_default), pub_options); #else image_pub_ = image_transport::create_publisher( - this, "image_tags", rmw_qos_profile_default); + this, "image_tags", convert_profile(rmw_qos_profile_default)); // Since the early ROS2 image transport does not call back when // subscribers come and go: must check by polling @@ -139,7 +151,7 @@ void ApriltagDraw::subscribe() image_transport::create_subscription( this, "image", std::bind(&ApriltagDraw::imageCallback, this, std::placeholders::_1), - transport_, qos_profile_)); + transport_, convert_profile(qos_profile_))); is_subscribed_ = true; }