#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include <cstdlib>     // std::rand, RAND_MAX
#include <ctime>       // std::time
#include <chrono>

using namespace std::chrono_literals;

class CmdPub : public rclcpp::Node
{
public:
  CmdPub()
  : Node("cmd_pub")
  {
    std::srand(static_cast<unsigned int>(std::time(nullptr)));

    publisher_ = this->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 10);
    timer_ = this->create_wall_timer(1s, std::bind(&CmdPub::timer_callback, this));
  }

private:
  void timer_callback()
  {
    geometry_msgs::msg::Twist msg;

    double r = static_cast<double>(std::rand()) / static_cast<double>(RAND_MAX); // [0,1]
    double wz = 2.0 * r - 1.0;                                                   // [-1,1]

    msg.angular.z = wz;

    publisher_->publish(msg);
    RCLCPP_INFO(this->get_logger(), "Published cmd_vel angular z = %.3f rad/s", wz);
  }

  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<CmdPub>());
  rclcpp::shutdown();
  return 0;
}
  
