Skip to content
This repository has been archived by the owner on May 18, 2023. It is now read-only.

Commit

Permalink
Refactor get_timestamp (autowarefoundation#21) (autowarefoundation#50)
Browse files Browse the repository at this point in the history
  • Loading branch information
@y-okumura-isp
y-okumura-isp authored Jun 27, 2022
1 parent 9abd05d commit d0da900
Show file tree
Hide file tree
Showing 3 changed files with 47 additions and 67 deletions.
23 changes: 8 additions & 15 deletions src/tilde/include/tilde/tilde_node.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<MessageT>::get_timestamp_from_const(t, p_msg);
auto header_stamp = Process<MessageT>::get_timestamp_from_const(p_msg);

auto input_info = std::make_shared<InputInfo>();

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.
Expand Down Expand Up @@ -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<MessageT>::get_timestamp_from_const(t, p_msg);
auto header_stamp = Process<MessageT>::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) {
Expand Down
45 changes: 17 additions & 28 deletions src/tilde/include/tilde/tilde_publisher.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@

#include <map>
#include <memory>
#include <optional>
#include <string>
#include <type_traits>
#include <utility>
Expand Down Expand Up @@ -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<rclcpp::Time> 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<rclcpp::Time> get_timestamp_from_const(const M * m)
{
(void) m;
return t;
return {};
}
};

Expand All @@ -117,25 +114,21 @@ struct Process<M, typename std::enable_if<HasHeader<M>::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<rclcpp::Time> get_timestamp(M * m)
{
(void) t;
return m->header.stamp;
}

/// stamp getter for const pointer with header field
/**
* 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<rclcpp::Time> get_timestamp_from_const(const M * m)
{
(void) t;
return m->header.stamp;
}
};
Expand All @@ -148,25 +141,21 @@ struct Process<M, typename std::enable_if<HasStampWithoutHeader<M>::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<rclcpp::Time> get_timestamp(M * m)
{
(void) t;
return m->stamp;
}

/// stamp getter for const pointer with header field
/**
* 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<rclcpp::Time> get_timestamp_from_const(const M * m)
{
(void) t;
return m->stamp;
}
};
Expand Down Expand Up @@ -332,9 +321,8 @@ class TildePublisher : public TildePublisherBase
publish(std::unique_ptr<MessageT, MessageDeleter> msg)
{
if (enable_) {
rclcpp::Time t(0, 100, clock_->get_clock_type());
auto stamp = Process<MessageT>::get_timestamp(t, msg.get());
publish_info((t != stamp), std::move(stamp));
auto stamp = Process<MessageT>::get_timestamp(msg.get());
publish_info(stamp);
}
pub_->publish(std::move(msg));
}
Expand All @@ -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<MessageT>::get_timestamp_from_const(t, &msg);
publish_info((t != stamp), std::move(stamp));
auto stamp = Process<MessageT>::get_timestamp_from_const(&msg);
publish_info(stamp);
}
pub_->publish(msg);
}
Expand Down Expand Up @@ -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<rclcpp::Time> & t)
{
bool has_header_stamp = t ? true : false;

auto msg = std::make_unique<tilde_msg::msg::MessageTrackingTag>();
msg->header.stamp = clock_->now();
// msg->header.frame_id // Nothing todo
Expand All @@ -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);
Expand Down
46 changes: 22 additions & 24 deletions src/tilde/test/test_stamp_processor.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<PointCloud2>::get_timestamp(t, &msg);
auto stamp = Process<PointCloud2>::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<String>::get_timestamp(t, &msg);
auto stamp = Process<String>::get_timestamp(&msg);

EXPECT_EQ(stamp, t);
EXPECT_FALSE((stamp ? true : false));
}

TEST_F(TestStampProcessor, const_pointer_with_header_stamp) {
Expand All @@ -69,32 +68,31 @@ 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<PointCloud2>::get_timestamp_from_const(t, &msg);
auto stamp = Process<PointCloud2>::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<String>::get_timestamp_from_const(t, &msg);
auto stamp = Process<String>::get_timestamp_from_const(&msg);

EXPECT_EQ(stamp, t);
EXPECT_FALSE((stamp ? true : false));
}

TEST_F(TestStampProcessor, pointer_with_top_level_stamp) {
MsgWithTopLevelStamp msg;

rclcpp::Time expect(1, 2, RCL_ROS_TIME);
msg.stamp = expect;
rclcpp::Time t(3, 4, RCL_ROS_TIME);
auto stamp = Process<decltype(msg)>::get_timestamp(t, &msg);
auto stamp = Process<decltype(msg)>::get_timestamp(&msg);

EXPECT_FALSE(tilde::HasHeader<decltype(msg)>::value);
EXPECT_TRUE(tilde::HasStamp<decltype(msg)>::value);
EXPECT_TRUE(tilde::HasStampWithoutHeader<decltype(msg)>::value);
EXPECT_EQ(stamp, expect);
EXPECT_TRUE((stamp ? true : false));
EXPECT_EQ(*stamp, expect);
}

TEST_F(TestStampProcessor, const_pointer_with_top_level_stamp) {
Expand All @@ -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<decltype(msg)>::get_timestamp_from_const(t, &msg);
auto stamp = Process<decltype(msg)>::get_timestamp_from_const(&msg);

EXPECT_FALSE(tilde::HasHeader<decltype(msg)>::value);
EXPECT_TRUE(tilde::HasStamp<decltype(msg)>::value);
EXPECT_TRUE(tilde::HasStampWithoutHeader<decltype(msg)>::value);
EXPECT_EQ(stamp, expect);
EXPECT_TRUE((stamp ? true : false));
EXPECT_EQ(*stamp, expect);
}

TEST_F(TestStampProcessor, pointer_with_header_and_top_level_stamp) {
Expand All @@ -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<decltype(msg)>::get_timestamp(t, &msg);
auto stamp = Process<decltype(msg)>::get_timestamp(&msg);

EXPECT_TRUE(tilde::HasHeader<decltype(msg)>::value);
EXPECT_TRUE(tilde::HasStamp<decltype(msg)>::value);
EXPECT_FALSE(tilde::HasStampWithoutHeader<decltype(msg)>::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) {
Expand All @@ -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<decltype(msg)>::get_timestamp_from_const(t, &msg);
auto stamp = Process<decltype(msg)>::get_timestamp_from_const(&msg);

EXPECT_TRUE(tilde::HasHeader<decltype(msg)>::value);
EXPECT_TRUE(tilde::HasStamp<decltype(msg)>::value);
EXPECT_FALSE(tilde::HasStampWithoutHeader<decltype(msg)>::value);
EXPECT_EQ(stamp, expect);
EXPECT_TRUE((stamp ? true : false));
EXPECT_EQ(*stamp, expect);
}

0 comments on commit d0da900

Please sign in to comment.