git commit -m "first commit for v2"

Devices/Libraries/Ros/libserial/.gitignore

@@ -0,0 +1,14 @@
+# ---> VisualStudioCode
+.vscode/*
+!.vscode/settings.json
+!.vscode/tasks.json
+!.vscode/launch.json
+!.vscode/extensions.json
+!.vscode/*.code-snippets
+
+# Local History for Visual Studio Code
+.history/
+
+# Built Visual Studio Code Extensions
+*.vsix
+

Devices/Libraries/Ros/libserial/CMakeLists.txt

@@ -0,0 +1,45 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(libserial)
+
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+)
+
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES ${PROJECT_NAME}
+  CATKIN_DEPENDS roscpp
+#  DEPENDS other non-ROS libs
+)
+include_directories(
+  include
+  ${Boost_INCLUDE_DIRS}
+  ${catkin_INCLUDE_DIRS}
+  ${console_bridge_INCLUDE_DIRS}
+)
+
+#modbustcp
+set(YOUR_LIB_SOURCES
+    src/serial.cpp
+    src/rs485.cpp
+)
+add_library(${PROJECT_NAME} ${YOUR_LIB_SOURCES})
+SET(-ludev udev)
+target_link_libraries(${PROJECT_NAME} 
+                      ${catkin_LIBRARIES}
+                      ${Boost_LIBRARIES}
+                      ${GSTREAMER_LIBRARIES}
+                      ${-ludev}
+)
+
+install(TARGETS ${PROJECT_NAME}
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+)
+
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+  FILES_MATCHING PATTERN "*.h"
+  PATTERN ".svn" EXCLUDE
+)

Devices/Libraries/Ros/libserial/README.md

@@ -0,0 +1,2 @@
+# libserial
+

Devices/Libraries/Ros/libserial/include/libserial/define.h

@@ -0,0 +1,43 @@
+#pragma once
+
+#define OFF 0
+#define ON 1
+#define MAX_MSG_LENGTH 2048
+
+typedef enum parity {
+	PARITY_NONE,
+	PARITY_EVEN,
+	PARITY_ODD
+} parity_t;
+
+typedef enum stop_bits {
+	STOPBIT_1,
+	STOPBIT_2
+} stop_bits_t;
+
+typedef enum data_bits {
+    DATABIT_5,
+    DATABIT_6,
+	DATABIT_7,
+    DATABIT_8
+} data_bits_t;
+
+struct _rs485 {
+    /* Socket or file descriptor */
+    int s;
+    /* Device: "/dev/ttyS0", "/dev/ttyUSB0" or "/dev/tty.USA19*" on Mac OS X. */
+    const char* _port;
+    /* Bauds: 9600, 19200, 57600, 115200, etc */
+    unsigned int _baud;
+    /* Data bit */
+    data_bits_t _data_bit;
+    /* Stop bit */
+    stop_bits_t _stop_bit;
+    /* Parity:*/
+    parity_t _parity;
+    /* Save old termios settings */
+    struct termios _old_tios;
+};
+
+
+

Devices/Libraries/Ros/libserial/include/libserial/rs485.h

@@ -0,0 +1,88 @@
+//
+// Created by Hiep  on 5/12/2020.
+//
+#pragma once
+#include <ros/ros.h>
+#include "serial.h"
+
+typedef struct _rs485 rs485_t;
+
+class rs485: public Serial
+{
+private:  
+    /**
+     * Configure
+     * @param devfile   Path name of dev  
+     * @param baud      Baudrate 
+     * @param parity    Parity
+     * @param data_bit
+     * @param stop_bit 
+     */
+    virtual int _rs485_connect(rs485_t &ctx); 
+
+    /**
+     * create new port
+     * @param devfile   Path name of dev  
+     * @param baud      Baudrate 
+     * @param parity    Parity
+     * @param data_bit
+     * @param stop_bit 
+     */
+    virtual void new_port(const char* devfile, unsigned int baud, parity_t parity, 
+                                        data_bits_t data_bit,stop_bits_t stop_bit);
+    
+public:
+    /**
+     * Data Sender
+     * @param to_send Request to Be Sent to Server
+     * @param length  Length of the Request
+     * @return        Size of the request
+     */
+    virtual ssize_t sendMsgs(uint8_t *to_send, uint16_t length);
+
+    /**
+     * Data Receiver
+     * @param buffer Buffer to Store the Data` Retrieved
+     * @return       Size of Incoming Data
+     */
+    virtual ssize_t receiveMsgs(uint8_t *buffer) const;
+
+    /*
+     *  Reconnect to device if rs485 modules was disconected.
+     */
+    virtual void reconnect(void);
+
+    /*
+     * Set read character miximum
+     * @param[in] num The character minximum to receive.
+     */
+    virtual int setCharacterNumToRead(int num);
+
+    /**
+     * Close serial port
+     */
+    virtual void close_port();
+
+    /**
+     * @brief Constructor 
+     * @param[in] devfile   Example /dev/tty* 
+     * @param[in] baud      Number of transmitted bits  per a second
+     * @param[in] parity    The parity check bit {Even, None , Old }
+     * @param[in] data_bit  Number of bits in a transmission frame 
+     * @param[in] stop_bit  End bit
+     */
+    rs485(const char* devfile, unsigned int baud, parity_t parity, 
+                                        data_bits_t data_bit,stop_bits_t stop_bit);
+    /**
+     * Destructor.
+     */
+    virtual ~rs485();
+/* Properties */
+protected:
+    rs485_t ctx;
+    uint8_t size_pkg;
+    float param_safety;
+public:
+    bool _connected{};      /* Write status connection of devices */
+    std::string error_msg;  /* To saved error message */
+};

