feature: replace rosbag time stamper with sim time(current time)

ros2bag/ros2bag/verb/play.py

@@ -27,6 +27,9 @@ def add_arguments(self, parser, cli_name):  # noqa: D102
         parser.add_argument(
             '-s', '--storage', default='sqlite3',
             help='storage identifier to be used, defaults to "sqlite3"')
+        parser.add_argument(
+            '-c', '--current', action='store_true',
+            help='determine if current system time will be set in Header time stamper')
         parser.add_argument(
             '-r', '--read-ahead-queue-size', type=int, default=1000,
             help='size of message queue rosbag tries to hold in memory to help deterministic '
@@ -46,5 +49,6 @@ def main(self, *, args):  # noqa: D102
         rosbag2_transport_py.play(
             uri=bag_file,
             storage_id=args.storage,
+            use_current_time=args.current,
             node_prefix=NODE_NAME_PREFIX,
             read_ahead_queue_size=args.read_ahead_queue_size)

rosbag2_transport/CMakeLists.txt

@@ -25,6 +25,7 @@ find_package(ament_cmake_ros REQUIRED)
 find_package(rcl REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rcutils REQUIRED)
+find_package(rosidl_typesupport_introspection_cpp REQUIRED)
 find_package(rmw REQUIRED)
 find_package(rosbag2_compression REQUIRED)
 find_package(rosbag2_cpp REQUIRED)
@@ -46,6 +47,8 @@ ament_target_dependencies(${PROJECT_NAME}
   rcl
   rclcpp
   rcutils
+  rosidl_generator_cpp
+  rosidl_typesupport_introspection_cpp
   rmw
   rosbag2_compression
   rosbag2_cpp

rosbag2_transport/include/rosbag2_transport/play_options.hpp

@@ -26,6 +26,7 @@ struct PlayOptions
 public:
   size_t read_ahead_queue_size;
   std::string node_prefix = "";
+  bool use_current_time = false;
 };
 
 }  // namespace rosbag2_transport

rosbag2_transport/src/rosbag2_transport/player.cpp

@@ -60,6 +60,11 @@ void Player::play(const PlayOptions & options)
 {
   prepare_publishers();
 
+  if(options.use_current_time)
+  {
+    prepare_topic_ts_map();
+  }
+
   storage_loading_future_ = std::async(
     std::launch::async,
     [this, options]() {load_storage_content(options);});
@@ -132,17 +137,203 @@ void Player::play_messages_from_queue()
   } while (!is_storage_completely_loaded() && rclcpp::ok());
 }
 
