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

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
+ * @@file microRTPS_timesync.cpp
- * @brief source code for time sync implementation
+ * @@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() {
+@[if 1.5 <= fastrtpsgen_version <= 1.7]@
-	px4_msgs::msg::Timesync msg{};
+@[    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;

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
+ * @@file microRTPS_timesync.h
- * @brief Adds time sync for the microRTPS bridge
+ * @@brief Adds time sync for the microRTPS bridge
- * @author Nuno Marques <nuno.marques@dronesolutions.io>
+ * @@author Nuno Marques <nuno.marques@@dronesolutions.io>
- * @author Julian Kent <julian@auterion.com>
+ * @@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;
+@[if ros2_distro]@
-	Timesync_Subscriber _Timesync_sub;
+	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};

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