diff --git a/README.md b/README.md deleted file mode 100644 index 764060f..0000000 --- a/README.md +++ /dev/null @@ -1,2 +0,0 @@ -# micro-ros-agent - diff --git a/microxrcedds_agent_cmake_module/CMakeLists.txt b/microxrcedds_agent_cmake_module/CMakeLists.txt new file mode 100644 index 0000000..8d17615 --- /dev/null +++ b/microxrcedds_agent_cmake_module/CMakeLists.txt @@ -0,0 +1,32 @@ +# 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. + +cmake_minimum_required(VERSION 3.5) + +project(microxrcedds_agent_cmake_module) + +find_package(ament_cmake REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package( + CONFIG_EXTRAS "microxrcedds_agent_cmake_module-extras.cmake" + ) + +install(DIRECTORY cmake + DESTINATION share/${PROJECT_NAME} + ) diff --git a/microxrcedds_agent_cmake_module/cmake/Modules/FindMicroXRCEDDS_Agent.cmake b/microxrcedds_agent_cmake_module/cmake/Modules/FindMicroXRCEDDS_Agent.cmake new file mode 100644 index 0000000..8df1b34 --- /dev/null +++ b/microxrcedds_agent_cmake_module/cmake/Modules/FindMicroXRCEDDS_Agent.cmake @@ -0,0 +1,124 @@ +# 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. + +############################################################################### +# +# CMake module for finding eProsima MICROXRCEDDS_AGENT. +# +# Output variables: +# +# - MicroXRCEDDS_Agent_FOUND: flag indicating if the package was found +# - MicroXRCEDDS_Agent_INCLUDE_DIR: Paths to the header files +# +# Example usage: +# +# find_package(uros_agent_cmake_module REQUIRED) +# find_package(MicroXRCEDDS_Agent MODULE) +# # use MicroXRCEDDS_Agent_* variables +# +############################################################################### + +# lint_cmake: -convention/filename, -package/stdargs + +set(MicroXRCEDDS_Agent_FOUND FALSE) + +find_package(microxrcedds_agent REQUIRED CONFIG) +find_package(fastcdr REQUIRED CONFIG) +find_package(fastrtps REQUIRED CONFIG) + +find_path(MicroXRCEDDS_Agent_INCLUDE_DIR NAMES uxr) + +string(REGEX MATCH "^[0-9]+\\.[0-9]+" microxrcedds_agent_MAJOR_MINOR_VERSION "${microxrcedds_agent_VERSION}") +string(REGEX MATCH "^[0-9]+\\.[0-9]+" fastcdr_MAJOR_MINOR_VERSION "${fastcdr_VERSION}") +string(REGEX MATCH "^[0-9]+\\.[0-9]+" fastrtps_MAJOR_MINOR_VERSION "${fastrtps_VERSION}") + +## +find_library(MicroXRCEDDS_Agent_LIBRARY_RELEASE + NAMES microxrcedds_agent-${microxrcedds_agent_MAJOR_MINOR_VERSION} microxrcedds_agent) + +find_library(MicroXRCEDDS_Agent_LIBRARY_DEBUG + NAMES microxrcedds_agentd-${microxrcedds_agent_MAJOR_MINOR_VERSION} microxrcedds_agentd) + +if(MicroXRCEDDS_Agent_LIBRARY_RELEASE AND MicroXRCEDDS_Agent_LIBRARY_DEBUG) + set(MicroXRCEDDS_Agent_LIBRARIES + optimized ${MicroXRCEDDS_Agent_LIBRARY_RELEASE} + debug ${MicroXRCEDDS_Agent_LIBRARY_DEBUG} + ) +elseif(MicroXRCEDDS_Agent_LIBRARY_RELEASE) + set(MicroXRCEDDS_Agent_LIBRARIES + ${MicroXRCEDDS_Agent_LIBRARY_RELEASE} + ) +elseif(MicroXRCEDDS_Agent_LIBRARY_DEBUG) + set(MicroXRCEDDS_Agent_LIBRARIES + ${MicroXRCEDDS_Agent_LIBRARY_DEBUG} + ) +else() + set(MicroXRCEDDS_Agent_LIBRARIES "") +endif() + +## +find_library(FastRTPS_LIBRARY_RELEASE + NAMES fastrtps-${fastrtps_MAJOR_MINOR_VERSION} fastrtps) + +find_library(FastRTPS_LIBRARY_DEBUG + NAMES fastrtpsd-${fastrtps_MAJOR_MINOR_VERSION} fastrtpsd) + +if(FastRTPS_LIBRARY_RELEASE AND FastRTPS_LIBRARY_DEBUG) + set(FastRTPS_LIBRARIES + optimized ${FastRTPS_LIBRARY_RELEASE} + debug ${FastRTPS_LIBRARY_DEBUG} + ) +elseif(FastRTPS_LIBRARY_RELEASE) + set(FastRTPS_LIBRARIES + ${MicroXRCEDDS_Agent_LIBRARY_RELEASE} + ) +elseif(FastRTPS_LIBRARY_DEBUG) + set(FastRTPS_LIBRARIES + ${MicroXRCEDDS_Agent_LIBRARY_DEBUG} + ) +else() + set(FastRTPS_LIBRARIES "") +endif() + +## +find_library(FastCDR_LIBRARY_RELEASE + NAMES fastcdr-${fastcdr_MAJOR_MINOR_VERSION} fastcdr) + +find_library(FastCDR_LIBRARY_DEBUG + NAMES fastcdrd-${fastcdr_MAJOR_MINOR_VERSION} fastcdrd) + +if(FastCDR_LIBRARY_RELEASE AND FastCDR_LIBRARY_DEBUG) + set(FastCDR_LIBRARIES + optimized ${FastCDR_LIBRARY_RELEASE} + debug ${FastCDR_LIBRARY_DEBUG} + ) +elseif(FastCDR_LIBRARY_RELEASE) + set(FastCDR_LIBRARIES + ${FastCDR_LIBRARY_RELEASE} + ) +elseif(FastCDR_LIBRARY_DEBUG) + set(FastCDR_LIBRARIES + ${FastCDR_LIBRARY_DEBUG} + ) +else() + set(FastCDR_LIBRARIES "") +endif() + +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(MicroXRCEDDS_Agent + FOUND_VAR MicroXRCEDDS_Agent_FOUND + REQUIRED_VARS + MicroXRCEDDS_Agent_INCLUDE_DIR + MicroXRCEDDS_Agent_LIBRARIES +) \ No newline at end of file diff --git a/microxrcedds_agent_cmake_module/microxrcedds_agent_cmake_module-extras.cmake b/microxrcedds_agent_cmake_module/microxrcedds_agent_cmake_module-extras.cmake new file mode 100644 index 0000000..d8b7862 --- /dev/null +++ b/microxrcedds_agent_cmake_module/microxrcedds_agent_cmake_module-extras.cmake @@ -0,0 +1,15 @@ +# 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. + +list(INSERT CMAKE_MODULE_PATH 0 "${microxrcedds_agent_cmake_module_DIR}/Modules") diff --git a/microxrcedds_agent_cmake_module/package.xml b/microxrcedds_agent_cmake_module/package.xml new file mode 100644 index 0000000..6750cbe --- /dev/null +++ b/microxrcedds_agent_cmake_module/package.xml @@ -0,0 +1,22 @@ + + + + microxrcedds_agent_cmake_module + 0.1.0 + Provide CMake module to find eProsima Micro XRCE-DDS Agent + Javier Moreno + Apache License 2.0 + Javier Moreno + + ament_cmake + + ament_lint_auto + ament_lint_common + + microxrcedds_agent + microxrcedds_agent + + + ament_cmake + + diff --git a/uROS_Agent/CMakeLists.txt b/uROS_Agent/CMakeLists.txt new file mode 100644 index 0000000..368b5ba --- /dev/null +++ b/uROS_Agent/CMakeLists.txt @@ -0,0 +1,258 @@ +# 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. + + +# Set CMake version +cmake_minimum_required(VERSION 3.5) + +# Set proyect name +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) +find_package(fastcdr REQUIRED CONFIG) +find_package(fastrtps REQUIRED CONFIG) +find_package(microxrcedds_agent REQUIRED CONFIG) +find_package(ament_cmake_python REQUIRED) + + +# Export dependencies to downstream packages +ament_export_dependencies(fastcdr) +ament_export_dependencies(fastrtps) +ament_export_dependencies(microxrcedds_agent) + + +# Set variables +set(_OUTPUT_PATH "${CMAKE_CURRENT_BINARY_DIR}/python") + +set(_XML_INTERFACE_GEN_BIN "${_OUTPUT_PATH}/bin/Xml_interface_gen.py") +normalize_path(_XML_INTERFACE_GEN_BIN "${_XML_INTERFACE_GEN_BIN}") + +set(_XML_DEFAULT_READ_BIN "${_OUTPUT_PATH}/bin/Xml_read_default_profiles.py") +normalize_path(_XML_DEFAULT_READ_BIN "${_XML_DEFAULT_READ_BIN}") + +set(_PYTHON_PKG_TOOL ${PROJECT_NAME}) + +set(_RESOURCE_DIR "${_OUTPUT_PATH}/resource") +normalize_path(_RESOURCE_DIR "${_RESOURCE_DIR}") + + +set(_DEFAULT_FASTRTPS_PROFILES_PATH "${_OUTPUT_PATH}/gen/DEFAULT_FASTRTPS_PROFILES.xml") +normalize_path(_DEFAULT_FASTRTPS_PROFILES_PATH "${_DEFAULT_FASTRTPS_PROFILES_PATH}") + + +# Get colcon call dir +get_filename_component(_COLCON_CALL_DIR "${CMAKE_INSTALL_PREFIX}" DIRECTORY) +get_filename_component(_COLCON_CALL_DIR "${_COLCON_CALL_DIR}" DIRECTORY) + + +# Generate python header +set( +_PYTHON_SCRIPT_HEAD +" +import os\n +import sys\n +sys.path.append('${_OUTPUT_PATH}')\n +from ${_PYTHON_PKG_TOOL} import *\n +" +) + + +# Copy pyton files +file(COPY "bin" "${_PYTHON_PKG_TOOL}" "resource" DESTINATION "${CMAKE_CURRENT_BINARY_DIR}/python") + + +# Extract all packages and manifiest paths +execute_process( + COMMAND + "${PYTHON_EXECUTABLE}" + "-c" + "${_PYTHON_SCRIPT_HEAD}for package in GetInterfacePackages(GetPackageList('${_COLCON_CALL_DIR}')): print ('%s,%s' % (package, GetPackageName(package)))" + OUTPUT_VARIABLE _packages + RESULT_VARIABLE _result + OUTPUT_STRIP_TRAILING_WHITESPACE +) +if(NOT _result EQUAL 0) + message(FATAL_ERROR "Error finding xml packages") +endif() + + +# Convert output to list +string(REPLACE "\n" ";" _packages ${_packages}) + + +# Extract all msg from each package (stored in ${package}_MSG_FILES) +set(ALL_MSG_FILES "") +foreach(package ${_packages}) + + # Extract info + string(REPLACE "," ";" package ${package}) + list(GET package 0 package_file) + list(GET package 1 package) + + + # Get all msg files + execute_process( + COMMAND + "${PYTHON_EXECUTABLE}" + "-c" + "${_PYTHON_SCRIPT_HEAD}for msg in GetInterfacePackageMsgs('${package_file}'): print ('%s' % msg)" + OUTPUT_VARIABLE ${package}_MSG_FILES + RESULT_VARIABLE _result + OUTPUT_STRIP_TRAILING_WHITESPACE + ) + if(NOT _result EQUAL 0) + message(FATAL_ERROR "Error finding .msg files") + endif() + + # Skip if there are no msgs to create + if(${package}_MSG_FILES STREQUAL "") + continue() + endif() + + # Generate list + string(REPLACE "\n" ";" ${package}_MSG_FILES ${${package}_MSG_FILES}) + +endforeach() + + +# Append defaul xml profiles +execute_process( + COMMAND "${PYTHON_EXECUTABLE}" "${_XML_DEFAULT_READ_BIN}" "--default-xml-path" "${_RESOURCE_DIR}" + OUTPUT_VARIABLE _XmlDoc + RESULT_VARIABLE _result + ) +if(NOT _result EQUAL 0) + message(FATAL_ERROR "Error in typesuppor generation") +endif() + + +# Create one xml for each message +foreach(package ${_packages}) + string(REPLACE "," ";" package ${package}) + list(GET package 0 package_file) + list(GET package 1 package) + + + # Skip this generation if there are no msgs to process + if(${package}_MSG_FILES STREQUAL "") + continue() + endif() + + + + # generate script argument file + set(generator_arguments_file "${_OUTPUT_PATH}/XML_ArgFiles/${package}_Args.json") + rosidl_write_generator_arguments( + "${generator_arguments_file}" + PACKAGE_NAME "${package}" + ROS_INTERFACE_FILES "${${package}_MSG_FILES}" + ROS_INTERFACE_DEPENDENCIES "NULL" + OUTPUT_DIR "NULL" + TEMPLATE_DIR "NULL" + TARGET_DEPENDENCIES "NULL" + ADDITIONAL_FILES "" + ) + + + # execute python script + execute_process( + COMMAND "${PYTHON_EXECUTABLE}" "${_XML_INTERFACE_GEN_BIN}" --generator-arguments-file "${generator_arguments_file}" + OUTPUT_VARIABLE _XmlGen + RESULT_VARIABLE _result + ) + if(NOT _result EQUAL 0) + message(FATAL_ERROR "Error in typesuppor generation") + endif() + + + # Strore xml + set(_XmlDoc "${_XmlDoc}${_XmlGen}") + +endforeach() + + +# Close profile +set (_XmlDoc "\n${_XmlDoc}\n") + + +# Save +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}" + DESTINATION lib/${PROJECT_NAME} +) + + +# Install package +#install( +# DIRECTORY cmake resource +# DESTINATION share/${PROJECT_NAME} +#) diff --git a/uROS_Agent/README.md b/uROS_Agent/README.md new file mode 100644 index 0000000..70e7cfa --- /dev/null +++ b/uROS_Agent/README.md @@ -0,0 +1,31 @@ +# Micro ROS Agent + +## Overview + +This repository contains the Micro-ROS Agent package. +Micro-ROS Agent is a ROS 2 node that wraps the Micro XRCE-DDS Agent. +For further information about Micro XRCE-DDS Agent click [here](https://github.com/eProsima/Micro-XRCE-DDS-Agent) +This package is a part of the Micro-ROS project stack. +For more information about Micro-ROS project click [here](https://microros.github.io/micro-ROS/). + +The node acts as a server between DDS Network and Micro-ROS nodes inside MCU. +It receives and send messages from Micro-ROS nodes, and keep track of the Micro-ROS nodes exposing them to the ROS 2 network. +The node interacts with DDS Global Data Space on behalf of the Micro-ROS nodes. + +## Package features + +### XML generation + +During the build process, the package looks for all ROS 2 messages to generate an initial list of XML profiles. +These profiles can are referenced in the Agent-Client communication to avoid sending the full XML content. +This reference mechanism can be switched on and off from the Micro XRCE-DDS middleware layer. + +### Agent-Client communication mechanism + +Communication between the Micro-ROS Agent and the Micro-ROS nodes supports two types of transport: + +- UDP. +- SerialPort. + +All available configurations are supported directly by the Micro XRCE-DDS agent. +For further information on how to configure the agent click [here](TODO). \ No newline at end of file diff --git a/uROS_Agent/bin/Xml_interface_gen.py b/uROS_Agent/bin/Xml_interface_gen.py new file mode 100644 index 0000000..07fdb7d --- /dev/null +++ b/uROS_Agent/bin/Xml_interface_gen.py @@ -0,0 +1,30 @@ +#!/usr/bin/env python3 + +import argparse +import os +import sys + +sys.path.append(os.path.join(os.path.dirname(os.path.realpath(__file__)),'..')) + +from rosidl_cmake import read_generator_arguments +from rosidl_parser import UnknownMessageType +from uros_agent import generate_XML + + +def main(argv=sys.argv[1:]): + parser = argparse.ArgumentParser( + description='Generate the C interfaces for Micro RTPS.', + formatter_class=argparse.ArgumentDefaultsHelpFormatter) + parser.add_argument( + '--generator-arguments-file', + required=True, + help='The location of the file containing the generator arguments') + args = parser.parse_args(argv) + + generator_args = read_generator_arguments(args.generator_arguments_file) + rc = generate_XML(generator_args) + return rc + + +if __name__ == '__main__': + sys.exit(main()) diff --git a/uROS_Agent/bin/Xml_read_default_profiles.py b/uROS_Agent/bin/Xml_read_default_profiles.py new file mode 100644 index 0000000..001d256 --- /dev/null +++ b/uROS_Agent/bin/Xml_read_default_profiles.py @@ -0,0 +1,26 @@ +#!/usr/bin/env python3 + +import os +import sys +import argparse +import xml.etree.ElementTree + +sys.path.append(os.path.join(os.path.dirname(os.path.realpath(__file__)),'..')) + +from uros_agent import * + +def main(argv=sys.argv[1:]): + parser = argparse.ArgumentParser( + description='Import default xml files', + formatter_class=argparse.ArgumentDefaultsHelpFormatter) + parser.add_argument( + '--default-xml-path', + required=True, + help='The location of the default profiles') + args = parser.parse_args(argv) + ReadDefaultXMLs(args.default_xml_path) + + return 0 + +if __name__ == '__main__': + sys.exit(main()) \ No newline at end of file diff --git a/uROS_Agent/package.xml b/uROS_Agent/package.xml new file mode 100644 index 0000000..afb3e2f --- /dev/null +++ b/uROS_Agent/package.xml @@ -0,0 +1,36 @@ + + + + uros_agent + 0.0.2 + DDS-XCRE agent implementation + Javier Moreno + Apache License 2.0 + + ament_cmake + ament_cmake + + rosidl_parser + + fastcdr + fastrtps + microxrcedds_agent_cmake_module + rosidl_cmake + + rclcpp + + rclcpp + + ament_lint_auto + ament_lint_common + + rosidl_interface_packages + + + ament_cmake + + + + + + diff --git a/uROS_Agent/resource/StaticValues.xml b/uROS_Agent/resource/StaticValues.xml new file mode 100644 index 0000000..4d9c1ab --- /dev/null +++ b/uROS_Agent/resource/StaticValues.xml @@ -0,0 +1,24 @@ + + + participant_profile + + + + + WITH_KEY + Square + ShapeType + + + + + WITH_KEY + Square + ShapeType + + + + WITH_KEY + Square + ShapeType + \ No newline at end of file diff --git a/uROS_Agent/resource/rclcpp.xml b/uROS_Agent/resource/rclcpp.xml new file mode 100644 index 0000000..54a4c50 --- /dev/null +++ b/uROS_Agent/resource/rclcpp.xml @@ -0,0 +1,29 @@ + + rt/parameter_events + rcl_interfaces::msg::dds_::ParameterEvent_ + + + rt/clock + rosgraph_msgs::msg::dds_::Clock_ + + + + NO_KEY + rt/parameter_events + rcl_interfaces::msg::dds_::ParameterEvent_ + + + + + NO_KEY + rt/clock + rosgraph_msgs::msg::dds_::Clock_ + + + + + NO_KEY + rt/parameter_events + rcl_interfaces::msg::dds_::ParameterEvent_ + + \ No newline at end of file diff --git a/uROS_Agent/src/main.cpp b/uROS_Agent/src/main.cpp new file mode 100644 index 0000000..e9d986f --- /dev/null +++ b/uROS_Agent/src/main.cpp @@ -0,0 +1,286 @@ + +// 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 + +#elif __unix__ +#include +#include + +#include +#include +#include +#include +#include +#endif + +#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) +{ + +#ifdef __unix__ + char exe_path[200]; + ssize_t count = readlink("/proc/self/exe", exe_path, sizeof(exe_path)); + if (count != -1) { + chdir(dirname(exe_path)); + } +#endif + + bool initialized = false; + +#ifdef __unix__ + 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(); + } +#elif _WIN32 + (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; +} diff --git a/uROS_Agent/uros_agent/__init__.py b/uROS_Agent/uros_agent/__init__.py new file mode 100644 index 0000000..612359e --- /dev/null +++ b/uROS_Agent/uros_agent/__init__.py @@ -0,0 +1,217 @@ +# Copyright 2016 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. + +import os +import sys +import xml.etree.ElementTree + +from rosidl_cmake import convert_camel_case_to_lower_case_underscore +from rosidl_cmake import expand_template +from rosidl_cmake import extract_message_types +from rosidl_cmake import get_newest_modification_time +from rosidl_parser import parse_message_file +from rosidl_parser import parse_service_file +from rosidl_parser import validate_field_types + + +def GetPackage(Dir): + + # ignore? + for f in os.listdir(Dir): + full_path = os.path.join(Dir, f) + if os.path.isfile(full_path): + if f == "COLCON_IGNORE": + return "COLCON_IGNORE" + + # found package + found_package_path = "" + for f in os.listdir(Dir): + if f == "package.xml": + found_package_path = os.path.join(Dir, f) + found_package_path = os.path.abspath(found_package_path) + break + + + return found_package_path + + +def GetPackageList(Dir): + package_list = [] + package_path = GetPackage(Dir) + + if package_path == "": + for f in os.listdir(Dir): + full_path = os.path.join(Dir, f) + if os.path.isdir(full_path): + for l in GetPackageList(full_path): + package_list.append(l) + elif package_path != "COLCON_IGNORE": + package_list.append(package_path) + + return package_list + + +def GetInterfacePackages(packages_list): + package_interface_list = [] + for package in packages_list: + xml_root = xml.etree.ElementTree.parse(package).getroot() + for element in xml_root.findall('member_of_group'): + if element.text == "rosidl_interface_packages": + package_interface_list.append(package) + return package_interface_list + + +def GetPackageName(package_path): + xml_root = xml.etree.ElementTree.parse(package_path).getroot() + xml_name_elements = xml_root.findall('name') + if len(xml_name_elements) != 1: + return "" + else: + return xml_name_elements[0].text + + +def GetInterfacePackageMsgs(package_path): + msg_list = [] + package_dir = os.path.dirname(package_path) + for root, dirs, files in os.walk(package_dir): + for file in files: + if file.endswith(".msg"): + full_path = os.path.join(root, file) + msg_list.append(full_path) + + return msg_list + +def GetInterfacePackageSrvs(package_path): + msg_list = [] + package_dir = os.path.dirname(package_path) + for root, dirs, files in os.walk(package_dir): + for file in files: + if file.endswith(".srv"): + full_path = os.path.join(root, file) + msg_list.append(full_path) + + return msg_list + + +def ReadDefaultXMLs(Path): + for f in os.listdir(Path): + full_path = os.path.join(Path, f) + if os.path.isfile(full_path) and f.endswith(".xml"): + #xml_root = xml.etree.ElementTree.parse(full_path).getroot() + #if xml_root.iselement(): + fd = open(full_path) + print ("%s" % fd.read()) + fd.close() + + + +def generate_XML(args): + pkg_name = args['package_name'] + #known_msg_types = extract_message_types( + # pkg_name, args['ros_interface_files'], args.get('ros_interface_dependencies', [])) + + functions = { + 'get_header_filename_from_msg_name': convert_camel_case_to_lower_case_underscore, + } + + + # Set file format + ros2_prefix = "rt/" + + + # Check destination dir + dest_dir = args['output_dir'] + if not os.path.exists(dest_dir): + os.makedirs(dest_dir) + + + # Check source dir + srcs_dir = os.path.join(dest_dir, "srcs") + if not os.path.exists(srcs_dir): + os.makedirs(srcs_dir) + + + + # Iterate throw all msgs/srvs + for idl_file in args['ros_interface_files']: + extension = os.path.splitext(idl_file)[1] + if extension == '.msg': + spec = parse_message_file(pkg_name, idl_file) + #validate_field_types(spec, known_msg_types) + subfolder = os.path.basename(os.path.dirname(idl_file)) + + data = { + 'spec': spec, + 'pkg': spec.base_type.pkg_name, + 'msg': spec.msg_name, + 'type': spec.base_type.type, + 'subfolder': subfolder, + } + data.update(functions) + + + # Generate source file path + src_file = os.path.join(srcs_dir, "%s_%s_%s.xml" % (spec.base_type.pkg_name, subfolder, spec.msg_name)) + + + # Data writer + #file_content = " \n" + file_content = " \n" % (spec.base_type.pkg_name, subfolder, spec.msg_name) + file_content += " \n" % (spec.base_type.pkg_name, subfolder, spec.msg_name) + file_content += " NO_KEY\n" + file_content += " %s%s_%s_%s\n" % (ros2_prefix, spec.base_type.pkg_name, subfolder, spec.msg_name) + file_content += " %s::%s::dds_::%s_\n" % (spec.base_type.pkg_name, subfolder, spec.msg_name) + file_content += " \n" + file_content += " \n" + #file_content += " \n" + + + # Data reader + #file_content += " \n" + file_content += " \n" % (spec.base_type.pkg_name, subfolder, spec.msg_name) + file_content += " \n" % (spec.base_type.pkg_name, subfolder, spec.msg_name) + file_content += " NO_KEY\n" + file_content += " %s%s_%s_%s\n" % (ros2_prefix, spec.base_type.pkg_name, subfolder, spec.msg_name) + file_content += " %s::%s::dds_::%s_\n" % (spec.base_type.pkg_name, subfolder, spec.msg_name) + file_content += " \n" + file_content += " \n" + #file_content += " \n" + + + # Topic + #file_content += " \n" + file_content += " \n" % (spec.base_type.pkg_name, subfolder, spec.msg_name) + file_content += " %s%s_%s_%s\n" % (ros2_prefix, spec.base_type.pkg_name, subfolder, spec.msg_name) + file_content += " %s::%s::dds_::%s_\n" % (spec.base_type.pkg_name, subfolder, spec.msg_name) + file_content += " \n" + #file_content += " \n" + + + # Write file content + print ("%s" % file_content) + + + + #elif extension == '.srv': + + #data = {'spec': spec} + #data.update(functions) + + #if not os.path.exists(args['output_dir']): + # os.makedirs(args['output_dir']) + + #f = open(os.path.join(args['output_dir'], 'demofile.txt'), 'w+') + #f.write("%s\n" % spec) + #f.close() + return 0 \ No newline at end of file