Using Positional Tracking with ROS 2

1
#include <rclcpp/rclcpp.hpp>
2
#include <rclcpp/qos.hpp>
3
#include <nav_msgs/msg/odometry.hpp>
4
#include <geometry_msgs/msg/pose_stamped.hpp>
5
6
#include <tf2/LinearMath/Quaternion.h>
7
#include <tf2/LinearMath/Matrix3x3.h>
8
9
using namespace std::placeholders;
10
11
#define RAD2DEG 57.295779513
12
13
class MinimalPoseOdomSubscriber : public rclcpp::Node {
14
public:
15
    MinimalPoseOdomSubscriber()
16
        : Node("zed_odom_pose_tutorial") {
17
18
        /* Note: it is very important to use a QOS profile for the subscriber that is compatible
19
         * with the QOS profile of the publisher.
20
         * The ZED component node uses a default QoS profile with reliability set as "RELIABLE"
21
         * and durability set as "VOLATILE".
22
         * To be able to receive the subscribed topic the subscriber must use compatible
23
         * parameters.
24
         */
25
26
        // https://github.com/ros2/ros2/wiki/About-Quality-of-Service-Settings
27
28
        rclcpp::QoS qos(10);
29
        qos.keep_last(10);
30
        qos.best_effort();
31
        qos.durability_volatile();
32
33
        // Create pose subscriber
34
        mPoseSub = create_subscription<geometry_msgs::msg::PoseStamped>(
35
                    "pose", qos,
36
                    std::bind(&MinimalPoseOdomSubscriber::poseCallback, this, _1) );
37
38
        // Create odom subscriber
39
        mOdomSub = create_subscription<nav_msgs::msg::Odometry>(
40
                    "odom", qos,
41
                    std::bind(&MinimalPoseOdomSubscriber::odomCallback, this, _1) );
42
    }
43
44
protected:
45
    void poseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg) {
46
        // Camera position in map frame
47
        double tx = msg->pose.position.x;
48
        double ty = msg->pose.position.y;
49
        double tz = msg->pose.position.z;
50
51
        // Orientation quaternion
52
        tf2::Quaternion q(
53
                    msg->pose.orientation.x,
54
                    msg->pose.orientation.y,
55
                    msg->pose.orientation.z,
56
                    msg->pose.orientation.w);
57
58
        // 3x3 Rotation matrix from quaternion
59
        tf2::Matrix3x3 m(q);
60
61
        // Roll Pitch and Yaw from rotation matrix
62
        double roll, pitch, yaw;
63
        m.getRPY(roll, pitch, yaw);
64
65
        // Output the measure
66
        RCLCPP_INFO(get_logger(), "Received pose in '%s' frame : X: %.2f Y: %.2f Z: %.2f - R: %.2f P: %.2f Y: %.2f - Timestamp: %u.%u sec ",
67
                 msg->header.frame_id.c_str(),
68
                 tx, ty, tz,
69
                 roll * RAD2DEG, pitch * RAD2DEG, yaw * RAD2DEG,
70
                    msg->header.stamp.sec,msg->header.stamp.nanosec);
71
    }
72
73
    void odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg) {
74
        // Camera position in map frame
75
        double tx = msg->pose.pose.position.x;
76
        double ty = msg->pose.pose.position.y;
77
        double tz = msg->pose.pose.position.z;
78
79
        // Orientation quaternion
80
        tf2::Quaternion q(
81
                    msg->pose.pose.orientation.x,
82
                    msg->pose.pose.orientation.y,
83
                    msg->pose.pose.orientation.z,
84
                    msg->pose.pose.orientation.w);
85
86
        // 3x3 Rotation matrix from quaternion
87
        tf2::Matrix3x3 m(q);
88
89
        // Roll Pitch and Yaw from rotation matrix
90
        double roll, pitch, yaw;
91
        m.getRPY(roll, pitch, yaw);
92
93
        // Output the measure
94
        RCLCPP_INFO(get_logger(), "Received odometry in '%s' frame : X: %.2f Y: %.2f Z: %.2f - R: %.2f P: %.2f Y: %.2f - Timestamp: %u.%u sec ",
95
                 msg->header.frame_id.c_str(),
96
                 tx, ty, tz,
97
                 roll * RAD2DEG, pitch * RAD2DEG, yaw * RAD2DEG,
98
                    msg->header.stamp.sec,msg->header.stamp.nanosec);
99
    }
100
101
private:
102
    rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr mPoseSub;
103
    rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr mOdomSub;
104
};
105
106
107
108
int main(int argc, char* argv[]) {
109
    rclcpp::init(argc, argv);
110
    rclcpp::spin(std::make_shared<MinimalPoseOdomSubscriber>());
111
    rclcpp::shutdown();
112
    return 0;
113
}

