Skip to content

Commit

Permalink
fix: Disable usage of rmw source timestamp on windows when using conn…
Browse files Browse the repository at this point in the history
…extdds.

The windows rmw connextdds does not provide a value in source timestamp
nor in receive timestamp, therefore we fall back to current node time.

Signed-off-by: Janosch Machowinski <[email protected]>
  • Loading branch information
Janosch Machowinski authored and Janosch Machowinski committed Mar 11, 2024
1 parent aa9fed8 commit 76c351e
Showing 1 changed file with 27 additions and 0 deletions.
27 changes: 27 additions & 0 deletions rosbag2_transport/src/rosbag2_transport/recorder.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -134,6 +134,7 @@ class RecorderImpl
rclcpp::Service<rosbag2_interfaces::srv::Resume>::SharedPtr srv_resume_;
rclcpp::Service<rosbag2_interfaces::srv::Snapshot>::SharedPtr srv_snapshot_;
rclcpp::Service<rosbag2_interfaces::srv::SplitBagfile>::SharedPtr srv_split_bagfile_;
bool rmw_has_time_stamping_support_ = true;

std::atomic<bool> paused_ = false;
std::atomic<bool> in_recording_ = false;
Expand Down Expand Up @@ -171,6 +172,15 @@ RecorderImpl::RecorderImpl(
"The /clock topic needs to be discovered to record with sim time.");
}

#if defined _WIN32
// ConnvextDDS on Windows does currently not support time stamping, therefore we disable the feature
if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") !=
std::string::npos)
{
rmw_has_time_stamping_support_ = false;
}
#endif

std::string key_str = enum_key_code_to_str(Recorder::kPauseResumeToggleKey);
toggle_paused_key_callback_handle_ =
keyboard_handler_->add_key_press_callback(
Expand Down Expand Up @@ -523,6 +533,23 @@ std::shared_ptr<rclcpp::GenericSubscription>
RecorderImpl::create_subscription(
const std::string & topic_name, const std::string & topic_type, const rclcpp::QoS & qos)
{
#ifdef _WIN32
if (!rmw_has_time_stamping_support_) {
return node->create_generic_subscription(
topic_name,
topic_type,
qos,
[this, topic_name, topic_type](std::shared_ptr<const rclcpp::SerializedMessage> message,
const rclcpp::MessageInfo &) {
if (!paused_.load()) {
writer_->write(
std::move(message), topic_name, topic_type, node->now().nanoseconds(),
0);
}
});
}
#endif

if (record_options_.use_sim_time) {
return node->create_generic_subscription(
topic_name,
Expand Down

0 comments on commit 76c351e

Please sign in to comment.