ROS 进阶学习笔记（15） - Use Service to play ROS-Serial communication

Use Service to play ROS-Serial communication

Continue for this blog :

ROS 进阶学习笔记（12） - Communication with ROS through USART Serial Port

In that blog above, we discussed ROS-Serial communication through r2serial_driver node (C++). There is a problem that the r2serial made too much cpu expenses. Like below:

----------

 Kevin mentioned a question: "Interesting, but a lot of my serial stuff is send a message and receive a response back. Instead of separate publish/subscribes. have you tried a service? That seems like it would work nicely for this."

I also noticed a problem: MY Linux's System RAM and CPUseems to be much costed by r2serial_driver when it is running.

This time, we'll see how to use Kevin's service to play ROS serial communication.

Use Service to play ROS-Serial communication

=============

-- Check out and compile this package --

Use this commands to check out the rosbuild package (rather than catkin package), androsmakethem:

exbot@my_robot:~/rosbuild_ws$ cd sandbox/exbot@my_robot:~/rosbuild_ws/sandbox$ git clone https://github.com/walchko/serial_nodeexbot@my_robot:~/rosbuild_ws/sandbox$ cd serial_node/exbot@my_robot:~/rosbuild_ws/sandbox/serial_node$ rosmake......[ rosmake ] /home/exbot/.ros/rosmake/rosmake_output-20160511-173021             exbot@my_robot:~/rosbuild_ws/sandbox/serial_node$ rospack find serial_node/home/exbot/rosbuild_ws/sandbox/serial_nodeexbot@my_robot:~/rosbuild_ws/sandbox/serial_node$roscore && rosrun serial_node serial_node 0 /dev/ttyUSB0 9600 true[ INFO] [1462959095.157208573]: Service uc0_serial on /dev/ttyUSB0 @ 9600 baud

We can see the expense of serial_node by command "top": [ The expenses has been very small. ]

====================================================================================

-- How to use this service for our works --

1.  Understand what is ros service:

• Here, I recommend the newer of ROS to read four links carefully and understand them as deep as you can:
  
• http://wiki.ros.org/ROS/Tutorials/UnderstandingServicesParams#Using_rosservice
  Above, you need to understand the service_node need a request and will give back a response.
  
  http://wiki.ros.org/ROS/Tutorials/CreatingMsgAndSrv#Using_srv
  Above, you need to understand the .srv file defines the data type of req. and rep.
  
  http://wiki.ros.org/ROS/Tutorials/WritingServiceClient%28c%2B%2B%29
  Above, you need to understand the key points of a source code for an ROS Service node.
  
  http://wiki.ros.org/ROS/Tutorials/ExaminingServiceClient
  Above, describes the source code for client to examine if the service node works fine.
  
  

2.  Read the source code of "serial_node_service.cpp" and use "rosservice call" command to test it

          run the service by:

        > $ rosrun seal_node serial_node 0 /dev/ttyUSB0 9600 true

          call this service by (open another terminal):

        > $ rosservice call /uc0_serial d 5 300

          If you send "3C 6D 05 73 6F 6E 69 63 3E" from your micro controller to your usb-serial port of ROS running on machine, you will see the info as below:

         

            3C  -- ASCII Code for the "<"
            6D  -- ASCII Code for the "m"
            05  -- number 5  #This is strange, but the servier is designed like this.
            73  -- ASCII Code for the "s"
            6F  -- ASCII Code for the "o"
            6E  -- ASCII Code for the "n"
            69  -- ASCII Code for the "i"
            63  -- ASCII Code for the "c"
            3E  -- ASCII Code for the ">"

3.  I modified the code for the serial_node_service.cpp file before running the rosservice call command above：

