#include "runL.h"


namespace uwb_slam{
    RunL::RunL(){
        cmd_vel_pub_ = nh_.advertise<geometry_msgs::Twist>("cmd_vel",50);
    }

    void RunL::Run(){
        ros::Rate rate(10);

        cmd_vel_.linear.x = 0.2; 
        cmd_vel_.angular.z = 0;
        for (int i = 0; i < 100; ++i) {  
            cmd_vel_pub_.publish(cmd_vel_);
            rate.sleep();
        }

        cmd_vel_.linear.x = 0; 
        cmd_vel_.angular.z = 1;

        for (int i = 0; i < 17; ++i) {  
            cmd_vel_pub_.publish(cmd_vel_);
            rate.sleep();
        }

        cmd_vel_.linear.x = 0.2; 
        cmd_vel_.angular.z = 0;
        for (int i = 0; i < 100; ++i) {  
            cmd_vel_pub_.publish(cmd_vel_);
            rate.sleep();
        }

        cmd_vel_.linear.x = 0; 
        cmd_vel_.angular.z = 0;
        cmd_vel_pub_.publish(cmd_vel_);

        stop = true;
    }
}
