Unreliable UART communication in C++

edit

I have written a small code in C++ that reads the data in serial port and sometimes writes to the serial port when the callback is being called. I am using this library in C++ for the serial communication:

http://wjwwood.io/serial/doc/1.1.0/in...

The problem I have is that sometimes the serial port stops writing to the data (does not happen often but happens) and also I need to write twice to the serial port in order for the data to be changed in the serial. For debugging purposes, I have added outputs that shows the data that has been read and the data that has been written.

Here is my code:

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;
    }

The code always runs in the run() function and pauses when the callback happens.

The only data in the serial for now is: !s 23 where 23 is any integer and I am simply writing to change the integer when callback is being called.

So what can be wrong with my code? Why do I need to call the callback twice in order to change the data in the serial and why does sometime the writing does not happen (my assumption is that the while loop gets stuck)? Is it due to the subscriber issue? Cannot figure out