yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 01:12:11 +00:00
Code Issues Packages Projects Releases Wiki Activity

Extracted non-template code from template

Browse Source 
Signed-off-by: Mark Charlebois <charlebm@gmail.com>
This commit is contained in:
Mark Charlebois
2017-07-31 12:58:36 -07:00
committed by Lorenz Meier
parent b31006dd78
commit d24503242e
4 changed files with 297 additions and 190 deletions
Download Patch File Download Diff File Expand all files Collapse all files

192
msg/templates/uorb/microRTPS_client.cpp.template
View File

@@ -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 <cinttypes>
#include <cstdio>
#include <ctime>
#include <pthread.h>
#include <termios.h>
#include <microcdr/microCdr.h>
#include <px4_config.h>
#include <px4_getopt.h>
#include <px4_posix.h>
#include <px4_tasks.h>
#include <px4_time.h>
#include <uORB/uORB.h>
@@ -73,85 +69,8 @@ recv_topics = [s.short_name for idx, s in enumerate(spec) if scope[idx] == MsgSc
#include <uORB/topics/@(topic).h>
@[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 <transport> [UART|UDP] Default UART\n"
" -d <device> UART device. Default /dev/ttyACM0\n"
" -u <update_time_ms> Time in ms for uORB subscribed topics update. Default 0\n"
" -l <loops> How many iterations will this program have. -1 for infinite. Default 10000\n"
" -w <sleep_time_ms> Time in ms for which each iteration sleep. Default 1ms\n"
" -b <baudrate> UART device baudrate. Default 460800\n"
" -p <poll_ms> Time in ms to poll over UART. Default 1ms\n"
" -r <reception port> UDP port for receiving. Default 2019\n"
" -s <sending port> 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(&microCDRReader, &microBufferReader);
@[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;
}

3
src/modules/micrortps_bridge/micrortps_client/CMakeLists.txt
View File

@@ -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
)

82
src/modules/micrortps_bridge/micrortps_client/microRTPS_client.h Normal file
View File

@@ -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 <cinttypes>
#include <cstdio>
#include <ctime>
#include <pthread.h>
#include <termios.h>
#include <microcdr/microCdr.h>
#include <px4_config.h>
#include <px4_getopt.h>
#include <px4_posix.h>
#include <px4_tasks.h>
#include <px4_time.h>
#include <uORB/uORB.h>
#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;

210
src/modules/micrortps_bridge/micrortps_client/microRTPS_client_main.cpp Normal file
View File

@@ -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 <cinttypes>
#include <cstdio>
#include <ctime>
#include <termios.h>
#include <px4_config.h>
#include <px4_getopt.h>
#include <px4_posix.h>
#include <px4_tasks.h>
#include <px4_time.h>
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 <transport> [UART|UDP] Default UART\n"
" -d <device> UART device. Default /dev/ttyACM0\n"
" -u <update_time_ms> Time in ms for uORB subscribed topics update. Default 0\n"
" -l <loops> How many iterations will this program have. -1 for infinite. Default 10000\n"
" -w <sleep_time_ms> Time in ms for which each iteration sleep. Default 1ms\n"
" -b <baudrate> UART device baudrate. Default 460800\n"
" -p <poll_ms> Time in ms to poll over UART. Default 1ms\n"
" -r <reception port> UDP port for receiving. Default 2019\n"
" -s <sending port> 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;
}