add wust typr mpc and mutipule x

wust_vision-main/ros2/ros2.hpp

@@ -0,0 +1,136 @@
+#pragma once
+
+#include "geometry_msgs/msg/transform_stamped.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "tf2_ros/buffer.h"
+#include "tf2_ros/transform_broadcaster.h"
+#include "tf2_ros/transform_listener.h"
+
+#include <chrono>
+#include <functional>
+#include <memory>
+#include <mutex>
+#include <string>
+#include <thread>
+#include <typeindex>
+#include <unordered_map>
+#include <vector>
+
+class Ros2Node: public rclcpp::Node {
+public:
+    explicit Ros2Node(const std::string& node_name = "vision"):
+        Node(node_name),
+        tf_buffer_(this->get_clock()),
+        tf_listener_(tf_buffer_),
+        tf_broadcaster_(std::make_shared<tf2_ros::TransformBroadcaster>(this)) {}
+    using Ptr = std::shared_ptr<Ros2Node>;
+
+    static Ptr instance() {
+        static Ptr inst = std::make_shared<Ros2Node>();
+        return inst;
+    }
+    ~Ros2Node() {
+        stop();
+    }
+
+    template<typename MsgT>
+    void add_subscription(
+        const std::string& topic_name,
+        std::function<void(const typename MsgT::SharedPtr)> callback,
+        const rclcpp::QoS& qos = rclcpp::QoS(rclcpp::KeepLast(10))
+    ) {
+        auto sub = this->create_subscription<MsgT>(topic_name, qos, callback);
+        std::lock_guard<std::mutex> lock(map_mutex_);
+        subscriptions_[topic_name] = sub;
+    }
+
+    template<typename MsgT>
+    void add_publisher(
+        const std::string& topic_name,
+        const rclcpp::QoS& qos = rclcpp::QoS(rclcpp::KeepLast(10))
+    ) {
+        auto pub = this->create_publisher<MsgT>(topic_name, qos);
+        std::lock_guard<std::mutex> lock(map_mutex_);
+        publishers_[topic_name] = pub;
+    }
+
+    template<typename MsgT>
+    void publish(const std::string& topic_name, const MsgT& msg) {
+        std::lock_guard<std::mutex> lock(map_mutex_);
+        auto it = publishers_.find(topic_name);
+        if (it != publishers_.end()) {
+            auto typed_pub = std::dynamic_pointer_cast<rclcpp::Publisher<MsgT>>(it->second);
+            if (typed_pub)
+                typed_pub->publish(msg);
+        }
+    }
+
+    template<typename Rep, typename Period>
+    void
+    add_timer(const std::chrono::duration<Rep, Period>& interval, std::function<void()> callback) {
+        auto timer = this->create_wall_timer(interval, callback);
+        std::lock_guard<std::mutex> lock(map_mutex_);
+        timers_.push_back(timer);
+    }
+
+    bool lookup_transform(
+        const std::string& target_frame,
+        const std::string& source_frame,
+        geometry_msgs::msg::TransformStamped& out_tf,
+        std::chrono::milliseconds timeout = std::chrono::milliseconds(200)
+    ) const {
+        try {
+            out_tf =
+                tf_buffer_.lookupTransform(target_frame, source_frame, tf2::TimePointZero, timeout);
+            return true;
+        } catch (const tf2::TransformException& ex) {
+            RCLCPP_WARN(
+                this->get_logger(),
+                "lookup_transform failed %s -> %s: %s",
+                source_frame.c_str(),
+                target_frame.c_str(),
+                ex.what()
+            );
+            return false;
+        }
+    }
+
+    void broadcast_tf(const geometry_msgs::msg::TransformStamped& tf_msg) {
+        tf_broadcaster_->sendTransform(tf_msg);
+    }
+
+    template<typename Rep, typename Period>
+    void broadcast_tf_periodic(
+        const geometry_msgs::msg::TransformStamped& tf_msg,
+        const std::chrono::duration<Rep, Period>& interval
+    ) {
+        add_timer(interval, [this, tf_msg]() { broadcast_tf(tf_msg); });
+    }
+
+    void start() {
+        if (!spin_thread_.joinable()) {
+            auto node = shared_from_this();
+            spin_thread_ = std::thread([node]() { rclcpp::spin(node); });
+        }
+    }
+
+    void stop() {
+        rclcpp::shutdown();
+        if (spin_thread_.joinable()) {
+            spin_thread_.join();
+        }
+    }
+
+private:
+    mutable std::mutex map_mutex_;
+
+    std::unordered_map<std::string, rclcpp::SubscriptionBase::SharedPtr> subscriptions_;
+    std::unordered_map<std::string, rclcpp::PublisherBase::SharedPtr> publishers_;
+    std::vector<rclcpp::TimerBase::SharedPtr> timers_;
+
+    std::thread spin_thread_;
+
+    tf2_ros::Buffer tf_buffer_;
+    tf2_ros::TransformListener tf_listener_;
+    std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
+};

