11diff --git a/src/ros2_foxglove_bridge.cpp b/src/ros2_foxglove_bridge.cpp
2- index 2229aadc..3a1c838c 100644
2+ index 601db6c..fc18e1c 100644
33--- a/src/ros2_foxglove_bridge.cpp
44+++ b/src/ros2_foxglove_bridge.cpp
5- @@ -305,7 +305,7 @@ void FoxgloveBridge::updateAdvertisedTopics(
6- std::string topic(channel.topic());
7- const TopicAndDatatype topicAndSchemaName = {topic, schemaName};
8- if (latestTopics.find(topicAndSchemaName) == latestTopics.end()) {
9- - RCLCPP_INFO(this->get_logger(), "Removing channel %lu for topic \"%s\" (%s)", channel.id(),
10- + RCLCPP_INFO(this->get_logger(), "Removing channel %llu for topic \"%s\" (%s)", channel.id(),
11- topic.c_str(), schemaName.c_str());
12- channel.close();
13- channelIt = _channels.erase(channelIt);
14- @@ -372,7 +372,7 @@ void FoxgloveBridge::updateAdvertisedTopics(
5+ @@ -492,7 +492,7 @@ void FoxgloveBridge::updateAdvertisedTopics(
6+ const TopicAndDatatype topicAndSchemaName = {topic, schemaName};
7+ if (latestTopics.find(topicAndSchemaName) == latestTopics.end()) {
8+ const auto channelId = channel.id();
9+ - RCLCPP_INFO(this->get_logger(), "Removing channel %lu for topic \"%s\" (%s)", channelId,
10+ + RCLCPP_INFO(this->get_logger(), "Removing channel %llu for topic \"%s\" (%s)", channelId,
11+ topic.c_str(), schemaName.c_str());
12+ // Remove any active subscriptions for this channel
13+ _subscriptions.erase(channelId);
14+ @@ -562,7 +562,7 @@ void FoxgloveBridge::updateAdvertisedTopics(
15+ }
16+
17+ const ChannelId channelId = channelResult.value().id();
18+ - RCLCPP_INFO(this->get_logger(), "Advertising new channel %lu for topic \"%s\"", channelId,
19+ + RCLCPP_INFO(this->get_logger(), "Advertising new channel %llu for topic \"%s\"", channelId,
20+ topic.c_str());
21+ _channels.insert({channelId, std::move(channelResult.value())});
1522 }
16-
17- const ChannelId channelId = channelResult.value().id();
18- - RCLCPP_INFO(this->get_logger(), "Advertising new channel %lu for topic \"%s\"", channelId,
19- + RCLCPP_INFO(this->get_logger(), "Advertising new channel %llu for topic \"%s\"", channelId,
20- topic.c_str());
21- _channels.insert({channelId, std::move(channelResult.value())});
22- }
23- @@ -593,14 +593,14 @@ void FoxgloveBridge::subscribeConnectionGraph(bool subscribe) {
23+ @@ -827,7 +827,7 @@ void FoxgloveBridge::subscribeConnectionGraph(bool subscribe) {
24+ }
25+
2426 void FoxgloveBridge::subscribe(ChannelId channelId, const foxglove::ClientMetadata& client) {
25- if (!client.sink_id.has_value()) {
26- RCLCPP_ERROR(this->get_logger(),
27- - "received subscribe request from client %u for channel %lu but client "
28- + "received subscribe request from client %u for channel %llu but client "
29- "has no sink ID",
30- client.id, channelId);
31- return;
32- }
33-
34- RCLCPP_INFO(this->get_logger(),
35- - "received subscribe request for channel %lu from client %u (sink %lu)", channelId,
36- + "received subscribe request for channel %llu from client %u (sink %llu)", channelId,
37- client.id, client.sink_id.value());
38- std::lock_guard<std::mutex> lock(_subscriptionsMutex);
39-
40- @@ -608,7 +608,7 @@ void FoxgloveBridge::subscribe(ChannelId channelId, const foxglove::ClientMetada
41- // calling this callback?
42- auto it = _channels.find(channelId);
43- if (it == _channels.end()) {
27+ - RCLCPP_INFO(this->get_logger(), "received subscribe request for channel %lu from client %u",
28+ + RCLCPP_INFO(this->get_logger(), "received subscribe request for channel %llu from client %u",
29+ channelId, client.id);
30+ createOrIncrementSubscription(channelId, client.id, false, client.sink_id);
31+ }
32+ @@ -880,7 +880,7 @@ void FoxgloveBridge::createOrIncrementSubscriptionLocked(ChannelId channelId, Cl
33+ std::optional<SinkId> sinkId) {
34+ auto channelIt = _channels.find(channelId);
35+ if (channelIt == _channels.end()) {
4436- RCLCPP_ERROR(this->get_logger(), "received subscribe request for unknown channel: %lu",
4537+ RCLCPP_ERROR(this->get_logger(), "received subscribe request for unknown channel: %llu",
4638 channelId);
4739 return;
4840 }
49- @@ -638,7 +638,7 @@ void FoxgloveBridge::subscribe(ChannelId channelId, const foxglove::ClientMetada
50-
51- if (!client.sink_id.has_value()) {
52- RCLCPP_ERROR(this->get_logger(),
53- - "received subscribe request for channel %lu but client "
54- + "received subscribe request for channel %llu but client "
55- "has no sink ID",
56- channelId);
57- return;
58- @@ -646,19 +646,19 @@ void FoxgloveBridge::subscribe(ChannelId channelId, const foxglove::ClientMetada
41+ @@ -911,7 +911,7 @@ void FoxgloveBridge::createOrIncrementSubscriptionLocked(ChannelId channelId, Cl
42+ auto [it, inserted] = _subscriptions.emplace(channelId, std::move(channelSub));
43+ subIt = it;
5944
60- _subscriptions.insert({{channelId, client.id}, subscription});
61- RCLCPP_INFO(this->get_logger(),
62- - "created ROS subscription on %s (%s) successfully for channel %lu (client "
63- - "%u, sink %lu)",
64- + "created ROS subscription on %s (%s) successfully for channel %llu (client "
65- + "%u, sink %llu)",
66- topic.c_str(), datatype.c_str(), channelId, client.id, client.sink_id.value());
45+ - RCLCPP_INFO(this->get_logger(), "Created ROS subscription on %s (%s) for channel %lu",
46+ + RCLCPP_INFO(this->get_logger(), "Created ROS subscription on %s (%s) for channel %llu",
47+ topic.c_str(), datatype.c_str(), channelId);
48+ }
49+
50+ @@ -935,7 +935,7 @@ void FoxgloveBridge::createOrIncrementSubscriptionLocked(ChannelId channelId, Cl
6751 }
6852
6953 void FoxgloveBridge::unsubscribe(ChannelId channelId, const foxglove::ClientMetadata& client) {
70- std::lock_guard<std::mutex> lock(_subscriptionsMutex);
71-
72- - RCLCPP_INFO(this->get_logger(), "received unsubscribe request for channel %lu", channelId);
73- + RCLCPP_INFO(this->get_logger(), "received unsubscribe request for channel %llu", channelId);
74-
75- auto it = _channels.find(channelId);
76- if (it == _channels.end()) {
77- - RCLCPP_ERROR(this->get_logger(), "received unsubscribe request for unknown channel %lu",
78- + RCLCPP_ERROR(this->get_logger(), "received unsubscribe request for unknown channel %llu",
79- channelId);
80- return;
81- }
82- @@ -666,7 +666,7 @@ void FoxgloveBridge::unsubscribe(ChannelId channelId, const foxglove::ClientMeta
83- auto subscriptionIt = _subscriptions.find({channelId, client.id});
84- if (subscriptionIt == _subscriptions.end()) {
54+ - RCLCPP_INFO(this->get_logger(), "received unsubscribe request for channel %lu from client %u",
55+ + RCLCPP_INFO(this->get_logger(), "received unsubscribe request for channel %llu from client %u",
56+ channelId, client.id);
57+ removeOrDecrementSubscription(channelId, client.id, false);
58+ }
59+ @@ -951,7 +951,7 @@ void FoxgloveBridge::removeOrDecrementSubscriptionLocked(ChannelId channelId, Cl
60+ auto subIt = _subscriptions.find(channelId);
61+ if (subIt == _subscriptions.end()) {
8562 RCLCPP_ERROR(this->get_logger(),
86- - "Client %u tried unsubscribing from channel %lu but a corresponding ROS "
87- + "Client %u tried unsubscribing from channel %llu but a corresponding ROS "
88- "subscription doesn't exist",
89- client.id, channelId);
63+ - "Client %u tried unsubscribing from channel %lu but no subscription exists",
64+ + "Client %u tried unsubscribing from channel %llu but no subscription exists",
65+ clientId, channelId);
9066 return;
91- @@ -674,7 +674,7 @@ void FoxgloveBridge::unsubscribe(ChannelId channelId, const foxglove::ClientMeta
92-
93- const std::string& topic = subscriptionIt->second->get_topic_name();
94- RCLCPP_INFO(this->get_logger(),
95- - "Cleaned up subscription to topic %s for client %u on channel %lu", topic.c_str(),
96- + "Cleaned up subscription to topic %s for client %u on channel %llu", topic.c_str(),
97- client.id, channelId);
98- _subscriptions.erase(subscriptionIt);
99- }
100- @@ -752,7 +752,7 @@ void FoxgloveBridge::clientAdvertise(ClientId clientId, const foxglove::ClientCh
101- auto publisher = this->create_generic_publisher(topicName, topicType, qos, publisherOptions);
102-
67+ }
68+ @@ -966,7 +966,7 @@ void FoxgloveBridge::removeOrDecrementSubscriptionLocked(ChannelId channelId, Cl
69+ // If no more subscribers, destroy the ROS subscription
70+ if (subIt->second.wsClientIds.empty() && subIt->second.gatewayClientIds.empty()) {
10371 RCLCPP_INFO(this->get_logger(),
104- - "Client ID %d is advertising \"%s\" (%s) on channel %d with encoding \"%s\"",
105- + "Client ID %u is advertising \"%s\" (%s) on channel %u with encoding \"%s\"",
106- clientId, topicName.c_str(), topicType.c_str(), channel.id, encoding.c_str() );
107-
108- // Store the new topic advertisement
109- @@ -778 ,7 +778 ,7 @@ void FoxgloveBridge::clientUnadvertise(ClientId clientId, ChannelId clientChanne
72+ - "Cleaned up ROS subscription for channel %lu (no more subscribers)", channelId);
73+ + "Cleaned up ROS subscription for channel %llu (no more subscribers)", channelId);
74+ _subscriptions.erase(subIt );
75+ }
76+ }
77+ @@ -1105 ,7 +1105 ,7 @@ void FoxgloveBridge::clientUnadvertise(ClientId clientId, ChannelId clientChanne
11078
11179 const auto& publisher = it->second.publisher;
11280 RCLCPP_INFO(this->get_logger(),
@@ -115,3 +83,95 @@ index 2229aadc..3a1c838c 100644
11583 publisher->get_topic_name(), publisher->get_subscription_count(), clientChannelId);
11684
11785 _clientAdvertisedTopics.erase(it);
86+ @@ -1585,13 +1585,13 @@ void FoxgloveBridge::gatewaySubscribe(uint32_t clientId,
87+ auto sinkId = _gateway->sinkId();
88+ if (!sinkId.has_value()) {
89+ RCLCPP_WARN(this->get_logger(),
90+ - "Gateway: subscribe request for channel %lu (\"%s\") from client %u "
91+ + "Gateway: subscribe request for channel %llu (\"%s\") from client %u "
92+ "but gateway session has no sink ID (reconnecting?); "
93+ "cached transient_local messages will not be replayed",
94+ channel.id(), std::string(channel.topic()).c_str(), clientId);
95+ }
96+ RCLCPP_INFO(this->get_logger(),
97+ - "Gateway: received subscribe request for channel %lu (\"%s\") from client %u",
98+ + "Gateway: received subscribe request for channel %llu (\"%s\") from client %u",
99+ channel.id(), std::string(channel.topic()).c_str(), clientId);
100+ createOrIncrementSubscription(channel.id(), clientId, true, sinkId);
101+ }
102+ @@ -1599,7 +1599,7 @@ void FoxgloveBridge::gatewaySubscribe(uint32_t clientId,
103+ void FoxgloveBridge::gatewayUnsubscribe(uint32_t clientId,
104+ const foxglove::ChannelDescriptor& channel) {
105+ RCLCPP_INFO(this->get_logger(),
106+ - "Gateway: received unsubscribe request for channel %lu (\"%s\") from client %u",
107+ + "Gateway: received unsubscribe request for channel %llu (\"%s\") from client %u",
108+ channel.id(), std::string(channel.topic()).c_str(), clientId);
109+ removeOrDecrementSubscription(channel.id(), clientId, true);
110+ }
111+ @@ -1614,7 +1614,7 @@ void FoxgloveBridge::gatewayClientAdvertise(uint32_t clientId,
112+
113+ ChannelAndClientId key = {channelId, clientId};
114+ if (_gatewayClientAdvertisedTopics.find(key) != _gatewayClientAdvertisedTopics.end()) {
115+ - RCLCPP_WARN(this->get_logger(), "Gateway: client %u already advertised channel %lu (\"%s\")",
116+ + RCLCPP_WARN(this->get_logger(), "Gateway: client %u already advertised channel %llu (\"%s\")",
117+ clientId, channelId, topicName.c_str());
118+ return;
119+ }
120+ @@ -1631,7 +1631,7 @@ void FoxgloveBridge::gatewayClientAdvertise(uint32_t clientId,
121+
122+ if (topicType.empty()) {
123+ RCLCPP_ERROR(this->get_logger(),
124+ - "Gateway: client %u advertised channel %lu (\"%s\") with empty schema name",
125+ + "Gateway: client %u advertised channel %llu (\"%s\") with empty schema name",
126+ clientId, channelId, topicName.c_str());
127+ return;
128+ }
129+ @@ -1639,12 +1639,12 @@ void FoxgloveBridge::gatewayClientAdvertise(uint32_t clientId,
130+ try {
131+ auto ad = createClientPublisher(topicName, topicType, encoding, schemaData, schemaLen);
132+ RCLCPP_INFO(this->get_logger(),
133+ - "Gateway: client %u is advertising channel %lu \"%s\" (%s) with encoding \"%s\"",
134+ + "Gateway: client %u is advertising channel %llu \"%s\" (%s) with encoding \"%s\"",
135+ clientId, channelId, topicName.c_str(), topicType.c_str(), encoding.c_str());
136+ _gatewayClientAdvertisedTopics.emplace(key, std::move(ad));
137+ } catch (const std::exception& ex) {
138+ RCLCPP_ERROR(this->get_logger(),
139+ - "Gateway: failed to create publisher for client %u channel %lu (\"%s\"): %s",
140+ + "Gateway: failed to create publisher for client %u channel %llu (\"%s\"): %s",
141+ clientId, channelId, topicName.c_str(), ex.what());
142+ }
143+ }
144+ @@ -1658,13 +1658,13 @@ void FoxgloveBridge::gatewayClientUnadvertise(uint32_t clientId,
145+
146+ auto it = _gatewayClientAdvertisedTopics.find(key);
147+ if (it == _gatewayClientAdvertisedTopics.end()) {
148+ - RCLCPP_WARN(this->get_logger(), "Gateway: client %u unadvertised unknown channel %lu (\"%s\")",
149+ + RCLCPP_WARN(this->get_logger(), "Gateway: client %u unadvertised unknown channel %llu (\"%s\")",
150+ clientId, channelId, std::string(channel.topic()).c_str());
151+ return;
152+ }
153+
154+ RCLCPP_INFO(this->get_logger(),
155+ - "Gateway: client %u is no longer advertising channel %lu (\"%s\")", clientId,
156+ + "Gateway: client %u is no longer advertising channel %llu (\"%s\")", clientId,
157+ channelId, it->second.topicName.c_str());
158+ _gatewayClientAdvertisedTopics.erase(it);
159+
160+ @@ -1688,7 +1688,7 @@ void FoxgloveBridge::gatewayClientMessage(uint32_t clientId,
161+ auto it = _gatewayClientAdvertisedTopics.find(key);
162+ if (it == _gatewayClientAdvertisedTopics.end()) {
163+ RCLCPP_ERROR(this->get_logger(),
164+ - "Gateway: dropping message from client %u for unknown channel %lu", clientId,
165+ + "Gateway: dropping message from client %u for unknown channel %llu", clientId,
166+ channelId);
167+ return;
168+ }
169+ @@ -1699,7 +1699,7 @@ void FoxgloveBridge::gatewayClientMessage(uint32_t clientId,
170+ try {
171+ publishClientData(ad, data, dataLen);
172+ } catch (const std::exception& ex) {
173+ - RCLCPP_ERROR(this->get_logger(), "Gateway: dropping message from client %u for channel %lu: %s",
174+ + RCLCPP_ERROR(this->get_logger(), "Gateway: dropping message from client %u for channel %llu: %s",
175+ clientId, channelId, ex.what());
176+ }
177+ }
0 commit comments