/* * Copyright (c) 2010, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * *     * Redistributions of source code must retain the above copyright *       notice, this list of conditions and the following disclaimer. *     * Redistributions in binary form must reproduce the above copyright *       notice, this list of conditions and the following disclaimer in the *       documentation and/or other materials provided with the distribution. *     * Neither the name of the Willow Garage, Inc. nor the names of its *       contributors may be used to endorse or promote products derived from *       this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *//*********************************************** * * rosrun serial_node <uC> <port> <baud> * * uC   = 0, 1, 2, ... what number micro controller  * port = serial port, /dev/cu.usbserial * baud = baud rate, 115200 * ***********************************************/#include "ros/ros.h"#include "std_msgs/String.h"#include <pthread.h>#include <sys/types.h>#include <sys/stat.h>#include <sys/ioctl.h>#include <sys/time.h>#include <fcntl.h>#include <termios.h>#include <stdio.h>#include <stdlib.h>#include <queue> // C++ FIFO#include <iostream>#include "serial_node/serial.h"class Serial {public:    Serial(ros::NodeHandle& n, int i=0){        char sname[30];        sprintf(sname, "uc%d_serial",i);        name = sname;        service = n.advertiseService(name, &Serial::callback, this);        debug = false;        read_error = read_good = 0;    }        ~Serial(){        ::close(fd);        ROS_INFO("%s stopping", name.c_str());    }        inline void setDebug(bool b=true){ debug = b; }    inline void flush(void){ //discard data that was not readtcflush (fd, TCIFLUSH);}    bool callback(serial_node::serial::Request& req,            serial_node::serial::Response& res);        int read(char *buf, const int numbytes, const int trys=3);    bool readMsg(std::string&, int size, const int trys=3);    //int read2(char *buf, const int numbytes, const int trys=3);    int write(const char* buf, const int numbytes, const int trys=3);    bool open(char *port, int baud);    unsigned int available();    std::string name;    protected:    unsigned short checksum(char* buffer, int cnt);    ros::ServiceServer service;    int fd;   //serial port file pointer    bool debug;    unsigned long read_error;    unsigned long read_good;};// check this!!unsigned short Serial::checksum(char* buffer, int cnt){    unsigned int sum = 0; // 32 bit int        for(int i=0;i<cnt;i+=2) sum += *((unsigned short*)buffer++);    sum = (sum & 0xFFFF) + (sum >> 16);        return(~sum);}//Initialize serial port, return file descriptorbool Serial::open(char *port, int baud){        char msg[70];struct termios options;fd = ::open( port, O_RDWR | O_NOCTTY | O_NDELAY /*| O_NONBLOCK*/ ); //may not need the nodelayif( fd <= 0 ){    // [FIXME 20120322] errno not defined in linuxsprintf(msg,"ERROR: Unable to open port %s:%d - %s",port,baud,strerror(errno));perror(msg);return false;}fcntl(fd, F_SETFL, FNDELAY); // return if nothing there to read//get config from fd and put into optionstcgetattr (fd, &options); // Enable the receiver and set local modeoptions.c_cflag |= (CLOCAL | CREAD);//give raw data pathcfmakeraw (&options);//set baudcfsetspeed(&options, baud);//send options back to fdtcsetattr (fd, TCSANOW, &options);Serial::flush();        return true;} //serialInit#define __K_SERIAL_DEBUG__ 0int Serial::read(char *buf, const int numbytes, const int trys){    int i, numread = 0, n = 0, numzeroes = 0;        while (numread < numbytes){        //printf("%d .\n", fd);        n = ::read (fd, (buf + numread), (numbytes - numread));        if (n < 0)            return -1;        else if (0 == n){            numzeroes++;            if (numzeroes > trys)                break;        }        else numread += n;    }        if (__K_SERIAL_DEBUG__){        printf ("Read:   ");        for (i = 0; i < numread; i++)            printf ("%d ", buf[i]);        printf ("\nRead %d of %d bytes\n", numread, numbytes);    }        //tcflush (fd, TCIFLUSH);//discard data that was not read    return numread;}bool getChar(char& c, int fd, const int trys=20){    int n = 0;    int numzeroes = 0;        // wait until start char found    while (numzeroes++ < trys){        n = ::read (fd, &c, 1);        if(n == 1){            return true;        }        usleep(1000);    }        return false;}inline char makeReadable(char c){    return (c < 32 ? '.' : (c > 126 ? '.' : c));}void printMsg(std::string& msg){    std::cout<<"---[[ "<<msg[1]<<" ]]-----------------------------------"<<std::endl;    std::cout<<"Size data: "<<(int)msg[2]<<"\t"<<"Size Msg: "<<msg.size()<<std::endl;    std::cout<<"Start/End characters: "<<msg[0]<<" / "<<msg[msg.size()-1]<<std::endl;    int size = (int)msg[2];    if(size > 0){        std::cout<<"Raw:  ";        for(int i=0;i<size;i++) std::cout<<(int)msg[3+i]<<' ';        std::cout<<std::endl;        std::cout<<"ASCII: ";        for(int i=0;i<size;i++) std::cout<<makeReadable(msg[3+i])<<' ';        std::cout<<std::endl;    }    std::cout<<"-------------------------------------------"<<std::endl;}bool Serial::readMsg(std::string& ucString, int size, const int trys){    int numread = 0, n = 0, numzeroes = 0;    char c = 0;    char buf[128];        // wait until start char found    while (numzeroes++ < trys){        n = ::read (fd, &c, 1);        if(c == '<') break;        usleep(1000);ROS_INFO("wait uC's: < ");    }        // get start char    while(c != '<'){        if(!getChar(c,fd)) return false;    }    buf[0] = c;    numread++;    ROS_INFO("Serial::readMsg: got start char <");        // get message char    if(!getChar(c,fd)) return false;    buf[1] = c;    numread++;        //get message size    if(!getChar(c,fd)) return false;    buf[2] = c;    numread++;    n = (int)c;    ROS_INFO("uC data size: %d",n);        // get data section    if(n){        for(int i=0;i<n;i++){            if(!getChar(c,fd)) return false;            buf[3+i] = c;            numread++;        }    }        // get end char    if(!getChar(c,fd)) return false;    if(c != '>') return false;    buf[numread] = c;    numread++;    ROS_INFO("got end char >");        ucString.assign(buf,numread);        if(debug) printMsg(ucString);        return true;}/** * Determine the number of bytes in the input buffer without the need to  * read it. */unsigned int Serial::available(void){    int bytes = 0;    ioctl(fd, FIONREAD, &bytes);        //ROS_INFO("bytes avail: %d",bytes);        return (unsigned int)bytes;}/** * Write a specific number of bytes from a buffer * \note write() will attempt to write to the serial port several *       times before failing * \return number of bytes written */int Serial::write(const char* buf, const int numbytes, const int trys){    int i, numwritten = 0, n = 0, numzeroes = 0;    while (numwritten < numbytes){        n = ::write (fd, (buf + numwritten), (numbytes - numwritten));                if (n < 0) return -1;                else if (0 == n){            numzeroes++;                        if (numzeroes > trys) break;        }        else numwritten += n;    }        if (__K_SERIAL_DEBUG__){        printf ("cwrite[%d]: ", numbytes);        for (i = 0; i < numbytes; i++) printf ("%d ", buf[i]);        printf ("\n");    }        return numwritten;}//Process ROS command message, send to uControllerbool Serial::callback(serial_node::serial::Request& req,            serial_node::serial::Response& res){                Serial::flush();        ROS_INFO("%s command: %s", name.c_str(), req.str.c_str());    Serial::write(req.str.c_str(),req.str.size());    usleep(3000);        ROS_INFO("send: %s[%d]",req.str.c_str(),req.str.size());        // wait x msec    int miss = 0;    while( available() < req.size ){        if(miss++ < req.time) usleep(1000);        else { // resend            Serial::flush();  //Commented originally*********            Serial::write(req.str.c_str(),req.str.size());            miss = 0;            ROS_INFO("Resending: avail[%d] need[%d]",                Serial::available(), req.size);            // QoS            ++read_error;    usleep(1000); //commented originally*************        }                //if(!ros::ok()) return 0;        //ROS_INFO("loop: %d", available());    }            if(req.size > 0){        usleep(1000);                ROS_INFO("going to read");        int rcvBufSize = 200;        char ucResponse[rcvBufSize];        std::string ucString;        int num = (req.size > available() ? available() : req.size);ROS_INFO("num= %d, going to readMsg.", num);        bool ok = Serial::readMsg(ucString,req.time);        ROS_INFO("got:%s",ucResponse);        ROS_INFO("done to read");                      if (ok) {             ROS_INFO("%s response: %s", name.c_str(), ucResponse);            res.str = ucString;                        // QoS            ++read_good;        }        else{            ROS_ERROR("Error: %s",req.str.c_str());            res.str = "error";            Serial::flush();                        // QoS            ++read_error;            return false;        }    }    else res.str = "success";            Serial::flush();    return true;        } //ucCommandCallbackint main(int argc, char **argv){    char *port;    //port name    int baud;     //baud rate    int uC;    //Initialize ROS    ros::init(argc, argv, "serial_node");    ros::NodeHandle rosNode;        Serial serial(rosNode,0);    //Open and initialize the serial port to the uController    if (argc > 1) uC = atoi(argv[1]);    else {        ROS_ERROR("ucontroller index parameter invalid");        return 1;    }        //strcpy(port, DEFAULT_SERIALPORT);    if (argc > 2) port = argv[2];    else {         ROS_ERROR("need port defined");        return 1;    }    if (argc > 3) baud = atoi(argv[3]);    else {            ROS_ERROR("ucontroller baud rate parameter invalid");            return 1;    }        bool debug;    if (argc > 4) debug = (strcmp(argv[4],"true") ? false : true);    else debug = false;    serial.setDebug(debug);    //ROS_INFO("Service %s on %s @ %d baud",serial.name.c_str(), port, baud);        if (serial.open(port, baud) == false){        ROS_ERROR("unable to create a new serial port");        return 1;    }    else ROS_INFO("Service %s on %s @ %d baud",serial.name.c_str(), port, baud);    //Process ROS messages and send serial commands to uController    ros::spin();    return 0;}

