From c7d86b73d4a1d807406a56cfe1c0ec9a20c6ab37 Mon Sep 17 00:00:00 2001 From: TSC21 Date: Tue, 10 Mar 2020 18:21:18 +0000 Subject: [PATCH] microRTPS: RtpsTopics: generalize types and member functions access --- msg/templates/urtps/RtpsTopics.cpp.em | 43 ++++----------------- msg/templates/urtps/RtpsTopics.h.em | 54 ++++++++++++++++++++++++++- 2 files changed, 61 insertions(+), 36 deletions(-) diff --git a/msg/templates/urtps/RtpsTopics.cpp.em b/msg/templates/urtps/RtpsTopics.cpp.em index afd1951381..76b6e41177 100644 --- a/msg/templates/urtps/RtpsTopics.cpp.em +++ b/msg/templates/urtps/RtpsTopics.cpp.em @@ -20,11 +20,6 @@ from px_generate_uorb_topic_files import MsgScope # this is in Tools/ send_topics = [(alias[idx] if alias[idx] else s.short_name) for idx, s in enumerate(spec) if scope[idx] == MsgScope.SEND] recv_topics = [(alias[idx] if alias[idx] else s.short_name) for idx, s in enumerate(spec) if scope[idx] == MsgScope.RECEIVE] package = package[0] -fastrtpsgen_version = fastrtpsgen_version[0] -try: - ros2_distro = ros2_distro[0].decode("utf-8") -except AttributeError: - ros2_distro = ros2_distro[0] }@ /**************************************************************************** * @@ -103,29 +98,18 @@ void RtpsTopics::publish(uint8_t topic_ID, char data_buffer[], size_t len) @[for topic in send_topics]@ case @(rtps_message_id(ids, topic)): // @(topic) { -@[ if 1.5 <= fastrtpsgen_version <= 1.7]@ -@[ if ros2_distro]@ - @(package)::msg::dds_::@(topic)_ st; -@[ else]@ - @(topic)_ st; -@[ end if]@ -@[ else]@ -@[ if ros2_distro]@ - @(package)::msg::@(topic) st; -@[ else]@ - @(topic) st; -@[ end if]@ -@[ end if]@ + @(topic)_msg_t st; eprosima::fastcdr::FastBuffer cdrbuffer(data_buffer, len); eprosima::fastcdr::Cdr cdr_des(cdrbuffer); st.deserialize(cdr_des); @[ if topic == 'Timesync' or topic == 'timesync']@ _timesync->processTimesyncMsg(&st); - if (st.sys_id() == 1) { + if (getMsgSysID(&st) == 1) { @[ end if]@ // apply timestamp offset - _timesync->subtractOffset(st.timestamp()); + uint64_t timestamp = getMsgTimestamp(&st); + _timesync->subtractOffset(timestamp); _@(topic)_pub.publish(&st); @[ if topic == 'Timesync' or topic == 'timesync']@ } @@ -150,24 +134,13 @@ bool RtpsTopics::getMsg(const uint8_t topic_ID, eprosima::fastcdr::Cdr &scdr) case @(rtps_message_id(ids, topic)): // @(topic) if (_@(topic)_sub.hasMsg()) { -@[ if 1.5 <= fastrtpsgen_version <= 1.7]@ -@[ if ros2_distro]@ - @(package)::msg::dds_::@(topic)_ msg = _@(topic)_sub.getMsg(); -@[ else]@ - @(topic)_ msg = _@(topic)_sub.getMsg(); -@[ end if]@ -@[ else]@ -@[ if ros2_distro]@ - @(package)::msg::@(topic) msg = _@(topic)_sub.getMsg(); -@[ else]@ - @(topic) msg = _@(topic)_sub.getMsg(); -@[ end if]@ -@[ end if]@ + @(topic)_msg_t msg = _@(topic)_sub.getMsg(); @[ if topic == 'Timesync' or topic == 'timesync']@ - if (msg.sys_id() == 0) { + if (getMsgSysID(&msg) == 0) { @[ end if]@ // apply timestamp offset - _timesync->addOffset(msg.timestamp()); + uint64_t timestamp = getMsgTimestamp(&msg); + _timesync->addOffset(timestamp); msg.serialize(scdr); ret = true; @[ if topic == 'Timesync' or topic == 'timesync']@ diff --git a/msg/templates/urtps/RtpsTopics.h.em b/msg/templates/urtps/RtpsTopics.h.em index a1fc2db19b..fc1b385b06 100644 --- a/msg/templates/urtps/RtpsTopics.h.em +++ b/msg/templates/urtps/RtpsTopics.h.em @@ -19,6 +19,12 @@ from px_generate_uorb_topic_files import MsgScope # this is in Tools/ send_topics = [(alias[idx] if alias[idx] else s.short_name) for idx, s in enumerate(spec) if scope[idx] == MsgScope.SEND] recv_topics = [(alias[idx] if alias[idx] else s.short_name) for idx, s in enumerate(spec) if scope[idx] == MsgScope.RECEIVE] +package = package[0] +fastrtpsgen_version = fastrtpsgen_version[0] +try: + ros2_distro = ros2_distro[0].decode("utf-8") +except AttributeError: + ros2_distro = ros2_distro[0] }@ /**************************************************************************** * @@ -66,6 +72,39 @@ recv_topics = [(alias[idx] if alias[idx] else s.short_name) for idx, s in enumer #include "@(topic)_Subscriber.h" @[end for]@ + +@[for topic in recv_topics]@ +@[ if 1.5 <= fastrtpsgen_version <= 1.7]@ +@[ if ros2_distro]@ +using @(topic)_msg_t = @(package)::msg::dds_::@(topic)_; +@[ else]@ +using @(topic)_msg_t = @(topic)_; +@[ end if]@ +@[ else]@ +@[ if ros2_distro]@ +using @(topic)_msg_t = @(package)::msg::@(topic) ; +@[ else]@ +using @(topic)_msg_t = @(topic); +@[ end if]@ +@[ end if]@ +@[end for]@ +@[for topic in send_topics]@ +@[ if 1.5 <= fastrtpsgen_version <= 1.7]@ +@[ if ros2_distro]@ +using @(topic)_msg_t = @(package)::msg::dds_::@(topic)_; +@[ else]@ +using @(topic)_msg_t = @(topic)_; +@[ end if]@ +@[ else]@ +@[ if ros2_distro]@ +using @(topic)_msg_t = @(package)::msg::@(topic); +@[ else]@ +using @(topic)_msg_t = @(topic); +@[ end if]@ +@[ end if]@ +@[end for]@ + + class RtpsTopics { public: bool init(std::condition_variable* t_send_queue_cv, std::mutex* t_send_queue_mutex, std::queue* t_send_queue); @@ -90,8 +129,21 @@ private: @[for topic in recv_topics]@ @(topic)_Subscriber _@(topic)_sub; @[end for]@ +@[end if]@ - std::shared_ptr _timesync; +@[if 1.5 <= fastrtpsgen_version <= 1.7 or not ros2_distro]@ + template + uint8_t getMsgSysID(T* msg) { return msg->sys_id_(); } + template + uint64_t getMsgTimestamp(T* msg) { return msg->timestamp_(); } +@[elif ros2_distro]@ + template + uint8_t getMsgSysID(T* msg) { return msg->sys_id(); } + + template + uint64_t getMsgTimestamp(T* msg) { return msg->timestamp(); } @[end if]@ + + std::shared_ptr _timesync; };