Devices/Libraries/Ros/libserial/include/libserial/serial.h

@@ -0,0 +1,44 @@
+//
+// Created by Hiep  on 4/12/2020.
+//
+#pragma once 
+// #include <bits/stdc++.h>
+#include <iostream>  /* std::cout */
+#include <stdio.h>   /* Standard input/output definitions */
+#include <string.h>  /* String function definitions */
+#include <unistd.h>  /* UNIX standard function definitions */
+#include <fcntl.h>   /* File control definitions */
+#include <errno.h>   /* Error number definitions */
+#include <termios.h> /* POSIX terminal control definitions */
+#include <cstdint>	/* uin8_t */
+#include <stdlib.h>  
+#include <sys/stat.h>
+#include "define.h"
+
+class Serial
+{
+public:
+   /*
+    * @brief Setting baudrate
+    * @param[in] b baud speed input
+    */
+   virtual speed_t setBaudRate(unsigned int b);
+
+   /*
+    * @brief Setting parity bits type
+    * @param[in] b baud speed input
+    */
+   virtual void setParity(termios &tios, parity p);
+
+   /*
+    * @brief Setting stop bits type
+    * @param[in] b baud speed input
+    */
+   virtual void setStopBits(termios &tios, stop_bits s);
+
+   /*
+    * @brief Setting data bits type
+    * @param[in] b baud speed input
+    */
+   virtual void setDataBits(termios &tios, data_bits d);
+};

Devices/Libraries/Ros/libserial/include/libserial/udevadm_info.h

