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

class CmdVelPublisher : public rclcpp::Node
{
public:
  CmdVelPublisher()
  : Node("cmd_pub")
  {
    std::srand(std::time(0));

    publisher_ = this->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 10);

    timer_ = this->create_wall_timer(
      std::chrono::seconds(1),
      std::bind(&CmdVelPublisher::timer_callback, this));

    RCLCPP_INFO(this->get_logger(), "Command Publisher Node initialized. Publishing random rotation to /cmd_vel every 1s...");
  }

private:
  void timer_callback()
  {
    auto twist_msg = geometry_msgs::msg::Twist();

    double random_z = (rand() / (double)RAND_MAX) * 2.0 - 1.0;
    
    twist_msg.angular.z = random_z;

    publisher_->publish(twist_msg);

    RCLCPP_INFO(this->get_logger(), "Published cmd_vel: angular.z = %f", random_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<CmdVelPublisher>());

  rclcpp::shutdown();
  return 0;
}