Weird serial behavior, and ROS loop rate

edit

Hello everyone.

I am currently working on a project which involves me to write a C++ differential driver and encoder publisher for an old custom DSP board, which communicates through serial port. It constantly displays the encoder info in the form of VXX=YYYY,YYYY lines ending with an \n, XX being the encoder number in DEC (in my case there are two encoders: numbers 11 and 12), and YYYY,YYYY is the encoder tick count, displayed in HEX. So if there are two encoders, the DSP would display:

V11=0000,0000
V12=0000,0000
V11=0000,0000
V12=0000,0000
V11=0000,0000
V12=0000,0000
...

My goal would be to: - Extract the serial string - Define which encoder it is about with the VXX info - Extract the encoder counts and convert them as follows: STRING -> HEX -> std_msgs::Int32.

For this matter, I am using wjwwood's serial.h library. I've got my DSP board to work correctly on Cutecom and HyperTerminal (Windows), and hence I am 99.99% sure of the accuracy of the info that I receive. So far, I wrote a code that I think should work, but I am facing issues regarding catching the info from the serial port: I get incomplete data. Also I was suspecting the ROS loop rate to play a role on this, which in fact has, but when changing it, either the refresh rate is too low, or the second encoder isn't read, or I loose all the data (and get weird values when rostopic echo lwheel for example).

I would be eternally thankful if anyone could help me on this. I think I've wasted way too much time on it trying to figure it out, and I'm starting to be really bothered by it. :/

Please find below the entire code I wrote. Note that I have never written a serial driver before, and therefore my knowledge on this matter is quite limited.

Thank you ever so much.

#include <ros/ros.h>
#include <serial/serial.h>
#include <std_msgs/Int32.h>
#include <std_msgs/Int16.h>
#include <std_msgs/String.h>
#include <ros/console.h>
#include <stdlib.h>

#include <sstream>

/*******************/
/*  Serial setup   */
/*******************/

serial::Serial ser;
serial::bytesize_t bytesize = serial::bytesize_t(8);
serial::parity_t parity = serial::parity_t(1);
serial::stopbits_t stopbits = serial::stopbits_t(1);
serial::flowcontrol_t flow = serial::flowcontrol_t(0);

/**********************/
/*  Variables setup   */
/**********************/

std_msgs::Int32 lenc;
std_msgs::Int32 renc;
std_msgs::Int32 lsp;
std_msgs::Int32 rsp;

std_msgs::String test;

/////////////////////

void write_callback(const std_msgs::String::ConstPtr& msg){
    ROS_INFO_STREAM("Writing to serial port" << msg->data);
    ser.write(msg->data);
}

void rmotorCb(const std_msgs::Int16& rmotor_msg){
    //TBD
}

void lmotorCb(const std_msgs::Int16& lmotor_msg){
    //TBD
}

int main (int argc, char** argv){

    /////////////////////

    //ROS Node initialization

    ros::init(argc, argv, "AMC-8004");
    ros::NodeHandle nh;

    /******************/
    /*  Topics setup  */
    /******************/

    ros::Subscriber rmotor = nh.subscribe("rmotor", 1000, rmotorCb);
    ros::Subscriber lmotor = nh.subscribe("lmotor", 1000, lmotorCb);
    ros::Publisher rwheel = nh.advertise<std_msgs::String>/*<std_msgs::Int32>*/("rwheel", 1000);
    ros::Publisher lwheel = nh.advertise/*<std_msgs::String>*/<std_msgs::Int32>("lwheel", 1000);
    ros::Publisher test_pub = nh.advertise ...