@@ -0,0 +1,119 @@
+#ifndef    __UDEVADM_INFO_H_INCLUDE__
+#define    __UDEVADM_INFO_H_INCLUDE__
+
+#include <ros/ros.h>
+#include <unistd.h>
+#include <poll.h>
+#include <cstdio>
+#include <cstdlib>
+#include <cerrno>
+#include <cstring>
+#include <sys/signalfd.h>
+#include <csignal>
+#include <libudev.h>
+#include <string>
+
+class udevadmInfo {
+public:
+    udevadmInfo(const char *product_name);
+    virtual ~udevadmInfo(void);
+    virtual int init();
+    virtual bool scanDevices(void);
+    const char *port;
+protected:
+    struct udev *udev;
+    char *product_name_;
+};
+
+udevadmInfo::udevadmInfo(const char *product_name)
+ : product_name_((char *)product_name)
+{
+    
+}
+
+udevadmInfo::~udevadmInfo(void){
+    udev_unref(udev);
+}
+
+int udevadmInfo::init() {
+    udev = udev_new();
+    if (!udev) {
+        printf("Error while initialization!\n");
+        return EXIT_FAILURE;
+    }
+    sigset_t mask;
+
+    // Set signals we want to catch
+    sigemptyset(&mask);
+    sigaddset(&mask, SIGTERM);
+    sigaddset(&mask, SIGINT);
+
+    // Change the signal mask and check
+    if (sigprocmask(SIG_BLOCK, &mask, nullptr) < 0) {
+        ROS_ERROR("UDEV-Error while sigprocmask(): %s\n", std::strerror(errno));
+        return EXIT_FAILURE;
+    }
+    // Get a signal file descriptor
+    int signal_fd = signalfd(-1, &mask, 0);
+    // Check the signal file descriptor
+    if (signal_fd < 0) {
+        ROS_ERROR("UDEV-Error while signalfd(): %s\n", std::strerror(errno));
+        return EXIT_FAILURE;
+    }
+    return EXIT_SUCCESS;
+}
+
+bool udevadmInfo::scanDevices(void) {
+    struct udev_device *device;
+    struct udev_enumerate *enumerate;
+    struct udev_list_entry *devices, *dev_list_entry;
+
+    // Create enumerate object
+    enumerate = udev_enumerate_new(udev);
+    if (!enumerate) {
+        ROS_ERROR("Error while creating udev enumerate\n");
+        return false;
+    }
+
+    // Scan devices
+    udev_enumerate_scan_devices(enumerate);
+
+    // Fill up device list
+    devices = udev_enumerate_get_list_entry(enumerate);
+    if (!devices) {
+        ROS_ERROR("Error while getting device list\n");
+        return false;
+    }
+
+    udev_list_entry_foreach(dev_list_entry, devices) {
+        const char* path = udev_list_entry_get_name(dev_list_entry);
+        // Get the device
+        device = udev_device_new_from_syspath(udev, udev_list_entry_get_name(dev_list_entry));
+        // Print device information
+        if(udev_device_get_devnode(device) != NULL && strstr(udev_device_get_sysname(device),"ttyUSB") != NULL) {
+            // ROS_INFO("DEVNODE=%s", udev_device_get_devnode(device));
+            struct udev_device* usb = udev_device_get_parent_with_subsystem_devtype(device,"usb","usb_device");
+                // ROS_INFO("usb = %s:%s, manufacturer = %s, product = %s, serial = %s, speed = %s, bMaxPower = %s",
+                //         udev_device_get_sysattr_value(usb, "idVendor"),
+                //         udev_device_get_sysattr_value(usb, "idProduct"),
+                //         udev_device_get_sysattr_value(usb, "manufacturer"),
+                //         udev_device_get_sysattr_value(usb, "product"),
+                //         udev_device_get_sysattr_value(usb, "serial"),
+                //         udev_device_get_sysattr_value(usb, "speed"),
+                //         udev_device_get_sysattr_value(usb, "bMaxPower"));
+                if(strstr(udev_device_get_sysattr_value(usb, "product"), (const char *)product_name_) != NULL) {
+                    port = udev_device_get_devnode(device);
+                    return true;
+                }
+        }
+        else {
+            continue;
+        }
+        // Free the device
+        udev_device_unref(device);
+    }
+    // Free enumerate
+    udev_enumerate_unref(enumerate);
+    return false;
+}
+#endif

Devices/Libraries/Ros/libserial/package.xml

@@ -0,0 +1,62 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>libserial</name>
+  <version>0.0.0</version>
+  <description>The libserial package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="robotics@todo.todo">robotics</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/libserial</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <exec_depend>roscpp</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

Devices/Libraries/Ros/libserial/src/rs485.cpp