+unsigned long alignment(unsigned long data_size, unsigned long last_data_size, unsigned long current_position)
+{
+  return data_size > last_data_size ? (data_size - current_position % data_size) & (data_size-1):0;
+}
+
+void Player::deal_with_string(const uint8_t *dds_buffer, bool is_wstring)
+{
+  uint32_t length;
+  size_t string_header = sizeof(uint32_t);
+
+  unsigned long one_offset = alignment(string_header, last_data_size_, current_position_);
+  current_position_ = current_position_ + one_offset;
+  memcpy(&length, (dds_buffer + current_position_ + 4), string_header);
+
+  if(!is_wstring)
+    {
+      last_data_size_ = sizeof(char);
+      current_position_ += (string_header + length);
+    }
+  else {
+      last_data_size_ = sizeof(uint32_t);
+      current_position_ += (string_header + length * sizeof(uint32_t));
+  }
+}
+
+void Player::calculate_position_with_align(const uint8_t * dds_buffer_ptr, const rosidl_typesupport_introspection_cpp::MessageMember *message_member, unsigned long stop_index)
+{
+  unsigned long one_offset = 0;
+  unsigned long data_size = 0;
+
+  for (unsigned int i = 0; i < stop_index; i++) {
+    bool is_string = false;
+    bool is_wstring = false;
+    bool is_ros_msg_type = false;
+    const rosidl_typesupport_introspection_cpp::MessageMembers * sub_members;
+    switch (message_member[i].type_id_) {
+      case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_BOOL:
+        data_size = sizeof(bool);
+        break;
+      case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_BYTE:
+      case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT8:
+        data_size = sizeof (uint8_t);
+        break;
+      case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_CHAR:
+      case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT8:
+        data_size = sizeof (char);
+        break;
+      case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_FLOAT:
+        data_size = sizeof (float);
+        break;
+      case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_FLOAT64:
+        data_size = sizeof (uint64_t);
+        break;
+      case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT16:
+        data_size = sizeof (int16_t);
+        break;
+      case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT16:
+        data_size = sizeof (uint16_t);
+        break;
+      case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT32:
+        data_size = sizeof (int32_t);
+        break;
+      case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT32:
+        data_size = sizeof (uint32_t);
+        break;
+      case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT64:
+        data_size = sizeof (int64_t);
+        break;
+      case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT64:
+        data_size = sizeof (uint64_t);
+        break;
+      case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_LONG_DOUBLE:
+        data_size = 8;
+        break;
+      case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_STRING:
+        deal_with_string(dds_buffer_ptr, is_wstring);
+        is_string = true;
+        break;
+      case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_WCHAR:
+        data_size = sizeof(uint16_t);
+        break;
+      case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_WSTRING:
+        deal_with_string(dds_buffer_ptr, is_wstring);
+        is_wstring = true;
+        break;
+      case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_MESSAGE:
+        sub_members = static_cast<const rosidl_typesupport_introspection_cpp::MessageMembers *>(message_member[i].members_->data);
+        calculate_position_with_align(dds_buffer_ptr, sub_members->members_, sub_members->member_count_);
+        is_ros_msg_type = true;
+        break;
+      }
+    //standard element
+    if(!is_string && !is_wstring && !is_ros_msg_type && !message_member[i].is_array_)
+      {
+        one_offset = alignment(data_size, last_data_size_, current_position_);
+        current_position_ += (one_offset + data_size);
+        last_data_size_ = data_size;
+      }
+    //standard array
+    else if(!is_string && !is_wstring && !is_ros_msg_type && message_member[i].is_array_)
+      {
+        for (uint j = 0;j < message_member[i].array_size_; j++) {
+          one_offset = alignment(data_size, last_data_size_, current_position_);
+          current_position_ += (one_offset + data_size);
+          last_data_size_ = data_size;
+        }
+      }
+    //array of string
+    else if (is_string && !is_wstring && !is_ros_msg_type && message_member[i].is_array_)
+      {
+        for (uint j = 0;j < message_member[i].array_size_ - 1; j++) {
+          deal_with_string(dds_buffer_ptr, false);
+        }
+      }
+    //array of wstring
+    else if (is_wstring && !is_string && !is_ros_msg_type && message_member[i].is_array_)
+      {
+        for (uint j = 0;j < message_member[i].array_size_ - 1; j++) {
+          deal_with_string(dds_buffer_ptr, true);
+        }
+      }
+    else if (is_ros_msg_type && !is_string && !is_wstring && message_member[i].is_array_)
+      {
+        for (uint j = 0;j < message_member[i].array_size_ - 1; j++) {
+          calculate_position_with_align(dds_buffer_ptr, sub_members->members_, sub_members->member_count_);
+        }
+      }
+  }
+
+}
+
 void Player::play_messages_until_queue_empty()
 {
   ReplayableMessage message;
   while (message_queue_.try_dequeue(message) && rclcpp::ok()) {
     std::this_thread::sleep_until(start_time_ + message.time_since_start);
+
+    auto it = topics_ts_map_.find(message.message->topic_name);
+    if (it != topics_ts_map_.end() ) {
+      builtin_interfaces::msg::Time ros_time_to_set;
+
+      std::chrono::time_point<std::chrono::high_resolution_clock> time_to_be_set = start_time_ + message.time_since_start;
+
+      auto offset_index = it->second.stop_index;
+      const rosidl_typesupport_introspection_cpp::MessageMember * msg_ptr = it->second.msg_member_ptr;
+
+      std::chrono::duration_cast<std::chrono::nanoseconds>(time_to_be_set.time_since_epoch());
+
+      ros_time_to_set.sec = static_cast<int32_t>(floor(time_to_be_set.time_since_epoch().count()/1e9));
+      ros_time_to_set.nanosec = static_cast<uint32_t>(round(time_to_be_set.time_since_epoch().count() - ros_time_to_set.sec*1e9));
+
+      //memcpy
+      uint8_t * buffer_temp = message.message->serialized_data->buffer;
+      dds_buffer_ptr_ = message.message->serialized_data->buffer;
+      calculate_position_with_align(dds_buffer_ptr_, msg_ptr, offset_index);
+      size_t header_time_sec_size = sizeof (int32_t);
+      unsigned long last_offset = alignment(header_time_sec_size, last_data_size_, current_position_);
+      current_position_ += last_offset;
+      buffer_temp = buffer_temp + current_position_ + 4;
+
+      memcpy(buffer_temp, &ros_time_to_set, sizeof(builtin_interfaces::msg::Time));
+
+      current_position_ = 0;
+      last_data_size_ = ULONG_MAX;
+    }
+
     if (rclcpp::ok()) {
       publishers_[message.message->topic_name]->publish(message.message->serialized_data);
     }
   }
 }
 
