Skip to content

Enable ros2topic delay command#139

Merged
clalancette merged 3 commits intoros2:masterfrom
yechun1:port_rostopic_delay
Oct 4, 2018
Merged

Enable ros2topic delay command#139
clalancette merged 3 commits intoros2:masterfrom
yechun1:port_rostopic_delay

Conversation

@yechun1
Copy link
Copy Markdown
Contributor

@yechun1 yechun1 commented Aug 24, 2018

Porting rostopic delay command from ROS to ROS2. Display delay of topic from timestamp in header.

Usage: ros2 topic delay /topic_name

Signed-off-by: Chris Ye chris.ye@intel.com

Connects to: #132

@tfoote tfoote added the in review Waiting for review (Kanban column) label Aug 24, 2018
@dirk-thomas dirk-thomas self-requested a review September 6, 2018 17:22
@yechun1 yechun1 force-pushed the port_rostopic_delay branch 2 times, most recently from aa2656c to 623d3ef Compare September 7, 2018 07:03
@dirk-thomas dirk-thomas added question Further information is requested in review Waiting for review (Kanban column) and removed in review Waiting for review (Kanban column) question Further information is requested labels Sep 7, 2018
@dirk-thomas
Copy link
Copy Markdown
Member

Thank you for working on this feature.

In the current form the patch is pretty difficult to review. Please split the patch into separate commit with roughly the following structure:

  • first commit containing the exact copy of the upstream file
  • additional commits for any changes you applied, e.g.
    • style changes, comments, removing obsolete change in one commit
    • ROS 2 API changes in a second commit
    • functional changes in a third commit

@dirk-thomas dirk-thomas added in progress Actively being worked on (Kanban column) requires-changes and removed in review Waiting for review (Kanban column) labels Sep 7, 2018
@yechun1
Copy link
Copy Markdown
Contributor Author

yechun1 commented Sep 10, 2018

Thanks for guide, I'll resubmit commits early this week.

@yechun1 yechun1 force-pushed the port_rostopic_delay branch from 623d3ef to 3b02cf8 Compare September 10, 2018 11:20
@yechun1
Copy link
Copy Markdown
Contributor Author

yechun1 commented Sep 10, 2018

patch splitted, be ready for review.

'topic',
help='Topic name to be calcurated the delay')
arg.completer = TopicNameCompleter(
include_hidden_topics_key='include_hidden_topics')
Copy link
Copy Markdown
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

There is currently no include_hidden_topics argument here? I guess you need to add it in order to work.

Copy link
Copy Markdown
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Could work, same usage ref to pub.py and list.py
from ros2topic.api import TopicNameCompleter

include_hidden_topics=getattr(
parsed_args, self.include_hidden_topics_key))

Copy link
Copy Markdown
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Never mind, the argument is added one level above in

parser.add_argument(
'--include-hidden-topics', action='store_true',
help='Consider hidden topics as well')
and will work for all subverbs under topic.