@@ -0,0 +1,309 @@
+#include "libserial/rs485.h"
+
+/*
+ * Configure
+ * @param devfile   Path name of dev  
+ * @param baud      Baudrate 
+ * @param parity    Parity
+ * @param data_bit
+ * @param stop_bit 
+ */
+int rs485::_rs485_connect(rs485_t &ctx)
+{
+    struct termios tios;
+    int flags;
+    /* The O_NOCTTY flag tells UNIX that this program doesn't want
+       to be the "controlling terminal" for that port. If you
+       don't specify this then any input (such as keyboard abort
+       signals and so forth) will affect your process
+       Timeouts are ignored in canonical input mode or when the
+       NDELAY option is set on the file via open or fcntl 
+	*/
+    flags = O_RDWR | O_NOCTTY | O_NONBLOCK | O_NDELAY;
+    ctx.s = open(ctx._port,flags);       
+    if (ctx.s < 0) 
+    {
+        std::stringstream e;
+        e << ctx._port <<" is failed: " << strerror(errno);
+        error_msg = e.str();
+        _connected = false;
+        return -1;
+    }
+    else
+    {
+        fcntl(ctx.s, F_SETFL, 0);
+    }
+    /* Save */
+    if(tcgetattr(ctx.s, &ctx._old_tios) <0)
+    {
+        ROS_ERROR("Can't get terminal parameters");
+    }
+
+    /*
+    * Enable the receiver and set local mode...
+    */
+    bzero(&tios, sizeof(tios));
+    
+    /*  C_ISPEED     Input baud (new interface)
+     *  C_OSPEED     Output baud (new interface)
+     */
+    speed_t speed = setBaudRate(ctx._baud);
+    /* Set the baud rate */
+    if ((cfsetispeed(&tios, speed) < 0) ||
+        (cfsetospeed(&tios, speed) < 0)) 
+    {
+        close(ctx.s);
+        ctx.s = -1;
+        _connected = false;
+        return -1;
+    }
+    setDataBits(tios,ctx._data_bit);
+    setStopBits(tios,ctx._stop_bit);
+    setParity(tios, ctx._parity);
+    
+    /* Read the man page of termios if you need more information.
+     * This field isn't used on POSIX systems
+     * tios.c_line = 0;
+     */
+
+    /* C_LFLAG      Line options
+       ISIG Enable SIGINTR, SIGSUSP, SIGDSUSP, and SIGQUIT signals
+       ICANON       Enable canonical input (else raw)
+       XCASE        Map uppercase \lowercase (obsolete)
+       ECHO Enable echoing of input characters
+       ECHOE        Echo erase character as BS-SP-BS
+       ECHOK        Echo NL after kill character
+       ECHONL       Echo NL
+       NOFLSH       Disable flushing of input buffers after
+       interrupt or quit characters
+       IEXTEN       Enable extended functions
+       ECHOCTL      Echo control characters as ^char and delete as ~?
+       ECHOPRT      Echo erased character as character erased
+       ECHOKE       BS-SP-BS entire line on line kill
+       FLUSHO       Output being flushed
+       PENDIN       Retype pending input at next read or input char
+       TOSTOP       Send SIGTTOU for background output
+       Canonical input is line-oriented. Input characters are put
+       into a buffer which can be edited interactively by the user
+       until a CR (carriage return) or LF (line feed) character is
+       received.
+       Raw input is unprocessed. Input characters are passed
+       through exactly as they are received, when they are
+       received. Generally you'll deselect the ICANON, ECHO,
+       ECHOE, and ISIG options when using raw input
+    */
+
+    /* Raw input */
+    tios.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);;
+
+    /* Software flow control is disabled */
+    tios.c_iflag &= ~(IXON | IXOFF | IXANY);
+
+    /* C_OFLAG      Output options
+       OPOST        Postprocess output (not set = raw output)
+       ONLCR        Map NL to CR-NL
+       ONCLR ant others needs OPOST to be enabled
+    */
+
+    /* Raw ouput */
+    tios.c_oflag = 0;
+
+    /* C_CC         Control characters
+       VMIN         Minimum number of characters to read
+       VTIME        Time to wait for data (tenths of seconds)
+       UNIX serial interface drivers provide the ability to
+       specify character and packet timeouts. Two elements of the
+       c_cc array are used for timeouts: VMIN and VTIME. Timeouts
+       are ignored in canonical input mode or when the NDELAY
+       option is set on the file via open or fcntl.
+       VMIN specifies the minimum number of characters to read. If
+       it is set to 0, then the VTIME value specifies the time to
+       wait for every character read. Note that this does not mean
+       that a read call for N bytes will wait for N characters to
+       come in. Rather, the timeout will apply to the first
+       character and the read call will return the number of
+       characters immediately available (up to the number you
+       request).
+       If VMIN is non-zero, VTIME specifies the time to wait for
+       the first character read. If a character is read within the
+       time given, any read will block (wait) until all VMIN
+       characters are read. That is, once the first character is
+       read, the serial interface driver expects to receive an
+       entire packet of characters (VMIN bytes total). If no
+       character is read within the time allowed, then the call to
+       read returns 0. This method allows you to tell the serial
+       driver you need exactly N bytes and any read call will
+       return 0 or N bytes. However, the timeout only applies to
+       the first character read, so if for some reason the driver
+       misses one character inside the N byte packet then the read
+       call could block forever waiting for additional input
+       characters.
+       VTIME specifies the amount of time to wait for incoming
+       characters in tenths of seconds. If VTIME is set to 0 (the
+       default), reads will block (wait) indefinitely unless the
+       NDELAY option is set on the port with open or fcntl.
+    */
+    /* Unused because we use open with the NDELAY option */
+    tios.c_cc[VMIN] = 5;        /* The character minximum to read*/
+    tios.c_cc[VTIME] = 20;      /* The wait time maximum to read*/
+    /* clean port */
+  	tcflush(ctx.s, TCIFLUSH);
+    /* activate the settings port */
+    if (tcsetattr(ctx.s, TCSANOW, &tios) < 0) {
+        ROS_ERROR("Can't get terminal parameters");
+        close(ctx.s);
+        ctx.s = -1;
+        _connected = false;
+        return -1;
+    }
+    ctx._old_tios = tios;
+    _connected = true;
+    return 0;
+}
+
+/*
+ *  Reconnect to device if rs485 modules was disconected.
+ */
+void rs485::reconnect(void)
+{
+    _rs485_connect(ctx);
+}
+
+/*
+ * create new port
+ * @param devfile   Path name of dev  
+ * @param baud      Baudrate 
+ * @param parity    Parity
+ * @param data_bit
+ * @param stop_bit 
+ */
+void rs485::new_port(const char* devfile,unsigned int baud, parity_t parity, 
+                                        data_bits_t data_bit,stop_bits_t stop_bit)
+{
+    ctx._port = devfile;
+    ctx._baud = baud;
+    ctx._parity = parity;
+    ctx._data_bit = data_bit;
+    ctx._stop_bit = stop_bit;
+    _connected = false;
+    if(parity == PARITY_EVEN || parity == PARITY_ODD)
+        size_pkg++;
+    switch(stop_bit)
+    {
+        case STOPBIT_1:
+            size_pkg++;
+            break;
+        case STOPBIT_2:
+            size_pkg +=2;
+            break;
+        default:
+            break;
+    }
+
+    switch(data_bit)
+    {
+        case DATABIT_5:
+            size_pkg += 5;
+            break;
+        case DATABIT_6:
+            size_pkg += 6;
+            break;
+        case DATABIT_7:
+            size_pkg += 7;
+            break;
+        case DATABIT_8:
+            size_pkg += 8;
+            break;
+        default:
+            break;
+    }
+    size_pkg +=2;  // bit start and stop
+
+    switch(baud)
+    {
+    case 19200:
+        param_safety = 1.5;
+        break;
+    case 115200:
+        param_safety = 3.5;
+        break;
+    default:
+        param_safety = 1.5;
+        break;
+    }
+}
+
+/*
+ * Close serial port
+ */
+void rs485::close_port()
+{
+    close(this->ctx.s);
+}
+
+/*
+ * Data Sender
+ * @param to_send Request to Be Sent to Server
+ * @param length  Length of the Request
+ * @return        Size of the request
+ */
+ssize_t rs485::sendMsgs(uint8_t *to_send, uint16_t length)
+{
+    float time = param_safety*1000000/(ctx._baud / (size_pkg * length));
+    usleep(time);
+    memset((to_send + length),'\0', 1);
+    ssize_t num = write(ctx.s, to_send, (size_t)length);
+    usleep(time);
+    return num;
+}
+
+/*
+ * Data Receiver
+ * @param buffer Buffer to Store the Data` Retrieved
+ * @return       Size of Incoming Data
+ */
+ssize_t rs485::receiveMsgs(uint8_t *buffer) const 
+{
+    memset(buffer,'\0',sizeof(buffer));
+    return read(ctx.s, (char *) buffer,MAX_MSG_LENGTH);
+}
+
+/*
+ * @brief Constructor 
+ * @param[in] devfile   Example /dev/tty* 
+ * @param[in] baud      Number of transmitted bits  per a second
+ * @param[in] parity    The parity check bit {Even, None , Old }
+ * @param[in] data_bit  Number of bits in a transmission frame 
+ * @param[in] stop_bit  End bit
+ */
+rs485::rs485(const char* devfile, unsigned int baud, parity_t parity, data_bits_t data_bit,stop_bits_t stop_bit)
+{
+    new_port(devfile, baud, parity, data_bit, stop_bit);
+    _rs485_connect(ctx);
+}
+
+/*
+ * Destructor.
+ */
+rs485::~rs485()
+{
+   
+}
+
+/*
+ * Set read character miximum
+ * @param[in] num The character minximum to receive.
+ */
+int rs485::setCharacterNumToRead(int num)
+{
+    ctx._old_tios.c_cc[VMIN] = num;
+    /* clean port */
+    tcflush(ctx.s, TCIFLUSH);
+    /* activate the settings port */
+    if (tcsetattr(ctx.s, TCSANOW, &ctx._old_tios) < 0) {
+        ROS_ERROR("Can't get terminal parameters");
+        close(ctx.s);
+        ctx.s = -1;
+    }
+    return 0;
+}

