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

scoopySHI · 2020-02-18T15:18:39Z

The feature is mentioned in issue #299.

kikass13

this is just a little view on things

rosbag2_transport/CMakeLists.txt

kikass13 · 2020-02-18T15:24:29Z

rosbag2_transport/src/rosbag2_transport/player.cpp

+//Dynamic Alignment as Fastrtps
+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;


can we do this vendor specific thing here?

how can we use the dds vendor implementation instead of using copied code in here

How is this alignment related to time? Just trying to keep the set of changes semantically separated.

in our version of this feature, we are changing the timestamp inside the serialized message directly, that way we achieve a "simulated" time while publishing messages via the player class. We therefore need to be able to find out exactly where to put the header.stamp, which depends on a dynamic alignment inside the dds serialization process based on underlying message types.
This is related to issue #299

Because the offset in typesupport does not provides actual offset in fastrtps serialized data. (It provides the offset in machine memory, using the alignment of machine) In order to figure out the actual offset of "Header", we must calculate the offset according to Fastrtps alignment rule.

rosbag2_transport/src/rosbag2_transport/player.cpp

kikass13 · 2020-02-18T15:31:51Z

rosbag2_transport/src/rosbag2_transport/player.cpp

+      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; //plus dds header


can this +4 dds header be looked up in any way?

rosbag2_transport/src/rosbag2_transport/player.cpp

rosbag2_transport/src/rosbag2_transport/player.hpp

kikass13

cool

emersonknapp

A few comments outside the feature itself, will take a second pass at the meat of it

.gitignore

ros2bag/CHANGELOG.rst

ros2bag/package.xml

emersonknapp · 2020-02-18T18:33:54Z

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 = "";
+  std::string clock_type = "past";


What values are accepted here? I am thinking this should probably be an enum.

Could this be part of the actual clock types being available in ros2? http://docs.ros2.org/eloquent/api/rcl/time_8h.html#a5c734f508ce06aec7974af10ad09d071

this was @scoopySHI 's way of getting the simulated clock argument into the core player class. I do agree that this should be done in a better way.

Maybe the name clock_type can lead to misunderstand. It suggests which time is in the time stamper in Header (current system time or the time while recording). It has actually nothing to do with clock types in ROS2. @Karsten1987 @emersonknapp
I will remove the parameter input and change this to a switch argument(-c argument) which indicates whether this feature is applied or not.

rosbag2_transport/src/rosbag2_transport/player.cpp

Karsten1987 · 2020-02-18T19:59:06Z

this PR has to be rebased on top of master. This should also make the diff way easier to read.

Karsten1987

a few comments, but I am mainly requesting changes for the rebasing.

ros2bag/package.xml

Karsten1987 · 2020-02-18T20:00:53Z

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', '--clock', default='past',


what does past mean here? Is there a way to describe this more accurately? Or at least the help text should reflect this. Default is set to past and the help text mentions current time.

"past" means the time in time stamper in Header is the time when recording happened. "current time" means the time in time stamper in Header is current system time. Some packages like robot_localization deals with current system time, therefore we want to replace the time stamper with current time so that the rosbag can be directly played and as data input in those packages. @Karsten1987
I will modify the help text and more.

rosbag2_transport/CMakeLists.txt

Karsten1987 · 2020-02-18T20:02:44Z

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 = "";
+  std::string clock_type = "past";


Could this be part of the actual clock types being available in ros2? http://docs.ros2.org/eloquent/api/rcl/time_8h.html#a5c734f508ce06aec7974af10ad09d071

rosbag2_transport/src/rosbag2_transport/player.cpp

Karsten1987 · 2020-02-18T20:04:46Z

rosbag2_transport/src/rosbag2_transport/player.cpp

+//Dynamic Alignment as Fastrtps
+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;


How is this alignment related to time? Just trying to keep the set of changes semantically separated.

Karsten1987 · 2020-02-18T20:06:30Z

rosbag2_transport/src/rosbag2_transport/player.cpp

+}
+
+//find out the real position of header in fastrtps serialized data
+void Player::calculate_position_with_align(const uint8_t * dds_buffer_ptr, const rosidl_typesupport_introspection_cpp::MessageMember *message_member, unsigned long stop_index)


I think this change should actually land in rosidl_typesupport_introspection_cpp rather than in rosbag2.
We've worked on a similar change for the serialized bag message, which resulted in here: ros2/rosidl#416

So I am very hesitant to bring back this logic

