Fix subframes disappearing when object is detached/scaled/renamed #1866

felixvd · 2020-01-24T20:12:22Z

Description

Subframes were being forgotten when objects were detached/removed from the robot, and when they were moved. They also would have overwritten the old subframes during an APPEND call. This PR should fix all of that, and adds a line to the Rviz plugin that says "The object has subframes X, Y, Z."

Subframes are not updated if the object is moved from the Rviz plugin. I am filing another PR about this in a minute.

Checklist

Required by CI: Code is auto formatted using clang-format
Create tests, which fail without this PR reference
While waiting for someone to review your request, please help review another open pull request to support the maintainers

felixvd · 2020-02-18T09:42:48Z

The tests on this PR succeed, only the code coverage is complaining. The tests for the functions affected by this should be implemented in #1757.

v4hn · 2020-03-20T20:16:21Z

moveit_core/planning_scene/src/planning_scene.cpp

+      ROS_WARN_NAMED(LOGNAME, "Object '%s' had subframes, but no new ones were given for Move operation. ",
+                     object.id.c_str());
+    }
+


i am not happy with the need to have the names vector transmitted here, but there's no way around that.

~~at least, it should be a normal use case of this to specify no subframes, even if the world object includes some.~~

On the other hand, moving shapes, typically also invalidates subframes and thus it makes sense to update the latter in the same call

I agree with @rhaschke, If the object contains subframes, they should be updated indeed.

moveit_core/planning_scene/src/planning_scene.cpp

rhaschke · 2020-03-26T13:41:09Z

moveit_core/planning_scene/src/planning_scene.cpp

@@ -1822,12 +1807,32 @@ bool PlanningScene::processCollisionObjectMove(const moveit_msgs::CollisionObjec
    }

    collision_detection::World::ObjectConstPtr obj = world_->getObject(object.id);
+
+    // Update subframe poses
+    moveit::core::FixedTransformsMap subframe_poses = obj->subframe_poses_;


This whole code snipped on subframe updates is only relevant if a shape update is actually performed below.
Hence, I think this code should be moved into the if condition below and a warning should be issued if a user just tries to move subframes (but no shapes).

The data structures actually allow to handle the additional use-case to specify only subframe poses and no shapes.
It's not really an important use-case (maybe for tcp online estimation?), but I think we could include it here?
Either way, the current patch need updating as it mixes both interpretations, doing the computations independent and then update them only if new shape poses exist...

If you just want to update subframes, you better use APPEND mode.

Yes, the MOVE operation shouldn't add anything to the object. I removed the whole if block and just updated the world object.

rhaschke

I suggest to split this PR into two:

fixing the subframe handling on planning scene updates
displaying corresponding subframes in rviz

felixvd · 2020-04-15T11:23:11Z

I split this as requested without any other changes for now.

codecov-io · 2020-04-16T14:44:52Z

Codecov Report

Merging #1866 into master will increase coverage by 0.00%.
The diff coverage is 40.00%.

@@           Coverage Diff           @@
##           master    #1866   +/-   ##
=======================================
  Coverage   54.04%   54.04%           
=======================================
  Files         319      319           
  Lines       24997    25003    +6     
=======================================
+ Hits        13509    13514    +5     
- Misses      11488    11489    +1

Impacted Files	Coverage Δ
moveit_core/planning_scene/src/planning_scene.cpp	`57.50% <36.84%> (-0.19%)`	⬇️
...t_state/include/moveit/robot_state/attached_body.h	`100.00% <100.00%> (ø)`
...ove_group_interface/src/wrap_python_move_group.cpp	`39.87% <0.00%> (-3.17%)`	⬇️
...anning_scene_monitor/src/current_state_monitor.cpp	`51.28% <0.00%> (-1.54%)`	⬇️
.../move_group_interface/src/move_group_interface.cpp	`40.56% <0.00%> (-0.22%)`	⬇️
...raint_samplers/src/default_constraint_samplers.cpp	`82.90% <0.00%> (+0.36%)`	⬆️
...veit_experimental/moveit_jog_arm/src/jog_calcs.cpp	`75.00% <0.00%> (+5.37%)`	⬆️

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update 4d30621...87e0c86. Read the comment docs.

v4hn · 2020-05-18T20:08:21Z

@felixvd is (part of) this still unmerged? could you resolve the conflict?

codecov-commenter · 2020-05-21T16:33:31Z

Codecov Report

Merging #1866 into master will decrease coverage by 0.06%.
The diff coverage is 57.14%.

