From cfd8e368dfcece446d642b320e63722235d9068b Mon Sep 17 00:00:00 2001 From: TSC21 Date: Thu, 5 Mar 2020 15:33:04 +0000 Subject: [PATCH] microRTPS: timesync: template generalization for both ROS2 and non-ROS2 --- ...timesync.cpp => microRTPS_timesync.cpp.em} | 103 +++++++++++++++--- ...TPS_timesync.h => microRTPS_timesync.h.em} | 74 +++++++++++-- msg/tools/generate_microRTPS_bridge.py | 16 +-- 3 files changed, 162 insertions(+), 31 deletions(-) rename msg/templates/urtps/{microRTPS_timesync.cpp => microRTPS_timesync.cpp.em} (66%) rename msg/templates/urtps/{microRTPS_timesync.h => microRTPS_timesync.h.em} (64%) diff --git a/msg/templates/urtps/microRTPS_timesync.cpp b/msg/templates/urtps/microRTPS_timesync.cpp.em similarity index 66% rename from msg/templates/urtps/microRTPS_timesync.cpp rename to msg/templates/urtps/microRTPS_timesync.cpp.em index e001ab3c91..c70273c709 100644 --- a/msg/templates/urtps/microRTPS_timesync.cpp +++ b/msg/templates/urtps/microRTPS_timesync.cpp.em @@ -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 @@ -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 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; diff --git a/msg/templates/urtps/microRTPS_timesync.h b/msg/templates/urtps/microRTPS_timesync.h.em similarity index 64% rename from msg/templates/urtps/microRTPS_timesync.h rename to msg/templates/urtps/microRTPS_timesync.h.em index 2fd00fe8ff..e8cef660a3 100644 --- a/msg/templates/urtps/microRTPS_timesync.h +++ b/msg/templates/urtps/microRTPS_timesync.h.em @@ -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 - * @author Julian Kent + * @@file microRTPS_timesync.h + * @@brief Adds time sync for the microRTPS bridge + * @@author Nuno Marques + * @@author Julian Kent */ #pragma once @@ -43,8 +68,13 @@ #include #include +@[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 ×tamp) { 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 _send_timesync_thread; std::atomic _request_stop{false}; diff --git a/msg/tools/generate_microRTPS_bridge.py b/msg/tools/generate_microRTPS_bridge.py index c72d928424..4c8fd7fd46 100644 --- a/msg/tools/generate_microRTPS_bridge.py +++ b/msg/tools/generate_microRTPS_bridge.py @@ -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