From a54276224412c989dc0c95efeef1a70088c6281d Mon Sep 17 00:00:00 2001 From: a1012112796 <1012112796@qq.com> Date: Fri, 3 Sep 2021 18:11:12 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BB=A3=E7=A0=81=E5=88=9D=E7=A8=BF?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: a1012112796 <1012112796@qq.com> --- .catkin_workspace | 1 + .gitignore | 4 + src/CMakeLists.txt | 1 + src/test_topic/CMakeLists.txt | 212 ++++++++++++++ .../include/test_topic/WzSerialportPlus.h | 71 +++++ .../include/test_topic/minPublisher.h | 21 ++ .../include/test_topic/minSubscriber.h | 21 ++ src/test_topic/package.xml | 65 +++++ src/test_topic/src/WzSerialportPlus.cpp | 269 ++++++++++++++++++ src/test_topic/src/minPublisher.cpp | 49 ++++ src/test_topic/src/minSubscriber.cpp | 33 +++ src/test_topic/src/timerPublisher.cpp | 54 ++++ 12 files changed, 801 insertions(+) create mode 100644 .catkin_workspace create mode 100644 .gitignore create mode 120000 src/CMakeLists.txt create mode 100644 src/test_topic/CMakeLists.txt create mode 100755 src/test_topic/include/test_topic/WzSerialportPlus.h create mode 100644 src/test_topic/include/test_topic/minPublisher.h create mode 100644 src/test_topic/include/test_topic/minSubscriber.h create mode 100644 src/test_topic/package.xml create mode 100644 src/test_topic/src/WzSerialportPlus.cpp create mode 100644 src/test_topic/src/minPublisher.cpp create mode 100644 src/test_topic/src/minSubscriber.cpp create mode 100644 src/test_topic/src/timerPublisher.cpp diff --git a/.catkin_workspace b/.catkin_workspace new file mode 100644 index 0000000..52fd97e --- /dev/null +++ b/.catkin_workspace @@ -0,0 +1 @@ +# This file currently only serves to mark the location of a catkin workspace for tool integration diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..b2b3844 --- /dev/null +++ b/.gitignore @@ -0,0 +1,4 @@ +.vscode/ +build/ +devel/ +*.o diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt new file mode 120000 index 0000000..2016816 --- /dev/null +++ b/src/CMakeLists.txt @@ -0,0 +1 @@ +/opt/ros/noetic/share/catkin/cmake/toplevel.cmake \ No newline at end of file diff --git a/src/test_topic/CMakeLists.txt b/src/test_topic/CMakeLists.txt new file mode 100644 index 0000000..0a62baf --- /dev/null +++ b/src/test_topic/CMakeLists.txt @@ -0,0 +1,212 @@ +cmake_minimum_required(VERSION 3.0.2) +project(test_topic) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + roscpp + std_msgs +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES test_topic +# CATKIN_DEPENDS roscpp std_msgs +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( include ${catkin_INCLUDE_DIRS}) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/test_topic.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +# add_executable(${PROJECT_NAME}_node src/test_topic_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_node +# ${catkin_LIBRARIES} +# ) + +add_executable(talker src/minPublisher.cpp src/WzSerialportPlus.cpp) +target_link_libraries(talker ${catkin_LIBRARIES}) + +add_executable(listener src/minSubscriber.cpp src/WzSerialportPlus.cpp) +target_link_libraries(listener ${catkin_LIBRARIES}) + +add_executable(time_tacker src/timerPublisher.cpp) +target_link_libraries(time_tacker ${catkin_LIBRARIES}) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# catkin_install_python(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html +# install(TARGETS ${PROJECT_NAME}_node +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark libraries for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html +# install(TARGETS ${PROJECT_NAME} +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +install(FILES + talker + listener + time_tacker + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_test_topic.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/src/test_topic/include/test_topic/WzSerialportPlus.h b/src/test_topic/include/test_topic/WzSerialportPlus.h new file mode 100755 index 0000000..5b49e16 --- /dev/null +++ b/src/test_topic/include/test_topic/WzSerialportPlus.h @@ -0,0 +1,71 @@ +/** + * @file WzSerialportPlus.h + * @author ouyangwei + * @brief library for serial port in linux + * @version 0.1 + * @date 2020-06-15 + * @copyright Copyright (c) 2020 + */ + +#ifndef __WZSERIALPORTPLUS_H__ +#define __WZSERIALPORTPLUS_H__ + +#include +#include +#include +#include + +using ReceiveCallback = std::function; + +class WzSerialportPlus +{ +public: + WzSerialportPlus(); + /** + * @param name: serialport name , such as: /dev/ttyS1 + * @param baudrate: baud rate , valid: 2400,4800,9600,19200,38400,57600,115200,230400 + * @param stopbit: stop bit , valid: 1,2 + * @param databit: data bit , valid: 7,8 + * @param paritybit: parity bit , valid: 'o'/'O' for odd,'e'/'E' for even,'n'/'N' for none + */ + WzSerialportPlus(const std::string& name, + const int& baudrate, + const int& stopbit, + const int& databit, + const int& paritybit); + ~WzSerialportPlus(); + + bool open(); + + /** + * @brief parameters same as constructor + */ + bool open(const std::string& name, + const int& baudrate, + const int& stopbit, + const int& databit, + const int& paritybit); + + void close(); + + int send(const char* data,int length); + + void setReceiveCalback(ReceiveCallback receiveCallback); + +protected: + virtual void onReceive(char* data,int length); + +private: + int serialportFd; + std::string name; + int baudrate; + int stopbit; + int databit; + int paritybit; + bool receivable; + int receiveMaxlength; + int receiveTimeout; /* unit: ms */ + ReceiveCallback receiveCallback; +}; + +#endif \ No newline at end of file diff --git a/src/test_topic/include/test_topic/minPublisher.h b/src/test_topic/include/test_topic/minPublisher.h new file mode 100644 index 0000000..e9a6e8a --- /dev/null +++ b/src/test_topic/include/test_topic/minPublisher.h @@ -0,0 +1,21 @@ +#ifndef __MIN_PUBLISHER_H__ +#define __MIN_PUBLISHER_H__ + +#include +#include +#include +#include "test_topic/WzSerialportPlus.h" + +using MsgType = std_msgs::String; +class MinPublisher: ros::NodeHandle { +public: + MinPublisher(); + ~MinPublisher(); + void run(const char *device); +private: + ros::Publisher _sender; + std::queue _queue; + WzSerialportPlus _serial; +}; + +#endif \ No newline at end of file diff --git a/src/test_topic/include/test_topic/minSubscriber.h b/src/test_topic/include/test_topic/minSubscriber.h new file mode 100644 index 0000000..53b8034 --- /dev/null +++ b/src/test_topic/include/test_topic/minSubscriber.h @@ -0,0 +1,21 @@ +#ifndef __MIN_SUBSCRIBER_H__ +#define __MIN_SUBSCRIBER_H__ + +#include +#include +#include +#include "test_topic/WzSerialportPlus.h" + +using MsgType = std_msgs::String; +class MinSubscriber: ros::NodeHandle { +public: + MinSubscriber(); + ~MinSubscriber(); + void run(const char *device); +private: + ros::Subscriber _sub; + WzSerialportPlus _serial; + void callback(const boost::shared_ptr &msg); +}; + +#endif diff --git a/src/test_topic/package.xml b/src/test_topic/package.xml new file mode 100644 index 0000000..15f067c --- /dev/null +++ b/src/test_topic/package.xml @@ -0,0 +1,65 @@ + + + test_topic + 0.0.0 + The test_topic package + + + + + zzc + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + std_msgs + roscpp + std_msgs + roscpp + std_msgs + + + + + + + + diff --git a/src/test_topic/src/WzSerialportPlus.cpp b/src/test_topic/src/WzSerialportPlus.cpp new file mode 100644 index 0000000..0570dec --- /dev/null +++ b/src/test_topic/src/WzSerialportPlus.cpp @@ -0,0 +1,269 @@ + +#include "test_topic/WzSerialportPlus.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +WzSerialportPlus::WzSerialportPlus() + : serialportFd(-1), + name(""), + baudrate(9600), + stopbit(1), + databit(8), + paritybit('n'), + receivable(false), + receiveMaxlength(2048), + receiveTimeout(5000), + receiveCallback(nullptr) +{ + +} + +WzSerialportPlus::WzSerialportPlus(const std::string& name, + const int& baudrate, + const int& stopbit, + const int& databit, + const int& paritybit): + serialportFd(-1), + name(name), + baudrate(baudrate), + stopbit(stopbit), + databit(databit), + paritybit(paritybit), + receivable(false), + receiveMaxlength(2048), + receiveTimeout(5000), + receiveCallback(nullptr) +{ + +} + +WzSerialportPlus::~WzSerialportPlus() +{ + close(); + + while(receivable) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + printf("[WzSerialportPlus::~WzSerialportPlus()]: destructed...\n"); +} + +bool WzSerialportPlus::open() +{ + serialportFd = ::open(name.c_str(), O_RDWR | O_NOCTTY | O_NDELAY); + if(serialportFd < 0) + { + printf("[WzSerialportPlus::open()]: open failed with serialportFd is %d , maybe permission denied or this serialport is opened!\n",serialportFd); + return false; + } + + if (fcntl(serialportFd, F_SETFL, 0) < 0) + { + printf("[WzSerialportPlus::open()]: open failed with fcntl failed!\n"); + return false; + } + + if (isatty(serialportFd) == 0) + { + printf("[WzSerialportPlus::open()]: open failed with standard input is not a terminal device!\n"); + ::close(serialportFd); + return false; + } + + struct termios newtio, oldtio; + if (tcgetattr(serialportFd, &oldtio) != 0) + { + printf("[WzSerialportPlus::open()]: open failed with tcgetattr failed!\n"); + return false; + } + + bzero(&newtio, sizeof(newtio)); + newtio.c_cflag |= CLOCAL | CREAD; + newtio.c_cflag &= ~CSIZE; + /* databit init */ + switch (databit) + { + case 7: + newtio.c_cflag |= CS7; + break; + case 8: + newtio.c_cflag |= CS8; + break; + default: + printf("[WzSerialportPlus::open()]: open failed with invalid databit %d!\n",databit); + return false; + } + /* paritybit init */ + switch (paritybit) + { + case 'o': + case 'O': + newtio.c_cflag |= PARENB; + newtio.c_cflag |= PARODD; + newtio.c_iflag |= (INPCK | ISTRIP); + break; + case 'e': + case 'E': + newtio.c_iflag |= (INPCK | ISTRIP); + newtio.c_cflag |= PARENB; + newtio.c_cflag &= ~PARODD; + break; + case 'n': + case 'N': + newtio.c_cflag &= ~PARENB; + break; + default: + printf("[WzSerialportPlus::open()]: open failed with invalid paritybit %d!\n",paritybit); + return false; + } + /* stopbit init */ + switch (stopbit) + { + case 1: + newtio.c_cflag &= ~CSTOPB; + break; + case 2: + newtio.c_cflag |= CSTOPB; + break; + default: + printf("[WzSerialportPlus::open()]: open failed with invalid stopbit %d!\n",stopbit); + return false; + } + /* baudrate init */ + switch (baudrate) + { + case 2400: + cfsetispeed(&newtio, B2400); + cfsetospeed(&newtio, B2400); + break; + case 4800: + cfsetispeed(&newtio, B4800); + cfsetospeed(&newtio, B4800); + break; + case 9600: + cfsetispeed(&newtio, B9600); + cfsetospeed(&newtio, B9600); + break; + case 19200: + cfsetispeed(&newtio, B19200); + cfsetospeed(&newtio, B19200); + break; + case 38400: + cfsetispeed(&newtio, B38400); + cfsetospeed(&newtio, B38400); + break; + case 57600: + cfsetispeed(&newtio, B57600); + cfsetospeed(&newtio, B57600); + break; + case 115200: + cfsetispeed(&newtio, B115200); + cfsetospeed(&newtio, B115200); + break; + case 230400: + cfsetispeed(&newtio, B230400); + cfsetospeed(&newtio, B230400); + break; + default: + printf("[WzSerialportPlus::open()]: open failed with invalid baudrate %d!\n",baudrate); + return false; + } + + newtio.c_cc[VTIME] = 1; + newtio.c_cc[VMIN] = 1; + + tcflush(serialportFd,TCIFLUSH); + if (tcsetattr(serialportFd, TCSANOW, &newtio) != 0) + { + printf("[WzSerialportPlus::open()]: open failed with tcgetattr failed!\n"); + return false; + } + + tcflush(serialportFd, TCIOFLUSH); + fcntl(serialportFd, F_SETFL, 0); + + receivable = true; + + std::thread([&]{ + char* receiveData = new char[receiveMaxlength]; + int receivedLength = 0; + int selectResult = -1; + + while (receivable) + { + memset(receiveData,0,receiveMaxlength); + receivedLength = read(serialportFd, receiveData, receiveMaxlength); /* block util data received */ + if(receivedLength > 0) + { + onReceive(receiveData,receivedLength); + if(nullptr != receiveCallback) + { + receiveCallback(receiveData,receivedLength); + } + } + receivedLength = 0; + } + + delete[] receiveData; + receiveData = nullptr; + }).detach(); + + printf("[WzSerialportPlus::open()]: open success.\n"); + return true; +} + +bool WzSerialportPlus::open(const std::string& name, + const int& baudrate, + const int& stopbit, + const int& databit, + const int& paritybit) +{ + this->name = name; + this->baudrate = baudrate; + this->stopbit = stopbit; + this->databit = databit; + this->paritybit = paritybit; + return open(); +} + +void WzSerialportPlus::close() +{ + if(receivable) + { + receivable = false; + } + + if(serialportFd >= 0) + { + ::close(serialportFd); + serialportFd = -1; + } +} + +int WzSerialportPlus::send(const char* data,int length) +{ + int lengthSent = 0; + lengthSent = write(serialportFd, data, length); + return lengthSent; +} + +void WzSerialportPlus::setReceiveCalback(ReceiveCallback receiveCallback) +{ + this->receiveCallback = receiveCallback; +} + +void WzSerialportPlus::onReceive(char* data,int length) +{ + +} diff --git a/src/test_topic/src/minPublisher.cpp b/src/test_topic/src/minPublisher.cpp new file mode 100644 index 0000000..dc04dbc --- /dev/null +++ b/src/test_topic/src/minPublisher.cpp @@ -0,0 +1,49 @@ +#include "test_topic/minPublisher.h" + +MinPublisher::MinPublisher() +{ + _sender = this->advertise("test_topic", 200); +} + +MinPublisher::~MinPublisher() +{ + _serial.close(); +} + +void MinPublisher::run(const char *device) +{ + _serial.setReceiveCalback([this](char* str,int length){ + MsgType msg; + std::stringstream ss; + ss << str; + + msg.data = ss.str(); + + this->_queue.push(msg); + }); + + _serial.open(device, 115200, 1, 8, 'N'); + + while (ros::ok()) + { + while(_queue.size() > 0) + { + MsgType msg = _queue.front(); + ROS_INFO("send: '%s'", msg.data.c_str()); + _sender.publish(msg); + _queue.pop(); + } + + usleep(50); + } +} + +int main(int argc, char *argv[]) +{ + ros::init(argc, argv, "test_pub_node"); + MinPublisher nh; + + nh.run("/dev/ttyUSB0"); + + return 0; +} diff --git a/src/test_topic/src/minSubscriber.cpp b/src/test_topic/src/minSubscriber.cpp new file mode 100644 index 0000000..3ec54e2 --- /dev/null +++ b/src/test_topic/src/minSubscriber.cpp @@ -0,0 +1,33 @@ +#include "test_topic/minSubscriber.h" + +MinSubscriber::MinSubscriber() +{ + _sub = this->subscribe("test_topic", 200, boost::bind(&MinSubscriber::callback, this, _1)); +} + +MinSubscriber::~MinSubscriber() +{ + _serial.close(); +} + +void MinSubscriber::callback(const boost::shared_ptr &msg) +{ + ROS_INFO("I heard: [%s]", msg->data.c_str()); + _serial.send(msg->data.c_str(), strlen(msg->data.c_str())); +} + +void MinSubscriber::run(const char *device) +{ + _serial.open(device, 115200, 1, 8, 'N'); + ros::spin(); +} + +int main(int argc, char *argv[]) +{ + ros::init(argc, argv, "test_sub_node"); + MinSubscriber nh; + + nh.run("/dev/ttyUSB0"); + + return 0; +} diff --git a/src/test_topic/src/timerPublisher.cpp b/src/test_topic/src/timerPublisher.cpp new file mode 100644 index 0000000..c8faa7f --- /dev/null +++ b/src/test_topic/src/timerPublisher.cpp @@ -0,0 +1,54 @@ +#include +#include + +using MsgType = std_msgs::String; +class TimerPublisher: ros::NodeHandle { +public: + TimerPublisher(); + ~TimerPublisher(); + void run(); +private: + ros::Publisher _sender; + int _count = 0; +}; + + +TimerPublisher::TimerPublisher() +{ + _sender = this->advertise("test_topic", 200); + _count = 0; +} + +TimerPublisher::~TimerPublisher() +{ + +} + +void TimerPublisher::run() +{ + ros::Rate loop_rate(10); + while (ros::ok()) + { + std_msgs::String msg; + + std::stringstream ss; + ss << "hello world " << _count++; + msg.data = ss.str(); + + ROS_INFO("%s", msg.data.c_str()); + _sender.publish(msg); + + ros::spinOnce(); + loop_rate.sleep(); + } +} + +int main(int argc, char *argv[]) +{ + ros::init(argc, argv, "timer_pub_node"); + TimerPublisher nh; + + nh.run(); + + return 0; +}