From d0da9006c8b569b3c16d2aece14b86cbb25f7726 Mon Sep 17 00:00:00 2001 From: y-okumura-isp <60122040+y-okumura-isp@users.noreply.github.com> Date: Mon, 27 Jun 2022 09:42:20 +0900 Subject: [PATCH] Refactor get_timestamp (#21) (#50) --- src/tilde/include/tilde/tilde_node.hpp | 23 ++++------- src/tilde/include/tilde/tilde_publisher.hpp | 45 ++++++++------------ src/tilde/test/test_stamp_processor.cpp | 46 ++++++++++----------- 3 files changed, 47 insertions(+), 67 deletions(-) diff --git a/src/tilde/include/tilde/tilde_node.hpp b/src/tilde/include/tilde/tilde_node.hpp index 49f3877f996..fe2c4c53e3b 100644 --- a/src/tilde/include/tilde/tilde_node.hpp +++ b/src/tilde/include/tilde/tilde_node.hpp @@ -214,19 +214,15 @@ class TildeNode : public rclcpp::Node { // prepare InputInfo - rclcpp::Time header_stamp; - // TODO(y-okumura-isp): introduce has_timestamp() - rclcpp::Time t(0, 100, this->now().get_clock_type()); - - header_stamp = Process::get_timestamp_from_const(t, p_msg); + auto header_stamp = Process::get_timestamp_from_const(p_msg); auto input_info = std::make_shared(); input_info->sub_time = subscription_time; input_info->sub_time_steady = subscription_time_steady; - if (header_stamp != t) { + if (header_stamp) { input_info->has_header_stamp = true; - input_info->header_stamp = header_stamp; + input_info->header_stamp = *header_stamp; } // TODO(y-okumura-isp): consider race condition in multi threaded executor. @@ -261,22 +257,19 @@ class TildeNode : public rclcpp::Node rclcpp::Time & subscription_time_steady) { // get header stamp - rclcpp::Time header_stamp; - rclcpp::Time t(0, 100, this->now().get_clock_type()); - - header_stamp = Process::get_timestamp_from_const(t, p_msg); + auto header_stamp = Process::get_timestamp_from_const(p_msg); InputInfo input_info; - if (header_stamp != t) { + if (header_stamp) { input_info.has_header_stamp = true; - input_info.header_stamp = header_stamp; + input_info.header_stamp = *header_stamp; } bool found = false; - if (header_stamp != t && tilde_pubs_.size() > 0) { + if (header_stamp && tilde_pubs_.size() > 0) { auto pub = tilde_pubs_.begin()->second; - found = pub->get_input_info(resolved_topic_name, header_stamp, input_info); + found = pub->get_input_info(resolved_topic_name, *header_stamp, input_info); } if (found) { diff --git a/src/tilde/include/tilde/tilde_publisher.hpp b/src/tilde/include/tilde/tilde_publisher.hpp index 3b8def0f38d..1a0e0d611d9 100644 --- a/src/tilde/include/tilde/tilde_publisher.hpp +++ b/src/tilde/include/tilde/tilde_publisher.hpp @@ -17,6 +17,7 @@ #include #include +#include #include #include #include @@ -84,28 +85,24 @@ struct Process { /// stamp getter for non-const pointer without header field /** - * Return dummy stamp as M has no header field. - * - * \param[in] t dummy stamp * \param[in] m message */ - static rclcpp::Time get_timestamp(rclcpp::Time t, M * m) + static std::optional get_timestamp(M * m) { (void) m; - return t; + return {}; } /// stamp getter for const pointer without header field /** * Return dummy stamp as M has no header field. * - * \param[in] t dummy stamp * \param[in] m message */ - static rclcpp::Time get_timestamp_from_const(rclcpp::Time t, const M * m) + static std::optional get_timestamp_from_const(const M * m) { (void) m; - return t; + return {}; } }; @@ -117,12 +114,10 @@ struct Process::value>::type> /** * Return header.stamp * - * \param[in] t dummy stamp * \param[in] m message */ - static rclcpp::Time get_timestamp(rclcpp::Time t, M * m) + static std::optional get_timestamp(M * m) { - (void) t; return m->header.stamp; } @@ -130,12 +125,10 @@ struct Process::value>::type> /** * Return header.stamp * - * \param[in] t dummy stamp * \param[in] m message */ - static rclcpp::Time get_timestamp_from_const(rclcpp::Time t, const M * m) + static std::optional get_timestamp_from_const(const M * m) { - (void) t; return m->header.stamp; } }; @@ -148,12 +141,10 @@ struct Process::value>::type /** * Return header.stamp * - * \param[in] t dummy stamp * \param[in] m message */ - static rclcpp::Time get_timestamp(rclcpp::Time t, M * m) + static std::optional get_timestamp(M * m) { - (void) t; return m->stamp; } @@ -161,12 +152,10 @@ struct Process::value>::type /** * Return header.stamp * - * \param[in] t dummy stamp * \param[in] m message */ - static rclcpp::Time get_timestamp_from_const(rclcpp::Time t, const M * m) + static std::optional get_timestamp_from_const(const M * m) { - (void) t; return m->stamp; } }; @@ -332,9 +321,8 @@ class TildePublisher : public TildePublisherBase publish(std::unique_ptr msg) { if (enable_) { - rclcpp::Time t(0, 100, clock_->get_clock_type()); - auto stamp = Process::get_timestamp(t, msg.get()); - publish_info((t != stamp), std::move(stamp)); + auto stamp = Process::get_timestamp(msg.get()); + publish_info(stamp); } pub_->publish(std::move(msg)); } @@ -343,9 +331,8 @@ class TildePublisher : public TildePublisherBase publish(const MessageT & msg) { if (enable_) { - rclcpp::Time t(0, 100, clock_->get_clock_type()); - auto stamp = Process::get_timestamp_from_const(t, &msg); - publish_info((t != stamp), std::move(stamp)); + auto stamp = Process::get_timestamp_from_const(&msg); + publish_info(stamp); } pub_->publish(msg); } @@ -416,8 +403,10 @@ class TildePublisher : public TildePublisherBase * \param has_header_stamp whether main message has header.stamp * \param t header stamp */ - void publish_info(bool has_header_stamp, rclcpp::Time && t) + void publish_info(const std::optional & t) { + bool has_header_stamp = t ? true : false; + auto msg = std::make_unique(); msg->header.stamp = clock_->now(); // msg->header.frame_id // Nothing todo @@ -430,7 +419,7 @@ class TildePublisher : public TildePublisherBase msg->output_info.pub_time_steady = steady_clock_->now(); msg->output_info.has_header_stamp = has_header_stamp; if (has_header_stamp) { - msg->output_info.header_stamp = t; + msg->output_info.header_stamp = *t; } fill_input_info(*msg); diff --git a/src/tilde/test/test_stamp_processor.cpp b/src/tilde/test/test_stamp_processor.cpp index 06c79a77b84..b6d0cce459b 100644 --- a/src/tilde/test/test_stamp_processor.cpp +++ b/src/tilde/test/test_stamp_processor.cpp @@ -49,18 +49,17 @@ TEST_F(TestStampProcessor, pointer_with_header_stamp) { rclcpp::Time expect(1, 2, RCL_ROS_TIME); msg.header.stamp = expect; - rclcpp::Time t(3, 4, RCL_ROS_TIME); - auto stamp = Process::get_timestamp(t, &msg); + auto stamp = Process::get_timestamp(&msg); - EXPECT_EQ(stamp, expect); + EXPECT_TRUE(stamp ? true : false); + EXPECT_EQ(*stamp, expect); } TEST_F(TestStampProcessor, pointer_without_header_stamp) { String msg; - rclcpp::Time t(3, 4, RCL_ROS_TIME); - auto stamp = Process::get_timestamp(t, &msg); + auto stamp = Process::get_timestamp(&msg); - EXPECT_EQ(stamp, t); + EXPECT_FALSE((stamp ? true : false)); } TEST_F(TestStampProcessor, const_pointer_with_header_stamp) { @@ -69,18 +68,17 @@ TEST_F(TestStampProcessor, const_pointer_with_header_stamp) { _msg.header.stamp = expect; const PointCloud2 msg(_msg); - rclcpp::Time t(3, 4, RCL_ROS_TIME); - auto stamp = Process::get_timestamp_from_const(t, &msg); + auto stamp = Process::get_timestamp_from_const(&msg); - EXPECT_EQ(stamp, expect); + EXPECT_TRUE((stamp ? true : false)); + EXPECT_EQ(*stamp, expect); } TEST_F(TestStampProcessor, const_pointer_without_header_stamp) { const String msg; - rclcpp::Time t(3, 4, RCL_ROS_TIME); - auto stamp = Process::get_timestamp_from_const(t, &msg); + auto stamp = Process::get_timestamp_from_const(&msg); - EXPECT_EQ(stamp, t); + EXPECT_FALSE((stamp ? true : false)); } TEST_F(TestStampProcessor, pointer_with_top_level_stamp) { @@ -88,13 +86,13 @@ TEST_F(TestStampProcessor, pointer_with_top_level_stamp) { rclcpp::Time expect(1, 2, RCL_ROS_TIME); msg.stamp = expect; - rclcpp::Time t(3, 4, RCL_ROS_TIME); - auto stamp = Process::get_timestamp(t, &msg); + auto stamp = Process::get_timestamp(&msg); EXPECT_FALSE(tilde::HasHeader::value); EXPECT_TRUE(tilde::HasStamp::value); EXPECT_TRUE(tilde::HasStampWithoutHeader::value); - EXPECT_EQ(stamp, expect); + EXPECT_TRUE((stamp ? true : false)); + EXPECT_EQ(*stamp, expect); } TEST_F(TestStampProcessor, const_pointer_with_top_level_stamp) { @@ -103,13 +101,13 @@ TEST_F(TestStampProcessor, const_pointer_with_top_level_stamp) { _msg.stamp = expect; const MsgWithTopLevelStamp msg(_msg); - rclcpp::Time t(3, 4, RCL_ROS_TIME); - auto stamp = Process::get_timestamp_from_const(t, &msg); + auto stamp = Process::get_timestamp_from_const(&msg); EXPECT_FALSE(tilde::HasHeader::value); EXPECT_TRUE(tilde::HasStamp::value); EXPECT_TRUE(tilde::HasStampWithoutHeader::value); - EXPECT_EQ(stamp, expect); + EXPECT_TRUE((stamp ? true : false)); + EXPECT_EQ(*stamp, expect); } TEST_F(TestStampProcessor, pointer_with_header_and_top_level_stamp) { @@ -119,13 +117,13 @@ TEST_F(TestStampProcessor, pointer_with_header_and_top_level_stamp) { rclcpp::Time unexpected(3, 4, RCL_ROS_TIME); msg.header.stamp = expect; msg.stamp = unexpected; - rclcpp::Time t(5, 6, RCL_ROS_TIME); - auto stamp = Process::get_timestamp(t, &msg); + auto stamp = Process::get_timestamp(&msg); EXPECT_TRUE(tilde::HasHeader::value); EXPECT_TRUE(tilde::HasStamp::value); EXPECT_FALSE(tilde::HasStampWithoutHeader::value); - EXPECT_EQ(stamp, expect); + EXPECT_TRUE((stamp ? true : false)); + EXPECT_EQ(*stamp, expect); } TEST_F(TestStampProcessor, const_pointer_with_header_and_top_level_stamp) { @@ -137,11 +135,11 @@ TEST_F(TestStampProcessor, const_pointer_with_header_and_top_level_stamp) { _msg.stamp = unexpected; const MsgWithHeaderAndTopLevelStamp msg(_msg); - rclcpp::Time t(5, 6, RCL_ROS_TIME); - auto stamp = Process::get_timestamp_from_const(t, &msg); + auto stamp = Process::get_timestamp_from_const(&msg); EXPECT_TRUE(tilde::HasHeader::value); EXPECT_TRUE(tilde::HasStamp::value); EXPECT_FALSE(tilde::HasStampWithoutHeader::value); - EXPECT_EQ(stamp, expect); + EXPECT_TRUE((stamp ? true : false)); + EXPECT_EQ(*stamp, expect); }