@@            Coverage Diff             @@
##           master    #1866      +/-   ##
==========================================
- Coverage   57.79%   57.72%   -0.07%     
==========================================
  Files         326      326              
  Lines       25615    25618       +3     
==========================================
- Hits        14803    14787      -16     
- Misses      10812    10831      +19

Impacted Files	Coverage Δ
...t_state/include/moveit/robot_state/attached_body.h	`100.00% <ø> (ø)`
moveit_core/planning_scene/src/planning_scene.cpp	`59.08% <57.14%> (-0.40%)`	⬇️
.../ompl_interface/src/detail/constrained_sampler.cpp	`46.15% <0.00%> (-17.95%)`	⬇️
...meterization/work_space/pose_model_state_space.cpp	`82.31% <0.00%> (-1.37%)`	⬇️
...on/pick_place/src/approach_and_translate_stage.cpp	`72.80% <0.00%> (-0.80%)`	⬇️
...dl_kinematics_plugin/src/kdl_kinematics_plugin.cpp	`78.92% <0.00%> (-0.45%)`	⬇️
...raint_samplers/src/default_constraint_samplers.cpp	`82.54% <0.00%> (-0.37%)`	⬇️
moveit_core/robot_state/src/robot_state.cpp	`47.00% <0.00%> (-0.10%)`	⬇️
...ove_group_interface/src/wrap_python_move_group.cpp	`42.94% <0.00%> (-0.10%)`	⬇️
...lanners/ompl/ompl_interface/src/ompl_interface.cpp	`71.42% <0.00%> (ø)`
... and 1 more

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update dc909df...af3a569. Read the comment docs.

felixvd · 2020-07-04T18:48:15Z

I updated and rebased this. The PR now only fixes the problem that subframes disappear when objects are detached objects from the robot, or scaled/renamed in Rviz. The changes to the MOVE operation will be in #2037 instead.

felixvd · 2020-07-29T14:04:34Z

Ping

v4hn

Sorry, more feedback inline.

I should have reviewed properly before...

moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp

moveit_core/robot_state/include/moveit/robot_state/attached_body.h

v4hn · 2020-07-29T15:17:05Z

moveit_core/planning_scene/src/planning_scene.cpp

-      }
+      const moveit::core::AttachedBody* body = robot_state_->getAttachedBody(object.object.id);
+      if (body)
+        attached_bodies.push_back(body);


Could you please check for whether the id is actually attached to object.link_name and trigger a warning if that's not the case?
The previous version would simply ignore the object if a wrong link is specified, but a warning is of course preferred. :)

Sure, seems like a rare error though. Please have a look and confirm that this is what you meant.

The condition is good, but I would say we either treat it as a proper error the user has to fix before they can continue (so the method should return), or we specify in the warning that the object attached to the other link is used instead.

I don't know which way will be better, but I chose to make it an error and fixed the condition (it would have fired for the default value otherwise).

moveit_core/planning_scene/src/planning_scene.cpp

- remove code duplication, when fetching attached bodies - simplify function calls when detaching (attached) bodies

v4hn

I added some minor adjustments and rebased to resolve the merge conflict.
Please have a final look @felixvd .

Thanks as always 😎

I approve as long as CI succeeds now.

felixvd · 2020-07-30T13:55:04Z

Perfect, thanks for the review :)

v4hn · 2020-07-30T14:18:25Z

Perfect

Not really. I just had to fix one misuse of ROS_ERROR_NAMED.

The second review might take some time because Robert is on vacation.
That is to say, unless some other maintainer is willing to look at this. :-)

felixvd · 2020-07-30T14:58:09Z

I see. I figured it would also concatenate. The more you know

tylerjw

I tested this and it works for me. One tiny thing I found. I might just be misreading what you are trying to do there.

moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp

felixvd requested review from jonbinney and rhaschke as code owners January 24, 2020 20:12

felixvd mentioned this pull request Mar 3, 2020

PlanningScene: "Move for attached objects not yet implemented" #1581

Open

felixvd mentioned this pull request Mar 13, 2020

Rviz Plugin: "Object Status" field in "Scene Objects" tab does not wrap lines #1945

Closed

v4hn reviewed Mar 20, 2020

View reviewed changes

moveit_core/planning_scene/src/planning_scene.cpp Outdated Show resolved Hide resolved

felixvd force-pushed the fix-subframes-during-moves branch from 76abf88 to a9f8745 Compare March 23, 2020 14:54

