This PR adds support for the new callback feature

[juanjqo]

Hi, @mmmarinho

This PR adds support for the new callback feature. The modifications include a new protected member std::function<void()> control_loop_callback_ and three new public methods.

Now, a concrete implementation of RobotDriver can set a callback function to be executed in the main control loop handled by RobotDriverROS.

For instance, in the constructor, the user can define a callback to publish IMU data

set_control_loop_callback([this]() {

        // Static local variables - persist between iterations
        static int iteration_counter = 0;
        static double accumulated_time = 0.0;

        iteration_counter++;
        accumulated_time += configuration_.thread_sampling_time_sec;

        if (iteration_counter % 100 == 0) {
            RCLCPP_INFO_STREAM(node_->get_logger(),
                               "Iteration: " << iteration_counter
                                             << ", Accumulated time: " << accumulated_time << "s");
        }

        double time = accumulated_time;
        impl_->imu_orientation_(0) = std::cos(time * 0.1);  // w
        impl_->imu_orientation_(1) = 0.0;                   // x
        impl_->imu_orientation_(2) = 0.0;                   // y
        impl_->imu_orientation_(3) = std::sin(time * 0.1);  // z

        // Simulate angular velocity
        impl_->imu_angular_velocity_(0) = 0.1 * std::cos(time * 0.5);
        impl_->imu_angular_velocity_(1) = 0.05 * std::sin(time * 0.7);
        impl_->imu_angular_velocity_(2) = 0.2 * std::cos(time * 0.3);

        // Simulate linear acceleration (gravity + motion)
        impl_->imu_linear_acceleration_(0) = 0.1 * std::sin(time * 0.2);
        impl_->imu_linear_acceleration_(1) = 0.1 * std::cos(time * 0.4);
        impl_->imu_linear_acceleration_(2) = 9.81;  // gravity
        publish_imu(impl_->imu_orientation_, impl_->imu_angular_velocity_, impl_->imu_linear_acceleration_);


    });

Kind regards,

Juancho

[mmmarinho]

Seems ok, if it doesn't work well please adjust.