Devices/Libraries/Ros/libserial/src/serial.cpp

@@ -0,0 +1,181 @@
+#include "libserial/serial.h"
+
+/*
+ * @brief Setting baudrate
+ * @param[in] b baud speed input
+ */
+speed_t Serial::setBaudRate(unsigned int b) {
+  speed_t speed;
+  switch (b) {
+  case 110:
+    speed = B110;
+    break;
+  case 300:
+    speed = B300;
+    break;
+  case 600:
+    speed = B600;
+    break;
+  case 1200:
+    speed = B1200;
+    break;
+  case 2400:
+    speed = B2400;
+    break;
+  case 4800:
+    speed = B4800;
+    break;
+  case 9600:
+    speed = B9600;
+    break;
+  case 19200:
+    speed = B19200;
+    break;
+  case 8400:
+    speed = B38400;
+    break;
+  case 57600:
+    speed = B57600;
+    break;
+  case 115200:
+    speed = B115200;
+    break;
+  case 230400:
+    speed = B230400;
+    break;
+  case 460800:
+    speed = B460800;
+    break;
+  case 500000:
+    speed = B500000;
+    break;
+  case 576000:
+    speed = B576000;
+    break;
+  case 921600:
+    speed = B921600;
+    break;
+  case 1000000:
+    speed = B1000000;
+    break;
+  case 1152000:
+    speed = B1152000;
+    break;
+  case 1500000:
+    speed = B1500000;
+    break;
+  case 2500000:
+    speed = B2500000;
+    break;
+  case 3000000:
+    speed = B3000000;
+    break;
+  case 3500000:
+    speed = B3500000;
+    break;
+  case 4000000:
+    speed = B4000000;
+    break;
+  default:
+    speed = B9600;
+    break;
+  }
+  return speed;
+}
+
+/*
+ * @brief Setting parity bits type
+ * @param[in] b baud speed input
+ */
+void Serial::setParity(termios& tios, parity p) {
+  /* PARENB       Enable parity bit
+  PARODD       Use odd parity instead of even */
+  switch (p)
+  {
+    /* None */
+  case PARITY_NONE:
+    tios.c_cflag &= ~PARENB;
+    tios.c_iflag &= ~INPCK;
+    break;
+    /* Even */
+  case PARITY_EVEN:
+    tios.c_cflag |= PARENB;
+    tios.c_cflag &= ~PARODD;
+    tios.c_iflag |= INPCK;
+    break;
+    /* Odd */
+  case PARITY_ODD:
+    tios.c_cflag |= PARENB;
+    tios.c_cflag |= PARODD;
+    break;
+    /* Default None */
+  default:
+    tios.c_cflag &= ~PARENB;
+    tios.c_iflag &= ~INPCK;
+    break;
+  }
+}
+
+/*
+ * @brief Setting stop bits type
+ * @param[in] b baud speed input
+ */
+void Serial::setStopBits(termios& tios, stop_bits s) {
+  /* Stop bit (1 or 2) */
+  switch (s)
+  {
+  case STOPBIT_1:
+    tios.c_cflag &= ~CSTOPB;
+    break;
+
+  case STOPBIT_2:
+    tios.c_cflag |= CSTOPB;
+    break;
+
+  default:
+    tios.c_cflag &= ~CSTOPB;
+    break;
+  }
+}
+
+/*
+ * @brief Setting data bits type
+ * @param[in] b baud speed input
+ */
+void Serial::setDataBits(termios& tios, data_bits d)
+{
+  /* C_CFLAG   Control options
+   * CLOCAL    Local line - do not change "owner" of port
+   * CREAD     Enable receiver
+   */
+  tios.c_cflag |= (CREAD | CLOCAL);
+  /* CSIZE, HUPCL, CRTSCTS (hardware flow control) */
+
+  /* Set data bits (5, 6, 7, 8 bits)
+   * CSIZE         Bit mask for data bits
+   */
+  tios.c_cflag &= ~CSIZE;
+  switch (d)
+  {
+  case DATABIT_8:
+    tios.c_cflag |= CS8;
+    break;
+
+  case DATABIT_7:
+    tios.c_cflag |= CS7;
+    break;
+
+  case DATABIT_6:
+    tios.c_cflag |= CS6;
+    break;
+
+  case DATABIT_5:
+    tios.c_cflag |= CS5;
+    break;
+
+  default:
+    tios.c_cflag |= CS8;
+    break;
+  }
+}
+