I can confirm that this is probably not the right place for a lot of this logic. the point is that we don't really know if this PR is even necessary. There is currently no way (as far as we know) of achieving what #299 needs without compromising the relatively clean rosbag functionality. So this is just the first shot we have for this problem so any suggestion is helpful :)

Our initial thought was also to generate the specific message type buffer solely from the rosidl_typesupport_introspection_cpp. Then deserialize the fastrtps data in the buffer, replace header time stamper, serialize it and publish. Considering the processing speed we stick to the plan above using memcpy.
I totally agree that it is worth some discussion about where and how this feature should be implemented. This PR is just our try to solve the issue and it works. Any advise or suggestion is helpful for us :)

@Karsten1987 the ros2/rosidl#416 you mentioned does not solve the problem. We do not know the type at hand (we cannot static cast something unknown into something unknown) ...
std_msgs__msg__String msg = *static_cast<std_msgs__msg__String *>(message_memory);
in your solution does not help when dealing with fully unknown type deserialization. This could only work if we switch(type_string) case 1, case 2, case 3 all possible message types - which is not possible due to the existence of custom messages (which are not known by default).

This works well for subscribers (where I give the expected Template type of the deserialization) ... but it should not work in places where i deserialize something random and look for fields.

Am I missing something here? Thanks :)

rosbag2_transport/src/rosbag2_transport/player.hpp

Signed-off-by: Shi <[email protected]>

zmichaels11

There's a lot going on in this PR; I'm going to give it another pass in 1-2hrs.

zmichaels11 · 2020-02-19T18:15:47Z

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)


Is REQUIRED not required here?

zmichaels11 · 2020-02-19T18:20:32Z

rosbag2_transport/src/rosbag2_transport/player.cpp

+        data_size = sizeof (float);
+        break;
+      case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_FLOAT64:
+        data_size = sizeof (double);


sizeof(double) may be sizes different than 64bits. Is this intentional?
I would have expected it to be 8 considering the switch case is FLOAT64

https://github.com/ros2/rmw_fastrtps/blob/9438cca2a6a21b2436684607fea8a78624363f80/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/TypeSupport_impl.hpp#L525

👍
Is an example for that:

we should replicate what was done here @scoopySHI

Signed-off-by: Shi <[email protected]>

kikass13 · 2020-02-20T08:50:00Z

Thank you guys for the first feedback, mostly regarding style and broader implementation details.
@scoopySHI and myself are not really sure about our approach regarding the solution to #299 and #99. We are not experts in the DDS specifics and if you guys have some nice ideas we are open for them :)

To summarice what @scoopySHI said in #299, we are searching for a fast way to get a "simulated" time in ros bag 2 similia to the feature ros1 had, so we have implemented a method of changing the timestamp of the current ros message to the current time without deserializing it. The solution follows these steps:

initialize rosbag player
- remember start time of first bag message and start time of current system clock
- type introspect each messae in the bagfile and lookup type-supports via dynamic symbol lookup get_symbol(type_str)
- use each type-support to find header a header field in the message
- this information will be kept inside a lookup hashtable unordered_map
play rosbag messages by dequeuing serialized messages
- before publish, check each message for it's type
- to do that, we need to do a lookup into our hashtable (header fields message types are already in there)
- calculate the header field offset in memory using the same dynamic alignment functions that the de-serialization uses.
- create our stamp message and set it to the stamp = current time + (current_message_stamp_in_bagfile - first_message_time_in_bagfile - ), that way we can replicate the time difference of the past and propagate it into the future
- memcopy our stamp into the offset calculated
- publish the message as normal

zmichaels11 · 2020-02-20T17:04:53Z

rosbag2_transport/src/rosbag2_transport/player.cpp

+      current_position_ += last_offset;
+      buffer_temp = buffer_temp + current_position_ + 4;
+
+      memcpy(buffer_temp, &ros_time_to_set, sizeof(builtin_interfaces::msg::Time));


Nit: Use memcpy_s I believe Windows will complain about not using it.

zmichaels11 · 2020-02-20T17:09:39Z

rosbag2_transport/src/rosbag2_transport/player.hpp


  static constexpr double read_ahead_lower_bound_percentage_ = 0.9;
+  size_t last_data_size_ = ULONG_MAX;
+  unsigned long current_position_ = 0;


Nit: I believe current_position_ should be std::ptrdiff_t since it is used in pointer math.

zmichaels11 · 2020-02-20T17:11:56Z

rosbag2_transport/src/rosbag2_transport/player.hpp


  static constexpr double read_ahead_lower_bound_percentage_ = 0.9;
