Loading files
This file is pinned to the selected release. Download the original to open it in your design tool.
#include <aruco_opencv_msgs/msg/aruco_detection.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_components/register_node_macro.hpp>
#include <tf2/LinearMath/Quaternion.hpp>
#include <tf2/LinearMath/Transform.hpp>
#include <tf2/LinearMath/Vector3.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/transform_listener.h>
#include <algorithm>
#include <array>
#include <cmath>
#include <optional>
#include <string>
#include <utility>
#include <vector>
namespace kalman_arm2 {
class PanelTracker : public rclcpp::Node {
public:
PanelTracker(const rclcpp::NodeOptions &options)
: Node("panel_tracker", options) {
declare_parameter<std::string>("tracking_frame", "odom");
declare_parameter<std::string>("board_frame", "aruco_board");
declare_parameter<std::string>(
"detection_topic", "/d455_arm/aruco_detections"
);
declare_parameter<double>("ema_alpha", 0.2);
declare_parameter<int>("ee_marker_id", 31);
declare_parameter<std::string>("ee_marker_frame", "aruco_under_j6");
declare_parameter<std::string>("ee_frame", "arm_link_end");
get_parameter("tracking_frame", tracking_frame_);
get_parameter("board_frame", board_frame_);
get_parameter("detection_topic", detection_topic_);
get_parameter("ema_alpha", ema_alpha_);
get_parameter("ee_frame", ee_frame_);
EeMarker primary_ee_marker;
get_parameter("ee_marker_id", primary_ee_marker.id);
get_parameter("ee_marker_frame", primary_ee_marker.frame);
ee_markers_.push_back(std::move(primary_ee_marker));
EeMarker secondary_ee_marker;
get_parameter("secondary_ee_marker_id", secondary_ee_marker.id);
get_parameter(
"secondary_ee_marker_frame", secondary_ee_marker.frame
);
ee_markers_.push_back(std::move(secondary_ee_marker));
ema_alpha_ = std::clamp(ema_alpha_, 1e-6, 1.0);
tf_buffer_ = std::make_shared<tf2_ros::Buffer>(get_clock());
tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
tf_broadcaster_ =
std::make_shared<tf2_ros::TransformBroadcaster>(this);
pose_pub_ = create_publisher<
geometry_msgs::msg::PoseWithCovarianceStamped>("panel_pose", 10);
visual_ee_pub_ = create_publisher<geometry_msgs::msg::PoseStamped>(
"visual_ee_pose", 10
);
detection_sub_ =
create_subscription<aruco_opencv_msgs::msg::ArucoDetection>(
detection_topic_,
10,
std::bind(
&PanelTracker::on_detection,
this,
std::placeholders::_1
)
);
RCLCPP_INFO(
get_logger(),
"PanelTracker tracking '%s' in frame '%s' and publishing '%s'",
detection_topic_.c_str(),
tracking_frame_.c_str(),
board_frame_.c_str()
);
}
private:
struct EeMarker {
int id{0};
std::string frame;
std::optional<tf2::Transform> cached_marker_to_ee;
};
struct MarkerLayout {
int id;
double u;
double v;
};
static constexpr std::array<MarkerLayout, 3> kMarkers{{
{13, 0.0, 0.0},
{14, 0.26, 0.0},
{15, 0.0, 0.383},
}};
static constexpr double kBoardWidth = 0.4;
static constexpr double kBoardHeight = 0.6;
std::string tracking_frame_;
std::string board_frame_;
std::string detection_topic_;
std::string ee_frame_;
std::vector<EeMarker> ee_markers_;
double ema_alpha_;
bool filter_initialized_{false};
tf2::Transform filtered_board_pose_;
rclcpp::Subscription<aruco_opencv_msgs::msg::ArucoDetection>::SharedPtr
detection_sub_;
rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
pose_pub_;
rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr visual_ee_pub_;
std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
std::optional<tf2::Transform> board_to_marker_from_layout(int marker_id) const {
for (const auto &marker : kMarkers) {
if (marker.id != marker_id) {
continue;
}
tf2::Transform transform;
transform.setIdentity();
transform.setOrigin(
tf2::Vector3(
marker.u - kBoardWidth * 0.5,
-(marker.v - kBoardHeight * 0.5),
0.0
)
);
return transform;
}
return std::nullopt;
}
tf2::Transform average_transforms(
const std::vector<tf2::Transform> &transforms
) const {
tf2::Vector3 mean_translation(0.0, 0.0, 0.0);
tf2::Quaternion mean_rotation(
transforms.front().getRotation().x(),
transforms.front().getRotation().y(),
transforms.front().getRotation().z(),
transforms.front().getRotation().w()
);
for (const auto &transform : transforms) {
mean_translation += transform.getOrigin();
}
mean_translation /= static_cast<double>(transforms.size());
double qx = 0.0;
double qy = 0.0;
double qz = 0.0;
double qw = 0.0;
const tf2::Quaternion reference = transforms.front().getRotation();
for (const auto &transform : transforms) {
tf2::Quaternion q = transform.getRotation();
if (reference.dot(q) < 0.0) {
q = tf2::Quaternion(-q.x(), -q.y(), -q.z(), -q.w());
}
qx += q.x();
qy += q.y();
qz += q.z();
qw += q.w();
}
mean_rotation = tf2::Quaternion(qx, qy, qz, qw);
mean_rotation.normalize();
tf2::Transform averaged;
averaged.setOrigin(mean_translation);
averaged.setRotation(mean_rotation);
return averaged;
}
tf2::Transform apply_ema(const tf2::Transform &measurement) {
if (!filter_initialized_) {
filtered_board_pose_ = measurement;
filter_initialized_ = true;
return filtered_board_pose_;
}
const tf2::Vector3 translation =
(1.0 - ema_alpha_) * filtered_board_pose_.getOrigin() +
ema_alpha_ * measurement.getOrigin();
tf2::Quaternion current = filtered_board_pose_.getRotation();
tf2::Quaternion incoming = measurement.getRotation();
if (current.dot(incoming) < 0.0) {
incoming = tf2::Quaternion(
-incoming.x(),
-incoming.y(),
-incoming.z(),
-incoming.w()
);
}
tf2::Quaternion rotation = current.slerp(incoming, ema_alpha_);
rotation.normalize();
filtered_board_pose_.setOrigin(translation);
filtered_board_pose_.setRotation(rotation);
return filtered_board_pose_;
}
std::array<double, 36> estimate_covariance(
const std::vector<tf2::Transform> &board_estimates,
const tf2::Transform &mean_board_pose,
const std::vector<aruco_opencv_msgs::msg::MarkerPose> &used_markers
) const {
std::array<double, 36> covariance{};
covariance.fill(0.0);
const size_t count = board_estimates.size();
const double marker_count_scale =
1.0 / std::sqrt(static_cast<double>(std::max<size_t>(1, count)));
double mean_distance = 0.0;
for (const auto &marker : used_markers) {
mean_distance += std::sqrt(
marker.pose.position.x * marker.pose.position.x +
marker.pose.position.y * marker.pose.position.y +
marker.pose.position.z * marker.pose.position.z
);
}
mean_distance /=
static_cast<double>(std::max<size_t>(1, used_markers.size()));
const double distance_scale = std::max(0.2, mean_distance);
double tx_var = 0.0;
double ty_var = 0.0;
double tz_var = 0.0;
double rx_var = 0.0;
double ry_var = 0.0;
double rz_var = 0.0;
for (const auto &estimate : board_estimates) {
const tf2::Vector3 dt =
estimate.getOrigin() - mean_board_pose.getOrigin();
tx_var += dt.x() * dt.x();
ty_var += dt.y() * dt.y();
tz_var += dt.z() * dt.z();
tf2::Quaternion q_err =
mean_board_pose.getRotation().inverse() * estimate.getRotation();
q_err.normalize();
if (q_err.getW() < 0.0) {
q_err = tf2::Quaternion(
-q_err.x(), -q_err.y(), -q_err.z(), -q_err.w()
);
}
const double w =
std::clamp(static_cast<double>(q_err.getW()), -1.0, 1.0);
const double angle = 2.0 * std::acos(w);
const double s = std::sqrt(std::max(1e-16, 1.0 - w * w));
tf2::Vector3 axis(1.0, 0.0, 0.0);
if (s > 1e-8) {
axis = tf2::Vector3(
q_err.getX() / s, q_err.getY() / s, q_err.getZ() / s
);
}
const tf2::Vector3 rotvec = axis * angle;
rx_var += rotvec.x() * rotvec.x();
ry_var += rotvec.y() * rotvec.y();
rz_var += rotvec.z() * rotvec.z();
}
const double denom = static_cast<double>(std::max<size_t>(1, count - 1));
tx_var /= denom;
ty_var /= denom;
tz_var /= denom;
rx_var /= denom;
ry_var /= denom;
rz_var /= denom;
const double pos_floor =
std::pow(0.005 * distance_scale * marker_count_scale, 2);
const double rot_floor =
std::pow(0.03 * distance_scale * marker_count_scale, 2);
covariance[0] = tx_var + pos_floor;
covariance[7] = ty_var + pos_floor;
covariance[14] = tz_var + pos_floor * 2.0;
covariance[21] = rx_var + rot_floor;
covariance[28] = ry_var + rot_floor;
covariance[35] = rz_var + rot_floor;
return covariance;
}
std::optional<tf2::Transform> marker_to_ee(EeMarker &marker) {
try {
const auto transform = tf_buffer_->lookupTransform(
marker.frame,
ee_frame_,
tf2::TimePointZero,
tf2::durationFromSec(0.1)
);
tf2::Transform current;
tf2::fromMsg(transform.transform, current);
if (marker.cached_marker_to_ee) {
const tf2::Transform delta =
marker.cached_marker_to_ee->inverse() * current;
const double translation_change = delta.getOrigin().length();
tf2::Quaternion rotation_change = delta.getRotation();
rotation_change.normalize();
const double rotation_angle = 2.0 * std::acos(std::clamp(
std::abs(static_cast<double>(rotation_change.w())), 0.0, 1.0
));
if (translation_change > 1e-5 || rotation_angle > 1e-4) {
RCLCPP_WARN_THROTTLE(
get_logger(),
*get_clock(),
2000,
"Transform %s -> %s is not static (delta %.6f m, %.6f rad)",
marker.frame.c_str(),
ee_frame_.c_str(),
translation_change,
rotation_angle
);
}
} else {
marker.cached_marker_to_ee = current;
}
return current;
} catch (const tf2::TransformException &ex) {
RCLCPP_WARN_THROTTLE(
get_logger(),
*get_clock(),
1000,
"Could not resolve %s -> %s: %s",
marker.frame.c_str(),
ee_frame_.c_str(),
ex.what()
);
return std::nullopt;
}
}
void on_detection(
const aruco_opencv_msgs::msg::ArucoDetection::SharedPtr msg
) {
if (msg->markers.empty()) {
return;
}
const rclcpp::Time stamp = msg->header.stamp;
const std::string camera_frame = msg->header.frame_id;
geometry_msgs::msg::TransformStamped tracking_to_camera;
try {
tracking_to_camera = tf_buffer_->lookupTransform(
tracking_frame_,
camera_frame,
stamp,
rclcpp::Duration::from_seconds(0.1)
);
} catch (const tf2::TransformException &ex) {
RCLCPP_WARN_THROTTLE(
get_logger(),
*get_clock(),
1000,
"Could not transform '%s' to '%s': %s",
camera_frame.c_str(),
tracking_frame_.c_str(),
ex.what()
);
return;
}
tf2::Transform t_tracking_camera;
tf2::fromMsg(tracking_to_camera.transform, t_tracking_camera);
std::vector<tf2::Transform> board_estimates;
std::vector<int> marker_ids;
std::vector<aruco_opencv_msgs::msg::MarkerPose> used_markers;
std::vector<tf2::Transform> camera_to_ee_estimates;
for (const auto &marker : msg->markers) {
geometry_msgs::msg::Transform marker_transform_msg;
marker_transform_msg.translation.x = marker.pose.position.x;
marker_transform_msg.translation.y = marker.pose.position.y;
marker_transform_msg.translation.z = marker.pose.position.z;
marker_transform_msg.rotation = marker.pose.orientation;
tf2::Transform t_camera_marker;
tf2::fromMsg(marker_transform_msg, t_camera_marker);
const auto ee_marker = std::find_if(
ee_markers_.begin(),
ee_markers_.end(),
[&marker](const EeMarker &candidate) {
return candidate.id == marker.marker_id;
}
);
if (ee_marker != ee_markers_.end()) {
const auto marker_to_end_effector = marker_to_ee(*ee_marker);
if (marker_to_end_effector) {
camera_to_ee_estimates.push_back(
t_camera_marker * *marker_to_end_effector
);
}
}
const auto board_to_marker =
board_to_marker_from_layout(marker.marker_id);
if (!board_to_marker.has_value()) {
continue;
}
board_estimates.push_back(
t_camera_marker * board_to_marker->inverse()
);
marker_ids.push_back(marker.marker_id);
used_markers.push_back(marker);
}
if (board_estimates.empty()) {
return;
}
const tf2::Transform averaged_measurement =
average_transforms(board_estimates);
const tf2::Transform filtered_measurement =
apply_ema(averaged_measurement);
const auto covariance =
estimate_covariance(board_estimates, averaged_measurement, used_markers);
const tf2::Transform filtered_tracking_board =
t_tracking_camera * filtered_measurement;
geometry_msgs::msg::TransformStamped board_tf;
board_tf.header.stamp = stamp;
board_tf.header.frame_id = tracking_frame_;
board_tf.child_frame_id = board_frame_;
board_tf.transform = tf2::toMsg(filtered_tracking_board);
tf_broadcaster_->sendTransform(board_tf);
geometry_msgs::msg::PoseWithCovarianceStamped pose_msg;
pose_msg.header = board_tf.header;
pose_msg.pose.pose.position.x = board_tf.transform.translation.x;
pose_msg.pose.pose.position.y = board_tf.transform.translation.y;
pose_msg.pose.pose.position.z = board_tf.transform.translation.z;
pose_msg.pose.pose.orientation = board_tf.transform.rotation;
pose_msg.pose.covariance = covariance;
pose_pub_->publish(pose_msg);
if (!camera_to_ee_estimates.empty()) {
const tf2::Transform camera_to_ee =
average_transforms(camera_to_ee_estimates);
const tf2::Transform panel_to_ee =
filtered_measurement.inverse() * camera_to_ee;
geometry_msgs::msg::PoseStamped visual_ee;
visual_ee.header.stamp = stamp;
visual_ee.header.frame_id = board_frame_;
visual_ee.pose.position.x = panel_to_ee.getOrigin().x();
visual_ee.pose.position.y = panel_to_ee.getOrigin().y();
visual_ee.pose.position.z = panel_to_ee.getOrigin().z();
visual_ee.pose.orientation = tf2::toMsg(
panel_to_ee.getRotation()
);
visual_ee_pub_->publish(visual_ee);
}
RCLCPP_DEBUG(
get_logger(),
"Updated board pose from %zu markers in '%s'; visual EE from %zu "
"marker(s)",
marker_ids.size(),
tracking_frame_.c_str(),
camera_to_ee_estimates.size()
);
}
};
} // namespace kalman_arm2
RCLCPP_COMPONENTS_REGISTER_NODE(kalman_arm2::PanelTracker)
657be5395This file is pinned to the selected release. Download the original to open it in your design tool.