0
$\begingroup$

Rosanswers logo

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<std_msgs::String>("test", 1000);

    try
    {
        ser.setPort("/dev/ttyUSB0");
        ser.setBaudrate(38400);
        ser.setBytesize(bytesize);
        ser.setFlowcontrol(flow);
        ser.setParity(parity);
        ser.setStopbits(stopbits);
        serial::Timeout to = serial::Timeout::simpleTimeout(0);
        ser.setTimeout(to);
        ser.open();
    }
    catch (serial::IOException& e)
    {
        ROS_ERROR_STREAM("Unable to open port lol");
        return -1;
    }

    if(ser.isOpen()){
        ROS_INFO_STREAM("Serial is now initialized, okay?");
    }else{
        return -1;
    }

    std::string V = "";
    std::string buffer = "";
    std::string var_check;

    renc.data = 456;
    char *ptr;
    long ret;

    ros::Rate loop_rate(1000);

    while(ros::ok()){

        ros::spinOnce();

        test_pub.publish(line);

        while(V != "V") if(ser.available()) V = ser.read(1);


        if(V == "V"){

            ser.waitReadable();
            if(ser.available()) ser.read(1);

            ser.waitReadable();
            if(ser.available()) buffer = ser.read(1);

            char *cstr = new char[buffer.length()+1];
            strcpy(cstr, buffer.c_str());

            int i = strtol(cstr, &ptr, 16);

            switch(i){
                case 1 : {
                    ser.waitReadable();
                    ser.read(6);

                    ser.waitReadable();
                    buffer = ser.read(4);

                    char *cstr = new char[buffer.length()+1];
                    strcpy(cstr, buffer.c_str());
                    
                    lenc.data = strtol(cstr, &ptr, 16);
                    if(lenc.data != 0) lwheel.publish(lenc);

                    ros::spinOnce();

                    delete [] cstr;
                    buffer = "";
                }

                case 2 : {
                    ser.waitReadable();
                    ser.read(6);

                    ser.waitReadable();
                    buffer = ser.read(4);

                    char *cstr = new char[buffer.length()+1];
                    strcpy(cstr, buffer.c_str());
                    
                    lenc.data = strtol(cstr, &ptr, 16);
                    if(lenc.data != 0) rwheel.publish(renc);

                    ros::spinOnce();

                    delete [] cstr;
                    buffer = "";     
                }

            }

            i = 0;
        }

        loop_rate.sleep();

    }
}

Originally posted by Tsuko G. on ROS Answers with karma: 3 on 2016-08-08

Post score: 0

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

You're doing a lot of strange things that might be affecting your ability to read the data effectively. The system has a limited serial buffer, so if you're reading the data slower than it is being sent over serial, then you'll lose some. You're calling ros::spinOnce() in your loops where you're reading the serial data. There are also a lot of cases where you're reading data but not checking to see what it is.

My recommendation is to start by creating a thread to read from the serial port and do ros::spin() in a separate thread. Then you can write a simpler loop that just reads one byte at a time. I'd also recommend using a timeout rather than doing waitReadable() between calls to read.


Originally posted by William with karma: 17335 on 2016-08-08

This answer was ACCEPTED on the original site

Post score: 1


Original comments

Comment by Tsuko G. on 2016-08-09:
Thank you so much for your insight William! By thread, you mean std::thread, right? Does ROS support this, and does it have rules to follow in this framework?

Also, I'm really not familiar with serial communication. What do you mean by using a timeout, how does it affect the read and how to do?

Comment by William on 2016-08-09:
You can use std::thread or boost::thread. Most of the ROS's functions are thread-safe. You can also use an async spinner and do the serial port reading in the main thread. http://wiki.ros.org/roscpp/Overview/Callbacks%20and%20Spinning#Multi-threaded_Spinning

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.