Skip to content
Open
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
7 changes: 7 additions & 0 deletions conda_build_config.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,13 @@ numpy:
- 2
assimp:
- 6.0.3
# Boost 1.90 migration: conda-forge's global pin is still 1.88, but the
# boost-python / geometry stack (eigenpy 3.13, pinocchio, ...) has already
# moved to 1.90, so we pin ahead to build the whole distro against 1.90.
libboost_devel:
- '1.90'
libboost_python_devel:
- '1.90'
libprotobuf:
- 6.33.5
protobuf:
Expand Down
67 changes: 1 addition & 66 deletions patch/ros-lyrical-apriltag-ros.patch
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 20bdedf..4e9d53d 100644
index 75d5822..ff9d333 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -5,8 +5,10 @@ project(apriltag_ros)
Expand All @@ -15,68 +15,3 @@ index 20bdedf..4e9d53d 100644
endif()

option(ASAN "use AddressSanitizer to detect memory issues" OFF)
diff --git a/src/AprilTagNode.cpp b/src/AprilTagNode.cpp
index c13bf26..4dcbc0e 100644
--- a/src/AprilTagNode.cpp
+++ b/src/AprilTagNode.cpp
@@ -13,7 +13,7 @@
#include <rclcpp_components/register_node_macro.hpp>
#include <sensor_msgs/msg/camera_info.hpp>
#include <sensor_msgs/msg/image.hpp>
-#include <tf2_ros/transform_broadcaster.h>
+#include <tf2_ros/transform_broadcaster.hpp>

