Skip to content

Commit b9862f0

Browse files
authored
Merge pull request #4 from wolfv/refresh-snapshot-2026-06-08
Full rebuild June 2026 bump ros2-distro-mutex to 0.17.0 and build number to 21
2 parents ba6a604 + 2601327 commit b9862f0

21 files changed

Lines changed: 743 additions & 529 deletions

.github/workflows/testpr.yml

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -101,7 +101,7 @@ jobs:
101101
- name: Delete specific outdated cache entries
102102
shell: bash -l {0}
103103
run: |
104-
# rm -rf ${{ matrix.folder_cache }}/ros-lyrical-moveit-core* 2>/dev/null || true
104+
# rm -rf ${{ matrix.folder_cache }}/ros-lyrical-pcl-conversions* 2>/dev/null || true
105105
mkdir -p ${{ matrix.folder_cache }}
106106
pixi run rattler-index fs ${{ matrix.folder_cache }}/.. --force
107107

conda_build_config.yaml

Lines changed: 8 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,14 @@
11
numpy:
22
- 2
33
assimp:
4-
- 6.0.3
4+
- 6.0.5
5+
# Boost 1.90 migration: conda-forge's global pin is still 1.88, but the
6+
# boost-python / geometry stack (eigenpy 3.13, pinocchio, ...) has already
7+
# moved to 1.90, so we pin ahead to build the whole distro against 1.90.
8+
libboost_devel:
9+
- '1.90'
10+
libboost_python_devel:
11+
- '1.90'
512
libprotobuf:
613
- 6.33.5
714
protobuf:
Lines changed: 1 addition & 66 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,5 @@
11
diff --git a/CMakeLists.txt b/CMakeLists.txt
2-
index 20bdedf..4e9d53d 100644
2+
index 75d5822..ff9d333 100644
33
--- a/CMakeLists.txt
44
+++ b/CMakeLists.txt
55
@@ -5,8 +5,10 @@ project(apriltag_ros)
@@ -15,68 +15,3 @@ index 20bdedf..4e9d53d 100644
1515
endif()
1616

1717
option(ASAN "use AddressSanitizer to detect memory issues" OFF)
18-
diff --git a/src/AprilTagNode.cpp b/src/AprilTagNode.cpp
19-
index c13bf26..4dcbc0e 100644
20-
--- a/src/AprilTagNode.cpp
21-
+++ b/src/AprilTagNode.cpp
22-
@@ -13,7 +13,7 @@
23-
#include <rclcpp_components/register_node_macro.hpp>
24-
#include <sensor_msgs/msg/camera_info.hpp>
25-
#include <sensor_msgs/msg/image.hpp>
26-
-#include <tf2_ros/transform_broadcaster.h>
27-
+#include <tf2_ros/transform_broadcaster.hpp>
28-
29-
// apriltag
30-
#include "tag_functions.hpp"
31-
@@ -99,13 +99,13 @@ AprilTagNode::AprilTagNode(const rclcpp::NodeOptions& options)
32-
td(apriltag_detector_create()),
33-
// topics
34-
sub_cam(image_transport::create_camera_subscription(
35-
- this,
36-
+ *this,
37-
this->get_node_topics_interface()->resolve_topic_name("image_rect"),
38-
std::bind(&AprilTagNode::onCamera, this, std::placeholders::_1, std::placeholders::_2),
39-
declare_parameter("image_transport", "raw", descr({}, true)),
40-
- rmw_qos_profile_sensor_data)),
41-
+ rclcpp::SensorDataQoS())),
42-
pub_detections(create_publisher<apriltag_msgs::msg::AprilTagDetectionArray>("detections", rclcpp::QoS(1))),
43-
- tf_broadcaster(this)
44-
+ tf_broadcaster(*this)
45-
{
46-
// read-only parameters
47-
const std::string tag_family = declare_parameter("family", "36h11", descr("tag family", true));
48-
diff --git a/src/conversion.cpp b/src/conversion.cpp
49-
index b9b30a8..16a8d66 100644
50-
--- a/src/conversion.cpp
51-
+++ b/src/conversion.cpp
52-
@@ -4,7 +4,7 @@
53-
#include <geometry_msgs/msg/quaternion.hpp>
54-
#include <geometry_msgs/msg/vector3.hpp>
55-
#include <opencv2/core/mat.hpp>
56-
-#include <tf2/convert.h>
57-
+#include <tf2/convert.hpp>
58-
59-
template<>
60-
void tf2::convert(const Eigen::Quaterniond& eigen_quat, geometry_msgs::msg::Quaternion& msg_quat)
61-
@@ -56,7 +56,7 @@ tf2::toMsg(const std::pair<cv::Mat_<double>, cv::Mat_<double>>& pose)
62-
63-
// convert compact rotation vector to angle-axis to quaternion
64-
const Eigen::Map<const Eigen::Vector3d> rvec(reinterpret_cast<double*>(pose.second.data));
65-
- const Eigen::Quaterniond q({rvec.norm(), rvec.normalized()});
66-
+ const Eigen::Quaterniond q(Eigen::AngleAxisd(rvec.norm(), rvec.normalized()));
67-
68-
geometry_msgs::msg::Transform t;
69-
tf2::convert(pose.first, t.translation);
70-
diff --git a/src/pose_estimation.cpp b/src/pose_estimation.cpp
71-
index 35a9e11..3fb8446 100644
72-
--- a/src/pose_estimation.cpp
73-
+++ b/src/pose_estimation.cpp
74-
@@ -3,7 +3,7 @@
75-
#include <apriltag/apriltag_pose.h>
76-
#include <apriltag/common/homography.h>
77-
#include <opencv2/calib3d.hpp>
78-
-#include <tf2/convert.h>
79-
+#include <tf2/convert.hpp>
80-
81-
82-
geometry_msgs::msg::Transform
Lines changed: 152 additions & 92 deletions
Original file line numberDiff line numberDiff line change
@@ -1,112 +1,80 @@
11
diff --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

Comments
 (0)