├── .gitignore ├── .github └── FUNDING.yml ├── src ├── node.cpp ├── main.cpp ├── factory.cpp ├── interface.cpp ├── interface_remote.cpp ├── implementation.cpp ├── port.cpp └── worker.cpp ├── include └── remote_serial │ ├── factory.hpp │ ├── node.hpp │ ├── utils.hpp │ ├── interface.hpp │ ├── port.hpp │ ├── worker.hpp │ ├── interface_remote.hpp │ └── implementation.hpp ├── package.xml ├── apache20.svg ├── CHANGELOG.rst ├── test ├── lib │ └── serial_test.py └── pipe1.py ├── CMakeLists.txt ├── README.md └── LICENSE.txt /.gitignore: -------------------------------------------------------------------------------- 1 | __pycache__ 2 | *.pyc 3 | -------------------------------------------------------------------------------- /.github/FUNDING.yml: -------------------------------------------------------------------------------- 1 | patreon: openvmp 2 | -------------------------------------------------------------------------------- /src/node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2022 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2022-09-24 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #include "remote_serial/node.hpp" 11 | 12 | namespace remote_serial { 13 | 14 | Node::Node() : rclcpp::Node::Node("serial") { 15 | impl_ = std::make_shared(this); 16 | } 17 | 18 | } // namespace remote_serial 19 | -------------------------------------------------------------------------------- /src/main.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2022 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2022-09-24 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #include "rclcpp/rclcpp.hpp" 11 | #include "remote_serial/node.hpp" 12 | 13 | int main(int argc, char **argv) { 14 | rclcpp::init(argc, argv); 15 | auto node = std::make_shared(); 16 | 17 | rclcpp::executors::MultiThreadedExecutor exec; 18 | exec.add_node(node); 19 | exec.spin(); 20 | 21 | rclcpp::shutdown(); 22 | return 0; 23 | } 24 | -------------------------------------------------------------------------------- /include/remote_serial/factory.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2022 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2022-10-01 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #ifndef OPENVMP_SERIAL_FACTORY_H 11 | #define OPENVMP_SERIAL_FACTORY_H 12 | 13 | #include 14 | 15 | #include "rclcpp/rclcpp.hpp" 16 | #include "remote_serial/interface.hpp" 17 | 18 | namespace remote_serial { 19 | 20 | class Factory { 21 | public: 22 | static std::shared_ptr New(rclcpp::Node *node); 23 | }; 24 | 25 | } // namespace remote_serial 26 | 27 | #endif // OPENVMP_SERIAL_FACTORY_H 28 | -------------------------------------------------------------------------------- /include/remote_serial/node.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2022 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2022-09-24 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #ifndef OPENVMP_SERIAL_NODE_H 11 | #define OPENVMP_SERIAL_NODE_H 12 | 13 | #include 14 | #include 15 | 16 | #include "rclcpp/rclcpp.hpp" 17 | #include "remote_serial/port.hpp" 18 | 19 | namespace remote_serial { 20 | 21 | class Node : public rclcpp::Node { 22 | public: 23 | Node(); 24 | 25 | private: 26 | std::shared_ptr impl_; 27 | }; 28 | 29 | } // namespace remote_serial 30 | 31 | #endif // OPENVMP_SERIAL_NODE_H 32 | -------------------------------------------------------------------------------- /src/factory.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2022 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2022-10-01 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #include "remote_serial/factory.hpp" 11 | 12 | #include 13 | 14 | #include "remote_serial/interface_remote.hpp" 15 | #include "remote_serial/port.hpp" 16 | 17 | namespace remote_serial { 18 | 19 | std::shared_ptr Factory::New(rclcpp::Node *node) { 20 | rclcpp::Parameter is_remote; 21 | node->declare_parameter("serial_is_remote", true); 22 | node->get_parameter("serial_is_remote", is_remote); 23 | 24 | if (is_remote.as_bool()) { 25 | return std::make_shared(node); 26 | } else { 27 | return std::make_shared(node); 28 | } 29 | } 30 | 31 | } // namespace remote_serial 32 | -------------------------------------------------------------------------------- /include/remote_serial/utils.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2022 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2022-09-24 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #ifndef OPENVMP_SERIAL_UTILS_H 11 | #define OPENVMP_SERIAL_UTILS_H 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | namespace remote_serial { 18 | 19 | namespace utils { 20 | 21 | static inline std::string bin2hex(const std::string &bin) { 22 | std::stringstream ss; 23 | 24 | ss << std::hex; 25 | 26 | for (size_t i = 0; i < bin.size(); i++) { 27 | ss << std::setfill('0') << std::setw(2) << (int)(uint8_t)((int)bin[i]) 28 | << " "; 29 | } 30 | return ss.str(); 31 | } 32 | 33 | } // namespace utils 34 | 35 | } // namespace remote_serial 36 | 37 | #endif // OPENVMP_SERIAL_UTILS_H 38 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | remote_serial 5 | 1.0.1 6 | Interface to serial lines 7 | Roman Kuzmenko 8 | Apache License 2.0 9 | 10 | ament_cmake 11 | rclcpp 12 | std_msgs 13 | std_srvs 14 | 15 | ament_lint_auto 16 | ament_lint_common 17 | 18 | rosidl_default_generators 19 | action_msgs 20 | rosidl_default_runtime 21 | rosidl_interface_packages 22 | 23 | 24 | ament_cmake 25 | 26 | -------------------------------------------------------------------------------- /apache20.svg: -------------------------------------------------------------------------------- 1 | License: Apache 2.0LicenseApache 2.0 -------------------------------------------------------------------------------- /CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package remote_serial 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.0.1 (2023-05-06) 6 | ------------------ 7 | 8 | 1.0.0 (2023-05-01) 9 | ------------------ 10 | * Renamed the package to better align with REP-0144 guidelines 11 | * Fixed missing class member initialization for the remote interface. Minor performance improvements. 12 | * Compiles on foxy 13 | * Refactored the boundary between the serial protocol implementation class and the serial port class as deemed necessary by an implementation of UART on a microcontroller. 14 | * Flow control refactoring. Reduced overhead from debug logging. 15 | * Refactored DDS path for all resources 16 | * Work around a timing issue in the test cases 17 | * Interface is refactored to support a factory and to coexist with other OpenVMP modules 18 | * Made it work as a dependency for other packages. Made it work with binary data. 19 | * updated the documentation to reflect the recent internal terminology change 20 | * added the first test case and made it work 21 | * First version that compiles 22 | * added the openvmp banner 23 | * added README.md 24 | * Initial revision 25 | * Contributors: Roman Kuzmenko 26 | -------------------------------------------------------------------------------- /src/interface.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2022 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2022-10-01 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #include "remote_serial/interface.hpp" 11 | 12 | #include 13 | 14 | namespace remote_serial { 15 | 16 | Interface::Interface(rclcpp::Node *node, const std::string &default_prefix) 17 | : node_{node} { 18 | auto prefix = default_prefix; 19 | if (prefix == "") { 20 | prefix = "/serial/" + std::string(node_->get_name()); 21 | } 22 | 23 | int index = 0; 24 | std::string parameter_name; 25 | do { 26 | parameter_name = "serial_prefix"; 27 | if (index++ != 0) { 28 | parameter_name += "_" + std::to_string(index); 29 | } 30 | } while (node->has_parameter(parameter_name)); 31 | 32 | node->declare_parameter(parameter_name, prefix); 33 | node->get_parameter(parameter_name, interface_prefix_); 34 | } 35 | 36 | std::string Interface::get_prefix_() { 37 | std::string prefix = std::string(node_->get_namespace()); 38 | if (prefix.length() > 0 && prefix[prefix.length() - 1] == '/') { 39 | prefix = prefix.substr(0, prefix.length() - 1); 40 | } 41 | prefix += interface_prefix_.as_string(); 42 | return prefix; 43 | } 44 | 45 | } // namespace remote_serial 46 | -------------------------------------------------------------------------------- /include/remote_serial/interface.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2022 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2022-09-24 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #ifndef OPENVMP_SERIAL_INTERFACE_H 11 | #define OPENVMP_SERIAL_INTERFACE_H 12 | 13 | #include 14 | #include 15 | 16 | #include "rclcpp/rclcpp.hpp" 17 | 18 | #define SERIAL_TOPIC_INPUT "/inspect/input" 19 | #define SERIAL_TOPIC_OUTPUT "/inspect/output" 20 | #define SERIAL_TOPIC_INJECT_INPUT "/inject/input" 21 | #define SERIAL_TOPIC_INJECT_OUTPUT "/inject/output" 22 | #define SERIAL_SERVICE_FLUSH "/flush" 23 | 24 | namespace remote_serial { 25 | 26 | class Interface { 27 | public: 28 | Interface(rclcpp::Node *node, const std::string &default_prefix = ""); 29 | virtual ~Interface() {} 30 | 31 | // output writes bytes to the serial line 32 | virtual void output(const std::string &) = 0; 33 | 34 | // register_input_cb is used to set the callback function which 35 | // is called every time data is received from the serial line 36 | virtual void register_input_cb(void (*)(const std::string &msg, 37 | void *user_data), 38 | void *user_data) = 0; 39 | 40 | virtual void inject_input(const std::string &) = 0; 41 | 42 | protected: 43 | rclcpp::Node *node_; 44 | 45 | std::string get_prefix_(); 46 | 47 | private: 48 | rclcpp::Parameter interface_prefix_; 49 | }; 50 | 51 | } // namespace remote_serial 52 | 53 | #endif // OPENVMP_SERIAL_INTERFACE_H 54 | -------------------------------------------------------------------------------- /test/lib/serial_test.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from rclpy.node import Node as rclpyNode 3 | 4 | from remote_serial.srv import InjectOutput 5 | import std_msgs.msg 6 | 7 | 8 | class SerialTesterNode(rclpyNode): 9 | test_context = {} 10 | 11 | def __init__(self, name="serial_tester_node"): 12 | super().__init__(name) 13 | 14 | def _save(self, id, direction, msg): 15 | print("Data received on com" + str(id) + " on " + direction) 16 | if not id in self.test_context: 17 | self.test_context[id] = {} 18 | if not direction in self.test_context[id]: 19 | self.test_context[id][direction] = "" 20 | self.test_context[id][direction] += msg.data 21 | 22 | def subscribe( 23 | self, 24 | id=1, 25 | direction="input", 26 | ): 27 | this_node = self 28 | subscription = self.create_subscription( 29 | std_msgs.msg.String, 30 | "/serial/com" + str(id) + "/inspect/" + direction, 31 | lambda msg: this_node._save(id, direction, msg), 32 | 10, 33 | ) 34 | return subscription 35 | 36 | def inject( 37 | self, id=1, direction="output", text=bytes("test", "latin1"), timeout=10.0 38 | ): 39 | client = self.create_client( 40 | InjectOutput, "/serial/com" + str(id) + "/inject/" + str(direction) 41 | ) 42 | ready = client.wait_for_service(timeout_sec=timeout) 43 | if not ready: 44 | raise RuntimeError("Wait for service timed out") 45 | 46 | request = InjectOutput.Request() 47 | request.data = str(text, "latin_1", "ignore") 48 | future = client.call_async(request) 49 | return future 50 | -------------------------------------------------------------------------------- /include/remote_serial/port.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2022 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2022-09-24 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #ifndef OPENVMP_SERIAL_PORT_H 11 | #define OPENVMP_SERIAL_PORT_H 12 | 13 | #include 14 | #include 15 | 16 | #include "rclcpp/rclcpp.hpp" 17 | #include "remote_serial/implementation.hpp" 18 | 19 | namespace remote_serial { 20 | 21 | class PortSettings { 22 | public: 23 | rclcpp::Parameter dev_name; 24 | rclcpp::Parameter skip_init; 25 | rclcpp::Parameter baud_rate; 26 | rclcpp::Parameter data; 27 | rclcpp::Parameter parity; 28 | rclcpp::Parameter stop; 29 | rclcpp::Parameter flow_control; 30 | rclcpp::Parameter sw_flow_control; 31 | 32 | static const int BUFFER_SIZE_MAX = 1048576; // 1MB 33 | rclcpp::Parameter bs; 34 | 35 | int setup(int old_fd); 36 | }; 37 | 38 | class Worker; 39 | 40 | class Port final : public Implementation { 41 | friend Worker; // Let the Worker class access 'node_' 42 | 43 | public: 44 | Port(rclcpp::Node *node); 45 | 46 | virtual void output(const std::string &msg) override; 47 | 48 | // having pure pointers would improve performance here 49 | // but it would be against the religion of so many 50 | virtual void register_input_cb(void (*input_cb)(const std::string &msg, 51 | void *user_data), 52 | void *user_data) override; 53 | 54 | virtual void inject_input(const std::string &msg) override; 55 | 56 | private: 57 | // node parameters 58 | std::shared_ptr port_settings_; 59 | 60 | // port worker 61 | std::shared_ptr worker_; 62 | }; 63 | 64 | } // namespace remote_serial 65 | 66 | #endif // OPENVMP_SERIAL_PORT_H 67 | -------------------------------------------------------------------------------- /include/remote_serial/worker.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2022 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2022-09-24 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #ifndef OPENVMP_SERIAL_WORKER_H 11 | #define OPENVMP_SERIAL_WORKER_H 12 | 13 | #include 14 | #include 15 | 16 | #include "rclcpp/logger.hpp" 17 | #include "rclcpp/rclcpp.hpp" 18 | #include "remote_serial/interface.hpp" 19 | #include "remote_serial/port.hpp" 20 | 21 | namespace remote_serial { 22 | 23 | class Worker final { 24 | public: 25 | Worker(Port *impl, std::shared_ptr settings); 26 | virtual ~Worker(); 27 | 28 | void stop(); 29 | 30 | /// @brief Place the byte array into the send queue for this serial line 31 | /// @param msg String object containing the byte array to be written to the 32 | /// serial line 33 | void output(const std::string &msg); 34 | void register_input_cb(void (*cb)(const std::string &, void *), 35 | void *user_data); 36 | void inject_input(const std::string &msg); 37 | 38 | private: 39 | // port 40 | Port *impl_; 41 | std::shared_ptr settings_; 42 | int fd_; 43 | 44 | // thread 45 | std::shared_ptr thread_; 46 | int signal_[2]; 47 | volatile bool do_stop_; 48 | void run_(); 49 | 50 | // queues 51 | std::vector> 54 | output_queue_; 55 | std::mutex output_queue_mutex_; 56 | 57 | // callbacks 58 | void (*volatile input_cb_)(const std::string &msg, void *user_data); 59 | void *volatile input_cb_user_data_; 60 | std::mutex input_cb_mutex_; 61 | 62 | // misc 63 | const rclcpp::Logger logger_; 64 | }; 65 | 66 | } // namespace remote_serial 67 | 68 | #endif // OPENVMP_SERIAL_WORKER_H 69 | -------------------------------------------------------------------------------- /include/remote_serial/interface_remote.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2022 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2022-10-01 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #ifndef OPENVMP_SERIAL_INTERFACE_REMOTE_H 11 | #define OPENVMP_SERIAL_INTERFACE_REMOTE_H 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | #include "rclcpp/callback_group.hpp" 18 | #include "rclcpp/rclcpp.hpp" 19 | #include "remote_serial/interface.hpp" 20 | #include "std_msgs/msg/empty.hpp" 21 | #include "std_msgs/msg/u_int8_multi_array.hpp" 22 | #include "std_srvs/srv/empty.hpp" 23 | 24 | namespace remote_serial { 25 | 26 | class RemoteInterface final : public Interface { 27 | public: 28 | RemoteInterface(rclcpp::Node *node); 29 | virtual ~RemoteInterface() {} 30 | 31 | virtual void output(const std::string &) override; 32 | virtual void inject_input(const std::string &) override; 33 | 34 | // having pure pointers would improve performance here 35 | // but it would be against the religion of so many 36 | virtual void register_input_cb(void (*)(const std::string &msg, 37 | void *user_data), 38 | void *user_data) override; 39 | 40 | protected: 41 | void input_handler(const std_msgs::msg::UInt8MultiArray::SharedPtr); 42 | 43 | private: 44 | std::mutex input_mutex_; 45 | void (*input_cb_)(const std::string &msg, void *user_data); 46 | void *input_cb_user_data_; 47 | rclcpp::CallbackGroup::SharedPtr callback_group_; 48 | 49 | rclcpp::Subscription::SharedPtr sub_input_; 50 | 51 | rclcpp::Publisher::SharedPtr 52 | pub_inject_input_; 53 | rclcpp::Publisher::SharedPtr 54 | pub_inject_output_; 55 | rclcpp::Client::SharedPtr clnt_flush_; 56 | }; 57 | 58 | } // namespace remote_serial 59 | 60 | #endif // OPENVMP_SERIAL_INTERFACE_REMOTE_H 61 | -------------------------------------------------------------------------------- /include/remote_serial/implementation.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2022 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2022-09-24 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #ifndef OPENVMP_SERIAL_IMPLEMENTATION_H 11 | #define OPENVMP_SERIAL_IMPLEMENTATION_H 12 | 13 | #include 14 | #include 15 | 16 | #include "rclcpp/rclcpp.hpp" 17 | #include "remote_serial/interface.hpp" 18 | #include "std_msgs/msg/empty.hpp" 19 | #include "std_msgs/msg/u_int8_multi_array.hpp" 20 | #include "std_srvs/srv/empty.hpp" 21 | 22 | namespace remote_serial { 23 | 24 | class Implementation : public Interface { 25 | public: 26 | Implementation(rclcpp::Node *node, const std::string &default_prefix = ""); 27 | 28 | rclcpp::Publisher::SharedPtr inspect_output; 29 | rclcpp::Publisher::SharedPtr inspect_input; 30 | 31 | protected: 32 | // init_serial is called by child constructors or other child members 33 | // when all the downstream modules are initialized and this modules is 34 | // ready to take requests from the upstreams. 35 | void init_serial_(); 36 | 37 | private: 38 | rclcpp::CallbackGroup::SharedPtr callback_group_; 39 | 40 | // topics 41 | rclcpp::Publisher::SharedPtr publisher_input_; 42 | rclcpp::Publisher::SharedPtr 43 | publisher_output_; 44 | 45 | rclcpp::Subscription::SharedPtr 46 | sub_inject_input_; 47 | rclcpp::Subscription::SharedPtr 48 | sub_inject_output_; 49 | rclcpp::Service::SharedPtr srv_flush_; 50 | 51 | void flush_handler_( 52 | const std::shared_ptr request, 53 | std::shared_ptr response); 54 | void inject_input_handler_(const std_msgs::msg::UInt8MultiArray::SharedPtr); 55 | void inject_output_handler_(const std_msgs::msg::UInt8MultiArray::SharedPtr); 56 | }; 57 | 58 | } // namespace remote_serial 59 | 60 | #endif // OPENVMP_SERIAL_IMPLEMENTATION_H 61 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(remote_serial) 3 | 4 | # Compiler settings 5 | if(NOT CMAKE_CXX_STANDARD) 6 | set(CMAKE_CXX_STANDARD 17) 7 | endif() 8 | 9 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 10 | add_compile_options(-Wall -Wextra -Wpedantic) 11 | endif() 12 | add_compile_options(-fPIC) 13 | 14 | # Dependencies 15 | find_package(ament_cmake REQUIRED) 16 | find_package(rclcpp REQUIRED) 17 | find_package(std_msgs REQUIRED) 18 | find_package(std_srvs REQUIRED) 19 | find_package(rosidl_default_generators REQUIRED) 20 | 21 | # Shared by all targets 22 | include_directories( 23 | include 24 | ) 25 | 26 | set(project_SOURCE_FILES 27 | src/node.cpp 28 | src/worker.cpp 29 | src/port.cpp 30 | src/implementation.cpp 31 | src/interface.cpp 32 | src/interface_remote.cpp 33 | src/factory.cpp 34 | ) 35 | 36 | set(project_DEPENDENCIES 37 | rclcpp 38 | std_msgs 39 | std_srvs 40 | ) 41 | 42 | # Executable target 43 | add_executable(${PROJECT_NAME}_standalone src/main.cpp ${project_SOURCE_FILES}) 44 | ament_target_dependencies(${PROJECT_NAME}_standalone ${project_DEPENDENCIES}) 45 | install(TARGETS 46 | ${PROJECT_NAME}_standalone 47 | DESTINATION lib/${PROJECT_NAME} 48 | ) 49 | 50 | # Library target 51 | add_library(${PROJECT_NAME}_native ${project_SOURCE_FILES}) 52 | ament_target_dependencies(${PROJECT_NAME}_native ${project_DEPENDENCIES}) 53 | ament_export_targets(${PROJECT_NAME}_native_library HAS_LIBRARY_TARGET) 54 | ament_export_dependencies(${project_DEPENDENCIES}) 55 | ament_export_include_directories(include) 56 | install(TARGETS ${PROJECT_NAME}_native 57 | EXPORT ${PROJECT_NAME}_native_library 58 | DESTINATION lib 59 | ) 60 | install( 61 | DIRECTORY include/${PROJECT_NAME}/ 62 | DESTINATION include/${PROJECT_NAME} 63 | ) 64 | 65 | 66 | # Testing 67 | if(BUILD_TESTING) 68 | find_package(ament_lint_auto REQUIRED) 69 | # the following line skips the linter which checks for copyrights 70 | # comment the line when a copyright and license is added to all source files 71 | set(ament_cmake_copyright_FOUND TRUE) 72 | # the following line skips cpplint (only works in a git repo) 73 | # comment the line when this package is in a git repo and when 74 | # a copyright and license is added to all source files 75 | set(ament_cmake_cpplint_FOUND TRUE) 76 | ament_lint_auto_find_test_dependencies() 77 | endif() 78 | 79 | ament_export_dependencies(rosidl_default_runtime) 80 | ament_package() 81 | -------------------------------------------------------------------------------- /src/interface_remote.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2022 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2022-10-01 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #include "remote_serial/interface_remote.hpp" 11 | 12 | #include 13 | 14 | namespace remote_serial { 15 | 16 | RemoteInterface::RemoteInterface(rclcpp::Node *node) 17 | : Interface(node), input_cb_{nullptr}, input_cb_user_data_{nullptr} { 18 | callback_group_ = 19 | node->create_callback_group(rclcpp::CallbackGroupType::Reentrant); 20 | 21 | auto prefix = get_prefix_(); 22 | 23 | RCLCPP_DEBUG(node_->get_logger(), 24 | "serial::RemoteInterface::RemoteInterface(): Connecting to the " 25 | "remote interface: %s", 26 | prefix.c_str()); 27 | 28 | sub_input_ = node->create_subscription( 29 | prefix + SERIAL_TOPIC_INPUT, 10, 30 | std::bind(&RemoteInterface::input_handler, this, std::placeholders::_1)); 31 | 32 | pub_inject_input_ = node->create_publisher( 33 | prefix + SERIAL_TOPIC_INJECT_INPUT, 10); 34 | pub_inject_output_ = node->create_publisher( 35 | prefix + SERIAL_TOPIC_INJECT_OUTPUT, 10); 36 | clnt_flush_ = node->create_client( 37 | prefix + SERIAL_SERVICE_FLUSH, ::rmw_qos_profile_default, 38 | callback_group_); 39 | 40 | clnt_flush_->wait_for_service(); 41 | 42 | RCLCPP_DEBUG(node_->get_logger(), "Connected to the remote interface: %s", 43 | prefix.c_str()); 44 | } 45 | 46 | void RemoteInterface::output(const std::string &data) { 47 | auto msg = std_msgs::msg::UInt8MultiArray(); 48 | msg.data = std::vector(data.begin(), data.end()); 49 | pub_inject_output_->publish(msg); 50 | } 51 | 52 | void RemoteInterface::inject_input(const std::string &data) { 53 | auto msg = std_msgs::msg::UInt8MultiArray(); 54 | msg.data = std::vector(data.begin(), data.end()); 55 | pub_inject_input_->publish(msg); 56 | } 57 | 58 | // having pure pointers would improve performance here 59 | // but it would be against the religion of so many 60 | void RemoteInterface::register_input_cb(void (*input_cb)(const std::string &msg, 61 | void *user_data), 62 | void *user_data) { 63 | input_mutex_.lock(); 64 | input_cb_ = input_cb; 65 | input_cb_user_data_ = user_data; 66 | input_mutex_.unlock(); 67 | } 68 | 69 | void RemoteInterface::input_handler( 70 | const std_msgs::msg::UInt8MultiArray::SharedPtr request) { 71 | input_mutex_.lock(); 72 | auto str = std::string(request->data.begin(), request->data.end()); 73 | if (input_cb_) input_cb_(str, input_cb_user_data_); 74 | input_mutex_.unlock(); 75 | } 76 | 77 | } // namespace remote_serial 78 | -------------------------------------------------------------------------------- /src/implementation.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2022 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2022-09-24 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #include "remote_serial/implementation.hpp" 11 | 12 | #include "remote_serial/utils.hpp" 13 | #include "remote_serial/worker.hpp" 14 | 15 | namespace remote_serial { 16 | 17 | Implementation::Implementation(rclcpp::Node *node, 18 | const std::string &default_prefix) 19 | : Interface(node, default_prefix) { 20 | callback_group_ = 21 | node->create_callback_group(rclcpp::CallbackGroupType::Reentrant); 22 | } 23 | 24 | void Implementation::init_serial_() { 25 | auto prefix = get_prefix_(); 26 | inspect_input = node_->create_publisher( 27 | prefix + SERIAL_TOPIC_INPUT, 10); 28 | inspect_output = node_->create_publisher( 29 | prefix + SERIAL_TOPIC_OUTPUT, 10); 30 | 31 | sub_inject_input_ = 32 | node_->create_subscription( 33 | prefix + SERIAL_TOPIC_INJECT_INPUT, 10, 34 | std::bind(&Implementation::inject_input_handler_, this, 35 | std::placeholders::_1)); 36 | sub_inject_output_ = 37 | node_->create_subscription( 38 | prefix + SERIAL_TOPIC_INJECT_OUTPUT, 10, 39 | std::bind(&Implementation::inject_output_handler_, this, 40 | std::placeholders::_1)); 41 | 42 | srv_flush_ = node_->create_service( 43 | prefix + SERIAL_SERVICE_FLUSH, 44 | std::bind(&Implementation::flush_handler_, this, std::placeholders::_1, 45 | std::placeholders::_2), 46 | ::rmw_qos_profile_default, callback_group_); 47 | } 48 | 49 | void Implementation::inject_input_handler_( 50 | const std_msgs::msg::UInt8MultiArray::SharedPtr msg) { 51 | auto str = std::string(msg->data.begin(), msg->data.end()); 52 | RCLCPP_DEBUG(node_->get_logger(), "Incoming request to inject input data: %s", 53 | utils::bin2hex(str).c_str()); 54 | inject_input(str); 55 | 56 | auto message = std_msgs::msg::UInt8MultiArray(); 57 | message.data = msg->data; 58 | inspect_input->publish(message); 59 | } 60 | 61 | void Implementation::inject_output_handler_( 62 | const std_msgs::msg::UInt8MultiArray::SharedPtr msg) { 63 | auto str = std::string(msg->data.begin(), msg->data.end()); 64 | RCLCPP_DEBUG(node_->get_logger(), 65 | "Incoming request to inject output data: %s", 66 | utils::bin2hex(str).c_str()); 67 | output(str); 68 | } 69 | 70 | void Implementation::flush_handler_( 71 | const std::shared_ptr request, 72 | std::shared_ptr response) { 73 | (void)request; 74 | (void)response; 75 | } 76 | 77 | } // namespace remote_serial 78 | -------------------------------------------------------------------------------- /test/pipe1.py: -------------------------------------------------------------------------------- 1 | import sys 2 | 3 | sys.path.append("test/lib") 4 | 5 | from serial_test import SerialTesterNode 6 | 7 | import os 8 | import tempfile 9 | from time import sleep 10 | 11 | import pytest 12 | import unittest 13 | 14 | import rclpy 15 | from launch import LaunchDescription 16 | from launch.actions import ( 17 | RegisterEventHandler, 18 | ExecuteProcess, 19 | ) 20 | from launch_testing.actions import ReadyToTest 21 | import launch_testing.markers 22 | from launch_ros.actions import Node 23 | from launch.event_handlers import OnProcessStart 24 | from launch.substitutions import FindExecutable 25 | 26 | 27 | TTY1 = "/tmp/ttyS21" 28 | TTY2 = "/tmp/ttyS22" 29 | 30 | 31 | @pytest.mark.launch_test 32 | @launch_testing.markers.keep_alive 33 | def generate_test_description(): 34 | socat = ExecuteProcess( 35 | name="socat", 36 | cmd=[ 37 | [ 38 | FindExecutable(name="socat"), 39 | " -s", 40 | " PTY,rawer,link=", 41 | TTY1, 42 | " PTY,rawer,link=", 43 | TTY2, 44 | ] 45 | ], 46 | shell=True, 47 | ) 48 | node1 = Node( 49 | name="serial_com1", 50 | package="remote_serial", 51 | executable="remote_serial_standalone", 52 | # arguments=["--ros-args", "--log-level", "debug"], 53 | parameters=[ 54 | { 55 | "serial_is_remote": False, 56 | "serial_prefix": "/serial/com1", 57 | "serial_dev_name": TTY1, 58 | # "serial_skip_init": True, 59 | "serial_baud_rate": 115200, 60 | "serial_data": 8, 61 | "serial_parity": False, 62 | "serial_stop": 1, 63 | "serial_flow_control": True, 64 | } 65 | ], 66 | output="screen", 67 | ) 68 | node2 = Node( 69 | name="serial_com2", 70 | package="remote_serial", 71 | executable="remote_serial_standalone", 72 | # arguments=["--ros-args", "--log-level", "debug"], 73 | parameters=[ 74 | { 75 | "serial_is_remote": False, 76 | "serial_prefix": "/serial/com2", 77 | "serial_dev_name": TTY2, 78 | # "serial_skip_init": True, 79 | "serial_baud_rate": 115200, 80 | "serial_data": 8, 81 | "serial_parity": False, 82 | "serial_stop": 1, 83 | "serial_flow_control": True, 84 | } 85 | ], 86 | output="screen", 87 | ) 88 | 89 | return ( 90 | LaunchDescription( 91 | [ 92 | socat, 93 | RegisterEventHandler( 94 | event_handler=OnProcessStart( 95 | target_action=socat, 96 | on_start=[ 97 | node1, 98 | node2, 99 | ], 100 | ) 101 | ), 102 | ReadyToTest(), 103 | ] 104 | ), 105 | {"socat": socat, "serial_com1": node1, "serial_com2": node2}, 106 | ) 107 | 108 | 109 | class TestInjectOutput(unittest.TestCase): 110 | def test_basic(self, proc_output): 111 | rclpy.init() 112 | try: 113 | sleep(3) 114 | node = SerialTesterNode("test_node") 115 | node.subscribe(1) 116 | node.subscribe(2) 117 | node.subscribe(1, "output") 118 | node.subscribe(2, "output") 119 | sleep(1) 120 | 121 | future = node.inject(1) 122 | sleep(1) 123 | rclpy.spin_until_future_complete(node, future, timeout_sec=10.0) 124 | 125 | assert future.done(), "Client request timed out" 126 | _response = future.result() 127 | # assert response, "Could not inject!" 128 | sleep(3) 129 | 130 | assert node.test_context[1]["output"] == "test", ( 131 | "com1 output mismatch: " + node.test_context[1]["output"] 132 | ) 133 | assert node.test_context[2]["input"] == "test", ( 134 | "com2 input mismatch: " + node.test_context[2]["input"] 135 | ) 136 | finally: 137 | rclpy.shutdown() 138 | _ignore = 1 139 | -------------------------------------------------------------------------------- /src/port.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2022 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2022-09-24 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #define _DEFAULT_SOURCE 11 | #include "remote_serial/port.hpp" 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | #include "remote_serial/worker.hpp" 22 | 23 | namespace remote_serial { 24 | 25 | Port::Port(rclcpp::Node *node) 26 | : Implementation(node), port_settings_(new PortSettings()) { 27 | node_->declare_parameter("serial_dev_name", "/dev/ttyS0"); 28 | node_->declare_parameter("serial_skip_init", false); 29 | node_->declare_parameter("serial_baud_rate", 115200); 30 | node_->declare_parameter("serial_data", 8); 31 | node_->declare_parameter("serial_parity", false); 32 | node_->declare_parameter("serial_stop", 1); 33 | node_->declare_parameter("serial_flow_control", false); 34 | node_->declare_parameter("serial_sw_flow_control", false); 35 | node_->declare_parameter("serial_bs", 1024); 36 | node_->get_parameter("serial_dev_name", port_settings_->dev_name); 37 | node_->get_parameter("serial_skip_init", port_settings_->skip_init); 38 | node_->get_parameter("serial_baud_rate", port_settings_->baud_rate); 39 | node_->get_parameter("serial_data", port_settings_->data); 40 | node_->get_parameter("serial_stop", port_settings_->stop); 41 | node_->get_parameter("serial_parity", port_settings_->parity); 42 | node_->get_parameter("serial_flow_control", port_settings_->flow_control); 43 | node_->get_parameter("serial_sw_flow_control", 44 | port_settings_->sw_flow_control); 45 | node_->get_parameter("serial_bs", port_settings_->bs); 46 | 47 | RCLCPP_INFO(node_->get_logger(), "Serial node initialization complete for %s", 48 | port_settings_->dev_name.as_string().c_str()); 49 | 50 | // Topics are initialized prior to the worker. 51 | // That is done to put the burden of null checks on the DDS side. 52 | // Should the burden be on the device driver side, 53 | // then the conditions for the serial line saturation will be met more often. 54 | worker_ = std::make_shared(this, port_settings_); 55 | 56 | init_serial_(); 57 | } 58 | 59 | void Port::output(const std::string &msg) { worker_->output(msg); } 60 | 61 | // having pure pointers would improve performance here 62 | // but it would be against the religion of so many 63 | void Port::register_input_cb(void (*input_cb)(const std::string &msg, 64 | void *user_data), 65 | void *user_data) { 66 | worker_->register_input_cb(input_cb, user_data); 67 | } 68 | 69 | void Port::inject_input(const std::string &msg) { worker_->inject_input(msg); } 70 | 71 | int PortSettings::setup(int old_fd) { 72 | if (old_fd != -1) { 73 | close(old_fd); 74 | } 75 | 76 | int fd = ::open(dev_name.as_string().data(), O_RDWR); 77 | if (fd < 0) { 78 | throw std::invalid_argument("failed to open the device"); 79 | } 80 | 81 | if (!skip_init.as_bool()) { 82 | // Get terminal attributes 83 | struct termios tty; 84 | ::memset(&tty, 0, sizeof(tty)); 85 | if (::tcgetattr(fd, &tty) != 0) { 86 | ::close(fd); 87 | throw std::invalid_argument("failed to get the device attributes"); 88 | } 89 | 90 | // Baud rate 91 | switch (baud_rate.as_int()) { 92 | case 0: 93 | cfsetispeed(&tty, B0); 94 | break; 95 | case 50: 96 | cfsetispeed(&tty, B50); 97 | break; 98 | case 75: 99 | cfsetispeed(&tty, B75); 100 | break; 101 | case 110: 102 | cfsetispeed(&tty, B110); 103 | break; 104 | case 134: 105 | cfsetispeed(&tty, B134); 106 | break; 107 | case 150: 108 | cfsetispeed(&tty, B150); 109 | break; 110 | case 200: 111 | cfsetispeed(&tty, B200); 112 | break; 113 | case 300: 114 | cfsetispeed(&tty, B300); 115 | break; 116 | case 600: 117 | cfsetispeed(&tty, B600); 118 | break; 119 | case 1200: 120 | cfsetispeed(&tty, B1200); 121 | break; 122 | case 1800: 123 | cfsetispeed(&tty, B1800); 124 | break; 125 | case 2400: 126 | cfsetispeed(&tty, B2400); 127 | break; 128 | case 4800: 129 | cfsetispeed(&tty, B4800); 130 | break; 131 | case 9600: 132 | cfsetispeed(&tty, B9600); 133 | break; 134 | case 19200: 135 | cfsetispeed(&tty, B19200); 136 | break; 137 | case 38400: 138 | cfsetispeed(&tty, B38400); 139 | break; 140 | case 57600: 141 | cfsetispeed(&tty, B57600); 142 | break; 143 | case 115200: 144 | cfsetispeed(&tty, B115200); 145 | break; 146 | case 230400: 147 | cfsetispeed(&tty, B230400); 148 | break; 149 | // case 460800: 150 | // cfsetispeed(&tty, B460800); 151 | // break; 152 | default: 153 | throw std::invalid_argument("unsupported baud rate"); 154 | } 155 | 156 | // Data bits 157 | tty.c_cflag &= ~CSIZE; 158 | switch (data.as_int()) { 159 | case 5: 160 | tty.c_cflag |= CS5; 161 | break; 162 | case 6: 163 | tty.c_cflag |= CS6; 164 | break; 165 | case 7: 166 | tty.c_cflag |= CS7; 167 | break; 168 | case 8: 169 | tty.c_cflag |= CS8; 170 | break; 171 | } 172 | 173 | // Stop bits 174 | if (stop.as_int() == 1) { 175 | tty.c_cflag &= ~CSTOPB; 176 | } else { 177 | tty.c_cflag |= CSTOPB; 178 | } 179 | 180 | // Parity 181 | // TODO(clairbee): handle this as a dictionary to support odd parity 182 | if (parity.as_bool()) { 183 | tty.c_cflag |= PARENB; 184 | tty.c_cflag &= ~PARODD; 185 | } else { 186 | tty.c_cflag &= ~PARENB; 187 | } 188 | 189 | // Flow control 190 | if (flow_control.as_bool()) { 191 | tty.c_cflag |= CRTSCTS; // Turn on h/w flow control 192 | } else { 193 | tty.c_cflag &= ~CRTSCTS; 194 | } 195 | if (sw_flow_control.as_bool()) { 196 | tty.c_iflag |= (IXON | IXOFF | IXANY); // Turn on s/w flow ctrl 197 | } else { 198 | tty.c_iflag &= ~(IXON | IXOFF | IXANY); // Turn off s/w flow ctrl 199 | } 200 | 201 | tty.c_cflag |= CREAD | CLOCAL; 202 | tty.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL); 203 | // Setting both to 0 will give a non-blocking read 204 | tty.c_cc[VTIME] = 0; 205 | tty.c_cc[VMIN] = 0; 206 | tty.c_lflag &= ~ICANON; // Turn off canonical input 207 | tty.c_lflag &= ~(ECHO); // Turn off echo 208 | tty.c_lflag &= ~ECHOE; // Disable erasure 209 | tty.c_lflag &= ~ECHONL; // Disable new-line echo 210 | tty.c_lflag &= ~ISIG; // Disables recognition of INTR (interrupt), QUIT and 211 | // SUSP (suspend) characters 212 | tty.c_oflag &= ~OPOST; // Prevent special interpretation of output bytes 213 | // (e.g. newline chars) 214 | tty.c_oflag &= 215 | ~ONLCR; // Prevent conversion of newline to carriage return/line feed 216 | 217 | // Set the terminal attributes 218 | ::tcflush(fd, TCIFLUSH); 219 | if (::tcsetattr(fd, TCSANOW, &tty) != 0) { 220 | ::close(fd); 221 | throw std::invalid_argument("failed to set the device attributes"); 222 | } 223 | } 224 | 225 | // Get the file descriptor attributes 226 | #if defined(F_NOCACHE) 227 | ::fcntl(fd, F_NOCACHE, 1); 228 | #endif 229 | int flags = ::fcntl(fd, F_GETFL, 0); 230 | if (flags == -1) { 231 | ::close(fd); 232 | throw std::invalid_argument("failed to get the file descriptor attributes"); 233 | } 234 | 235 | flags |= O_NONBLOCK; 236 | 237 | // Set the file attributes 238 | if (::fcntl(fd, F_SETFL, flags)) { 239 | ::close(fd); 240 | throw std::invalid_argument("failed to set the file descriptor attributes"); 241 | } 242 | 243 | return fd; 244 | } 245 | 246 | } // namespace remote_serial 247 | -------------------------------------------------------------------------------- /src/worker.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2022 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2022-09-24 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #include "remote_serial/worker.hpp" 11 | 12 | #include "remote_serial/interface.hpp" 13 | #include "remote_serial/utils.hpp" 14 | 15 | #define _BSD_SOURCE 16 | #include 17 | #include 18 | #include 19 | 20 | #ifndef DEBUG 21 | #undef RCLCPP_DEBUG 22 | #if 1 23 | #define RCLCPP_DEBUG(...) 24 | #else 25 | #define RCLCPP_DEBUG RCLCPP_INFO 26 | #define DEBUG 27 | #endif 28 | #endif 29 | 30 | namespace remote_serial { 31 | 32 | Worker::Worker(Port *impl, std::shared_ptr settings) 33 | : impl_(impl), 34 | settings_(settings), 35 | fd_(-1), 36 | signal_{-1, -1}, 37 | do_stop_(false), 38 | input_cb_{nullptr}, 39 | input_cb_user_data_{nullptr}, 40 | logger_{rclcpp::get_logger("serial(" + settings->dev_name.as_string() + 41 | ")")} { 42 | fd_ = settings->setup(fd_); 43 | if (fd_ < 0) { 44 | throw std::invalid_argument("failed to conigure the port"); 45 | } 46 | 47 | // Create a pipe to send signals into the worker threads 48 | if (::pipe(signal_) < 0) { 49 | ::close(fd_); 50 | fd_ = -1; 51 | throw std::invalid_argument("failed to creat a signalling channel"); 52 | } 53 | 54 | // Make the recieving side non-blocking 55 | int flags = ::fcntl(signal_[0], F_GETFL, 0); 56 | if (flags == -1) { 57 | throw std::invalid_argument("failed to get the pipe descriptor attributes"); 58 | } 59 | 60 | flags |= O_NONBLOCK; 61 | 62 | if (::fcntl(signal_[0], F_SETFL, flags)) { 63 | throw std::invalid_argument("failed to set the pipe descriptor attributes"); 64 | } 65 | 66 | thread_ = std::shared_ptr(new std::thread(&Worker::run_, this)); 67 | 68 | RCLCPP_INFO(logger_, "Worker initialization complete for %s", 69 | settings_->dev_name.as_string().c_str()); 70 | } 71 | 72 | Worker::~Worker() { stop(); } 73 | 74 | void Worker::output(const std::string &msg) { 75 | RCLCPP_DEBUG(logger_, "output() with %lu bytes", msg.size()); 76 | 77 | std::lock_guard guard(output_queue_mutex_); 78 | output_queue_.push_back(std::pair(msg, 0)); 79 | 80 | ::write(signal_[1], " ", 1); 81 | } 82 | 83 | void Worker::register_input_cb(void (*cb)(const std::string &, void *), 84 | void *user_data) { 85 | RCLCPP_DEBUG(logger_, "register_input_cb()"); 86 | 87 | std::lock_guard guard(input_cb_mutex_); 88 | input_cb_ = cb; 89 | input_cb_user_data_ = user_data; 90 | } 91 | 92 | void Worker::inject_input(const std::string &msg) { 93 | RCLCPP_DEBUG(logger_, "inject_input() with %lu bytes", msg.size()); 94 | 95 | std::lock_guard guard(input_cb_mutex_); 96 | if (input_cb_) { 97 | input_cb_(msg, input_cb_user_data_); 98 | } 99 | } 100 | 101 | void Worker::stop() { 102 | if (do_stop_) { 103 | // already stopped, ping the worker thread just in case 104 | ::write(signal_[1], " ", 1); 105 | return; 106 | } 107 | 108 | do_stop_ = true; 109 | ::write(signal_[1], " ", 1); 110 | thread_->join(); 111 | } 112 | 113 | void Worker::run_() { 114 | fd_set read_fds, write_fds, except_fds; 115 | int max_fds = fd_; 116 | if (signal_[0] > max_fds) { 117 | max_fds = signal_[0]; 118 | } 119 | max_fds++; 120 | 121 | while (true) { 122 | FD_ZERO(&read_fds); 123 | FD_ZERO(&write_fds); 124 | FD_ZERO(&except_fds); 125 | FD_SET(fd_, &read_fds); 126 | FD_SET(signal_[0], &read_fds); 127 | if (output_queue_.size() > 0) { 128 | FD_SET(fd_, &write_fds); 129 | } 130 | 131 | int nevents = 132 | ::select(max_fds, &read_fds, &write_fds, &except_fds, nullptr); 133 | if (nevents < 0) { 134 | RCLCPP_ERROR(logger_, "select() failed for %s", 135 | settings_->dev_name.as_string().c_str()); 136 | break; 137 | } 138 | RCLCPP_DEBUG(logger_, "woke up from select on %d events", nevents); 139 | 140 | if (FD_ISSET(signal_[0], &read_fds)) { 141 | // consume up to 32 signals at a time 142 | char c[32]; 143 | (void)read(signal_[0], &c[0], sizeof(c)); 144 | 145 | // check if it's a signal to terminate the threads 146 | if (do_stop_) { 147 | RCLCPP_INFO(logger_, "exiting IO loop gracefully"); 148 | break; 149 | } 150 | 151 | // start processing the send queue 152 | output_queue_mutex_.lock(); 153 | while (output_queue_.size() > 0) { 154 | auto next = output_queue_.begin(); 155 | output_queue_mutex_.unlock(); 156 | // We do not need to keep the lock during IO, 157 | // since the other threads use .push_back() and, 158 | // thus, do not invalidate this iterator. 159 | 160 | // Now attempt to write 161 | const char *read_pos = next->first.data() + next->second; 162 | const int remains_to_write = next->first.length() - next->second; 163 | 164 | RCLCPP_DEBUG(logger_, "attempting to output %d bytes", 165 | remains_to_write); 166 | 167 | int wrote = ::write(fd_, read_pos, remains_to_write); 168 | if (wrote < 0) { 169 | // TODO(clairbee): check the errno in this thread 170 | // TODO(clairbee): if not EAGAIN 171 | // TODO(clairbee): break the IO loop and re-open the file 172 | RCLCPP_INFO(logger_, "error while writing data: %d bytes", 173 | remains_to_write); 174 | output_queue_mutex_.lock(); 175 | continue; // hoping it was EAGAIN 176 | } 177 | if (wrote == 0) { 178 | // TODO(clairbee): break the IO loop and re-open the file 179 | RCLCPP_ERROR(logger_, "EOF while writing data: %d bytes", 180 | remains_to_write); 181 | output_queue_mutex_.lock(); 182 | break; 183 | } 184 | 185 | // Now publish what is written successfully 186 | auto message = std_msgs::msg::UInt8MultiArray(); 187 | // TODO(clairbee): optimize extra copy away 188 | auto str = std::string(read_pos, wrote); 189 | message.data = std::vector(str.begin(), str.end()); 190 | RCLCPP_DEBUG(logger_, "Publishing written data: '%s'", 191 | utils::bin2hex(str).c_str()); 192 | impl_->inspect_output->publish(message); 193 | 194 | // Advance the queue read position 195 | if (wrote == remains_to_write) { 196 | output_queue_mutex_.lock(); 197 | output_queue_.erase(next); 198 | } else { 199 | next->second += wrote; 200 | output_queue_mutex_.lock(); 201 | } 202 | } 203 | output_queue_mutex_.unlock(); 204 | } 205 | 206 | if (FD_ISSET(fd_, &except_fds)) { 207 | RCLCPP_INFO(logger_, "exiting IO loop due to an exception while reading"); 208 | break; 209 | } 210 | 211 | if (FD_ISSET(fd_, &read_fds)) { 212 | int buffer_size = settings_->bs.as_int(); 213 | if (buffer_size > PortSettings::BUFFER_SIZE_MAX) { 214 | buffer_size = PortSettings::BUFFER_SIZE_MAX; 215 | } 216 | auto buffer = std::unique_ptr(new char[buffer_size]); 217 | 218 | int total = ::read(fd_, buffer.get(), buffer_size); 219 | if (total < 0) { 220 | // TODO(clairbee): break the IO loop and re-open the file 221 | RCLCPP_INFO(logger_, "error while reading data"); 222 | continue; // hoping it was EAGAIN 223 | } 224 | 225 | if (total == 0) { 226 | RCLCPP_INFO(logger_, "exiting IO loop due to EOF"); 227 | break; 228 | } 229 | 230 | if (total > 0) { 231 | auto message = std_msgs::msg::UInt8MultiArray(); 232 | // TODO(clairbee): optimize extra copy away 233 | auto str = std::string(buffer.get(), total); 234 | message.data = std::vector(str.begin(), str.end()); 235 | 236 | RCLCPP_DEBUG(logger_, "received input of %d bytes", total); 237 | 238 | input_cb_mutex_.lock(); 239 | if (input_cb_ != nullptr) { 240 | // having pure pointers would improve performance here 241 | // by skipping data copying few lines above (message.data) 242 | 243 | input_cb_(str, input_cb_user_data_); 244 | } 245 | input_cb_mutex_.unlock(); 246 | RCLCPP_DEBUG(logger_, "received input of %d bytes: done", total); 247 | 248 | RCLCPP_DEBUG(logger_, "Publishing received: '%s'", 249 | utils::bin2hex(str).c_str()); 250 | impl_->inspect_input->publish(message); 251 | } 252 | } 253 | } 254 | } 255 | 256 | } // namespace remote_serial 257 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # remote_serial 2 | 3 | [![License](./apache20.svg)](./LICENSE.txt) 4 | 5 | This is an ultimate implementation of serial line interface for ROS2. 6 | 7 | It provides ROS2 interfaces for inter-process access, introspection and 8 | debugging. 9 | It performs wisely in case of serial line saturation in any of 10 | the I/O directions, minimizing data loses and blocking behavior, 11 | ensuring optimum performance. 12 | 13 | This package implements a generic purpose serial port driver using 14 | character device files. 15 | However this package also implements most of the logic required for other 16 | serial line drivers (see the "Examples" section below). 17 | 18 | ## ROS2 interfaces 19 | 20 | ### Parameters 21 | 22 | All classes: 23 | 24 | - `serial_prefix`: the prefix to ROS2 interfaces exposed by this driver 25 | ("/serial/<node-name>" by default) 26 | 27 | The factory class: 28 | 29 | - `serial_is_remote`: instructs whether 30 | to instantiate the driver locally or to connect to a remote instance 31 | ("true" by default) 32 | 33 | Serial port driver: 34 | 35 | - `serial_dev_name`: the character device path ("/dev/ttyS0" by default) 36 | - serial port settings: 37 | - `serial_skip_init`: skip the port initialization (ignore the below parameters) 38 | - `serial_baud_rate`: baud rate ("115200" by default) 39 | - `serial_data`: data bits ("8" by default) 40 | - `serial_parity`: parity bit ("false" by default) 41 | - `serial_stop`: stop bits ("1" by default) 42 | - `serial_flow_control`: hardware flow control ("false" by default) 43 | - `serial_sw_flow_control`: software flow control ("false" by default) 44 | - `serial_bs`: the size of read buffer ("1024" by default) 45 | 46 | ### Topics 47 | 48 | #### Subscribers 49 | 50 | - `//inject/input`: injects the given bytes into 51 | the serial line as if it was written to the line by the driver 52 | - `//inject/output`: injects the given bytes into 53 | the serial line as if it was received by the driver from the line 54 | 55 | #### Publishers 56 | 57 | - `//inspect/input`: publishes data 58 | received by the driver from the line 59 | - `//inspect/output`: publishes data 60 | sent by the driver to the line 61 | 62 | ## Basic setup 63 | 64 | Launch it as a separate node for each serial port: 65 | 66 | ``` 67 | $ ros2 run remote_serial remote_serial_standalone 68 | ``` 69 | 70 | or 71 | 72 | ``` 73 | $ ros2 run remote_serial remote_serial_standalone \ 74 | --ros-args \ 75 | --remap serial:__node:=serial_com1 \ 76 | -p serial_is_remote:=false \ 77 | -p serial_prefix:=/serial/com1 \ 78 | -p serial_dev_name:=/dev/ttyS0 \ 79 | -p serial_baud_rate:=115200 \ 80 | -p serial_data:=8 \ 81 | -p serial_parity:=false \ 82 | -p serial_stop:=1 \ 83 | -p serial_flow_control:=true 84 | ``` 85 | 86 | ```mermaid 87 | flowchart TB 88 | cli["

