50 lines
1.1 KiB
C++
50 lines
1.1 KiB
C++
#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;
|
|
} |