+
+void Player::prepare_topic_ts_map()
+{
+  auto topics = reader_->get_all_topics_and_types();
+
+  for (const auto & topic : topics){
+      auto type_support = rosbag2::get_typesupport(topic.type, "rosidl_typesupport_introspection_cpp");
+      auto msg_members = static_cast<const rosidl_typesupport_introspection_cpp::MessageMembers *>(type_support->data);
+
+      const rosidl_typesupport_introspection_cpp::MessageMember *msg_member_ptr = msg_members->members_;
+      auto msg_member_count = msg_members->member_count_;
+      for (unsigned int i = 0;i < msg_member_count;i++) {
+          if(strcmp(msg_member_ptr[i].name_, "header") == 0)
+          {
+            HeaderSupportStruct header_support;
+            header_support.stop_index = i;
+            header_support.msg_member_ptr = msg_member_ptr;
+            topics_ts_map_.insert(std::make_pair(topic.name, header_support));
+            break;
+          }
+      }
+  }
+
+}
+
 void Player::prepare_publishers()
 {
   auto topics = reader_->get_all_topics_and_types();

rosbag2_transport/src/rosbag2_transport/player.hpp

@@ -16,6 +16,9 @@
 #define ROSBAG2_TRANSPORT__PLAYER_HPP_
 
 #include <chrono>
+#include <climits>
+#include <cmath>
+#include <cstring>
 #include <future>
 #include <memory>
 #include <queue>
@@ -25,6 +28,13 @@
 #include "moodycamel/readerwriterqueue.h"
 
 #include "rosbag2_transport/play_options.hpp"
+#include "rosbag2_node.hpp"
+
+
+#include "rosidl_typesupport_cpp/message_type_support.hpp"
+#include "rosidl_typesupport_introspection_cpp/field_types.hpp"
+#include "rosidl_typesupport_introspection_cpp/message_introspection.hpp"
+#include "rosidl_typesupport_introspection_c/message_introspection.h"
 
 #include "replayable_message.hpp"
 
@@ -38,6 +48,11 @@ class Reader;
 namespace rosbag2_transport
 {
 
+typedef struct{
+    int stop_index;
+    const rosidl_typesupport_introspection_cpp::MessageMember * msg_member_ptr;
+}HeaderSupportStruct;
+
 class GenericPublisher;
 class Rosbag2Node;
 
@@ -58,8 +73,14 @@ class Player
   void play_messages_from_queue();
   void play_messages_until_queue_empty();
   void prepare_publishers();
+  void prepare_topic_ts_map();
+  void calculate_position_with_align(const uint8_t * dds_buffer, const rosidl_typesupport_introspection_cpp::MessageMember *message_member, unsigned long stop_index);
+  void deal_with_string(const uint8_t * dds_buffer, bool is_wstring);
 
   static constexpr double read_ahead_lower_bound_percentage_ = 0.9;
+  size_t last_data_size_ = ULONG_MAX;
+  unsigned long current_position_ = 0;
+  uint8_t *dds_buffer_ptr_;
   static const std::chrono::milliseconds queue_read_wait_period_;
 
   std::shared_ptr<rosbag2_cpp::Reader> reader_;
@@ -68,8 +89,10 @@ class Player
   mutable std::future<void> storage_loading_future_;
   std::shared_ptr<Rosbag2Node> rosbag2_transport_;
   std::unordered_map<std::string, std::shared_ptr<GenericPublisher>> publishers_;
+  std::unordered_map<std::string, header_support_struct> topics_ts_map_;
 };
 
 }  // namespace rosbag2_transport
 
+
 #endif  // ROSBAG2_TRANSPORT__PLAYER_HPP_

rosbag2_transport/src/rosbag2_transport/rosbag2_transport_python.cpp

@@ -144,19 +144,21 @@ rosbag2_transport_play(PyObject * Py_UNUSED(self), PyObject * args, PyObject * k
     "uri",
     "storage_id",
     "node_prefix",
+    "use_current_time",
     "read_ahead_queue_size",
     nullptr
   };
 
   char * uri;
   char * storage_id;
   char * node_prefix;
+  bool use_current_time = false;
   size_t read_ahead_queue_size;
-  if (!PyArg_ParseTupleAndKeywords(
-      args, kwargs, "sss|k", const_cast<char **>(kwlist),
+  if (!PyArg_ParseTupleAndKeywords(args, kwargs, "sss|bk", const_cast<char **>(kwlist),
       &uri,
       &storage_id,
       &node_prefix,
+      &use_current_time,
       &read_ahead_queue_size))
   {
     return nullptr;
@@ -167,6 +169,7 @@ rosbag2_transport_play(PyObject * Py_UNUSED(self), PyObject * args, PyObject * k
 
   play_options.node_prefix = std::string(node_prefix);
   play_options.read_ahead_queue_size = read_ahead_queue_size;
+  play_options.use_current_time = use_current_time;
 
   rosbag2_storage::MetadataIo metadata_io{};
   rosbag2_storage::BagMetadata metadata{};