From b483e7e007f19b396603a0e83b5349962c441c1a Mon Sep 17 00:00:00 2001 From: "Javier JM. Moreno" Date: Wed, 31 Oct 2018 15:49:30 +0100 Subject: [PATCH] Feature/u ros agent node --- uROS_Agent/CMakeLists.txt | 48 ++++++- uROS_Agent/package.xml | 6 +- uROS_Agent/src/main.cpp | 272 ++++++++++++++++++++++++++++++++++++++ 3 files changed, 324 insertions(+), 2 deletions(-) create mode 100644 uROS_Agent/src/main.cpp diff --git a/uROS_Agent/CMakeLists.txt b/uROS_Agent/CMakeLists.txt index 2717f90..ec0182f 100644 --- a/uROS_Agent/CMakeLists.txt +++ b/uROS_Agent/CMakeLists.txt @@ -20,6 +20,18 @@ cmake_minimum_required(VERSION 3.5) project(uros_agent) +set(CMAKE_C_CLANG_TIDY clang-tidy -checks=*) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + + # Find packages depencences find_package(rosidl_cmake REQUIRED) find_package(ament_cmake REQUIRED) @@ -193,11 +205,45 @@ set (_XmlDoc "\n${_XmlDoc}\n") file(WRITE "${_DEFAULT_FASTRTPS_PROFILES_PATH}" "${_XmlDoc}") +# Only compile uROS agent if rclcpp is found +find_package(rclcpp QUIET) +if (rclcpp_FOUND) + find_package(microxrcedds_agent_cmake_module REQUIRED) + find_package(MicroXRCEDDS_Agent REQUIRED MODULE) + + add_executable(${PROJECT_NAME} src/main.cpp) + ament_target_dependencies( + ${PROJECT_NAME} + rclcpp + microxrcedds_agent + ) + + # TODO(Javier) Temporal until thread error is solver + target_link_libraries( + ${PROJECT_NAME} + microxrcedds_agent + ) + +else() + message("uROS agent node will be not build") +endif() + + + # Install the package.xml file, and generate code for ``find_package`` so that other packages can get information about this package. ament_package() +# Only install if uROS_Agent is comiled +if (rclcpp_FOUND) + install(TARGETS + ${PROJECT_NAME} + DESTINATION lib/${PROJECT_NAME} + ) +endif() + + #Install install( FILES "${_DEFAULT_FASTRTPS_PROFILES_PATH}" @@ -209,4 +255,4 @@ install( #install( # DIRECTORY cmake resource # DESTINATION share/${PROJECT_NAME} -#) \ No newline at end of file +#) diff --git a/uROS_Agent/package.xml b/uROS_Agent/package.xml index 38534c8..afb3e2f 100644 --- a/uROS_Agent/package.xml +++ b/uROS_Agent/package.xml @@ -14,8 +14,12 @@ fastcdr fastrtps - microxrcedds_agent + microxrcedds_agent_cmake_module rosidl_cmake + + rclcpp + + rclcpp ament_lint_auto ament_lint_common diff --git a/uROS_Agent/src/main.cpp b/uROS_Agent/src/main.cpp new file mode 100644 index 0000000..0d412f6 --- /dev/null +++ b/uROS_Agent/src/main.cpp @@ -0,0 +1,272 @@ + +// Copyright 2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// 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. + +#ifdef _WIN32 +#include +#include +#else +#include +#include +#include +#include +#include +#endif //_WIN32 +#include +#include +#include +#include "rclcpp/rclcpp.hpp" + +void showHelp() +{ + std::cout << "Usage: program " << std::endl; + std::cout << "List of commands:" << std::endl; + std::cout << " serial " << std::endl; + std::cout << " pseudo-serial" << std::endl; + std::cout << " udp []" << std::endl; + std::cout << " tcp []" << std::endl; +} + +void initializationError() +{ + std::cout << "Error: Invalid arguments." << std::endl; + showHelp(); + std::exit(EXIT_FAILURE); +} + +uint16_t parsePort(const char* str_port) +{ + uint16_t valid_port = 0; + try + { + int port = std::stoi(str_port); + if(port > (std::numeric_limits::max)()) + { + std::cout << "Error: port number '" << port << "out of range." << std::endl; + initializationError(); + } + valid_port = uint16_t(port); + } + catch (const std::invalid_argument& ) + { + initializationError(); + } + return valid_port; +} + + +class uros_agent : public rclcpp::Node +{ +public: + uros_agent() : Node("uROS_Agent") + { + } + + ~uros_agent() + { + } + +private: + +}; + + +int main(int argc, char** argv) +{ + bool initialized = false; + +#ifndef _WIN32 + if(argc == 2 && (strcmp(argv[1], "-h") == 0 || strcmp(argv[1], "--help") == 0)) + { + showHelp(); + } + else if(argc >= 3 && strcmp(argv[1], "udp") == 0) + { + std::cout << "UDP agent initialization... "; + uint16_t port = parsePort(argv[2]); + eprosima::uxr::UDPServer* udp_server = (argc == 4) //discovery port + ? new eprosima::uxr::UDPServer(port, parsePort(argv[3])) + : new eprosima::uxr::UDPServer(port); + if (udp_server->run()) + { + initialized = true; + } + std::cout << ((!initialized) ? "ERROR" : "OK") << std::endl; + } + else if(argc >= 3 && strcmp(argv[1], "tcp") == 0) + { + std::cout << "TCP agent initialization... "; + uint16_t port = parsePort(argv[2]); + eprosima::uxr::TCPServer* tcp_server = (argc == 4) //discovery port + ? new eprosima::uxr::TCPServer(port, parsePort(argv[3])) + : new eprosima::uxr::TCPServer(port); + if (tcp_server->run()) + { + initialized = true; + } + std::cout << ((!initialized) ? "ERROR" : "OK") << std::endl; + } + else if(argc == 3 && strcmp(argv[1], "serial") == 0) + { + std::cout << "Serial agent initialization... "; + + /* Open serial device. */ + int fd = open(argv[2], O_RDWR | O_NOCTTY); + if (0 < fd) + { + struct termios tty_config; + memset(&tty_config, 0, sizeof(tty_config)); + if (0 == tcgetattr(fd, &tty_config)) + { + /* Setting CONTROL OPTIONS. */ + tty_config.c_cflag |= CREAD; // Enable read. + tty_config.c_cflag |= CLOCAL; // Set local mode. + tty_config.c_cflag &= ~PARENB; // Disable parity. + tty_config.c_cflag &= ~CSTOPB; // Set one stop bit. + tty_config.c_cflag &= ~CSIZE; // Mask the character size bits. + tty_config.c_cflag |= CS8; // Set 8 data bits. + tty_config.c_cflag &= ~CRTSCTS; // Disable hardware flow control. + + /* Setting LOCAL OPTIONS. */ + tty_config.c_lflag &= ~ICANON; // Set non-canonical input. + tty_config.c_lflag &= ~ECHO; // Disable echoing of input characters. + tty_config.c_lflag &= ~ECHOE; // Disable echoing the erase character. + tty_config.c_lflag &= ~ISIG; // Disable SIGINTR, SIGSUSP, SIGDSUSP and SIGQUIT signals. + + /* Setting INPUT OPTIONS. */ + tty_config.c_iflag &= ~IXON; // Disable output software flow control. + tty_config.c_iflag &= ~IXOFF; // Disable input software flow control. + tty_config.c_iflag &= ~INPCK; // Disable parity check. + tty_config.c_iflag &= ~ISTRIP; // Disable strip parity bits. + tty_config.c_iflag &= ~IGNBRK; // No ignore break condition. + tty_config.c_iflag &= ~IGNCR; // No ignore carrier return. + tty_config.c_iflag &= ~INLCR; // No map NL to CR. + tty_config.c_iflag &= ~ICRNL; // No map CR to NL. + + /* Setting OUTPUT OPTIONS. */ + tty_config.c_oflag &= ~OPOST; // Set raw output. + + /* Setting OUTPUT CHARACTERS. */ + tty_config.c_cc[VMIN] = 10; + tty_config.c_cc[VTIME] = 1; + + /* Setting BAUD RATE. */ + cfsetispeed(&tty_config, B115200); + cfsetospeed(&tty_config, B115200); + + if (0 == tcsetattr(fd, TCSANOW, &tty_config)) + { + eprosima::uxr::SerialServer* serial_server = new eprosima::uxr::SerialServer(fd, 0); + if (serial_server->run()) + { + initialized = true; + } + } + } + } + + std::cout << ((!initialized) ? "ERROR" : "OK") << std::endl; + } + else if (argc == 2 && strcmp(argv[1], "pseudo-serial") == 0) + { + std::cout << "Pseudo-Serial initialization... "; + + /* Open pseudo-terminal. */ + char* dev = NULL; + int fd = posix_openpt(O_RDWR | O_NOCTTY); + if (-1 != fd) + { + if (grantpt(fd) == 0 && unlockpt(fd) == 0 && (dev = ptsname(fd))) + { + struct termios attr; + tcgetattr(fd, &attr); + cfmakeraw(&attr); + tcflush(fd, TCIOFLUSH); + tcsetattr(fd, TCSANOW, &attr); + } + } + + /* Launch server. */ + eprosima::uxr::SerialServer* serial_server = new eprosima::uxr::SerialServer(fd, 0x00); + if (serial_server->run()) + { + initialized = true; + } + + if (initialized) + { + std::cout << "OK" << std::endl; + std::cout << "Device: " << dev << std::endl; + } + else + { + std::cout << "ERROR" << std::endl; + } + } + else + { + initializationError(); + } +#else + (void) argc; + (void) argv; + + std::string server_type = ""; + std::cout << "Select server type [udp|tcp]: "; + std::cin >> server_type; + + if ("udp" == server_type) + { + uint16_t port = 0; + std::cout << "Select port: "; + std::cin >> port; + + std::cout << "UDP agent initialization.... "; + eprosima::uxr::UDPServer* udp_server = new eprosima::uxr::UDPServer(port); + if (udp_server->run()) + { + initialized = true; + } + std::cout << ((!initialized) ? "ERROR" : "OK") << std::endl; + } + else if ("tcp" == server_type) + { + uint16_t port = 0; + std::cout << "Select port: "; + std::cin >> port; + + std::cout << "UDP agent initialization.... "; + eprosima::uxr::TCPServer* tcp_server = new eprosima::uxr::TCPServer(port); + if (tcp_server->run()) + { + initialized = true; + } + std::cout << ((!initialized) ? "ERROR" : "OK") << std::endl; + } + else + { + std::cout << "Error server type" << std::endl; + } + +#endif + + if(initialized) + { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + } + + return 0; +}