velocity_extimation.cxx

#include <iostream>
#include <algorithm>
#include <vector>
#include <ros/ros.h>
#include <apriltags_ros/AprilTagDetectionArray.h>
#include <apriltags_ros/AprilTagDetection.h>
#include <geometry_msgs/Twist.h>

double prev_position[3] = {0};
ros::Publisher vel_pub;

void getVelocities(const apriltags_ros::AprilTagDetectionArray msg)
{
    geometry_msgs::Twist computed_velocity;

    computed_velocity.linear.x = msg.detections[0].pose.pose.position.x - prev_position[0];
    computed_velocity.linear.y = msg.detections[0].pose.pose.position.y - prev_position[1];
    computed_velocity.linear.z = msg.detections[0].pose.pose.position.z - prev_position[2];
  
    prev_position[0] = msg.detections[0].pose.pose.position.x;
    prev_position[1] = msg.detections[0].pose.pose.position.y;
    prev_position[2] = msg.detections[0].pose.pose.position.z;
      
    vel_pub.publish(computed_velocity);
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "apriltags");
    ros::NodeHandle nh;
    ros::Subscriber tag_sub = nh.subscribe("tag_detections", 10, getVelocities);
    // Velocity publisher
    vel_pub = nh.advertise<geometry_msgs::Twist>("/computed_velocity",1000);
    ros::spin();
    ros::spin();
    return 0;
}