void UART::run()
    {

        while(ros::ok())
        {
            _letter = _serial.read();
            if (_serial.available() > 0)
            {
                _line.clear();
                _line = _serial.readline(65536, "!");
                _words.clear();
                std::istringstream f(_line);
                std::string s;
                while (getline(f,s,'t'))
                {
                    _words.push_back(s);
                }
                this->fillVars();
                _msg.velocity = _velocity;
                _msg.position = _position;
                _pub.publish(_msg);
                ros::spinOnce();
            }
            _serial.flush();
        }
    }

    void UART::fillVars()
    {
        if (_words[0] == "u")
        {
            _ultrasonic = std::stoi(_words[1]);
        }
        else if (_words[0] == "s")
        {
            std::cout << "reading " << _words[1] << std::endl;
        }

    }


    void UART::callback(const std_msgs::Int32::ConstPtr& speed)
    {
        // Convert m/s and serial write
        _effort = speed->data - 32;
        std::string toWrite = std::to_string(_effort);
        size_t temp = _serial.write(toWrite);
        std::cout << "Writing :" << toWrite << std::endl;
    }