mirror of
https://github.com/micro-ROS/micro-ROS-Agent.git
synced 2026-09-06 09:55:15 +02:00
Borja Outerelo
@@ -0,0 +1,8 @@
|
||||
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
Changelog for package micro-ros_agent
|
||||
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
|
||||
0.0.1 (2019-04-24)
|
||||
-----------------
|
||||
* Initial release
|
||||
|
||||
@@ -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 "${PROJECT_BINARY_DIR}" 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 "<profiles>\n${_XmlDoc}</profiles>\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}
|
||||
#)
|
||||
@@ -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).
|
||||
@@ -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())
|
||||
@@ -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())
|
||||
@@ -0,0 +1,36 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>micro-ros_agent</name>
|
||||
<version>0.0.1</version>
|
||||
<description>DDS-XCRE agent implementation </description>
|
||||
<maintainer email="borjaouterelo@eprosima.com">Borja Outerelo</maintainer>
|
||||
<license>Apache License 2.0</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
<buildtool_export_depend>ament_cmake</buildtool_export_depend>
|
||||
|
||||
<exec_depend>rosidl_parser</exec_depend>
|
||||
|
||||
<depend>fastcdr</depend>
|
||||
<depend>fastrtps</depend>
|
||||
<depend>microxrcedds_agent_cmake_module</depend>
|
||||
<depend>rosidl_cmake</depend>
|
||||
|
||||
<build_depend>rclcpp</build_depend>
|
||||
|
||||
<exec_depend>rclcpp</exec_depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<group_depend>rosidl_interface_packages</group_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,24 @@
|
||||
<participant profile_name="participant_profile">
|
||||
<rtps>
|
||||
<name>participant_profile</name>
|
||||
</rtps>
|
||||
</participant>
|
||||
<data_writer profile_name="shapetype_data_writer">
|
||||
<topic>
|
||||
<kind>WITH_KEY</kind>
|
||||
<name>Square</name>
|
||||
<dataType>ShapeType</dataType>
|
||||
</topic>
|
||||
</data_writer>
|
||||
<data_reader profile_name="shapetype_data_reader">
|
||||
<topic>
|
||||
<kind>WITH_KEY</kind>
|
||||
<name>Square</name>
|
||||
<dataType>ShapeType</dataType>
|
||||
</topic>
|
||||
</data_reader>
|
||||
<topic profile_name="shapetype_topic">
|
||||
<kind>WITH_KEY</kind>
|
||||
<name>Square</name>
|
||||
<dataType>ShapeType</dataType>
|
||||
</topic>
|
||||
@@ -0,0 +1,29 @@
|
||||
<topic profile_name="parameter_events_t">
|
||||
<name>rt/parameter_events</name>
|
||||
<dataType>rcl_interfaces::msg::dds_::ParameterEvent_</dataType>
|
||||
</topic>
|
||||
<topic profile_name="clock_t">
|
||||
<name>rt/clock</name>
|
||||
<dataType>rosgraph_msgs::msg::dds_::Clock_</dataType>
|
||||
</topic>
|
||||
<data_writer profile_name="parameter_events_p">
|
||||
<topic>
|
||||
<kind>NO_KEY</kind>
|
||||
<name>rt/parameter_events</name>
|
||||
<dataType>rcl_interfaces::msg::dds_::ParameterEvent_</dataType>
|
||||
</topic>
|
||||
</data_writer>
|
||||
<data_reader profile_name="clock_s">
|
||||
<topic>
|
||||
<kind>NO_KEY</kind>
|
||||
<name>rt/clock</name>
|
||||
<dataType>rosgraph_msgs::msg::dds_::Clock_</dataType>
|
||||
</topic>
|
||||
</data_reader>
|
||||
<data_reader profile_name="parameter_events_s">
|
||||
<topic>
|
||||
<kind>NO_KEY</kind>
|
||||
<name>rt/parameter_events</name>
|
||||
<dataType>rcl_interfaces::msg::dds_::ParameterEvent_</dataType>
|
||||
</topic>
|
||||
</data_reader>
|
||||
@@ -0,0 +1,237 @@
|
||||
|
||||
// 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 <uxr/agent/transport/udp/UDPServerWindows.hpp>
|
||||
#include <uxr/agent/transport/tcp/TCPServerWindows.hpp>
|
||||
|
||||
#elif __unix__
|
||||
#include <unistd.h>
|
||||
#include <libgen.h>
|
||||
|
||||
#include <uxr/agent/transport/serial/SerialServerLinux.hpp>
|
||||
#include <uxr/agent/transport/udp/UDPServerLinux.hpp>
|
||||
#include <uxr/agent/transport/tcp/TCPServerLinux.hpp>
|
||||
#include <termios.h>
|
||||
#include <fcntl.h>
|
||||
#endif
|
||||
|
||||
#include <iterator>
|
||||
#include <iostream>
|
||||
#include <string>
|
||||
#include <limits>
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
void showHelp()
|
||||
{
|
||||
std::cout << "Usage: program <command>" << std::endl;
|
||||
std::cout << "List of commands:" << std::endl;
|
||||
#ifdef _WIN32
|
||||
std::cout << " udp <local_port>" << std::endl;
|
||||
std::cout << " tcp <local_port>" << std::endl;
|
||||
#else
|
||||
std::cout << " serial <device_name>" << std::endl;
|
||||
std::cout << " pseudo-serial" << std::endl;
|
||||
std::cout << " udp <local_port> [<discovery_port>]" << std::endl;
|
||||
std::cout << " tcp <local_port> [<discovery_port>]" << std::endl;
|
||||
#endif
|
||||
}
|
||||
|
||||
void initializationError()
|
||||
{
|
||||
std::cout << "Error: Invalid arguments." << std::endl;
|
||||
showHelp();
|
||||
std::exit(EXIT_FAILURE);
|
||||
}
|
||||
|
||||
uint16_t parsePort(const std::string& str_port)
|
||||
{
|
||||
uint16_t valid_port = 0;
|
||||
try
|
||||
{
|
||||
int port = std::stoi(str_port);
|
||||
if(port > (std::numeric_limits<uint16_t>::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;
|
||||
}
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
eprosima::uxr::Server* server = nullptr;
|
||||
std::vector<std::string> cl(0);
|
||||
|
||||
if (1 == argc)
|
||||
{
|
||||
showHelp();
|
||||
std::cout << std::endl;
|
||||
std::cout << "Enter command: ";
|
||||
|
||||
std::string raw_cl;
|
||||
std::getline(std::cin, raw_cl);
|
||||
std::istringstream iss(raw_cl);
|
||||
cl.insert(cl.begin(), std::istream_iterator<std::string>(iss), std::istream_iterator<std::string>());
|
||||
std::cout << raw_cl << std::endl;
|
||||
}
|
||||
else
|
||||
{
|
||||
for (int i = 1; i < argc; ++i)
|
||||
{
|
||||
cl.push_back(argv[i]);
|
||||
}
|
||||
}
|
||||
|
||||
if((1 == cl.size()) && (("-h" == cl[0]) || ("--help" == cl[0])))
|
||||
{
|
||||
showHelp();
|
||||
}
|
||||
else if((2 <= cl.size()) && ("udp" == cl[0]))
|
||||
{
|
||||
std::cout << "UDP agent initialization... ";
|
||||
uint16_t port = parsePort(cl[1]);
|
||||
#ifdef _WIN32
|
||||
server = new eprosima::uxr::UDPServer(port);
|
||||
#else
|
||||
server = (3 == cl.size()) //discovery port
|
||||
? new eprosima::uxr::UDPServer(port, parsePort(cl[2]))
|
||||
: new eprosima::uxr::UDPServer(port);
|
||||
#endif
|
||||
}
|
||||
else if((2 <= cl.size()) && ("tcp" == cl[0]))
|
||||
{
|
||||
std::cout << "TCP agent initialization... ";
|
||||
uint16_t port = parsePort(cl[1]);
|
||||
#ifdef _WIN32
|
||||
server = new eprosima::uxr::TCPServer(port);
|
||||
#else
|
||||
server = (3 == cl.size()) //discovery port
|
||||
? new eprosima::uxr::TCPServer(port, parsePort(cl[2]))
|
||||
: new eprosima::uxr::TCPServer(port);
|
||||
#endif
|
||||
}
|
||||
#ifndef _WIN32
|
||||
else if((2 == cl.size()) && ("serial" == cl[0]))
|
||||
{
|
||||
std::cout << "Serial agent initialization... ";
|
||||
|
||||
/* Open serial device. */
|
||||
int fd = open(cl[1].c_str(), 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))
|
||||
{
|
||||
server = new eprosima::uxr::SerialServer(fd, 0);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
else if ((1 == cl.size()) && ("pseudo-serial" == cl[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);
|
||||
std::cout << "Device: " << dev << std::endl;
|
||||
}
|
||||
}
|
||||
server = new eprosima::uxr::SerialServer(fd, 0x00);
|
||||
}
|
||||
#endif
|
||||
else
|
||||
{
|
||||
initializationError();
|
||||
}
|
||||
|
||||
if (nullptr != server)
|
||||
{
|
||||
/* Launch server. */
|
||||
if (server->run())
|
||||
{
|
||||
std::cout << "OK" << std::endl;
|
||||
std::cin.clear();
|
||||
char exit_flag = 0;
|
||||
while ('q' != exit_flag)
|
||||
{
|
||||
std::cout << "Enter 'q' for exit" << std::endl;
|
||||
std::cin >> exit_flag;
|
||||
}
|
||||
server->stop();
|
||||
}
|
||||
else
|
||||
{
|
||||
std::cout << "ERROR" << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -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.replace("\\", "/")
|
||||
|
||||
|
||||
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.replace("\\", "/"))
|
||||
|
||||
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.replace("\\", "/"))
|
||||
|
||||
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 = " <dds>\n"
|
||||
file_content = " <data_writer profile_name=\"%s_%s_%s_p\">\n" % (spec.base_type.pkg_name, subfolder, spec.msg_name)
|
||||
file_content += " <topic profile_name=\"%s_%s_%s_t\">\n" % (spec.base_type.pkg_name, subfolder, spec.msg_name)
|
||||
file_content += " <kind>NO_KEY</kind>\n"
|
||||
file_content += " <name>%s%s_%s_%s</name>\n" % (ros2_prefix, spec.base_type.pkg_name, subfolder, spec.msg_name)
|
||||
file_content += " <dataType>%s::%s::dds_::%s_</dataType>\n" % (spec.base_type.pkg_name, subfolder, spec.msg_name)
|
||||
file_content += " </topic>\n"
|
||||
file_content += " </data_writer>\n"
|
||||
#file_content += " </dds>\n"
|
||||
|
||||
|
||||
# Data reader
|
||||
#file_content += " <dds>\n"
|
||||
file_content += " <data_reader profile_name=\"%s_%s_%s_s\">\n" % (spec.base_type.pkg_name, subfolder, spec.msg_name)
|
||||
file_content += " <topic profile_name=\"%s_%s_%s_t\">\n" % (spec.base_type.pkg_name, subfolder, spec.msg_name)
|
||||
file_content += " <kind>NO_KEY</kind>\n"
|
||||
file_content += " <name>%s%s_%s_%s</name>\n" % (ros2_prefix, spec.base_type.pkg_name, subfolder, spec.msg_name)
|
||||
file_content += " <dataType>%s::%s::dds_::%s_</dataType>\n" % (spec.base_type.pkg_name, subfolder, spec.msg_name)
|
||||
file_content += " </topic>\n"
|
||||
file_content += " </data_reader>\n"
|
||||
#file_content += " </dds>\n"
|
||||
|
||||
|
||||
# Topic
|
||||
#file_content += " <dds>\n"
|
||||
file_content += " <topic profile_name=\"%s_%s_%s_t\">\n" % (spec.base_type.pkg_name, subfolder, spec.msg_name)
|
||||
file_content += " <name>%s%s_%s_%s</name>\n" % (ros2_prefix, spec.base_type.pkg_name, subfolder, spec.msg_name)
|
||||
file_content += " <dataType>%s::%s::dds_::%s_</dataType>\n" % (spec.base_type.pkg_name, subfolder, spec.msg_name)
|
||||
file_content += " </topic>\n"
|
||||
#file_content += " </dds>\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
|
||||
Reference in New Issue
Block a user