Weird serial behavior, and ROS loop rate
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 ...