#include
#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <px4_msgs/msg/vehicle_odometry.hpp>
using namespace std::chrono_literals;
class OptitrackToOdometryMapper : public rclcpp::Node
{
public:
OptitrackToOdometryMapper() : Node(“optitrack_to_odometry_mapper”)
{
// Initialize the subscriber
subscription_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
“/Drone/pose”,
10,
std::bind(&OptitrackToOdometryMapper::listener_callback, this, std::placeholders::_1));
// Publisher for the uORB topic (vehicle_odometry)
publisher_ = this->create_publisher<px4_msgs::msg::VehicleOdometry>("/fmu/in/vehicle_visual_odometry", 10);
// Set up timer callback
timer_ = this->create_wall_timer(20ms, std::bind(&OptitrackToOdometryMapper::timer_callback, this));
}
private:
void listener_callback(const geometry_msgs::msg::PoseStamped::SharedPtr msg)
{
// Store latest message for timer callback use
last_received_pose_ = msg;
}
void timer_callback()
{
if (last_received_pose_)
{
auto odometry_msg = px4_msgs::msg::VehicleOdometry();
odometry_msg.timestamp = std::chrono::time_point_cast<std::chrono::microseconds>(
std::chrono::steady_clock::now()).time_since_epoch().count();
// Manually convert from ENU to NED for position
odometry_msg.position[0] = last_received_pose_->pose.position.y;
odometry_msg.position[1] = last_received_pose_->pose.position.x;
odometry_msg.position[2] = -last_received_pose_->pose.position.z;
// Manually convert from ENU to NED for orientation
odometry_msg.q[0] = last_received_pose_->pose.orientation.w;
odometry_msg.q[1] = last_received_pose_->pose.orientation.y;
odometry_msg.q[2] = last_received_pose_->pose.orientation.x;
odometry_msg.q[3] = -last_received_pose_->pose.orientation.z;
// Assuming you don't have the velocity info, set them to NaN
odometry_msg.velocity[0] = std::nan("");
odometry_msg.velocity[1] = std::nan("");
odometry_msg.velocity[2] = std::nan("");
RCLCPP_INFO(this->get_logger(), "Publishing Vehicle Odometry: timestamp: %lu, position: [%f, %f, %f], orientation: [%f, %f, %f, %f]",
odometry_msg.timestamp, odometry_msg.position[0], odometry_msg.position[1], odometry_msg.position[2],
odometry_msg.q[0], odometry_msg.q[1], odometry_msg.q[2], odometry_msg.q[3]);
publisher_->publish(odometry_msg);
}
}
rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr subscription_;
rclcpp::Publisher<px4_msgs::msg::VehicleOdometry>::SharedPtr publisher_;
rclcpp::TimerBase::SharedPtr timer_;
geometry_msgs::msg::PoseStamped::SharedPtr last_received_pose_ = nullptr;
};
int main(int argc, char *argv)
{
std::cout << “Starting optitrack_to_odometry_mapper node…” << std::endl;
setvbuf(stdout, NULL, _IONBF, BUFSIZ);
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared());
rclcpp::shutdown();
return 0;
}