$ ros2 topic echo /serial/com1/inspect/input\n$ ros2 topic echo /serial/com1/inspect/output\n$ ros2 topic pub /serial/com1/inject/output \\n   std_msgs/msg/UInt8MultiArray {'data':'ATH+CHUP\r\ n'}

"] -. "DDS" .-> topic_serial[/ROS2 interfaces:\n/serial/com1/.../] 89 | app["Your process"] -- "DDS\n(with context switch)" --> topic_serial 90 | subgraph serial["Process: remote_serial_standalone"] 91 | topic_serial --> driver["Serial port driver"] 92 | end 93 | driver --> file{{"Character device: /dev/ttyS0"}} 94 | ``` 95 | 96 | ## Advanced setup 97 | 98 | The more advanced setup is to initialize it as a separate node in the very executable which will be communicating with the port all the time 99 | (e.g. a MODBUS RTU implementation). 100 | 101 | This setup allows DDS to forward the messages between nodes 102 | without context switches should your DDS implementation support that. 103 | See an example of such a setup in the Modbus RTU package: 104 | 105 | ```mermaid 106 | flowchart TB 107 | cli_serial["

# Serial debugging and troubleshooting\n$ ros2 topic echo /serial/inspect/input\n...

"] -. "DDS" ..-> topic_serial[/ROS2 interfaces:\n/serial/.../] 108 | subgraph modbus_exe["Your process"] 109 | subgraph serial["Library: remote_serial"] 110 | topic_serial --> driver["Serial port driver"] 111 | end 112 | code["Your code"] -- "DDS\n(potentially without\ncontext switch)" --> topic_serial 113 | code -- "or native API calls" ---> driver 114 | driver --> file{{"Character device"}} 115 | end 116 | ``` 117 | 118 | 119 | ## Implementation details 120 | 121 | The following diagram shows the high level view on the internals of this package: 122 | 123 | ```mermaid 124 | flowchart TB 125 | owner["Native API"] 126 | external["ROS2 Interface API"] 127 | 128 | subgraph node["remote_serial::Node"] 129 | 130 | subgraph interface_ros2["remote_serial::Interface"] 131 | subgraph topics["Topics: /serial"] 132 | published_input[//inspect/input/] 133 | published_output[//inspect/output/] 134 | end 135 | 136 | subgraph services["Services: /serial"] 137 | service_send[//inject/output/] 138 | service_recv[//inject/input/] 139 | end 140 | end 141 | 142 | subgraph worker["remote_serial::Port"] 143 | output_queue["Output Queue"] 144 | thread["Worker thread\nrunning select()"] 145 | 146 | subgraph interface_native["remote_serial::Implementation"] 147 | subgraph register_input_cb["register_input_cb()"] 148 | input_cb["input_cb()"] 149 | end 150 | output("output()") 151 | end 152 | 153 | fd["File Descriptor"] 154 | end 155 | 156 | end 157 | file{{"Character device"}} 158 | 159 | %% owner -- "Constructor\nwith parameters" ----> node 160 | owner --> output --> output_queue 161 | %% owner --> register_input_cb 162 | 163 | external .-> published_input 164 | external .-> published_output 165 | external .-> service_send 166 | external .-> service_recv 167 | 168 | service_recv --> published_input 169 | service_recv --> input_cb 170 | 171 | service_send --> output_queue 172 | output_queue --> thread 173 | thread --> published_input 174 | thread --> published_output 175 | thread ---> input_cb --> owner 176 | 177 | file -- "input" ---> fd -- "input" ----> thread 178 | thread -- "output" --> fd -- "output" --> file 179 | 180 | ``` 181 | 182 | ## Implementing serial line drivers 183 | 184 | The serial line drivers implement the base class provided by this package. 185 | For C++ drivers, that means that they use 186 | `remote_serial::Implementation` as a parent class. 187 | Its constructor requires an instance of `rclcpp::Node` to read the 188 | parameters from. 189 | 190 | 191 | Note: 192 | There can be more than one instance of `remote_serial::Implementation` 193 | per node. However they will all be controlled by the same parameters. 194 | Though the default value of the parameter `serial_prefix` can be passed 195 | into each individual constructor which is sufficient for common use cases. 196 | 197 |
198 |
199 | 200 | The following methods need to be implemented by each driver: 201 | 202 | ```c++ 203 | public: 204 | // output writes bytes to the serial line 205 | virtual void output(const std::string &) override; 206 | 207 | // register_input_cb is used to set the callback function which 208 | // is called every time data is received from the serial line 209 | virtual void register_input_cb(void (*)(const std::string &msg, 210 | void *user_data), 211 | void *user_data) override; 212 | ``` 213 | 214 | The users can chose one of three ways to interact with child classes of `remote_serial::Implementation`: 215 | 216 | - Link with the driver directly to make native API calls: 217 | 218 | ```c++ 219 | auto serial = \ 220 | std::make_shared(node, "/serial"); 221 | serial->output("ATH+CHUP\r\n"); 222 | ``` 223 | 224 | In this case the driver is running within the same process and it is 225 | destroyed when the `serial` object is destroyed. 226 | 227 | - Link with `remote_serial` to make ROS2 interface (DDS) calls 228 | (locally or over a network): 229 | 230 | ```c++ 231 | auto serial = \ 232 | std::make_shared(node, "/serial"); 233 | serial->output("ATH+CHUP\r\n"); 234 | ``` 235 | 236 | In this case the driver can be elsewhere within DDS' reach 237 | (same process or another side of the globe). 238 | 239 | - Let the runtime make the choice between the above two options: 240 | 241 | ```c++ 242 | auto serial = \ 243 | remote_serial::Factory::New(node, "/serial"); 244 | // or 245 | auto serial = \ 246 | driver_package::Factory::New(node, "/serial"); 247 | ``` 248 | 249 | In this case the boolean value of the parameter `serial_is_remote` 250 | determines whether the driver is instantiated locally or if a remote 251 | interface is used to reach the driver instantiated elsewhere. 252 | Please, note, the trivial class `driver_package::Factory` 253 | (similar to `remote_serial::Factory`) has to be written 254 | to support this use case. 255 | 256 | ## Examples 257 | 258 | ### remote_microcontroller 259 | 260 | See the UART implementation in 261 | [remote_microcontroller](https://github.com/openvmp/microcontroller) 262 | for an example of a driver that implements `remote_serial`. 263 | -------------------------------------------------------------------------------- /LICENSE.txt: -------------------------------------------------------------------------------- 1 | 2 | Apache License 3 | Version 2.0, January 2004 4 | http://www.apache.org/licenses/ 5 | 6 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 7 | 8 | 1. Definitions. 9 | 10 | "License" shall mean the terms and conditions for use, reproduction, 11 | and distribution as defined by Sections 1 through 9 of this document. 12 | 13 | "Licensor" shall mean the copyright owner or entity authorized by 14 | the copyright owner that is granting the License. 15 | 16 | "Legal Entity" shall mean the union of the acting entity and all 17 | other entities that control, are controlled by, or are under common 18 | control with that entity. For the purposes of this definition, 19 | "control" means (i) the power, direct or indirect, to cause the 20 | direction or management of such entity, whether by contract or 21 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 22 | outstanding shares, or (iii) beneficial ownership of such entity. 23 | 24 | "You" (or "Your") shall mean an individual or Legal Entity 25 | exercising permissions granted by this License. 26 | 27 | "Source" form shall mean the preferred form for making modifications, 28 | including but not limited to software source code, documentation 29 | source, and configuration files. 30 | 31 | "Object" form shall mean any form resulting from mechanical 32 | transformation or translation of a Source form, including but 33 | not limited to compiled object code, generated documentation, 34 | and conversions to other media types. 35 | 36 | "Work" shall mean the work of authorship, whether in Source or 37 | Object form, made available under the License, as indicated by a 38 | copyright notice that is included in or attached to the work 39 | (an example is provided in the Appendix below). 40 | 41 | "Derivative Works" shall mean any work, whether in Source or Object 42 | form, that is based on (or derived from) the Work and for which the 43 | editorial revisions, annotations, elaborations, or other modifications 44 | represent, as a whole, an original work of authorship. For the purposes 45 | of this License, Derivative Works shall not include works that remain 46 | separable from, or merely link (or bind by name) to the interfaces of, 47 | the Work and Derivative Works thereof. 48 | 49 | "Contribution" shall mean any work of authorship, including 50 | the original version of the Work and any modifications or additions 51 | to that Work or Derivative Works thereof, that is intentionally 52 | submitted to Licensor for inclusion in the Work by the copyright owner 53 | or by an individual or Legal Entity authorized to submit on behalf of 54 | the copyright owner. For the purposes of this definition, "submitted" 55 | means any form of electronic, verbal, or written communication sent 56 | to the Licensor or its representatives, including but not limited to 57 | communication on electronic mailing lists, source code control systems, 58 | and issue tracking systems that are managed by, or on behalf of, the 59 | Licensor for the purpose of discussing and improving the Work, but 60 | excluding communication that is conspicuously marked or otherwise 61 | designated in writing by the copyright owner as "Not a Contribution." 62 | 63 | "Contributor" shall mean Licensor and any individual or Legal Entity 64 | on behalf of whom a Contribution has been received by Licensor and 65 | subsequently incorporated within the Work. 66 | 67 | 2. Grant of Copyright License. Subject to the terms and conditions of 68 | this License, each Contributor hereby grants to You a perpetual, 69 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 70 | copyright license to reproduce, prepare Derivative Works of, 71 | publicly display, publicly perform, sublicense, and distribute the 72 | Work and such Derivative Works in Source or Object form. 73 | 74 | 3. Grant of Patent License. Subject to the terms and conditions of 75 | this License, each Contributor hereby grants to You a perpetual, 76 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 77 | (except as stated in this section) patent license to make, have made, 78 | use, offer to sell, sell, import, and otherwise transfer the Work, 79 | where such license applies only to those patent claims licensable 80 | by such Contributor that are necessarily infringed by their 81 | Contribution(s) alone or by combination of their Contribution(s) 82 | with the Work to which such Contribution(s) was submitted. If You 83 | institute patent litigation against any entity (including a 84 | cross-claim or counterclaim in a lawsuit) alleging that the Work 85 | or a Contribution incorporated within the Work constitutes direct 86 | or contributory patent infringement, then any patent licenses 87 | granted to You under this License for that Work shall terminate 88 | as of the date such litigation is filed. 89 | 90 | 4. Redistribution. You may reproduce and distribute copies of the 91 | Work or Derivative Works thereof in any medium, with or without 92 | modifications, and in Source or Object form, provided that You 93 | meet the following conditions: 94 | 95 | (a) You must give any other recipients of the Work or 96 | Derivative Works a copy of this License; and 97 | 98 | (b) You must cause any modified files to carry prominent notices 99 | stating that You changed the files; and 100 | 101 | (c) You must retain, in the Source form of any Derivative Works 102 | that You distribute, all copyright, patent, trademark, and 103 | attribution notices from the Source form of the Work, 104 | excluding those notices that do not pertain to any part of 105 | the Derivative Works; and 106 | 107 | (d) If the Work includes a "NOTICE" text file as part of its 108 | distribution, then any Derivative Works that You distribute must 109 | include a readable copy of the attribution notices contained 110 | within such NOTICE file, excluding those notices that do not 111 | pertain to any part of the Derivative Works, in at least one 112 | of the following places: within a NOTICE text file distributed 113 | as part of the Derivative Works; within the Source form or 114 | documentation, if provided along with the Derivative Works; or, 115 | within a display generated by the Derivative Works, if and 116 | wherever such third-party notices normally appear. The contents 117 | of the NOTICE file are for informational purposes only and 118 | do not modify the License. You may add Your own attribution 119 | notices within Derivative Works that You distribute, alongside 120 | or as an addendum to the NOTICE text from the Work, provided 121 | that such additional attribution notices cannot be construed 122 | as modifying the License. 123 | 124 | You may add Your own copyright statement to Your modifications and 125 | may provide additional or different license terms and conditions 126 | for use, reproduction, or distribution of Your modifications, or 127 | for any such Derivative Works as a whole, provided Your use, 128 | reproduction, and distribution of the Work otherwise complies with 129 | the conditions stated in this License. 130 | 131 | 5. Submission of Contributions. Unless You explicitly state otherwise, 132 | any Contribution intentionally submitted for inclusion in the Work 133 | by You to the Licensor shall be under the terms and conditions of 134 | this License, without any additional terms or conditions. 135 | Notwithstanding the above, nothing herein shall supersede or modify 136 | the terms of any separate license agreement you may have executed 137 | with Licensor regarding such Contributions. 138 | 139 | 6. Trademarks. This License does not grant permission to use the trade 140 | names, trademarks, service marks, or product names of the Licensor, 141 | except as required for reasonable and customary use in describing the 142 | origin of the Work and reproducing the content of the NOTICE file. 143 | 144 | 7. Disclaimer of Warranty. Unless required by applicable law or 145 | agreed to in writing, Licensor provides the Work (and each 146 | Contributor provides its Contributions) on an "AS IS" BASIS, 147 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 148 | implied, including, without limitation, any warranties or conditions 149 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 150 | PARTICULAR PURPOSE. You are solely responsible for determining the 151 | appropriateness of using or redistributing the Work and assume any 152 | risks associated with Your exercise of permissions under this License. 153 | 154 | 8. Limitation of Liability. In no event and under no legal theory, 155 | whether in tort (including negligence), contract, or otherwise, 156 | unless required by applicable law (such as deliberate and grossly 157 | negligent acts) or agreed to in writing, shall any Contributor be 158 | liable to You for damages, including any direct, indirect, special, 159 | incidental, or consequential damages of any character arising as a 160 | result of this License or out of the use or inability to use the 161 | Work (including but not limited to damages for loss of goodwill, 162 | work stoppage, computer failure or malfunction, or any and all 163 | other commercial damages or losses), even if such Contributor 164 | has been advised of the possibility of such damages. 165 | 166 | 9. Accepting Warranty or Additional Liability. While redistributing 167 | the Work or Derivative Works thereof, You may choose to offer, 168 | and charge a fee for, acceptance of support, warranty, indemnity, 169 | or other liability obligations and/or rights consistent with this 170 | License. However, in accepting such obligations, You may act only 171 | on Your own behalf and on Your sole responsibility, not on behalf 172 | of any other Contributor, and only if You agree to indemnify, 173 | defend, and hold each Contributor harmless for any liability 174 | incurred by, or claims asserted against, such Contributor by reason 175 | of your accepting any such warranty or additional liability. 176 | 177 | END OF TERMS AND CONDITIONS 178 | 179 | APPENDIX: How to apply the Apache License to your work. 180 | 181 | To apply the Apache License to your work, attach the following 182 | boilerplate notice, with the fields enclosed by brackets "[]" 183 | replaced with your own identifying information. (Don't include 184 | the brackets!) The text should be enclosed in the appropriate 185 | comment syntax for the file format. We also recommend that a 186 | file or class name and description of purpose be included on the 187 | same "printed page" as the copyright notice for easier 188 | identification within third-party archives. 189 | 190 | Copyright [yyyy] [name of copyright owner] 191 | 192 | Licensed under the Apache License, Version 2.0 (the "License"); 193 | you may not use this file except in compliance with the License. 194 | You may obtain a copy of the License at 195 | 196 | http://www.apache.org/licenses/LICENSE-2.0 197 | 198 | Unless required by applicable law or agreed to in writing, software 199 | distributed under the License is distributed on an "AS IS" BASIS, 200 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 201 | See the License for the specific language governing permissions and 202 | limitations under the License. 203 | --------------------------------------------------------------------------------