arg.completer = TopicNameCompleter(
include_hidden_topics_key='include_hidden_topics')
parser.add_argument(
'--window', '-w', default=-1,
Copy link
Copy Markdown
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Please define the type of the argument as int than argparse will already ensure the type for you. It would make sense if the default value would be a valid value so that users don't have to specify one.

Copy link
Copy Markdown
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

fixed (5e7cc6a)


# can't have infinite window size due to memory restrictions
if window_size < 0:
if window_size < 0 or window_size > 50000:
Copy link
Copy Markdown
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The input of window size should be checked after argparse and if negative rejected (rather than silently changed).

Please don't hard code arbitrary limits like 50000. It should be the users choice to use a larger window if they know what they are doing.

Copy link
Copy Markdown
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

fixed a5aec05

return

curr = curr_rostime.to_sec()
curr = curr_rostime.nanoseconds * 1e-9
Copy link
Copy Markdown
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

You could avoid the conversion to floating point seconds and keep using the Time class for the operations. That would avoid any rounding errors.

Copy link
Copy Markdown
Contributor Author

@yechun1 yechun1 Sep 11, 2018

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

fixed 2c01515
As ROS there is to_sec and to_time method, but I not found similar in ROS2, especially about msg_header.stamp. this is builtin_interface/Time type, does there is function like stamp.to_time()?

Copy link
Copy Markdown
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Not at the moment: see ros2/rclpy#236

return t_type, t, msgevalgen(topic[len(t):])
else:
return None, None, None
timer = node.create_timer(1, timer_callback)
Copy link
Copy Markdown
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

You could register rt.print_delay here directly getting rid of the local function.

Copy link
Copy Markdown
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

fixed 9c6ad41

self.msg_tn = curr

if len(self.delays) > self.window_size - 1:
if len(self.delays) > self.window_size:
Copy link
Copy Markdown
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Please describe why this was changed.

Copy link
Copy Markdown
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

To fix the issue that no data print when set window as 1,
ros2 topic delay /image -w 1

Copy link
Copy Markdown
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Don't know why here need to window_size - 1 ?


self.last_msg_tn = self.msg_tn
return mean, min_delta, max_delta, std_dev, n + 1
return mean, min_delta, max_delta, std_dev, n
Copy link
Copy Markdown
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Please describe why this was changed.

@yechun1
Copy link
Copy Markdown
Contributor Author

yechun1 commented Sep 11, 2018

thanks for review, I have added changes for fixing.


# can't have infinite window size due to memory restrictions
if window_size < 0:
window_size = 50000
Copy link
Copy Markdown
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This "magic" upper bound is nowhere visible to the user. Please use it as a default value of the argument instead (and ensure in code that the user didn't pass a negative value).

Copy link
Copy Markdown
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

this is original design, I have not changed while porting. If user did not set the value. As previous design, the default value of window is -1, and here will re-set to max 50000 as memory restrictions.

Copy link
Copy Markdown
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Sure, it would still benefit from being improved.

% (delay, min_delta, max_delta, std_dev, window))


def _rostopic_delay(node, topic, window_size=-1, blocking=False):
Copy link
Copy Markdown
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I would suggest making window_size a required argument - otherwise this function needs to pick an opaque default value.

How does blocking=False work if the msg_class can't be determined yet?

Copy link
Copy Markdown
Contributor Author

@yechun1 yechun1 Sep 12, 2018

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This is original design. I think general user will not concern about window_size and just easy to use the tool. It will give user a little more trouble every time to type --window as required argument. It may also confuse user about the window meaning (first time I use rostopic, I really don't know what this window mean :))

Sorry but not considered when blocking=False, thanks for review, fixed by 0e79d06 (replaced by c3e506f )

Copy link
Copy Markdown
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

set blocking=True by default. commit again: c3e506f

Copy link
Copy Markdown
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

(commit: 2cd09fe): Is this fixing acceptable to set DEFULT_WINDOW_SIZE? (ref to cho.py code). User can input any value (unsigned int) or use default. The DEFULT_WINDOW_SIZE is set as 10000 and could be any other value.

curr_nanosec = curr_rostime.to_msg().nanosec

delay = ((curr_sec + curr_nanosec * 1e-9) -
(msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9))
Copy link
Copy Markdown
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

You should be able to compute the delay (as a Duration) by doing curr_rostime.to_msg() - rclpy.Time.from_msg(msg.header.stamp) (see https://github.com/ros2/rclpy/blob/62012d3650de790d89c725232451c4e998b396fb/rclpy/rclpy/time.py#L131) and store the duration instance in self.delays.

self.msg_tn = curr
self.delays = []
else:
# store the duration instrance in self.delays
Copy link
Copy Markdown
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Spelling: "instrance" -> "instance".

else:
# store the duration instrance in self.delays
duration = (curr_rostime - Time.from_msg(msg.header.stamp))
self.delays.append(duration.nanoseconds * 1e-9)
Copy link
Copy Markdown
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This isn't doing what the comment two lines above states - it still stores the duration as a floating point number in seconds.

Copy link
Copy Markdown
Contributor Author

@yechun1 yechun1 Sep 12, 2018

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Resubmitted the commit 34e400a


# can't have infinite window size due to memory restrictions
if window_size < 0:
window_size = 50000
Copy link
Copy Markdown
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Sure, it would still benefit from being improved.

@yechun1 yechun1 force-pushed the port_rostopic_delay branch from e9d5095 to 34e400a Compare September 12, 2018 01:55
@yechun1 yechun1 force-pushed the port_rostopic_delay branch 2 times, most recently from d057f2c to 2cd09fe Compare September 12, 2018 15:08
@yechun1
Copy link
Copy Markdown
Contributor Author

yechun1 commented Sep 12, 2018

@dirk-thomas commits with minor changes are submitted. Do you have any other suggests? so that I could sync with you and fix immediately when you are online:)

@dirk-thomas
Copy link
Copy Markdown
Member

Thank you for the updates!

I just ran CI builds for this branch and the results look good:

  • Linux Build Status
  • Linux-aarch64 Build Status
  • macOS Build Status
  • Windows Build Status

Can you please add the BSD license to the package manifest (as in https://github.com/ros2/ros2cli/pull/133/files#diff-27285248de861ae535a0217d2fa40c9cR11).

To keep the git history a bit shorter can you then reduce the commits as follows:

  • keep the first two commits (1-to-1 copy of file, add the link to "upstream")
  • squash the remaining commits into a third commit

After that this should be ready to be merged.

@dirk-thomas dirk-thomas added in review Waiting for review (Kanban column) and removed in progress Actively being worked on (Kanban column) requires-changes labels Sep 21, 2018
@yechun1 yechun1 force-pushed the port_rostopic_delay branch 2 times, most recently from 0e2f7ff to a317f3c Compare September 22, 2018 13:17
Signed-off-by: Chris Ye <chris.ye@intel.com>
* remove irrelevant code and reserve hz code (ros has only one __init__.py file include all topic commands, ros2 has splitted commands to isolated file)

* major functional changes of delay cmd with ROS2 API

* update license format to pass test_copyright

* use Time duration to compute the delay

* check window_size as non-negative integer, fix no print when set window as 1

Signed-off-by: Chris Ye <chris.ye@intel.com>
@yechun1
Copy link
Copy Markdown
Contributor Author

yechun1 commented Sep 29, 2018

To keep the git history a bit shorter can you then reduce the commits as follows:
keep the first two commits (1-to-1 copy of file, add the link to "upstream")
squash the remaining commits into a third commit

Added BSD license and merged the comments as
b805ac6, 3fd69ec, a317f3c

@clalancette
Copy link
Copy Markdown
Contributor

CI looks good, and the patch set has been redone according to what @dirk-thomas suggested. I also took a brief look at it, and it looks OK to me. I'm going to merge this; thanks for the contribution.

@clalancette clalancette merged commit 2049b2f into ros2:master Oct 4, 2018
@clalancette clalancette removed the in review Waiting for review (Kanban column) label Oct 4, 2018
@yechun1
Copy link
Copy Markdown
Contributor Author

yechun1 commented Oct 4, 2018

@dirk-thomas @clalancette thanks for review and merge.

@yechun1 yechun1 deleted the port_rostopic_delay branch October 4, 2018 11:54
@dirk-thomas dirk-thomas added the enhancement New feature or request label Oct 6, 2018
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

enhancement New feature or request

Projects

None yet

Development

Successfully merging this pull request may close these issues.

4 participants