mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-19 05:33:34 +08:00
microRTPS: agent: match the code style from the PX4 Firmware
This commit is contained in:
TSC21
@@ -91,89 +91,98 @@ 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 = "";
|
||||
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 <baudrate> UART device baudrate. Default 460800\n"
|
||||
" -d <device> UART device. Default /dev/ttyACM0\n"
|
||||
" -f <sw flow control> Activates UART link SW flow control\n"
|
||||
" -h <hw flow control> Activates UART link HW flow control\n"
|
||||
" -i <ip_address> Target IP for UDP. Default 127.0.0.1\n"
|
||||
" -n <namespace> ROS 2 topics namespace. Identifies the vehicle in a multi-agent network\n"
|
||||
" -p <poll_ms> Time in ms to poll over UART. Default 1ms\n"
|
||||
" -r <reception port> UDP port for receiving. Default 2020\n"
|
||||
" -s <sending port> UDP port for sending. Default 2019\n"
|
||||
" -t <transport> [UART|UDP] Default UART\n"
|
||||
" -v <debug verbosity> Add more verbosity\n"
|
||||
" -w <sleep_time_us> Time in us for which each iteration sleep. Default 1ms\n",
|
||||
name);
|
||||
printf("usage: %s [options]\n\n"
|
||||
" -b <baudrate> UART device baudrate. Default 460800\n"
|
||||
" -d <device> UART device. Default /dev/ttyACM0\n"
|
||||
" -f <sw flow control> Activates UART link SW flow control\n"
|
||||
" -h <hw flow control> Activates UART link HW flow control\n"
|
||||
" -i <ip_address> Target IP for UDP. Default 127.0.0.1\n"
|
||||
" -n <namespace> ROS 2 topics namespace. Identifies the vehicle in a multi-agent network\n"
|
||||
" -p <poll_ms> Time in ms to poll over UART. Default 1ms\n"
|
||||
" -r <reception port> UDP port for receiving. Default 2020\n"
|
||||
" -s <sending port> UDP port for sending. Default 2019\n"
|
||||
" -t <transport> [UART|UDP] Default UART\n"
|
||||
" -v <debug verbosity> Add more verbosity\n"
|
||||
" -w <sleep_time_us> Time in us for which each iteration sleep. Default 1ms\n",
|
||||
name);
|
||||
}
|
||||
|
||||
static int parse_options(int argc, char **argv)
|
||||
{
|
||||
int ch;
|
||||
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;
|
||||
}
|
||||
}
|
||||
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;
|
||||
|
||||
if (_options.poll_ms < 1) {
|
||||
_options.poll_ms = 1;
|
||||
printf("\033[1;33m[ micrortps_agent ]\tPoll timeout too low, using 1 ms\033[0m");
|
||||
}
|
||||
case 'd': if (nullptr != optarg) strcpy(_options.device, optarg); break;
|
||||
|
||||
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;
|
||||
}
|
||||
case 'w': _options.sleep_us = strtol(optarg, nullptr, 10); break;
|
||||
|
||||
return 0;
|
||||
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();
|
||||
printf("\033[1;33m[ micrortps_agent ]\tInterrupt signal (%d) received.\033[0m\n", signum);
|
||||
running = 0;
|
||||
transport_node->close();
|
||||
}
|
||||
|
||||
@[if recv_topics]@
|
||||
@@ -182,152 +191,150 @@ std::condition_variable t_send_queue_cv;
|
||||
std::mutex t_send_queue_mutex;
|
||||
std::queue<uint8_t> t_send_queue;
|
||||
|
||||
void t_send(void*)
|
||||
void t_send(void *)
|
||||
{
|
||||
char data_buffer[BUFFER_SIZE] = {};
|
||||
uint32_t length = 0;
|
||||
char data_buffer[BUFFER_SIZE] = {};
|
||||
uint32_t length = 0;
|
||||
|
||||
while (running && !exit_sender_thread.load())
|
||||
{
|
||||
std::unique_lock<std::mutex> 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();
|
||||
while (running && !exit_sender_thread.load()) {
|
||||
std::unique_lock<std::mutex> lk(t_send_queue_mutex);
|
||||
|
||||
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);
|
||||
while (t_send_queue.empty() && !exit_sender_thread.load()) {
|
||||
t_send_queue_cv.wait(lk);
|
||||
}
|
||||
|
||||
if (topics.getMsg(topic_ID, scdr))
|
||||
{
|
||||
length = scdr.getSerializedDataLength();
|
||||
if (0 < (length = transport_node->write(topic_ID, data_buffer, length)))
|
||||
{
|
||||
total_sent += length;
|
||||
++sent;
|
||||
}
|
||||
}
|
||||
}
|
||||
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)
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
if (-1 == parse_options(argc, argv))
|
||||
{
|
||||
printf("\033[1;33m[ micrortps_agent ]\tEXITING...\033[0m\n");
|
||||
return -1;
|
||||
}
|
||||
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);
|
||||
// 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");
|
||||
printf("\033[0;37m--- MicroRTPS Agent ---\033[0m\n");
|
||||
printf("[ micrortps_agent ]\tStarting link...\n");
|
||||
|
||||
const char* localhost_only = std::getenv("ROS_LOCALHOST_ONLY");
|
||||
if (localhost_only && strcmp(localhost_only, "1") == 0) {
|
||||
printf("[ micrortps_agent ]\tUsing only the localhost network for data sharing...\n");
|
||||
}
|
||||
const char *localhost_only = std::getenv("ROS_LOCALHOST_ONLY");
|
||||
|
||||
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 (localhost_only && strcmp(localhost_only, "1") == 0) {
|
||||
printf("[ micrortps_agent ]\tUsing only the localhost network for data sharing...\n");
|
||||
}
|
||||
|
||||
if (0 > transport_node->init())
|
||||
{
|
||||
printf("\033[0;37m[ micrortps_agent ]\tEXITING...\033[0m\n");
|
||||
return -1;
|
||||
}
|
||||
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;
|
||||
|
||||
sleep(1);
|
||||
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<std::chrono::steady_clock> start, end;
|
||||
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<std::chrono::steady_clock> start, end;
|
||||
@[end if]@
|
||||
|
||||
// Init timesync
|
||||
std::shared_ptr<TimeSync> timeSync = std::make_shared<TimeSync>(_options.verbose_debug);
|
||||
// Init timesync
|
||||
std::shared_ptr<TimeSync> timeSync = std::make_shared<TimeSync>(_options.verbose_debug);
|
||||
|
||||
topics.set_timesync(timeSync);
|
||||
topics.set_timesync(timeSync);
|
||||
|
||||
@[if recv_topics]@
|
||||
topics.init(&t_send_queue_cv, &t_send_queue_mutex, &t_send_queue, _options.ns);
|
||||
topics.init(&t_send_queue_cv, &t_send_queue_mutex, &t_send_queue, _options.ns);
|
||||
@[end if]@
|
||||
|
||||
running = true;
|
||||
running = true;
|
||||
@[if recv_topics]@
|
||||
std::thread sender_thread(t_send, nullptr);
|
||||
std::thread sender_thread(t_send, nullptr);
|
||||
@[end if]@
|
||||
|
||||
while (running)
|
||||
{
|
||||
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();
|
||||
}
|
||||
++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<double>(std::chrono::steady_clock::now() - end).count() > WAIT_CNST) ||
|
||||
(!running && loop > 1)) {
|
||||
std::chrono::duration<double> 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;
|
||||
}
|
||||
|
||||
if ((receiving && std::chrono::duration<double>(std::chrono::steady_clock::now() - end).count() > WAIT_CNST) ||
|
||||
(!running && loop > 1))
|
||||
{
|
||||
std::chrono::duration<double> 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);
|
||||
usleep(_options.sleep_us);
|
||||
@[end if]@
|
||||
}
|
||||
}
|
||||
|
||||
@[if recv_topics]@
|
||||
exit_sender_thread = true;
|
||||
t_send_queue_cv.notify_one();
|
||||
sender_thread.join();
|
||||
exit_sender_thread = true;
|
||||
t_send_queue_cv.notify_one();
|
||||
sender_thread.join();
|
||||
@[end if]@
|
||||
delete transport_node;
|
||||
transport_node = nullptr;
|
||||
delete transport_node;
|
||||
transport_node = nullptr;
|
||||
|
||||
timeSync->stop();
|
||||
timeSync->reset();
|
||||
timeSync->stop();
|
||||
timeSync->reset();
|
||||
|
||||
return 0;
|
||||
return 0;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user