+  size_t last_data_size_ = ULONG_MAX;


A more C++ way of doing this would be to use numeric_limits::max()

zmichaels11 · 2020-02-20T17:15:25Z

rosbag2_transport/src/rosbag2_transport/player.cpp

+    //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++) {


The iterating index should be the same type as array_size_

emersonknapp · 2020-02-20T20:01:55Z

As a high level question, I want to summarize what we're seeing here -

When playing back messages, if they have a Header, re-stamp the Header timestamp with the current ROS time

If I'm not mistaken, rosbag1 never had this feature, the feature it did expose is what's asked for in #99. This involves publishing an interpolated /clock topic so that other nodes in the system can use_sim_time to run along.

Do we actually want to implement this re-stamping feature, or would we be better off aiming at a solution for #99 instead?

kikass13 · 2020-02-21T00:37:09Z

to comment on what @emersonknapp said: Yes you are absolutely correct with your analysis. The relevance of this feature is ... questionable... for us at best.

As far as I can tell, there is no implementation for an underlying /clock topic in Nodes, but I could be wrong with that. As I said in my comment before, @scoopySHI and myself are not really sure about this approach either.

We implemented it currently because we are doing integration tests with the current robot_localisation (RL) in eloquent and issue #99 literally destroyed our ability to debug the RL, which is, in it's current state, so broken that we cannot use it properly.

So your question about re-implementing the clock topic feature for simulating time (in the Base rclcpp:Node as well as here in rosbag) is valid and needs to be addressed by some guys other than us because, we (myself and @scoopySHI) are not really an authority when it comes to ros2 guidelines of how to do stuff.

This solution though has one benefit in relation to the old /clock solution. It is standalone and does therefore not involve or compromise the nodes initialization behavior , the system clock or any other implementation. Nonetheless could our solution be seen as "dirty as fuck", which is not something I would personally disagree with :D

emersonknapp · 2020-02-21T03:13:08Z

The use_sim_time parameter works in ros2 on Nodes, which makes them use the /clock topic. The only change to make that work would be here in rosbag2 to publish the clock.

scoopySHI · 2020-02-21T14:15:30Z

The use_sim_time parameter works in ros2 on Nodes, which makes them use the /clock topic. The only change to make that work would be here in rosbag2 to publish the clock.

Thanks for the hint, i will test it with our stack and then try to implement the /clock publisher inside ros2bag. For now i will close this PR since i think right now it is not the best way to solve the problem referring to issue #99 . @emersonknapp

kikass13 reviewed Feb 18, 2020

View reviewed changes

emersonknapp reviewed Feb 18, 2020

View reviewed changes

emersonknapp requested review from Karsten1987 and zmichaels11 February 18, 2020 19:32

Karsten1987 requested changes Feb 18, 2020

View reviewed changes

Shi added 2 commits February 19, 2020 15:35

can choose if replace timestamper with current sys time

154d3f8

Signed-off-by: Shi <[email protected]>

improve code

264144f

Signed-off-by: Shi <[email protected]>

zmichaels11 reviewed Feb 19, 2020

View reviewed changes

zmichaels11 self-requested a review February 19, 2020 18:22

improve code

d2ffe19

Signed-off-by: Shi <[email protected]>

zmichaels11 reviewed Feb 20, 2020

View reviewed changes

scoopySHI closed this Feb 21, 2020

KenYN mentioned this pull request Sep 1, 2020

play - publish /clock topic #99

Closed


		static constexpr double read_ahead_lower_bound_percentage_ = 0.9;
		size_t last_data_size_ = ULONG_MAX;

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

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

Conversation

scoopySHI commented Feb 18, 2020

kikass13 left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

kikass13 Feb 18, 2020 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

kikass13 left a comment

Choose a reason for hiding this comment

emersonknapp left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

scoopySHI Feb 19, 2020 • edited Loading

Choose a reason for hiding this comment

Karsten1987 commented Feb 18, 2020

Karsten1987 left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

scoopySHI Feb 19, 2020 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

zmichaels11 left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

kikass13 commented Feb 20, 2020

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

emersonknapp commented Feb 20, 2020

kikass13 commented Feb 21, 2020 • edited Loading

emersonknapp commented Feb 21, 2020

scoopySHI commented Feb 21, 2020

kikass13 Feb 18, 2020 •

edited

Loading

scoopySHI Feb 19, 2020 •

edited

Loading

scoopySHI Feb 19, 2020 •

edited

Loading

kikass13 commented Feb 21, 2020 •

edited

Loading