@############################################### @# @# EmPy template for generating microRTPS_agent.cpp file @# @############################################### @# Start of Template @# @# Context: @# - msgs (List) list of all msg files @# - multi_topics (List) list of all multi-topic names @# - ids (List) list of all RTPS msg ids @############################################### @{ import genmsg.msgs from px_generate_uorb_topic_helper import * # this is in Tools/ from px_generate_uorb_topic_files import MsgScope # this is in Tools/ send_topics = [(alias[idx] if alias[idx] else s.short_name) for idx, s in enumerate(spec) if scope[idx] == MsgScope.SEND] recv_topics = [(alias[idx] if alias[idx] else s.short_name) for idx, s in enumerate(spec) if scope[idx] == MsgScope.RECEIVE] }@ /**************************************************************************** * * Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). * Copyright (c) 2018-2019 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * 1. Redistributions of source code must retain the above copyright notice, this * list of conditions and the following disclaimer. * * 2. Redistributions in binary form must reproduce the above copyright notice, * this list of conditions and the following disclaimer in the documentation * and/or other materials provided with the distribution. * * 3. Neither the name of the copyright holder nor the names of its contributors * may be used to endorse or promote products derived from this software without * specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include "microRTPS_transport.h" #include "microRTPS_timesync.h" #include "RtpsTopics.h" // Default values #define DEVICE "/dev/ttyACM0" #define SLEEP_US 1 #define BAUDRATE 460800 #define POLL_MS 1 #define WAIT_CNST 2 #define DEFAULT_RECV_PORT 2020 #define DEFAULT_SEND_PORT 2019 #define DEFAULT_IP "127.0.0.1" using namespace eprosima; using namespace eprosima::fastrtps; volatile sig_atomic_t running = 1; Transport_node *transport_node = nullptr; RtpsTopics topics; uint32_t total_sent = 0, sent = 0; struct options { enum class eTransports { UART, UDP }; eTransports transport = options::eTransports::UART; char device[64] = DEVICE; int sleep_us = SLEEP_US; uint32_t baudrate = BAUDRATE; int poll_ms = POLL_MS; uint16_t recv_port = DEFAULT_RECV_PORT; uint16_t send_port = DEFAULT_SEND_PORT; char ip[16] = DEFAULT_IP; bool sw_flow_control = false; bool hw_flow_control = false; bool verbose_debug = false; std::string ns = ""; } _options; static void usage(const char *name) { printf("usage: %s [options]\n\n" " -b UART device baudrate. Default 460800\n" " -d UART device. Default /dev/ttyACM0\n" " -f Activates UART link SW flow control\n" " -h Activates UART link HW flow control\n" " -i Target IP for UDP. Default 127.0.0.1\n" " -n ROS 2 topics namespace. Identifies the vehicle in a multi-agent network\n" " -p Time in ms to poll over UART. Default 1ms\n" " -r UDP port for receiving. Default 2019\n" " -s UDP port for sending. Default 2020\n" " -t [UART|UDP] Default UART\n" " -v Add more verbosity\n" " -w Time in us for which each iteration sleep. Default 1ms\n", name); } static int parse_options(int argc, char **argv) { int ch; while ((ch = getopt(argc, argv, "t:d:w:b:p:r:s:i:fhvn:")) != EOF) { switch (ch) { case 't': _options.transport = strcmp(optarg, "UDP") == 0? options::eTransports::UDP :options::eTransports::UART; break; case 'd': if (nullptr != optarg) strcpy(_options.device, optarg); break; case 'w': _options.sleep_us = strtol(optarg, nullptr, 10); break; case 'b': _options.baudrate = strtoul(optarg, nullptr, 10); break; case 'p': _options.poll_ms = strtol(optarg, nullptr, 10); break; case 'r': _options.recv_port = strtoul(optarg, nullptr, 10); break; case 's': _options.send_port = strtoul(optarg, nullptr, 10); break; case 'i': if (nullptr != optarg) strcpy(_options.ip, optarg); break; case 'f': _options.sw_flow_control = true; break; case 'h': _options.hw_flow_control = true; break; case 'v': _options.verbose_debug = true; break; case 'n': if (nullptr != optarg) _options.ns = std::string(optarg) + "/"; break; default: usage(argv[0]); return -1; } } if (_options.poll_ms < 1) { _options.poll_ms = 1; printf("\033[1;33m[ micrortps_agent ]\tPoll timeout too low, using 1 ms\033[0m"); } if (_options.hw_flow_control && _options.sw_flow_control) { printf("\033[0;31m[ micrortps_agent ]\tHW and SW flow control set. Please set only one or another\033[0m"); return -1; } return 0; } void signal_handler(int signum) { printf("\033[1;33m[ micrortps_agent ]\tInterrupt signal (%d) received.\033[0m\n", signum); running = 0; transport_node->close(); } @[if recv_topics]@ std::atomic exit_sender_thread(false); std::condition_variable t_send_queue_cv; std::mutex t_send_queue_mutex; std::queue t_send_queue; void t_send(void*) { char data_buffer[BUFFER_SIZE] = {}; uint32_t length = 0; while (running && !exit_sender_thread.load()) { std::unique_lock lk(t_send_queue_mutex); while (t_send_queue.empty() && !exit_sender_thread.load()) { t_send_queue_cv.wait(lk); } uint8_t topic_ID = t_send_queue.front(); t_send_queue.pop(); lk.unlock(); size_t header_length = transport_node->get_header_length(); /* make room for the header to fill in later */ eprosima::fastcdr::FastBuffer cdrbuffer(&data_buffer[header_length], sizeof(data_buffer)-header_length); eprosima::fastcdr::Cdr scdr(cdrbuffer); if (topics.getMsg(topic_ID, scdr)) { length = scdr.getSerializedDataLength(); if (0 < (length = transport_node->write(topic_ID, data_buffer, length))) { total_sent += length; ++sent; } } } } @[end if]@ int main(int argc, char** argv) { if (-1 == parse_options(argc, argv)) { printf("\033[1;33m[ micrortps_agent ]\tEXITING...\033[0m\n"); return -1; } // register signal SIGINT and signal handler signal(SIGINT, signal_handler); printf("\033[0;37m--- MicroRTPS Agent ---\033[0m\n"); printf("[ micrortps_agent ]\tStarting link...\n"); switch (_options.transport) { case options::eTransports::UART: { transport_node = new UART_node(_options.device, _options.baudrate, _options.poll_ms, _options.sw_flow_control, _options.hw_flow_control, _options.verbose_debug); printf("[ micrortps_agent ]\tUART transport: device: %s; baudrate: %d; sleep: %dus; poll: %dms; flow_control: %s\n", _options.device, _options.baudrate, _options.sleep_us, _options.poll_ms, _options.sw_flow_control ? "SW enabled" : (_options.hw_flow_control ? "HW enabled" : "No")); } break; case options::eTransports::UDP: { transport_node = new UDP_node(_options.ip, _options.recv_port, _options.send_port, _options.verbose_debug); printf("[ micrortps_agent ]\tUDP transport: ip address: %s; recv port: %u; send port: %u; sleep: %dus\n", _options.ip, _options.recv_port, _options.send_port, _options.sleep_us); } break; default: printf("\033[0;37m[ micrortps_agent ]\tEXITING...\033[0m\n"); return -1; } if (0 > transport_node->init()) { printf("\033[0;37m[ micrortps_agent ]\tEXITING...\033[0m\n"); return -1; } sleep(1); @[if send_topics]@ char data_buffer[BUFFER_SIZE] = {}; int received = 0, loop = 0; int length = 0, total_read = 0; bool receiving = false; uint8_t topic_ID = 255; std::chrono::time_point start, end; @[end if]@ // Init timesync std::shared_ptr timeSync = std::make_shared(_options.verbose_debug); topics.set_timesync(timeSync); @[if recv_topics]@ topics.init(&t_send_queue_cv, &t_send_queue_mutex, &t_send_queue, _options.ns); @[end if]@ running = true; @[if recv_topics]@ std::thread sender_thread(t_send, nullptr); @[end if]@ while (running) { @[if send_topics]@ ++loop; if (!receiving) start = std::chrono::steady_clock::now(); // Publish messages received from UART while (0 < (length = transport_node->read(&topic_ID, data_buffer, BUFFER_SIZE))) { topics.publish(topic_ID, data_buffer, sizeof(data_buffer)); ++received; total_read += length; receiving = true; end = std::chrono::steady_clock::now(); } if ((receiving && std::chrono::duration(std::chrono::steady_clock::now() - end).count() > WAIT_CNST) || (!running && loop > 1)) { std::chrono::duration elapsed_secs = end - start; printf("[ micrortps_agent ]\tSENT: %lumessages \t- %lubytes\n", (unsigned long)sent, (unsigned long)total_sent); printf("[ micrortps_agent ]\tRECEIVED: %dmessages \t- %dbytes; %d LOOPS - %.03f seconds - %.02fKB/s\n", received, total_read, loop, elapsed_secs.count(), (double)total_read/(1000*elapsed_secs.count())); received = sent = total_read = total_sent = 0; receiving = false; } @[else]@ usleep(_options.sleep_us); @[end if]@ } @[if recv_topics]@ exit_sender_thread = true; t_send_queue_cv.notify_one(); sender_thread.join(); @[end if]@ delete transport_node; transport_node = nullptr; timeSync->stop(); timeSync->reset(); return 0; }