-
Notifications
You must be signed in to change notification settings - Fork 243
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
feature: replace rosbag time stamper with sim time(current time) #300
Conversation
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
this is just a little view on things
//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; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
can we do this vendor specific thing here?
how can we use the dds vendor implementation instead of using copied code in here
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
How is this alignment related to time? Just trying to keep the set of changes semantically separated.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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.
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 |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
can this +4 dds header be looked up in any way?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
cool
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
A few comments outside the feature itself, will take a second pass at the meat of it
@@ -26,6 +26,7 @@ struct PlayOptions | |||
public: | |||
size_t read_ahead_queue_size; | |||
std::string node_prefix = ""; | |||
std::string clock_type = "past"; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
What values are accepted here? I am thinking this should probably be an enum.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Could this be part of the actual clock types being available in ros2? http://docs.ros2.org/eloquent/api/rcl/time_8h.html#a5c734f508ce06aec7974af10ad09d071
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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.
this PR has to be rebased on top of master. This should also make the diff way easier to read. |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
a few comments, but I am mainly requesting changes for the rebasing.
ros2bag/ros2bag/verb/play.py
Outdated
@@ -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', |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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
.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
"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.
@@ -26,6 +26,7 @@ struct PlayOptions | |||
public: | |||
size_t read_ahead_queue_size; | |||
std::string node_prefix = ""; | |||
std::string clock_type = "past"; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Could this be part of the actual clock types being available in ros2? http://docs.ros2.org/eloquent/api/rcl/time_8h.html#a5c734f508ce06aec7974af10ad09d071
//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; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
How is this alignment related to time? Just trying to keep the set of changes semantically separated.
} | ||
|
||
//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) |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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 :)
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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 :)
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
@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 :)
19e6566
to
5d452f2
Compare
Signed-off-by: Shi <Qiyao.Shi@streetscooter.com>
Signed-off-by: Shi <Qiyao.Shi@streetscooter.com>
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
There's a lot going on in this PR; I'm going to give it another pass in 1-2hrs.
rosbag2_transport/CMakeLists.txt
Outdated
@@ -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) |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Is REQUIRED
not required here?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
👍
data_size = sizeof (float); | ||
break; | ||
case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_FLOAT64: | ||
data_size = sizeof (double); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
👍
Is an example for that:
we should replicate what was done here @scoopySHI
Signed-off-by: Shi <Qiyao.Shi@streetscooter.com>
Thank you guys for the first feedback, mostly regarding style and broader implementation details. 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:
|
current_position_ += last_offset; | ||
buffer_temp = buffer_temp + current_position_ + 4; | ||
|
||
memcpy(buffer_temp, &ros_time_to_set, sizeof(builtin_interfaces::msg::Time)); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Nit: Use memcpy_s
I believe Windows will complain about not using it.
|
||
static constexpr double read_ahead_lower_bound_percentage_ = 0.9; | ||
size_t last_data_size_ = ULONG_MAX; | ||
unsigned long current_position_ = 0; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Nit: I believe current_position_
should be std::ptrdiff_t
since it is used in pointer math.
|
||
static constexpr double read_ahead_lower_bound_percentage_ = 0.9; | ||
size_t last_data_size_ = ULONG_MAX; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
A more C++
way of doing this would be to use numeric_limits::max()
//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++) { |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
The iterating index should be the same type as array_size_
As a high level question, I want to summarize what we're seeing here -
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 Do we actually want to implement this re-stamping feature, or would we be better off aiming at a solution for #99 instead? |
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 |
The |
Thanks for the hint, i will test it with our stack and then try to implement the |
The feature is mentioned in issue #299.