1
MinimalPoseOdomSubscriber()
2
        : Node("zed_odom_pose_tutorial") {
3
        rclcpp::QoS qos(10);
4
        qos.keep_last(10);
5
        qos.best_effort();
6
        qos.durability_volatile();
7
8
        // Create pose subscriber
9
        mPoseSub = create_subscription<geometry_msgs::msg::PoseStamped>(
10
                    "pose", qos,
11
                    std::bind(&MinimalPoseOdomSubscriber::poseCallback, this, _1) );
12
13
        // Create odom subscriber
14
        mOdomSub = create_subscription<nav_msgs::msg::Odometry>(
15
                    "odom", qos,
16
                    std::bind(&MinimalPoseOdomSubscriber::odomCallback, this, _1) );
17
    }

1
    void poseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg) {
2
        // Camera position in map frame
3
        double tx = msg->pose.position.x;
4
        double ty = msg->pose.position.y;
5
        double tz = msg->pose.position.z;
6
7
        // Orientation quaternion
8
        tf2::Quaternion q(
9
                    msg->pose.orientation.x,
10
                    msg->pose.orientation.y,
11
                    msg->pose.orientation.z,
12
                    msg->pose.orientation.w);
13
14
        // 3x3 Rotation matrix from quaternion
15
        tf2::Matrix3x3 m(q);
16
17
        // Roll Pitch and Yaw from rotation matrix
18
        double roll, pitch, yaw;
19
        m.getRPY(roll, pitch, yaw);
20
21
        // Output the measure
22
        RCLCPP_INFO(get_logger(), "Received pose in '%s' frame : X: %.2f Y: %.2f Z: %.2f - R: %.2f P: %.2f Y: %.2f - Timestamp: %u.%u sec ",
23
                 msg->header.frame_id.c_str(),
24
                 tx, ty, tz,
25
                 roll * RAD2DEG, pitch * RAD2DEG, yaw * RAD2DEG,
26
                    msg->header.stamp.sec,msg->header.stamp.nanosec);
27
    }
28
29
    void odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg) {
30
        // Camera position in map frame
31
        double tx = msg->pose.pose.position.x;
32
        double ty = msg->pose.pose.position.y;
33
        double tz = msg->pose.pose.position.z;
34
35
        // Orientation quaternion
36
        tf2::Quaternion q(
37
                    msg->pose.pose.orientation.x,
38
                    msg->pose.pose.orientation.y,
39
                    msg->pose.pose.orientation.z,
40
                    msg->pose.pose.orientation.w);
41
42
        // 3x3 Rotation matrix from quaternion
43
        tf2::Matrix3x3 m(q);
44
45
        // Roll Pitch and Yaw from rotation matrix
46
        double roll, pitch, yaw;
47
        m.getRPY(roll, pitch, yaw);
48
49
        // Output the measure
50
        RCLCPP_INFO(get_logger(), "Received odometry in '%s' frame : X: %.2f Y: %.2f Z: %.2f - R: %.2f P: %.2f Y: %.2f - Timestamp: %u.%u sec ",
51
                 msg->header.frame_id.c_str(),
52
                 tx, ty, tz,
53
                 roll * RAD2DEG, pitch * RAD2DEG, yaw * RAD2DEG,
54
                    msg->header.stamp.sec,msg->header.stamp.nanosec);
55
    }

1
    double tx = msg->pose.position.x;
2
    double ty = msg->pose.position.y;
3
    double tz = msg->pose.position.z;

1
    // Orientation quaternion
2
    tf2::Quaternion q(
3
        msg->pose.orientation.x,
4
        msg->pose.orientation.y,
5
        msg->pose.orientation.z,
6
        msg->pose.orientation.w);
7
8
    // 3x3 Rotation matrix from quaternion
9
    tf2::Matrix3x3 m(q);
10
11
    // Roll Pitch and Yaw from rotation matrix
12
    double roll, pitch, yaw;
13
    m.getRPY(roll, pitch, yaw);

1
        // Output the measure
2
        RCLCPP_INFO(get_logger(), "Received pose in '%s' frame : X: %.2f Y: %.2f Z: %.2f - R: %.2f P: %.2f Y: %.2f - Timestamp: %u.%u sec ",
3
                 msg->header.frame_id.c_str(),
4
                 tx, ty, tz,
5
                 roll * RAD2DEG, pitch * RAD2DEG, yaw * RAD2DEG,
6
                    msg->header.stamp.sec,msg->header.stamp.nanosec);

1
int main(int argc, char* argv[]) {
2
    rclcpp::init(argc, argv);
3
    
4
    auto pos_track_node = std::make_shared<MinimalPoseOdomSubscriber>();
5
6
    rclcpp::spin(pos_track_node);
7
    rclcpp::shutdown();
8
    return 0;
9
}