wust_vision-main/ros2/tf.hpp

@@ -0,0 +1,88 @@
+#pragma once
+#include "rclcpp/rclcpp.hpp"
+#include <Eigen/Dense>
+#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>
+template<typename PublisherT>
+inline bool publisherSubscribed(const PublisherT& publisher) noexcept {
+    return publisher && publisher->get_subscription_count() > 0;
+}
+inline Eigen::Isometry3f tf2ToEigen(const geometry_msgs::msg::TransformStamped& tf) noexcept {
+    Eigen::Isometry3f T = Eigen::Isometry3f::Identity();
+
+    const auto& t = tf.transform.translation;
+    T.translation() =
+        Eigen::Vector3f(static_cast<float>(t.x), static_cast<float>(t.y), static_cast<float>(t.z));
+
+    const auto& q = tf.transform.rotation;
+
+    Eigen::Quaternionf Q(
+        static_cast<float>(q.w),
+        static_cast<float>(q.x),
+        static_cast<float>(q.y),
+        static_cast<float>(q.z)
+    );
+
+    Q.normalize();
+
+    T.linear() = Q.toRotationMatrix();
+
+    return T;
+}
+class TF {
+public:
+    using Ptr = std::shared_ptr<TF>;
+    TF(rclcpp::Node& n) {
+        tf_buffer_ = std::make_unique<tf2_ros::Buffer>(n.get_clock());
+        tf_listener_ = std::make_unique<tf2_ros::TransformListener>(*tf_buffer_);
+        tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(n);
+        node_ = &n;
+    }
+    static Ptr create(rclcpp::Node& n) {
+        return std::make_shared<TF>(n);
+    }
+    std::optional<Eigen::Isometry3f>
+    getTransform(const std::string& target, const std::string& source, rclcpp::Time t)
+        const noexcept {
+        Eigen::Isometry3f T_out = Eigen::Isometry3f::Identity();
+        try {
+            geometry_msgs::msg::TransformStamped tf_msg =
+                tf_buffer_->lookupTransform(target, source, t, rclcpp::Duration::from_seconds(0.1));
+
+            T_out = tf2ToEigen(tf_msg);
+            return T_out;
+        } catch (tf2::TransformException& ex) {
+            RCLCPP_WARN(rclcpp::get_logger("tf"), "TF lookup failed: %s", ex.what());
+            return std::nullopt;
+        }
+        return T_out;
+    }
+
+    void publishTransform(
+        const Eigen::Isometry3d& transform,
+        const std::string& parent_frame,
+        const std::string& child_frame,
+        const rclcpp::Time& stamp
+    ) const noexcept {
+        geometry_msgs::msg::TransformStamped tmsg;
+        tmsg.header.stamp = stamp;
+        tmsg.header.frame_id = parent_frame;
+        tmsg.child_frame_id = child_frame;
+        const Eigen::Vector3d tr = transform.translation();
+        const Eigen::Quaterniond q(transform.rotation());
+        tmsg.transform.translation.x = tr.x();
+        tmsg.transform.translation.y = tr.y();
+        tmsg.transform.translation.z = tr.z();
+        tmsg.transform.rotation.x = q.x();
+        tmsg.transform.rotation.y = q.y();
+        tmsg.transform.rotation.z = q.z();
+        tmsg.transform.rotation.w = q.w();
+        tf_broadcaster_->sendTransform(tmsg);
+    }
+    rclcpp::Node* node_;
+    std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
+    std::unique_ptr<tf2_ros::TransformListener> tf_listener_;
+    std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
+};