For conclusion:

1.  There seems nothing to do about System Resouces Expenses and whether use service or not.
2.  This service node expenses less than r2serial_driver. The machanisms of calling serial port of this service_node can be refered when sometime we need to write a less-expense usb_serial driver in ROS.
3.  The way using this serial_node_service is pretty weird since it requires HEX input from the uController.
   
4.  I will not continue for this topic since this cost me too much time.
   

====================================================================================

====================================================================================

-- The Catkin way to compile (problem) --

The Catkin way to compile the serial_node service pack: (But I met a lot of errors...)

sonictl@my_robot_pc:~/catkin_ws/src$ git clone https://github.com/walchko/serial_nodeCloning into 'serial_node'...remote: Counting objects: 95, done.remote: Compressing objects: 100% (39/39), done.remote: Total 95 (delta 47), reused 95 (delta 47), pack-reused 0Unpacking objects: 100% (95/95), done.sonictl@my_robot_pc2:~/catkin_ws/src$ cd serial_node/sonictl@my_robot_pc2:~/catkin_ws/src/serial_node$ git checkout catkinBranch catkin set up to track remote branch catkin from origin.Switched to a new branch 'catkin'sonictl@my_robot_pc2:~/catkin_ws/src/serial_node$ cd ~/catkin_ws/sonictl@my_robot_pc2:~/catkin_ws$ catkin_make

But I met a lot of errors...

====================================================================================

====================================================================================

====================================================================================