rhaschke reviewed Mar 26, 2020

View reviewed changes

felixvd force-pushed the fix-subframes-during-moves branch from 7cb409f to 6957460 Compare April 15, 2020 08:08

felixvd requested a review from mlautman as a code owner April 16, 2020 13:53

felixvd force-pushed the fix-subframes-during-moves branch from 87e0c86 to 8e6222f Compare May 21, 2020 15:01

felixvd changed the title ~~Fix subframes during MOVE/REMOVE/APPEND operations, display them in the Rviz plugin~~ Fix subframes during MOVE/REMOVE/APPEND operations May 21, 2020

felixvd force-pushed the fix-subframes-during-moves branch from 8e6222f to e53a900 Compare July 4, 2020 18:35

felixvd requested a review from tylerjw as a code owner July 4, 2020 18:35

felixvd force-pushed the fix-subframes-during-moves branch from e53a900 to 04ec95d Compare July 4, 2020 18:45

felixvd changed the title ~~Fix subframes during MOVE/REMOVE/APPEND operations~~ Fix subframes disappearing when object is detached/scaled/renamed Jul 5, 2020

v4hn reviewed Jul 29, 2020

View reviewed changes

felixvd mentioned this pull request Jul 29, 2020

Add object pose #2037

Merged

5 tasks

felixvd and others added 4 commits July 30, 2020 15:17

Treat subframes when detaching objects

f0573ab

reduce code complexity

b90d4f5

- remove code duplication, when fetching attached bodies - simplify function calls when detaching (attached) bodies

Maintain subframes when scaling/renaming objects in Rviz

ad3105a

clang-format

bbacd03

felixvd and others added 3 commits July 30, 2020 15:19

Add review comments

cd154bb

More review comments

5674bdf

minor nits

880046f

v4hn force-pushed the fix-subframes-during-moves branch from d96c493 to 880046f Compare July 30, 2020 13:19

v4hn approved these changes Jul 30, 2020

View reviewed changes

v4hn added the awaits 2nd review one maintainer approved this request label Jul 30, 2020

fixup: use STREAM logging

41f3f89

tylerjw approved these changes Jul 30, 2020

View reviewed changes

moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp Show resolved Hide resolved

Add TODO

af3a569

tylerjw merged commit 7d40d82 into moveit:master Aug 4, 2020

felixvd deleted the fix-subframes-during-moves branch August 24, 2020 17:15

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Fix subframes disappearing when object is detached/scaled/renamed #1866

Fix subframes disappearing when object is detached/scaled/renamed #1866

felixvd commented Jan 24, 2020

felixvd commented Feb 18, 2020

v4hn Mar 20, 2020 •

edited

Loading

rhaschke Mar 26, 2020

v4hn Mar 26, 2020

rhaschke Mar 26, 2020

felixvd Jul 4, 2020

rhaschke left a comment

felixvd commented Apr 15, 2020

codecov-io commented Apr 16, 2020

v4hn commented May 18, 2020

codecov-commenter commented May 21, 2020 •

edited by codecov bot

Loading

felixvd commented Jul 4, 2020

felixvd commented Jul 29, 2020

v4hn left a comment

v4hn Jul 29, 2020

felixvd Jul 29, 2020

v4hn Jul 29, 2020

felixvd Jul 29, 2020

v4hn left a comment

felixvd commented Jul 30, 2020

v4hn commented Jul 30, 2020

felixvd commented Jul 30, 2020

tylerjw left a comment

Fix subframes disappearing when object is detached/scaled/renamed #1866

Fix subframes disappearing when object is detached/scaled/renamed #1866

Conversation

felixvd commented Jan 24, 2020

Description

Checklist

felixvd commented Feb 18, 2020

v4hn Mar 20, 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

rhaschke left a comment

Choose a reason for hiding this comment

felixvd commented Apr 15, 2020

codecov-io commented Apr 16, 2020

Codecov Report

v4hn commented May 18, 2020

codecov-commenter commented May 21, 2020 • edited by codecov bot Loading

Codecov Report

felixvd commented Jul 4, 2020

felixvd commented Jul 29, 2020

v4hn 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

v4hn left a comment

Choose a reason for hiding this comment

felixvd commented Jul 30, 2020

v4hn commented Jul 30, 2020

felixvd commented Jul 30, 2020

tylerjw left a comment

Choose a reason for hiding this comment

v4hn Mar 20, 2020 •

edited

Loading

codecov-commenter commented May 21, 2020 •

edited by codecov bot

Loading