diff --git a/msg/templates/uorb/microRTPS_client.cpp.template b/msg/templates/uorb/microRTPS_client.cpp.template index b6a9b9acf3..5674a439af 100644 --- a/msg/templates/uorb/microRTPS_client.cpp.template +++ b/msg/templates/uorb/microRTPS_client.cpp.template @@ -54,18 +54,14 @@ recv_topics = [s.short_name for idx, s in enumerate(spec) if scope[idx] == MsgSc ****************************************************************************/ #include "microRTPS_transport.h" +#include "microRTPS_client.h" #include #include #include #include -#include #include -#include -#include -#include -#include #include #include @@ -73,85 +69,8 @@ recv_topics = [s.short_name for idx, s in enumerate(spec) if scope[idx] == MsgSc #include @[end for]@ -#define BUFFER_SIZE 1024 -#define UPDATE_TIME_MS 0 -#define LOOPS 10000 -#define SLEEP_MS 1 -#define BAUDRATE 460800 -#define DEVICE "/dev/ttyACM0" -#define POLL_MS 1 -#define DEFAULT_RECV_PORT 2019 -#define DEFAULT_SEND_PORT 2020 - -extern "C" __EXPORT int micrortps_client_main(int argc, char *argv[]); void* send(void *data); -struct options { - enum class eTransports - { - UART, - UDP - }; - eTransports transport = options::eTransports::UART; - char device[64] = DEVICE; - int update_time_ms = UPDATE_TIME_MS; - int loops = LOOPS; - int sleep_ms = SLEEP_MS; - uint32_t baudrate = BAUDRATE; - int poll_ms = POLL_MS; - uint16_t recv_port = DEFAULT_RECV_PORT; - uint16_t send_port = DEFAULT_SEND_PORT; -} _options; - -static int _rtps_task = -1; -static bool _should_exit_task = false; -Transport_node *transport_node = nullptr; - -static void usage(const char *name) -{ - PX4_INFO("usage: %s start|stop [options]\n\n" - " -t [UART|UDP] Default UART\n" - " -d UART device. Default /dev/ttyACM0\n" - " -u Time in ms for uORB subscribed topics update. Default 0\n" - " -l How many iterations will this program have. -1 for infinite. Default 10000\n" - " -w Time in ms for which each iteration sleep. Default 1ms\n" - " -b UART device baudrate. Default 460800\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", - name); -} - -static int parse_options(int argc, char *argv[]) -{ - int ch; - int myoptind = 1; - const char *myoptarg = nullptr; - - while ((ch = px4_getopt(argc, argv, "t:d:u:l:w:b:p:r:s:", &myoptind, &myoptarg)) != EOF) - { - switch (ch) - { - case 't': _options.transport = strcmp(myoptarg, "UDP") == 0? - options::eTransports::UDP - :options::eTransports::UART; break; - case 'd': if (nullptr != myoptarg) strcpy(_options.device, myoptarg); break; - case 'u': _options.update_time_ms = strtol(myoptarg, nullptr, 10); break; - case 'l': _options.loops = strtol(myoptarg, nullptr, 10); break; - case 'w': _options.sleep_ms = strtol(myoptarg, 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; - default: - usage(argv[1]); - return -1; - } - } - - return 0; -} - @[if send_topics]@ void* send(void* /*unused*/) { @@ -228,45 +147,8 @@ static int launch_send_thread(pthread_t &sender_thread) } @[end if]@ -static int micrortps_start(int argc, char *argv[]) +void micrortps_start_topics(struct timespec &begin, int &total_read, uint32_t &received, int &loop) { - if (0 > parse_options(argc, argv)) - { - printf("EXITING...\n"); - _rtps_task = -1; - return -1; - } - - switch (_options.transport) - { - case options::eTransports::UART: - { - transport_node = new UART_node(_options.device, _options.baudrate, _options.poll_ms); - printf("\nUART transport: device: %s; baudrate: %d; sleep: %dms; poll: %dms\n\n", - _options.device, _options.baudrate, _options.sleep_ms, _options.poll_ms); - } - break; - case options::eTransports::UDP: - { - transport_node = new UDP_node(_options.recv_port, _options.send_port); - printf("\nUDP transport: recv port: %u; send port: %u; sleep: %dms\n\n", - _options.recv_port, _options.send_port, _options.sleep_ms); - } - break; - default: - _rtps_task = -1; - printf("EXITING...\n"); - return -1; - } - - if (0 > transport_node->init()) - { - printf("EXITING...\n"); - _rtps_task = -1; - return -1; - } - - sleep(1); @[if recv_topics]@ char data_buffer[BUFFER_SIZE] = {}; @@ -287,11 +169,7 @@ static int micrortps_start(int argc, char *argv[]) initMicroCDR(µCDRReader, µBufferReader); @[end if]@ - int total_read = 0; - uint32_t received = 0; - struct timespec begin; px4_clock_gettime(CLOCK_REALTIME, &begin); - int loop = 0; _should_exit_task = false; @[if send_topics]@ @@ -338,70 +216,4 @@ static int micrortps_start(int argc, char *argv[]) _should_exit_task = true; pthread_join(sender_thread, nullptr); @[end if]@ - - struct timespec end; - px4_clock_gettime(CLOCK_REALTIME, &end); - double elapsed_secs = double(end.tv_sec - begin.tv_sec) + double(end.tv_nsec - begin.tv_nsec)/double(1000000000); - printf("RECEIVED: %lu messages in %d LOOPS, %d bytes in %.03f seconds - %.02fKB/s\n\n", - (unsigned long)received, loop, total_read, elapsed_secs, (double)total_read/(1000*elapsed_secs)); - - delete transport_node; - transport_node = nullptr; - PX4_INFO("exiting"); - fflush(stdout); - - _rtps_task = -1; - - return 0; -} - -int micrortps_client_main(int argc, char *argv[]) -{ - if (argc < 2) - { - usage(argv[0]); - return -1; - } - - if (!strcmp(argv[1], "start")) - { - if (_rtps_task != -1) - { - PX4_INFO("Already running"); - return -1; - } - - _rtps_task = px4_task_spawn_cmd("rtps", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 4096, - (px4_main_t) micrortps_start, - (char *const *)argv); - - if (_rtps_task < 0) - { - PX4_WARN("Could not start task"); - _rtps_task = -1; - return -1; - } - - return 0; - } - - if (!strcmp(argv[1], "stop")) - { - if (_rtps_task == -1) - { - PX4_INFO("Not running"); - return -1; - } - - _should_exit_task = true; - if (nullptr != transport_node) transport_node->close(); - return 0; - } - - usage(argv[0]); - - return -1; } diff --git a/src/modules/micrortps_bridge/micrortps_client/CMakeLists.txt b/src/modules/micrortps_bridge/micrortps_client/CMakeLists.txt index 1e7c028d0f..458e42a3c7 100644 --- a/src/modules/micrortps_bridge/micrortps_client/CMakeLists.txt +++ b/src/modules/micrortps_bridge/micrortps_client/CMakeLists.txt @@ -33,6 +33,7 @@ if(NOT GENERATE_RTPS_BRIDGE MATCHES "off") include_directories(.) + include_directories(${CMAKE_CURRENT_BINARY_DIR}) # Do not delete everything in the current dir set(msg_out_path ${CMAKE_CURRENT_BINARY_DIR}) @@ -95,6 +96,8 @@ px4_add_module( SRCS microRTPS_transport.cpp microRTPS_client.cpp + microRTPS_client.h + microRTPS_client_main.cpp DEPENDS platforms__common ) diff --git a/src/modules/micrortps_bridge/micrortps_client/microRTPS_client.h b/src/modules/micrortps_bridge/micrortps_client/microRTPS_client.h new file mode 100644 index 0000000000..65a2ba3c65 --- /dev/null +++ b/src/modules/micrortps_bridge/micrortps_client/microRTPS_client.h @@ -0,0 +1,82 @@ +/**************************************************************************** + * + * Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). + * + * 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 "microRTPS_transport.h" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#define BUFFER_SIZE 1024 +#define UPDATE_TIME_MS 0 +#define LOOPS 10000 +#define SLEEP_MS 1 +#define BAUDRATE 460800 +#define DEVICE "/dev/ttyACM0" +#define POLL_MS 1 +#define DEFAULT_RECV_PORT 2019 +#define DEFAULT_SEND_PORT 2020 + +void* send(void *data); +void micrortps_start_topics(struct timespec &begin, int &total_read, uint32_t &received, int &loop); + +struct options { + enum class eTransports + { + UART, + UDP + }; + eTransports transport = options::eTransports::UART; + char device[64] = DEVICE; + int update_time_ms = UPDATE_TIME_MS; + int loops = LOOPS; + int sleep_ms = SLEEP_MS; + uint32_t baudrate = BAUDRATE; + int poll_ms = POLL_MS; + uint16_t recv_port = DEFAULT_RECV_PORT; + uint16_t send_port = DEFAULT_SEND_PORT; +}; + +extern struct options _options; +extern bool _should_exit_task; +extern Transport_node *transport_node; + diff --git a/src/modules/micrortps_bridge/micrortps_client/microRTPS_client_main.cpp b/src/modules/micrortps_bridge/micrortps_client/microRTPS_client_main.cpp new file mode 100644 index 0000000000..593d6df3dd --- /dev/null +++ b/src/modules/micrortps_bridge/micrortps_client/microRTPS_client_main.cpp @@ -0,0 +1,210 @@ +/**************************************************************************** + * + * Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). + * + * 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 "microRTPS_transport.h" +#include "microRTPS_client.h" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +extern "C" __EXPORT int micrortps_client_main(int argc, char *argv[]); + +static int _rtps_task = -1; +bool _should_exit_task = false; +Transport_node *transport_node = nullptr; +struct options _options; + +static void usage(const char *name) +{ + PX4_INFO("usage: %s start|stop [options]\n\n" + " -t [UART|UDP] Default UART\n" + " -d UART device. Default /dev/ttyACM0\n" + " -u Time in ms for uORB subscribed topics update. Default 0\n" + " -l How many iterations will this program have. -1 for infinite. Default 10000\n" + " -w Time in ms for which each iteration sleep. Default 1ms\n" + " -b UART device baudrate. Default 460800\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", + name); +} + +static int parse_options(int argc, char *argv[]) +{ + int ch; + int myoptind = 1; + const char *myoptarg = nullptr; + + while ((ch = px4_getopt(argc, argv, "t:d:u:l:w:b:p:r:s:", &myoptind, &myoptarg)) != EOF) + { + switch (ch) + { + case 't': _options.transport = strcmp(myoptarg, "UDP") == 0? + options::eTransports::UDP + :options::eTransports::UART; break; + case 'd': if (nullptr != myoptarg) strcpy(_options.device, myoptarg); break; + case 'u': _options.update_time_ms = strtol(myoptarg, nullptr, 10); break; + case 'l': _options.loops = strtol(myoptarg, nullptr, 10); break; + case 'w': _options.sleep_ms = strtol(myoptarg, 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; + default: + usage(argv[1]); + return -1; + } + } + + return 0; +} + +static int micrortps_start(int argc, char *argv[]) +{ + if (0 > parse_options(argc, argv)) + { + printf("EXITING...\n"); + _rtps_task = -1; + return -1; + } + + switch (_options.transport) + { + case options::eTransports::UART: + { + transport_node = new UART_node(_options.device, _options.baudrate, _options.poll_ms); + printf("\nUART transport: device: %s; baudrate: %d; sleep: %dms; poll: %dms\n\n", + _options.device, _options.baudrate, _options.sleep_ms, _options.poll_ms); + } + break; + case options::eTransports::UDP: + { + transport_node = new UDP_node(_options.recv_port, _options.send_port); + printf("\nUDP transport: recv port: %u; send port: %u; sleep: %dms\n\n", + _options.recv_port, _options.send_port, _options.sleep_ms); + } + break; + default: + _rtps_task = -1; + printf("EXITING...\n"); + return -1; + } + + if (0 > transport_node->init()) + { + printf("EXITING...\n"); + _rtps_task = -1; + return -1; + } + + sleep(1); + + struct timespec begin; + int total_read = 0; + uint32_t received = 0; + int loop = 0; + micrortps_start_topics(begin, total_read, received, loop); + + struct timespec end; + px4_clock_gettime(CLOCK_REALTIME, &end); + double elapsed_secs = double(end.tv_sec - begin.tv_sec) + double(end.tv_nsec - begin.tv_nsec)/double(1000000000); + printf("RECEIVED: %lu messages in %d LOOPS, %d bytes in %.03f seconds - %.02fKB/s\n\n", + (unsigned long)received, loop, total_read, elapsed_secs, (double)total_read/(1000*elapsed_secs)); + + delete transport_node; + transport_node = nullptr; + PX4_INFO("exiting"); + fflush(stdout); + + _rtps_task = -1; + + return 0; +} + +int micrortps_client_main(int argc, char *argv[]) +{ + if (argc < 2) + { + usage(argv[0]); + return -1; + } + + if (!strcmp(argv[1], "start")) + { + if (_rtps_task != -1) + { + PX4_INFO("Already running"); + return -1; + } + + _rtps_task = px4_task_spawn_cmd("rtps", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 4096, + (px4_main_t) micrortps_start, + (char *const *)argv); + + if (_rtps_task < 0) + { + PX4_WARN("Could not start task"); + _rtps_task = -1; + return -1; + } + + return 0; + } + + if (!strcmp(argv[1], "stop")) + { + if (_rtps_task == -1) + { + PX4_INFO("Not running"); + return -1; + } + + _should_exit_task = true; + if (nullptr != transport_node) transport_node->close(); + return 0; + } + + usage(argv[0]); + + return -1; +}