#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include <cstdlib>
#include <ctime>

class CmdPublisher : public rclcpp::Node
{
public:
    CmdPublisher() : Node("cmd_pub")
    {
        publisher_ = this->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 10);
        timer_ = this->create_wall_timer(
            std::chrono::seconds(1),
            std::bind(&CmdPublisher::timer_callback, this)
        );
        std::srand(std::time(nullptr));  // initialisation du générateur aléatoire
    }

private:
    void timer_callback()
    {
        geometry_msgs::msg::Twist msg;
        msg.angular.z = ((double)std::rand() / RAND_MAX) * 2.0 - 1.0;  // entre -1 et 1
        publisher_->publish(msg);
        RCLCPP_INFO(this->get_logger(), "Published angular velocity around z: %.3f rad/s", msg.angular.z);
    }

    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr publisher_;
    rclcpp::TimerBase::SharedPtr timer_;
};

int main(int argc, char *argv[])
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<CmdPublisher>());
    rclcpp::shutdown();
    return 0;
}