Skip to content

Commit 4e5b854

Browse files
committed
Automatically deduce QOS paramters for subscriptions based on publications
1 parent 8ed75d1 commit 4e5b854

File tree

1 file changed

+14
-6
lines changed

1 file changed

+14
-6
lines changed

mqtt_client/src/MqttClient.ros2.cpp

Lines changed: 14 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -28,6 +28,7 @@ SOFTWARE.
2828
#include <algorithm>
2929
#include <cstdint>
3030
#include <cstring>
31+
#include <memory>
3132
#include <stdexcept>
3233
#include <vector>
3334

@@ -649,27 +650,34 @@ void MqttClient::setupSubscriptions() {
649650
} catch (rclcpp::exceptions::RCLError& e) {
650651
RCLCPP_ERROR(get_logger(), "Failed to create generic subscriber: %s",
651652
e.what());
652-
return;
653+
continue;
653654
}
654655
RCLCPP_INFO(get_logger(), "Subscribed ROS topic '%s' of type '%s'",
655656
ros_topic.c_str(), ros2mqtt.ros.msg_type.c_str());
656657

657-
}
658-
if (all_topics_and_types.count(ros_topic)) {
658+
} else {
659+
const auto &pubs = get_publishers_info_by_topic(ros_topic);
660+
661+
if (pubs.empty()) continue;
659662

663+
const auto endpointInfo = pubs.front ();
660664
// check if message type has changed or if mapping is stale
661-
const std::string& msg_type = all_topics_and_types.at(ros_topic)[0];
665+
const std::string msg_type = endpointInfo.topic_type();
666+
662667
if (msg_type == ros2mqtt.ros.msg_type && !ros2mqtt.ros.is_stale) continue;
663668
ros2mqtt.ros.is_stale = false;
664669
ros2mqtt.ros.msg_type = msg_type;
665670

666671
try {
672+
auto qos = endpointInfo.qos_profile ();
667673
ros2mqtt.ros.subscriber = create_generic_subscription(
668-
ros_topic, msg_type, ros2mqtt.ros.queue_size, bound_callback_func);
674+
ros_topic, msg_type, qos.keep_last(ros2mqtt.ros.queue_size),
675+
bound_callback_func);
676+
669677
} catch (rclcpp::exceptions::RCLError& e) {
670678
RCLCPP_ERROR(get_logger(), "Failed to create generic subscriber: %s",
671679
e.what());
672-
return;
680+
continue;
673681
}
674682
RCLCPP_INFO(get_logger(), "Subscribed ROS topic '%s' of type '%s'",
675683
ros_topic.c_str(), msg_type.c_str());

0 commit comments

Comments
 (0)