Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 4 additions & 0 deletions apriltag_detector/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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
#
Expand Down
16 changes: 14 additions & 2 deletions apriltag_detector/src/detector_component.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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::Subscriber>(
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;
}

Expand Down
4 changes: 4 additions & 0 deletions apriltag_draw/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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)
#
Expand Down
18 changes: 15 additions & 3 deletions apriltag_draw/src/apriltag_draw.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{
Expand All @@ -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
Expand All @@ -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;
}

Expand Down