From 587ad308d7a92bc9311910c14a20b68068ec04a2 Mon Sep 17 00:00:00 2001 From: seanyen Date: Fri, 8 Nov 2019 16:58:31 -0800 Subject: [PATCH 1/2] Use portable serial lib. --- cob_sick_s300/CMakeLists.txt | 6 +- .../include/cob_sick_s300/ScannerSickS300.h | 4 +- .../common/include/cob_sick_s300/SerialIO.h | 214 --------- .../include/cob_sick_s300/TelegramS300.h | 4 + cob_sick_s300/common/src/ScannerSickS300.cpp | 44 +- cob_sick_s300/common/src/SerialIO.cpp | 414 ------------------ cob_sick_s300/package.xml | 1 + cob_sick_s300/ros/src/cob_sick_s300.cpp | 3 +- 8 files changed, 34 insertions(+), 656 deletions(-) delete mode 100644 cob_sick_s300/common/include/cob_sick_s300/SerialIO.h delete mode 100644 cob_sick_s300/common/src/SerialIO.cpp diff --git a/cob_sick_s300/CMakeLists.txt b/cob_sick_s300/CMakeLists.txt index dc6031bdd..39ec11efc 100644 --- a/cob_sick_s300/CMakeLists.txt +++ b/cob_sick_s300/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 2.8.3) project(cob_sick_s300) -find_package(catkin REQUIRED COMPONENTS diagnostic_msgs roscpp sensor_msgs std_msgs) +find_package(catkin REQUIRED COMPONENTS diagnostic_msgs roscpp sensor_msgs std_msgs serial) find_package(Boost REQUIRED COMPONENTS date_time thread) @@ -16,7 +16,6 @@ include_directories( add_executable(${PROJECT_NAME} common/src/ScannerSickS300.cpp - common/src/SerialIO.cpp ros/src/${PROJECT_NAME}.cpp ) @@ -26,6 +25,9 @@ add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) add_dependencies(cob_scan_filter ${catkin_EXPORTED_TARGETS}) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) +if(WIN32) + target_link_libraries(${PROJECT_NAME} ws2_32) +endif() target_link_libraries(cob_scan_filter ${catkin_LIBRARIES}) ### INSTALL ### diff --git a/cob_sick_s300/common/include/cob_sick_s300/ScannerSickS300.h b/cob_sick_s300/common/include/cob_sick_s300/ScannerSickS300.h index 9a6a523d7..d5f2bf633 100644 --- a/cob_sick_s300/common/include/cob_sick_s300/ScannerSickS300.h +++ b/cob_sick_s300/common/include/cob_sick_s300/ScannerSickS300.h @@ -27,7 +27,7 @@ #include #include -#include +#include #include /** @@ -119,7 +119,7 @@ class ScannerSickS300 bool m_bInStandby; // Components - SerialIO m_SerialIO; + serial::Serial m_SerialIO; TelegramParser tp_; // Functions diff --git a/cob_sick_s300/common/include/cob_sick_s300/SerialIO.h b/cob_sick_s300/common/include/cob_sick_s300/SerialIO.h deleted file mode 100644 index ba43be457..000000000 --- a/cob_sick_s300/common/include/cob_sick_s300/SerialIO.h +++ /dev/null @@ -1,214 +0,0 @@ -/* - * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -#ifndef _SerialIO_H -#define _SerialIO_H - -#include -#include - -#include -#include - -/** - * Wrapper class for serial communication. - */ -class SerialIO -{ -public: - // ---------------------- Constants - /// Constants for defining the handshake. - enum HandshakeFlags - { - HS_NONE, - HS_HARDWARE, - HS_XONXOFF - }; - - /// Constants for defining the parity bits. - enum ParityFlags - { - PA_NONE, - PA_EVEN, - PA_ODD, -// UNIX serial drivers only support even, odd, and no parity bit generation. - PA_MARK, - PA_SPACE - }; - - /// Constants for defining the stop bits. - enum StopBits - { - SB_ONE, - SB_ONE_5, // ????? returns an error ????? - SB_TWO - }; - - /// Default constructor - SerialIO(); - - /// Destructor - virtual ~SerialIO(); - - /** - * Sets the device name - * @param Name 'COM1', 'COM2', ... - */ - void setDeviceName(const char *Name) { m_DeviceName = Name; } - - /** - * Sets the baudrate. - * @param BaudRate baudrate. - */ - void setBaudRate(int BaudRate) { m_BaudRate = BaudRate; } - - /** - * Changes the baudrate. - * The serial port is allready open. - * @param BaudRate new baudrate. - */ - void changeBaudRate(int BaudRate); - - /** - * Sets a multiplier for the baudrate. - * Some serial cards need a specific multiplier for the baudrate. - * @param Multiplier default is one. - */ - void setMultiplier(double Multiplier = 1) { m_Multiplier = Multiplier; }; - - /** - * Sets the message format. - */ - void SetFormat(int ByteSize, ParityFlags Parity, int StopBits) - { m_ByteSize = ByteSize; m_Parity = Parity; m_StopBits = StopBits; } - - /** - * Defines the handshake type. - */ - void setHandshake(HandshakeFlags Handshake) { m_Handshake = Handshake; } - - /** - * Sets the buffer sizes. - * @param ReadBufSize number of bytes of the read buffer. - * @param WriteBufSize number of bytes of the write buffer. - */ - void setBufferSize(int ReadBufSize, int WriteBufSize) - { m_ReadBufSize = ReadBufSize; m_WriteBufSize = WriteBufSize; } - - /** - * Sets the timeout. - * @param Timeout in seconds - */ - void setTimeout(double Timeout); - - /** - * Sets the byte period for transmitting bytes. - * If the period is not equal to 0, the transmit will be repeated with the given - * period until all bytes are transmitted. - * @param default is 0. - */ - void setBytePeriod(double Period); - - /** - * Opens serial port. - * The port has to be configured before. - */ - int openIO(); - - /** - * Closes the serial port. - */ - void closeIO(); - - /** - * Reads the serial port blocking. - * The function blocks until the requested number of bytes have been - * read or the timeout occurs. - * @param Buffer pointer to the buffer. - * @param Length number of bytes to read - */ - int readBlocking(char *Buffer, int Length); - - - /** - * Reads the serial port non blocking. - * The function returns all avaiable bytes but not more than requested. - * @param Buffer pointer to the buffer. - * @param Length number of bytes to read - */ - int readNonBlocking(char *Buffer, int Length); - - /** - * Writes bytes to the serial port. - * @param Buffer buffer of the message - * @param Length number of bytes to send - */ - int writeIO(const char *Buffer, int Length); - - /** - * Returns the number of bytes available in the read buffer. - */ - int getSizeRXQueue(); - - - /** Clears the read and transmit buffer. - */ - void purge() - { - ::tcflush(m_Device, TCIOFLUSH); - } - - /** Clears the read buffer. - */ - void purgeRx() { - tcflush(m_Device, TCIFLUSH); -} - - /** - * Clears the transmit buffer. - * The content of the buffer will not be transmitted. - */ - void purgeTx() { - tcflush(m_Device, TCOFLUSH); - } - - /** - * Sends the transmit buffer. - * All bytes of the transmit buffer will be sent. - */ - void flushTx() { - tcdrain(m_Device); - } - -protected: - ::termios m_tio; - std::string m_DeviceName; - int m_Device; - int m_BaudRate; - double m_Multiplier; - int m_ByteSize, m_StopBits; - ParityFlags m_Parity; - HandshakeFlags m_Handshake; - int m_ReadBufSize, m_WriteBufSize; - double m_Timeout; - ::timeval m_BytePeriod; - bool m_ShortBytePeriod; -}; - - -#endif // - diff --git a/cob_sick_s300/common/include/cob_sick_s300/TelegramS300.h b/cob_sick_s300/common/include/cob_sick_s300/TelegramS300.h index 5238ca85b..b80b71d40 100644 --- a/cob_sick_s300/common/include/cob_sick_s300/TelegramS300.h +++ b/cob_sick_s300/common/include/cob_sick_s300/TelegramS300.h @@ -17,7 +17,11 @@ #pragma once +#ifdef _WIN32 +#include +#else #include +#endif /* * S300 header format in continuous mode: diff --git a/cob_sick_s300/common/src/ScannerSickS300.cpp b/cob_sick_s300/common/src/ScannerSickS300.cpp index 89371c1af..ce814fa84 100644 --- a/cob_sick_s300/common/src/ScannerSickS300.cpp +++ b/cob_sick_s300/common/src/ScannerSickS300.cpp @@ -95,7 +95,7 @@ ScannerSickS300::ScannerSickS300() //------------------------------------------- ScannerSickS300::~ScannerSickS300() { - m_SerialIO.closeIO(); + m_SerialIO.close(); } @@ -108,26 +108,25 @@ bool ScannerSickS300::open(const char* pcPort, int iBaudRate, int iScanId=7) m_iScanId = iScanId; // initialize Serial Interface - m_SerialIO.setBaudRate(iBaudRate); - m_SerialIO.setDeviceName(pcPort); - m_SerialIO.setBufferSize(READ_BUF_SIZE - 10 , WRITE_BUF_SIZE -10 ); - m_SerialIO.setHandshake(SerialIO::HS_NONE); - m_SerialIO.setMultiplier(m_dBaudMult); - bRetSerial = m_SerialIO.openIO(); - m_SerialIO.setTimeout(0.0); - m_SerialIO.SetFormat(8, SerialIO::PA_NONE, SerialIO::SB_ONE); - - if(bRetSerial == 0) - { - // Clears the read and transmit buffer. - m_iPosReadBuf2 = 0; - m_SerialIO.purge(); - return true; - } - else - { - return false; - } + try + { + m_SerialIO.setBaudrate(iBaudRate * m_dBaudMult); + m_SerialIO.setPort(pcPort); + m_SerialIO.setFlowcontrol(serial::flowcontrol_none); + m_SerialIO.setParity(serial::parity_none); + m_SerialIO.setStopbits(serial::stopbits_one); + m_SerialIO.setBytesize(serial::eightbits); + m_SerialIO.setTimeout(serial::Timeout::simpleTimeout(0)); + m_SerialIO.open(); + } + catch(...) + { + return false; + } + + // Clears the read and transmit buffer. + m_iPosReadBuf2 = 0; + return true; } @@ -135,7 +134,6 @@ bool ScannerSickS300::open(const char* pcPort, int iBaudRate, int iScanId=7) void ScannerSickS300::purgeScanBuf() { m_iPosReadBuf2 = 0; - m_SerialIO.purge(); } @@ -169,7 +167,7 @@ bool ScannerSickS300::getScan(std::vector &vdDistanceM, std::vector -#include -#include - -#include -#include -#include -#include -#include -#include - - -//#define _PRINT_BYTES - -/* -#ifdef _DEBUG -#define new DEBUG_NEW -#undef THIS_FILE -static char THIS_FILE[] = __FILE__; -#endif -*/ - - -bool getBaudrateCode(int iBaudrate, int* iBaudrateCode) -{ - // baudrate codes are defined in termios.h - // currently upto B1000000 - const int baudTable[] = { - 0, 50, 75, 110, 134, 150, 200, 300, 600, - 1200, 1800, 2400, 4800, - 9600, 19200, 38400, 57600, 115200, 230400, - 460800, 500000, 576000, 921600, 1000000 - }; - const int baudCodes[] = { - B0, B50, B75, B110, B134, B150, B200, B300, B600, - B1200, B1800, B2400, B4800, - B9600, B19200, B38400, B57600, B115200, B230400, - B460800, B500000, B576000, B921600, B1000000 - }; - const int iBaudsLen = sizeof(baudTable) / sizeof(int); - - bool ret = false; - *iBaudrateCode = B38400; - int i; - for( i=0; i= baudTable[iPos + 1]) - && iPos != 0) - { - if (iBaudrate < baudTable[iPos]) - { - iEnd = iPos; - } - else - { - iStart = iPos; - } - iPos = (iStart + iEnd) / 2; - } - - return baudCodes[iPos]; - */ - return ret; -} - - - - -////////////////////////////////////////////////////////////////////// -// Konstruktion/Destruktion -////////////////////////////////////////////////////////////////////// - -SerialIO::SerialIO() - : m_DeviceName(""), - m_Device(-1), - m_BaudRate(9600), - m_Multiplier(1.0), - m_ByteSize(8), - m_StopBits(SB_ONE), - m_Parity(PA_NONE), - m_Handshake(HS_NONE), - m_ReadBufSize(1024), - m_WriteBufSize(m_ReadBufSize), - m_Timeout(0), - m_ShortBytePeriod(false) -{ - m_BytePeriod.tv_sec = 0; - m_BytePeriod.tv_usec = 0; -} - -SerialIO::~SerialIO() -{ - closeIO(); -} - -int SerialIO::openIO() -{ - int Res; - - // open device - m_Device = open(m_DeviceName.c_str(), O_RDWR | O_NOCTTY /*| O_NONBLOCK*/); - - if(m_Device < 0) - { - //RF_ERR("Open " << m_DeviceName << " failed, error code " << errno); - std::cout << "Trying to open " << m_DeviceName << " failed: " - << strerror(errno) << " (Error code " << errno << ")" << std::endl; - - return -1; - } - - // set parameters - Res = tcgetattr(m_Device, &m_tio); - if (Res == -1) - { - std::cout << "tcgetattr of " << m_DeviceName << " failed: " - << strerror(errno) << " (Error code " << errno << ")" << std::endl; - - close(m_Device); - m_Device = -1; - - return -1; - } - - // Default values - m_tio.c_iflag = 0; - m_tio.c_oflag = 0; - m_tio.c_cflag = B9600 | CS8 | CREAD | HUPCL | CLOCAL; - m_tio.c_lflag = 0; - cfsetispeed(&m_tio, B9600); - cfsetospeed(&m_tio, B9600); - - m_tio.c_cc[VINTR] = 3; // Interrupt - m_tio.c_cc[VQUIT] = 28; // Quit - m_tio.c_cc[VERASE] = 127; // Erase - m_tio.c_cc[VKILL] = 21; // Kill-line - m_tio.c_cc[VEOF] = 4; // End-of-file - m_tio.c_cc[VTIME] = 0; // Time to wait for data (tenths of seconds) - m_tio.c_cc[VMIN] = 1; // Minimum number of characters to read - m_tio.c_cc[VSWTC] = 0; - m_tio.c_cc[VSTART] = 17; - m_tio.c_cc[VSTOP] = 19; - m_tio.c_cc[VSUSP] = 26; - m_tio.c_cc[VEOL] = 0; // End-of-line - m_tio.c_cc[VREPRINT] = 18; - m_tio.c_cc[VDISCARD] = 15; - m_tio.c_cc[VWERASE] = 23; - m_tio.c_cc[VLNEXT] = 22; - m_tio.c_cc[VEOL2] = 0; // Second end-of-line - - - // set baud rate - int iNewBaudrate = int(m_BaudRate * m_Multiplier + 0.5); - std::cout << "Setting Baudrate to " << iNewBaudrate << std::endl; - - int iBaudrateCode = 0; - bool bBaudrateValid = getBaudrateCode(iNewBaudrate, &iBaudrateCode); - - cfsetispeed(&m_tio, iBaudrateCode); - cfsetospeed(&m_tio, iBaudrateCode); - - if( !bBaudrateValid ) { - std::cout << "Baudrate code not available - setting baudrate directly" << std::endl; - struct serial_struct ss; - ioctl( m_Device, TIOCGSERIAL, &ss ); - ss.flags |= ASYNC_SPD_CUST; - ss.custom_divisor = ss.baud_base / iNewBaudrate; - ioctl( m_Device, TIOCSSERIAL, &ss ); - } - - - // set data format - m_tio.c_cflag &= ~CSIZE; - switch (m_ByteSize) - { - case 5: - m_tio.c_cflag |= CS5; - break; - case 6: - m_tio.c_cflag |= CS6; - break; - case 7: - m_tio.c_cflag |= CS7; - break; - case 8: - default: - m_tio.c_cflag |= CS8; - } - - m_tio.c_cflag &= ~ (PARENB | PARODD); - - switch (m_Parity) - { - case PA_ODD: - m_tio.c_cflag |= PARODD; - //break; // break must not be active here as we need the combination of PARODD and PARENB on odd parity. - - case PA_EVEN: - m_tio.c_cflag |= PARENB; - break; - - case PA_NONE: - default: {} - } - - switch (m_StopBits) - { - case SB_TWO: - m_tio.c_cflag |= CSTOPB; - break; - - case SB_ONE: - default: - m_tio.c_cflag &= ~CSTOPB; - } - - // hardware handshake - switch (m_Handshake) - { - case HS_NONE: - m_tio.c_cflag &= ~CRTSCTS; - m_tio.c_iflag &= ~(IXON | IXOFF | IXANY); - break; - case HS_HARDWARE: - m_tio.c_cflag |= CRTSCTS; - m_tio.c_iflag &= ~(IXON | IXOFF | IXANY); - break; - case HS_XONXOFF: - m_tio.c_cflag &= ~CRTSCTS; - m_tio.c_iflag |= (IXON | IXOFF | IXANY); - break; - } - - m_tio.c_oflag &= ~OPOST; - m_tio.c_lflag &= ~ICANON; - - // write parameters - Res = tcsetattr(m_Device, TCSANOW, &m_tio); - - if (Res == -1) - { - std::cout << "tcsetattr " << m_DeviceName << " failed: " - << strerror(errno) << " (Error code " << errno << ")" << std::endl; - - close(m_Device); - m_Device = -1; - - return -1; - } - - // set buffer sizes - // SetupComm(m_Device, m_ReadBufSize, m_WriteBufSize); - - // set timeout - setTimeout(m_Timeout); - - return 0; -} - -void SerialIO::closeIO() -{ - if (m_Device != -1) - { - close(m_Device); - m_Device = -1; - } -} - -void SerialIO::setTimeout(double Timeout) -{ - m_Timeout = Timeout; - if (m_Device != -1) - { - m_tio.c_cc[VTIME] = cc_t(ceil(m_Timeout * 10.0)); - tcsetattr(m_Device, TCSANOW, &m_tio); - } - -} - -void SerialIO::setBytePeriod(double Period) -{ - m_ShortBytePeriod = false; - m_BytePeriod.tv_sec = time_t(Period); - m_BytePeriod.tv_usec = suseconds_t((Period - m_BytePeriod.tv_sec) * 1000); -} - -//----------------------------------------------- -void SerialIO::changeBaudRate(int iBaudRate) -{ - /* - int iRetVal; - - m_BaudRate = iBaudRate; - - int iNewBaudrate = int(m_BaudRate * m_Multiplier + 0.5); - int iBaudrateCode = getBaudrateCode(iNewBaudrate); - cfsetispeed(&m_tio, iBaudrateCode); - cfsetospeed(&m_tio, iBaudrateCode); - - iRetVal = tcsetattr(m_Device, TCSANOW, &m_tio); - if(iRetVal == -1) - { - std::cout << "error in SerialCom::changeBaudRate()" << std::endl; - char c; - std::cin >> c; - exit(0); - }*/ -} - - -int SerialIO::readBlocking(char *Buffer, int Length) -{ - ssize_t BytesRead; - BytesRead = read(m_Device, Buffer, Length); -#ifdef PRINT_BYTES - printf("%2d Bytes read:", BytesRead); - for(int i=0; iroscpp sensor_msgs std_msgs + serial diff --git a/cob_sick_s300/ros/src/cob_sick_s300.cpp b/cob_sick_s300/ros/src/cob_sick_s300.cpp index 96eac2a7c..33ceceb95 100644 --- a/cob_sick_s300/ros/src/cob_sick_s300.cpp +++ b/cob_sick_s300/ros/src/cob_sick_s300.cpp @@ -356,7 +356,8 @@ int main(int argc, char** argv) ROS_ERROR("...scanner not available on port %s. Will retry every second.", nodeClass.port.c_str()); nodeClass.publishError("...scanner not available on port"); } - sleep(1); // wait for scan to get ready if successfull, or wait befor retrying + // wait for scan to get ready if successfull, or wait befor retrying + boost::this_thread::sleep_for(boost::chrono::seconds(1)); } ROS_INFO("...scanner opened successfully on port %s", nodeClass.port.c_str()); From aa3412b607f5a8d26b707e7816929bc21eba4fbf Mon Sep 17 00:00:00 2001 From: seanyen Date: Tue, 3 Dec 2019 01:41:06 -0800 Subject: [PATCH 2/2] Fix build break. --- cob_sick_s300/common/src/ScannerSickS300.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/cob_sick_s300/common/src/ScannerSickS300.cpp b/cob_sick_s300/common/src/ScannerSickS300.cpp index ce814fa84..e11dc817c 100644 --- a/cob_sick_s300/common/src/ScannerSickS300.cpp +++ b/cob_sick_s300/common/src/ScannerSickS300.cpp @@ -116,7 +116,8 @@ bool ScannerSickS300::open(const char* pcPort, int iBaudRate, int iScanId=7) m_SerialIO.setParity(serial::parity_none); m_SerialIO.setStopbits(serial::stopbits_one); m_SerialIO.setBytesize(serial::eightbits); - m_SerialIO.setTimeout(serial::Timeout::simpleTimeout(0)); + serial::Timeout timeout = serial::Timeout::simpleTimeout(0); + m_SerialIO.setTimeout(timeout); m_SerialIO.open(); } catch(...)