Reorder incoming out of order joint_names in trajectory messages #53

ddengster · 2020-06-03T08:30:57Z

As per title. For a use case, moveit sends joint messages with names out of order, so this would be nice to have.

siphoned from PR #36

gavanderhoorn · 2020-06-03T08:41:23Z

For a use case, moveit sends joint messages with names out of order, so this would be nice to have.

It's actually a requirement.

There is no default order, so it's also impossible to be "out of order".

Consumers must make sure to re-order these messages such that they align with whatever assumptions subsequent code has.

ddengster · 2020-06-03T08:50:35Z

It's actually a requirement.

Oh? To clarify, I'm talking about trajectory messages having their joint_names jumbled up and the corresponding trajectories moved according to joint_names. This may not align with the initialization parameters you gave to joint_trajectory_controller.

There was some joint remapping functionality in ros1 joint_trajectory_controller; it was also able to handle partial joints. So in some sense this is a port.

gavanderhoorn · 2020-06-03T08:59:45Z

Oh?

Yes.

To clarify, I'm talking about trajectory messages having their joint_names jumbled up and the corresponding trajectories moved according to joint_names.

Yes, I understand what you are referring to.

Consumers / Subscribers / Action servers cannot assume a particular order will be used for the joint names in incoming goals / messages. There has always been the requirement to check whether whatever comes in corresponds to what the server needs.

This may not align with the initialization parameters you gave to joint_trajectory_controller.

Of course.

Again: there has never been a requirement to maintain that order. So publishers/action clients should be able to use whatever order they want/need. Subscribers/servers have the responsibility to be able to process all incoming orders.

There was some joint remapping functionality in ros1 joint_trajectory_controller;

Not "some remapping functionality": that code was expressly there to address the point I'm making above.

it was also able to handle partial joints. So in some sense this is a port.

If the current implementation of the JTC in ros2_control cannot deal with orders other than the one in the configuration .yaml, that would be a regression.

ddengster · 2020-06-03T09:06:23Z

oh ok, sounds great to me! seems like i misunderstood what you meant there.

bmagyar

Thanks for the improvements!
My comments are a little intertwined, make sure to read all first, then they will make more sense :)

joint_trajectory_controller/src/joint_trajectory_controller.cpp

joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp

joint_trajectory_controller/src/joint_trajectory_controller.cpp

joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp

joint_trajectory_controller/test/test_trajectory_controller.cpp

bmagyar

Thanks @ddengster for this and the swift response to the review! I'm happy for this to go in.

joint_trajectory_controller/src/joint_trajectory_controller.cpp

Karsten1987

I don't fully understand what's happening, and might have to give this a second look.

For the time being, please enhance this with some doxygen and general comments, as stuff like this will be extremely tricky to revisit in the future.
At the same time, I appreciate that you went ahead and got some tests in, I would prefer to have more precise unit tests for the two main functions here (mapping and sort_to_local_joint_order)

joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp

Karsten1987 · 2020-06-05T15:37:23Z

joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp

+  std::vector<size_t> mapping_vector(t1.size());  // Return value
+  for (typename T::const_iterator t1_it = t1.begin(); t1_it != t1.end(); ++t1_it) {
+    typename T::const_iterator t2_it = std::find(t2.begin(), t2.end(), *t1_it);
+    if (t2.end() == t2_it) {return std::vector<size_t>();} else {


that format looks weird. Line break after the {

fix formatting 8628144

joint_trajectory_controller/src/joint_trajectory_controller.cpp

codecov-commenter · 2020-06-09T03:17:25Z

Codecov Report

All modified and coverable lines are covered by tests ✅

Project coverage is 32.92%. Comparing base (98b9a51) to head (507cb0e).
Report is 655 commits behind head on master.

Additional details and impacted files

@@            Coverage Diff             @@
##           master      #53      +/-   ##
==========================================
- Coverage   33.12%   32.92%   -0.21%     
==========================================
  Files          12       12              
  Lines        1123     1215      +92     
  Branches      740      802      +62     
==========================================
+ Hits          372      400      +28     
- Misses        103      114      +11     
- Partials      648      701      +53

Flag	Coverage Δ
unittests	`32.92% <ø> (-0.21%)`	⬇️

Flags with carried forward coverage won't be shown. Click here to find out more.

see 4 files with indirect coverage changes

Signed-off-by: Karsten Knese <[email protected]>

…o subscriber as well

Co-authored-by: Karsten Knese <[email protected]>

bmagyar

@ddengster I'm really happy how this turned out. I'm going to fix up the typos and merge

joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp

joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp

joint_trajectory_controller/src/joint_trajectory_controller.cpp

ddengster changed the title ~~Remap incoming out of order joint messages~~ Remap incoming out of order joint_names in trajectory messages Jun 3, 2020

bmagyar requested changes Jun 3, 2020

View reviewed changes

ddengster requested a review from bmagyar June 4, 2020 05:05

bmagyar approved these changes Jun 5, 2020

View reviewed changes

bmagyar requested a review from Karsten1987 June 5, 2020 08:59

bmagyar requested changes Jun 5, 2020

View reviewed changes

joint_trajectory_controller/src/joint_trajectory_controller.cpp Show resolved Hide resolved

Karsten1987 reviewed Jun 5, 2020

View reviewed changes

ddengster changed the title ~~Remap incoming out of order joint_names in trajectory messages~~ Reorder incoming out of order joint_names in trajectory messages Jun 8, 2020

v-lopez pushed a commit to pal-robotics-forks/ros2_controllers that referenced this pull request Jun 10, 2020

fix uncrustify test failures (ros-controls#53)

add5c5b

Signed-off-by: Karsten Knese <[email protected]>

ddengster and others added 12 commits June 11, 2020 16:13

incoming action messages are now remapped based on initialized order

8b45e4e

shift remapping code to remap_msg_trajectories function, applied it t…

e48ddd5

…o subscriber as well

added jumbled_joint_order test

0823893

linter and style fixes

641cfd2

renamed function, add docstring, type changes, simplify code

34efa43

error message for incoming goal with unmatched joint name

11a56eb

shorten decl #1

fd287c8

Co-authored-by: Karsten Knese <[email protected]>

shorten decl #2

1c7d8f3

Co-authored-by: Karsten Knese <[email protected]>

use std::find

d0cee6a

added warning for invalid sizes

cc2d08c

fix formatting

655042d

remove double input

4adabfa

ddengster mentioned this pull request Jun 12, 2020

[joint_trajectory_controller] Implement partial joint list handling #37

Closed

bmagyar approved these changes Jun 14, 2020

View reviewed changes

bmagyar added 2 commits June 14, 2020 08:26

Fix doc typos

0bcd354

Use const-ref of joint name in loop

507cb0e

bmagyar merged commit 7c52bec into ros-controls:master Jun 14, 2020

bmagyar mentioned this pull request Jun 14, 2020

Refactor test cases #64

Merged

Reorder incoming out of order joint_names in trajectory messages #53

Reorder incoming out of order joint_names in trajectory messages #53

Uh oh!

Conversation

ddengster commented Jun 3, 2020 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

gavanderhoorn commented Jun 3, 2020

Uh oh!

ddengster commented Jun 3, 2020 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

gavanderhoorn commented Jun 3, 2020 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

ddengster commented Jun 3, 2020

Uh oh!

bmagyar left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

bmagyar left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Karsten1987 left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Karsten1987 Jun 5, 2020

Choose a reason for hiding this comment

Uh oh!

ddengster Jun 9, 2020

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

codecov-commenter commented Jun 9, 2020 • edited by codecov bot Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Codecov Report

Uh oh!

bmagyar left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

5 participants

ddengster commented Jun 3, 2020 •

edited

Loading

ddengster commented Jun 3, 2020 •

edited

Loading

gavanderhoorn commented Jun 3, 2020 •

edited

Loading

codecov-commenter commented Jun 9, 2020 •

edited by codecov bot

Loading