fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

microRTPS: timesync: template generalization for both ROS2 and non-ROS2

Browse Source
This commit is contained in:
TSC21 2020-03-05 15:33:04 +00:00 committed by Nuno Marques
parent 01518bd009
commit cfd8e368df
3 changed files with 162 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

103
msg/templates/urtps/microRTPS_timesync.cpp → msg/templates/urtps/microRTPS_timesync.cpp.em
View File

@ -1,3 +1,28 @@
@###############################################
@#
@# EmPy template for generating microRTPS_timesync.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/
package = package[0]
fastrtpsgen_version = fastrtpsgen_version[0]
try:
ros2_distro = ros2_distro[0].decode("utf-8")
except AttributeError:
ros2_distro = ros2_distro[0]
}@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
@ -31,8 +56,8 @@
****************************************************************************/
/*!
* @file microRTPS_timesync.cpp
* @brief source code for time sync implementation
* @@file microRTPS_timesync.cpp
* @@brief source code for time sync implementation
*/
#include <time.h>
@ -41,9 +66,6 @@
#include "microRTPS_timesync.h"
#include "Timesync_Publisher.h"
#include "Timesync_Subscriber.h"
TimeSync::TimeSync()
: _offset_ns(-1),
_skew_ns_per_sync(0.0),
@ -56,16 +78,32 @@ TimeSync::~TimeSync() { stop(); }
void TimeSync::setNewOffsetCB(std::function<void(int64_t)> callback) { _notifyNewOffset = callback; }
@[if ros2_distro]@
void TimeSync::start(const Timesync_Publisher* pub) {
@[else]@
void TimeSync::start(const timesync_Publisher* pub) {
@[end if]@
stop();
_Timesync_pub = (*pub);
_timesync_pub = (*pub);
auto run = [this]() {
while (!_request_stop) {
px4_msgs::msg::Timesync msg = newTimesyncMsg();
@[if 1.5 <= fastrtpsgen_version <= 1.7]@
@[ if ros2_distro]@
@(package)::msg::dds_::Timesync_ msg = newTimesyncMsg();
@[ else]@
timesync_ msg = newTimesyncMsg();
@[ end if]@
@[else]@
@[ if ros2_distro]@
@(package)::msg::Timesync msg = newTimesyncMsg();
@[ else]@
timesync msg = newTimesyncMsg();
@[ end if]@
@[end if]@
_Timesync_pub.publish(&msg);
_timesync_pub.publish(&msg);
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
@ -147,19 +185,43 @@ bool TimeSync::addMeasurement(int64_t local_t1_ns, int64_t remote_t2_ns, int64_t
return true;
}
void TimeSync::processTimesyncMsg(const px4_msgs::msg::Timesync* msg) {
@[if 1.5 <= fastrtpsgen_version <= 1.7]@
@[ if ros2_distro]@
void TimeSync::processTimesyncMsg(const @(package)::msg::dds_::Timesync_* msg) {
@[ else]@
void TimeSync::processTimesyncMsg(const timesync_* msg) {
@[ end if]@
@[else]@
@[ if ros2_distro]@
void TimeSync::processTimesyncMsg(const @(package)::msg::Timesync* msg) {
@[ else]@
void TimeSync::processTimesyncMsg(const timesync* msg) {
@[ end if]@
@[end if]@
if (msg->seq() == _last_msg_seq) return;
_last_msg_seq = msg->seq();
int64_t now_time = getSystemMonoNanos();
if (msg->tc1() == 0) {
px4_msgs::msg::Timesync reply = (*msg);
@[if 1.5 <= fastrtpsgen_version <= 1.7]@
@[ if ros2_distro]@
@(package)::msg::dds_::Timesync_ reply = (*msg);
@[ else]@
timesync_ reply = (*msg);
@[ end if]@
@[else]@
@[ if ros2_distro]@
@(package)::msg::Timesync reply = (*msg);
@[ else]@
timesync reply = (*msg);
@[ end if]@
@[end if]@
reply.timestamp() = getSystemTime();
reply.seq() = _last_msg_seq;
reply.tc1() = now_time;
_Timesync_pub.publish(&reply);
_timesync_pub.publish(&reply);
} else if (msg->tc1() > 0) {
bool added = addMeasurement(msg->ts1(), msg->tc1(), now_time);
@ -167,8 +229,23 @@ void TimeSync::processTimesyncMsg(const px4_msgs::msg::Timesync* msg) {
}
}
px4_msgs::msg::Timesync TimeSync::newTimesyncMsg() {
px4_msgs::msg::Timesync msg{};
@[if 1.5 <= fastrtpsgen_version <= 1.7]@
@[ if ros2_distro]@
@(package)::msg::dds_::Timesync_ TimeSync::newTimesyncMsg() {
@(package)::msg::dds_::Timesync_ msg{};
@[ else]@
timesync_ TimeSync::newTimesyncMsg() {
timesync_ msg{};
@[ end if]@
@[else]@
@[ if ros2_distro]@
@(package)::msg::Timesync TimeSync::newTimesyncMsg() {
@(package)::msg::Timesync msg{};
@[ else]@
timesync TimeSync::newTimesyncMsg() {
timesync msg{};
@[ end if]@
@[end if]@
msg.timestamp() = getSystemTime();
msg.seq() = _last_msg_seq;
msg.tc1() = 0;

74
msg/templates/urtps/microRTPS_timesync.h → msg/templates/urtps/microRTPS_timesync.h.em
View File

@ -1,3 +1,28 @@
@###############################################
@#
@# EmPy template for generating microRTPS_timesync.h 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/
package = package[0]
fastrtpsgen_version = fastrtpsgen_version[0]
try:
ros2_distro = ros2_distro[0].decode("utf-8")
except AttributeError:
ros2_distro = ros2_distro[0]
}@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
@ -31,10 +56,10 @@
****************************************************************************/
/*!
* @file microRTPS_timesync.h
* @brief Adds time sync for the microRTPS bridge
* @author Nuno Marques <nuno.marques@dronesolutions.io>
* @author Julian Kent <julian@auterion.com>
* @@file microRTPS_timesync.h
* @@brief Adds time sync for the microRTPS bridge
* @@author Nuno Marques <nuno.marques@@dronesolutions.io>
* @@author Julian Kent <julian@@auterion.com>
*/
#pragma once
@ -43,8 +68,13 @@
#include <functional>
#include <thread>
@[if ros2_distro]@
#include "Timesync_Publisher.h"
#include "Timesync_Subscriber.h"
@[else]@
#include "timesync_Publisher.h"
#include "timesync_Subscriber.h"
@[end if]@
static constexpr double ALPHA_INITIAL = 0.05;
static constexpr double ALPHA_FINAL = 0.003;
@ -60,7 +90,12 @@ public:
TimeSync();
virtual ~TimeSync();
@[if ros2_distro]@
void start(const Timesync_Publisher* pub);
@[else]@
void start(const timesync_Publisher* pub);
@[end if]@
void reset();
void stop();
@ -86,9 +121,27 @@ public:
bool addMeasurement(int64_t local_t1_ns, int64_t remote_t2_ns, int64_t local_t3_ns);
void processTimesyncMsg(const px4_msgs::msg::Timesync* msg);
@[if 1.5 <= fastrtpsgen_version <= 1.7]@
@[ if ros2_distro]@
void processTimesyncMsg(const @(package)::msg::dds_::Timesync_* msg);
px4_msgs::msg::Timesync newTimesyncMsg();
@(package)::msg::dds_::Timesync_ newTimesyncMsg();
@[ else]@
void processTimesyncMsg(const timesync_* msg);
timesync_ newTimesyncMsg();
@[ end if]@
@[else]@
@[ if ros2_distro]@
void processTimesyncMsg(const @(package)::msg::Timesync* msg);
@(package)::msg::Timesync newTimesyncMsg();
@[ else]@
void processTimesyncMsg(const timesync* msg);
timesync newTimesyncMsg();
@[ end if]@
@[end if]@
inline void applyOffset(uint64_t &timestamp) { timestamp += _offset_ns.load(); }
@ -100,8 +153,13 @@ private:
int32_t _request_reset_counter;
uint8_t _last_msg_seq;
Timesync_Publisher _Timesync_pub;
Timesync_Subscriber _Timesync_sub;
@[if ros2_distro]@
Timesync_Publisher _timesync_pub;
Timesync_Subscriber _timesync_sub;
@[else]@
timesync_Publisher _timesync_pub;
timesync_Subscriber _timesync_sub;
@[end if]@
std::unique_ptr<std::thread> _send_timesync_thread;
std::atomic<bool> _request_stop{false};

16
msg/tools/generate_microRTPS_bridge.py
View File

@ -312,6 +312,8 @@ uRTPS_CLIENT_TEMPL_FILE = 'microRTPS_client.cpp.em'
uRTPS_AGENT_TOPICS_H_TEMPL_FILE = 'RtpsTopics.h.em'
uRTPS_AGENT_TOPICS_SRC_TEMPL_FILE = 'RtpsTopics.cpp.em'
uRTPS_AGENT_TEMPL_FILE = 'microRTPS_agent.cpp.em'
uRTPS_TIMESYNC_CPP_TEMPL_FILE = 'microRTPS_timesync.cpp.em'
uRTPS_TIMESYNC_H_TEMPL_FILE = 'microRTPS_timesync.h.em'
uRTPS_AGENT_CMAKELISTS_TEMPL_FILE = 'microRTPS_agent_CMakeLists.txt.em'
uRTPS_PUBLISHER_SRC_TEMPL_FILE = 'Publisher.cpp.em'
uRTPS_PUBLISHER_H_TEMPL_FILE = 'Publisher.h.em'
@ -384,6 +386,10 @@ def generate_agent(out_dir):
px_generate_uorb_topic_files.generate_uRTPS_general(classifier.msgs_to_send, classifier.alias_msgs_to_send, classifier.msgs_to_receive, classifier.alias_msgs_to_receive, msg_dir, out_dir,
urtps_templates_dir, package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_id_map, fastrtpsgen_version, ros2_distro, uRTPS_AGENT_TEMPL_FILE)
px_generate_uorb_topic_files.generate_uRTPS_general(classifier.msgs_to_send, classifier.alias_msgs_to_send, classifier.msgs_to_receive, classifier.alias_msgs_to_receive, msg_dir, out_dir,
urtps_templates_dir, package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_id_map, fastrtpsgen_version, ros2_distro, uRTPS_TIMESYNC_CPP_TEMPL_FILE)
px_generate_uorb_topic_files.generate_uRTPS_general(classifier.msgs_to_send, classifier.alias_msgs_to_send, classifier.msgs_to_receive, classifier.alias_msgs_to_receive, msg_dir, out_dir,
urtps_templates_dir, package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_id_map, fastrtpsgen_version, ros2_distro, uRTPS_TIMESYNC_H_TEMPL_FILE)
px_generate_uorb_topic_files.generate_uRTPS_general(classifier.msgs_to_send, classifier.alias_msgs_to_send, classifier.msgs_to_receive, classifier.alias_msgs_to_receive, msg_dir, out_dir,
urtps_templates_dir, package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_id_map, fastrtpsgen_version, ros2_distro, uRTPS_AGENT_TOPICS_H_TEMPL_FILE)
px_generate_uorb_topic_files.generate_uRTPS_general(classifier.msgs_to_send, classifier.alias_msgs_to_send, classifier.msgs_to_receive, classifier.alias_msgs_to_receive, msg_dir, out_dir,
@ -416,8 +422,6 @@ def generate_agent(out_dir):
shutil.rmtree(os.path.join(out_dir, "fastrtpsgen"))
cp_wildcard(os.path.join(urtps_templates_dir,
"microRTPS_transport.*"), agent_out_dir)
cp_wildcard(os.path.join(urtps_templates_dir,
"microRTPS_timesync.*"), agent_out_dir)
if cmakelists:
os.rename(os.path.join(out_dir, "microRTPS_agent_CMakeLists.txt"),
os.path.join(out_dir, "CMakeLists.txt"))
@ -459,12 +463,6 @@ def generate_client(out_dir):
if os.path.isfile(def_file):
os.rename(def_file, def_file.replace(".cpp", ".cpp_"))
def_file = os.path.join(default_client_out, "microRTPS_transport.h")
if os.path.isfile(def_file):
os.rename(def_file, def_file.replace(".cpp", ".cpp_"))
def_file = os.path.join(default_client_out, "microRTPS_timesync.cpp")
if os.path.isfile(def_file):
os.rename(def_file, def_file.replace(".cpp", ".cpp_"))
def_file = os.path.join(default_client_out, "microRTPS_timesync.h")
if os.path.isfile(def_file):
os.rename(def_file, def_file.replace(".h", ".h_"))
@ -474,8 +472,6 @@ def generate_client(out_dir):
# Final steps to install client
cp_wildcard(os.path.join(urtps_templates_dir,
"microRTPS_transport.*"), out_dir)
cp_wildcard(os.path.join(urtps_templates_dir,
"microRTPS_timesync.*"), out_dir)
return 0