Cannot call/use position callback in offboard code with ROS2, mavros, c++.

[msyuruk]

Mavros: 2.4.0 (if I am correct)
ROS: foxy
Ubuntu: 20.04

[ ] ArduPilot
[ X ] PX4
Version: 1.15 beta

I have written a simple offboard code to use with px4 autopilot in sitl. My code's state callback works, however position callback is not being called. Beside this, when I run mavros with command: ros2 run mavros mavros_node --ros-args --param fcu_url:=udp://:[email protected]:14557, I continuosly get this error: [ERROR] [1724271415.008343266] [mavros.odometry]: ODOM: Ex: Could not find a connection between 'map' and 'base_link_frd' because they are not part of the same tree.Tf has two or more unconnected trees.

Colcon builds this code successfully, switches vehicle to offboard, arms it and takes it off, but pose_cb is not called in this code. When I run ros2 topic echo command in another terminal I can see that the position is being published. Also, my code runs state callback successfully, when I cannot use the posiiton data in my code, did I installed mavros incorrectly? I added comment line where my code run in the simulation.
I hope I do not made a simple mistake with callbacks, thanks in advance to those who are interested.

This is my code:

#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <mavros_msgs/msg/state.hpp>
#include <mavros_msgs/srv/command_bool.hpp>
#include <mavros_msgs/srv/set_mode.hpp>

mavros_msgs::msg::State current_state;
void state_cb(const mavros_msgs::msg::State::SharedPtr msg) {
current_state = *msg;
}

geometry_msgs::msg::PoseStamped current_pose;
void pose_cb(const geometry_msgs::msg::PoseStamped::SharedPtr msg) {
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Current pose: x=%f, y=%f, z=%f", msg->pose.position.x, msg->pose.position.y, msg->pose.position.z);
current_pose = *msg;
}

struct Point {
double x, y;
};

int main(int argc, char **argv) {
rclcpp::init(argc, argv);
std::shared_ptrrclcpp::Node node = rclcpp::Node::make_shared("position_node");

rclcpp::Subscription<mavros_msgs::msg::State>::SharedPtr state_sub = node->create_subscription<mavros_msgs::msg::State>(
    "mavros/state", 10, state_cb);
rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr pose_sub = node->create_subscription<geometry_msgs::msg::PoseStamped>(
    "mavros/local_position/pose", 10, pose_cb);
rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr local_pos_pub = node->create_publisher<geometry_msgs::msg::PoseStamped>(
    "mavros/setpoint_position/local", 10);
rclcpp::Client<mavros_msgs::srv::CommandBool>::SharedPtr arming_client = node->create_client<mavros_msgs::srv::CommandBool>(
    "mavros/cmd/arming");
rclcpp::Client<mavros_msgs::srv::SetMode>::SharedPtr set_mode_client = node->create_client<mavros_msgs::srv::SetMode>(
    "mavros/set_mode");

double altitude = 10;
int sampling_degree = 10;
double r = 20;

Point c1, c2;
c1.x = 10;
c1.y = 10;
c2.x = 50;
c2.y = 10;

std::vector<Point> points;
for (int i = 0; i <= 360; i += sampling_degree) {
    Point temp;
    temp.x = c1.x + r * cos(i * M_PI / 180.0);
    temp.y = c1.y + r * sin(i * M_PI / 180.0);

    points.push_back(temp);
}

for (int i = 180; i >= -180; i -= sampling_degree) {
    Point temp;
    temp.x = c2.x + r * cos(i * M_PI / 180.0);
    temp.y = c2.y + r * sin(i * M_PI / 180.0);

    points.push_back(temp);
}

// Obtain size of the points vector as an integer
int points_size = points.size();

rclcpp::Rate rate(20.0);

RCLCPP_INFO(node->get_logger(), "Waiting for connection");
while (rclcpp::ok() && !current_state.connected) {
    rclcpp::spin_some(node);
    rate.sleep();
}
RCLCPP_INFO(node->get_logger(), "Connected");

geometry_msgs::msg::PoseStamped tkf_pose;
tkf_pose.pose.position.x = 0;
tkf_pose.pose.position.y = 0;
tkf_pose.pose.position.z = altitude;

RCLCPP_INFO(node->get_logger(), "Sending setpoints before requesting offboard");

for (int i = 100; rclcpp::ok() && i > 0; --i) {
    local_pos_pub->publish(tkf_pose);
    rclcpp::spin_some(node);
    rate.sleep();
}

std::shared_ptr<mavros_msgs::srv::SetMode::Request> offb_set_mode = std::make_shared<mavros_msgs::srv::SetMode::Request>();
offb_set_mode->custom_mode = "OFFBOARD";

std::shared_ptr<mavros_msgs::srv::CommandBool::Request> arm_cmd = std::make_shared<mavros_msgs::srv::CommandBool::Request>();
arm_cmd->value = true;

rclcpp::Time last_request = node->now();

while (rclcpp::ok() && current_pose.pose.position.z < altitude - 0.5) {
    if (current_state.mode != "OFFBOARD" &&
        (node->now() - last_request > rclcpp::Duration(5.0))) {
        rclcpp::Client<mavros_msgs::srv::SetMode>::SharedFuture result = set_mode_client->async_send_request(offb_set_mode);
        if (rclcpp::spin_until_future_complete(node, result) == rclcpp::FutureReturnCode::SUCCESS) {
            RCLCPP_INFO(node->get_logger(), "Offboard enabled");
        }
        last_request = node->now();
    } else {
        if (!current_state.armed &&
            (node->now() - last_request > rclcpp::Duration(5.0))) {
            rclcpp::Client<mavros_msgs::srv::CommandBool>::SharedFuture result = arming_client->async_send_request(arm_cmd);
            if (rclcpp::spin_until_future_complete(node, result) == rclcpp::FutureReturnCode::SUCCESS) {
                RCLCPP_INFO(node->get_logger(), "Vehicle armed");
            }
            last_request = node->now();
        }
    }
    local_pos_pub->publish(tkf_pose);

    rclcpp::spin_some(node);
    rate.sleep();
}

// The code does not pass here. Because it does not obtain local position data from code.

geometry_msgs::msg::PoseStamped pose;
RCLCPP_INFO(node->get_logger(), "Beginning circling");
for (int i = 0; i < points_size; i++) {
    RCLCPP_INFO(node->get_logger(), "point index: %d", i);
    pose.pose.position.x = points[i].x;
    pose.pose.position.y = points[i].y;
    pose.pose.position.z = altitude;

    while (rclcpp::ok() && sqrt(pow(points[i].x - current_pose.pose.position.x, 2) +
                                pow(points[i].y - current_pose.pose.position.y, 2)) > 1.0) {
        if (current_state.mode != "OFFBOARD" &&
            (node->now() - last_request > rclcpp::Duration(5.0))) {
            rclcpp::Client<mavros_msgs::srv::SetMode>::SharedFuture result = set_mode_client->async_send_request(offb_set_mode);
            if (rclcpp::spin_until_future_complete(node, result) == rclcpp::FutureReturnCode::SUCCESS) {
                RCLCPP_INFO(node->get_logger(), "Offboard enabled");
            }
            last_request = node->now();
        }
        local_pos_pub->publish(pose);
        rclcpp::spin_some(node);
        rate.sleep();
    }
}
rclcpp::shutdown();
return 0;

}