// apriltag
#include "tag_functions.hpp"
@@ -99,13 +99,13 @@ AprilTagNode::AprilTagNode(const rclcpp::NodeOptions& options)
td(apriltag_detector_create()),
// topics
sub_cam(image_transport::create_camera_subscription(
- this,
+ *this,
this->get_node_topics_interface()->resolve_topic_name("image_rect"),
std::bind(&AprilTagNode::onCamera, this, std::placeholders::_1, std::placeholders::_2),
declare_parameter("image_transport", "raw", descr({}, true)),
- rmw_qos_profile_sensor_data)),
+ rclcpp::SensorDataQoS())),
pub_detections(create_publisher<apriltag_msgs::msg::AprilTagDetectionArray>("detections", rclcpp::QoS(1))),
- tf_broadcaster(this)
+ tf_broadcaster(*this)
{
// read-only parameters
const std::string tag_family = declare_parameter("family", "36h11", descr("tag family", true));
diff --git a/src/conversion.cpp b/src/conversion.cpp
index b9b30a8..16a8d66 100644
--- a/src/conversion.cpp
+++ b/src/conversion.cpp
@@ -4,7 +4,7 @@
#include <geometry_msgs/msg/quaternion.hpp>
#include <geometry_msgs/msg/vector3.hpp>
#include <opencv2/core/mat.hpp>
-#include <tf2/convert.h>
+#include <tf2/convert.hpp>

template<>
void tf2::convert(const Eigen::Quaterniond& eigen_quat, geometry_msgs::msg::Quaternion& msg_quat)
@@ -56,7 +56,7 @@ tf2::toMsg(const std::pair<cv::Mat_<double>, cv::Mat_<double>>& pose)

// convert compact rotation vector to angle-axis to quaternion
const Eigen::Map<const Eigen::Vector3d> rvec(reinterpret_cast<double*>(pose.second.data));
- const Eigen::Quaterniond q({rvec.norm(), rvec.normalized()});
+ const Eigen::Quaterniond q(Eigen::AngleAxisd(rvec.norm(), rvec.normalized()));

geometry_msgs::msg::Transform t;
tf2::convert(pose.first, t.translation);
diff --git a/src/pose_estimation.cpp b/src/pose_estimation.cpp
index 35a9e11..3fb8446 100644
--- a/src/pose_estimation.cpp
+++ b/src/pose_estimation.cpp
@@ -3,7 +3,7 @@
#include <apriltag/apriltag_pose.h>
#include <apriltag/common/homography.h>
#include <opencv2/calib3d.hpp>
-#include <tf2/convert.h>
+#include <tf2/convert.hpp>


geometry_msgs::msg::Transform
244 changes: 152 additions & 92 deletions patch/ros-lyrical-foxglove-bridge.osx.patch
Original file line number Diff line number Diff line change
@@ -1,112 +1,80 @@
diff --git a/src/ros2_foxglove_bridge.cpp b/src/ros2_foxglove_bridge.cpp
index 2229aadc..3a1c838c 100644
index 601db6c..fc18e1c 100644
--- a/src/ros2_foxglove_bridge.cpp
+++ b/src/ros2_foxglove_bridge.cpp
@@ -305,7 +305,7 @@ void FoxgloveBridge::updateAdvertisedTopics(
std::string topic(channel.topic());
const TopicAndDatatype topicAndSchemaName = {topic, schemaName};
if (latestTopics.find(topicAndSchemaName) == latestTopics.end()) {
- RCLCPP_INFO(this->get_logger(), "Removing channel %lu for topic \"%s\" (%s)", channel.id(),
+ RCLCPP_INFO(this->get_logger(), "Removing channel %llu for topic \"%s\" (%s)", channel.id(),
topic.c_str(), schemaName.c_str());
channel.close();
channelIt = _channels.erase(channelIt);
@@ -372,7 +372,7 @@ void FoxgloveBridge::updateAdvertisedTopics(
@@ -492,7 +492,7 @@ void FoxgloveBridge::updateAdvertisedTopics(
const TopicAndDatatype topicAndSchemaName = {topic, schemaName};
if (latestTopics.find(topicAndSchemaName) == latestTopics.end()) {
const auto channelId = channel.id();
- RCLCPP_INFO(this->get_logger(), "Removing channel %lu for topic \"%s\" (%s)", channelId,
+ RCLCPP_INFO(this->get_logger(), "Removing channel %llu for topic \"%s\" (%s)", channelId,
topic.c_str(), schemaName.c_str());
// Remove any active subscriptions for this channel
_subscriptions.erase(channelId);
@@ -562,7 +562,7 @@ void FoxgloveBridge::updateAdvertisedTopics(
}

const ChannelId channelId = channelResult.value().id();
- RCLCPP_INFO(this->get_logger(), "Advertising new channel %lu for topic \"%s\"", channelId,
+ RCLCPP_INFO(this->get_logger(), "Advertising new channel %llu for topic \"%s\"", channelId,
topic.c_str());
_channels.insert({channelId, std::move(channelResult.value())});
}

const ChannelId channelId = channelResult.value().id();
- RCLCPP_INFO(this->get_logger(), "Advertising new channel %lu for topic \"%s\"", channelId,
+ RCLCPP_INFO(this->get_logger(), "Advertising new channel %llu for topic \"%s\"", channelId,
topic.c_str());
_channels.insert({channelId, std::move(channelResult.value())});
}
@@ -593,14 +593,14 @@ void FoxgloveBridge::subscribeConnectionGraph(bool subscribe) {
@@ -827,7 +827,7 @@ void FoxgloveBridge::subscribeConnectionGraph(bool subscribe) {
}

void FoxgloveBridge::subscribe(ChannelId channelId, const foxglove::ClientMetadata& client) {
if (!client.sink_id.has_value()) {
RCLCPP_ERROR(this->get_logger(),
- "received subscribe request from client %u for channel %lu but client "
+ "received subscribe request from client %u for channel %llu but client "
"has no sink ID",
client.id, channelId);
return;
}

RCLCPP_INFO(this->get_logger(),
- "received subscribe request for channel %lu from client %u (sink %lu)", channelId,
+ "received subscribe request for channel %llu from client %u (sink %llu)", channelId,
client.id, client.sink_id.value());
std::lock_guard<std::mutex> lock(_subscriptionsMutex);

@@ -608,7 +608,7 @@ void FoxgloveBridge::subscribe(ChannelId channelId, const foxglove::ClientMetada
// calling this callback?
auto it = _channels.find(channelId);
if (it == _channels.end()) {
- RCLCPP_INFO(this->get_logger(), "received subscribe request for channel %lu from client %u",
+ RCLCPP_INFO(this->get_logger(), "received subscribe request for channel %llu from client %u",
channelId, client.id);
createOrIncrementSubscription(channelId, client.id, false, client.sink_id);
}
@@ -880,7 +880,7 @@ void FoxgloveBridge::createOrIncrementSubscriptionLocked(ChannelId channelId, Cl
std::optional<SinkId> sinkId) {
auto channelIt = _channels.find(channelId);
if (channelIt == _channels.end()) {
- RCLCPP_ERROR(this->get_logger(), "received subscribe request for unknown channel: %lu",
+ RCLCPP_ERROR(this->get_logger(), "received subscribe request for unknown channel: %llu",
channelId);
return;
}
@@ -638,7 +638,7 @@ void FoxgloveBridge::subscribe(ChannelId channelId, const foxglove::ClientMetada

if (!client.sink_id.has_value()) {
RCLCPP_ERROR(this->get_logger(),
- "received subscribe request for channel %lu but client "
+ "received subscribe request for channel %llu but client "
"has no sink ID",
channelId);
return;
@@ -646,19 +646,19 @@ void FoxgloveBridge::subscribe(ChannelId channelId, const foxglove::ClientMetada
@@ -911,7 +911,7 @@ void FoxgloveBridge::createOrIncrementSubscriptionLocked(ChannelId channelId, Cl
auto [it, inserted] = _subscriptions.emplace(channelId, std::move(channelSub));
subIt = it;

_subscriptions.insert({{channelId, client.id}, subscription});
RCLCPP_INFO(this->get_logger(),
- "created ROS subscription on %s (%s) successfully for channel %lu (client "
- "%u, sink %lu)",
+ "created ROS subscription on %s (%s) successfully for channel %llu (client "
+ "%u, sink %llu)",
topic.c_str(), datatype.c_str(), channelId, client.id, client.sink_id.value());
- RCLCPP_INFO(this->get_logger(), "Created ROS subscription on %s (%s) for channel %lu",
+ RCLCPP_INFO(this->get_logger(), "Created ROS subscription on %s (%s) for channel %llu",
topic.c_str(), datatype.c_str(), channelId);
}

@@ -935,7 +935,7 @@ void FoxgloveBridge::createOrIncrementSubscriptionLocked(ChannelId channelId, Cl
}

void FoxgloveBridge::unsubscribe(ChannelId channelId, const foxglove::ClientMetadata& client) {
std::lock_guard<std::mutex> lock(_subscriptionsMutex);

- RCLCPP_INFO(this->get_logger(), "received unsubscribe request for channel %lu", channelId);
+ RCLCPP_INFO(this->get_logger(), "received unsubscribe request for channel %llu", channelId);

auto it = _channels.find(channelId);
if (it == _channels.end()) {
- RCLCPP_ERROR(this->get_logger(), "received unsubscribe request for unknown channel %lu",
+ RCLCPP_ERROR(this->get_logger(), "received unsubscribe request for unknown channel %llu",
channelId);
return;
}
@@ -666,7 +666,7 @@ void FoxgloveBridge::unsubscribe(ChannelId channelId, const foxglove::ClientMeta
auto subscriptionIt = _subscriptions.find({channelId, client.id});
if (subscriptionIt == _subscriptions.end()) {
- RCLCPP_INFO(this->get_logger(), "received unsubscribe request for channel %lu from client %u",
+ RCLCPP_INFO(this->get_logger(), "received unsubscribe request for channel %llu from client %u",
channelId, client.id);
removeOrDecrementSubscription(channelId, client.id, false);
}
@@ -951,7 +951,7 @@ void FoxgloveBridge::removeOrDecrementSubscriptionLocked(ChannelId channelId, Cl
auto subIt = _subscriptions.find(channelId);
if (subIt == _subscriptions.end()) {
RCLCPP_ERROR(this->get_logger(),
- "Client %u tried unsubscribing from channel %lu but a corresponding ROS "
+ "Client %u tried unsubscribing from channel %llu but a corresponding ROS "
"subscription doesn't exist",
client.id, channelId);
- "Client %u tried unsubscribing from channel %lu but no subscription exists",
+ "Client %u tried unsubscribing from channel %llu but no subscription exists",
clientId, channelId);
return;
@@ -674,7 +674,7 @@ void FoxgloveBridge::unsubscribe(ChannelId channelId, const foxglove::ClientMeta

const std::string& topic = subscriptionIt->second->get_topic_name();
RCLCPP_INFO(this->get_logger(),
- "Cleaned up subscription to topic %s for client %u on channel %lu", topic.c_str(),
+ "Cleaned up subscription to topic %s for client %u on channel %llu", topic.c_str(),
client.id, channelId);
_subscriptions.erase(subscriptionIt);
}
@@ -752,7 +752,7 @@ void FoxgloveBridge::clientAdvertise(ClientId clientId, const foxglove::ClientCh
auto publisher = this->create_generic_publisher(topicName, topicType, qos, publisherOptions);

}
@@ -966,7 +966,7 @@ void FoxgloveBridge::removeOrDecrementSubscriptionLocked(ChannelId channelId, Cl
// If no more subscribers, destroy the ROS subscription
if (subIt->second.wsClientIds.empty() && subIt->second.gatewayClientIds.empty()) {
RCLCPP_INFO(this->get_logger(),
- "Client ID %d is advertising \"%s\" (%s) on channel %d with encoding \"%s\"",
+ "Client ID %u is advertising \"%s\" (%s) on channel %u with encoding \"%s\"",
clientId, topicName.c_str(), topicType.c_str(), channel.id, encoding.c_str());

// Store the new topic advertisement
@@ -778,7 +778,7 @@ void FoxgloveBridge::clientUnadvertise(ClientId clientId, ChannelId clientChanne
- "Cleaned up ROS subscription for channel %lu (no more subscribers)", channelId);
+ "Cleaned up ROS subscription for channel %llu (no more subscribers)", channelId);
_subscriptions.erase(subIt);
}
}
@@ -1105,7 +1105,7 @@ void FoxgloveBridge::clientUnadvertise(ClientId clientId, ChannelId clientChanne

const auto& publisher = it->second.publisher;
RCLCPP_INFO(this->get_logger(),
Expand All @@ -115,3 +83,95 @@ index 2229aadc..3a1c838c 100644
publisher->get_topic_name(), publisher->get_subscription_count(), clientChannelId);

_clientAdvertisedTopics.erase(it);
@@ -1585,13 +1585,13 @@ void FoxgloveBridge::gatewaySubscribe(uint32_t clientId,
auto sinkId = _gateway->sinkId();
if (!sinkId.has_value()) {
RCLCPP_WARN(this->get_logger(),
- "Gateway: subscribe request for channel %lu (\"%s\") from client %u "
+ "Gateway: subscribe request for channel %llu (\"%s\") from client %u "
"but gateway session has no sink ID (reconnecting?); "
"cached transient_local messages will not be replayed",
channel.id(), std::string(channel.topic()).c_str(), clientId);
}
RCLCPP_INFO(this->get_logger(),
- "Gateway: received subscribe request for channel %lu (\"%s\") from client %u",
+ "Gateway: received subscribe request for channel %llu (\"%s\") from client %u",
channel.id(), std::string(channel.topic()).c_str(), clientId);
createOrIncrementSubscription(channel.id(), clientId, true, sinkId);
}
@@ -1599,7 +1599,7 @@ void FoxgloveBridge::gatewaySubscribe(uint32_t clientId,
void FoxgloveBridge::gatewayUnsubscribe(uint32_t clientId,
const foxglove::ChannelDescriptor& channel) {
RCLCPP_INFO(this->get_logger(),
- "Gateway: received unsubscribe request for channel %lu (\"%s\") from client %u",
+ "Gateway: received unsubscribe request for channel %llu (\"%s\") from client %u",
channel.id(), std::string(channel.topic()).c_str(), clientId);
removeOrDecrementSubscription(channel.id(), clientId, true);
}
@@ -1614,7 +1614,7 @@ void FoxgloveBridge::gatewayClientAdvertise(uint32_t clientId,

ChannelAndClientId key = {channelId, clientId};
if (_gatewayClientAdvertisedTopics.find(key) != _gatewayClientAdvertisedTopics.end()) {
- RCLCPP_WARN(this->get_logger(), "Gateway: client %u already advertised channel %lu (\"%s\")",
+ RCLCPP_WARN(this->get_logger(), "Gateway: client %u already advertised channel %llu (\"%s\")",
clientId, channelId, topicName.c_str());
return;
}
@@ -1631,7 +1631,7 @@ void FoxgloveBridge::gatewayClientAdvertise(uint32_t clientId,

if (topicType.empty()) {
RCLCPP_ERROR(this->get_logger(),
- "Gateway: client %u advertised channel %lu (\"%s\") with empty schema name",
+ "Gateway: client %u advertised channel %llu (\"%s\") with empty schema name",
clientId, channelId, topicName.c_str());
return;
}
@@ -1639,12 +1639,12 @@ void FoxgloveBridge::gatewayClientAdvertise(uint32_t clientId,
try {
auto ad = createClientPublisher(topicName, topicType, encoding, schemaData, schemaLen);
RCLCPP_INFO(this->get_logger(),
- "Gateway: client %u is advertising channel %lu \"%s\" (%s) with encoding \"%s\"",
+ "Gateway: client %u is advertising channel %llu \"%s\" (%s) with encoding \"%s\"",
clientId, channelId, topicName.c_str(), topicType.c_str(), encoding.c_str());
_gatewayClientAdvertisedTopics.emplace(key, std::move(ad));
} catch (const std::exception& ex) {
RCLCPP_ERROR(this->get_logger(),
- "Gateway: failed to create publisher for client %u channel %lu (\"%s\"): %s",
+ "Gateway: failed to create publisher for client %u channel %llu (\"%s\"): %s",
clientId, channelId, topicName.c_str(), ex.what());
}
}
@@ -1658,13 +1658,13 @@ void FoxgloveBridge::gatewayClientUnadvertise(uint32_t clientId,

auto it = _gatewayClientAdvertisedTopics.find(key);
if (it == _gatewayClientAdvertisedTopics.end()) {
- RCLCPP_WARN(this->get_logger(), "Gateway: client %u unadvertised unknown channel %lu (\"%s\")",
+ RCLCPP_WARN(this->get_logger(), "Gateway: client %u unadvertised unknown channel %llu (\"%s\")",
clientId, channelId, std::string(channel.topic()).c_str());
return;
}

RCLCPP_INFO(this->get_logger(),
- "Gateway: client %u is no longer advertising channel %lu (\"%s\")", clientId,
+ "Gateway: client %u is no longer advertising channel %llu (\"%s\")", clientId,
channelId, it->second.topicName.c_str());
_gatewayClientAdvertisedTopics.erase(it);

@@ -1688,7 +1688,7 @@ void FoxgloveBridge::gatewayClientMessage(uint32_t clientId,
auto it = _gatewayClientAdvertisedTopics.find(key);
if (it == _gatewayClientAdvertisedTopics.end()) {
RCLCPP_ERROR(this->get_logger(),
- "Gateway: dropping message from client %u for unknown channel %lu", clientId,
+ "Gateway: dropping message from client %u for unknown channel %llu", clientId,
channelId);
return;
}
@@ -1699,7 +1699,7 @@ void FoxgloveBridge::gatewayClientMessage(uint32_t clientId,
try {
publishClientData(ad, data, dataLen);
} catch (const std::exception& ex) {
- RCLCPP_ERROR(this->get_logger(), "Gateway: dropping message from client %u for channel %lu: %s",
+ RCLCPP_ERROR(this->get_logger(), "Gateway: dropping message from client %u for channel %llu: %s",
clientId, channelId, ex.what());
}
}
Loading
Loading