move_group capability for publishing planning scene frames to the tf system #1761

JonasTietz · 2019-11-21T14:56:11Z

Description

This PR points to issue #1735 . I have implemented a move_group capability, which publishes frames of planning scene objects to the tf system.

welcome · 2019-11-21T14:56:12Z

Thanks for helping in improving MoveIt and open source robotics!

rhaschke

Thanks. Looks like a great contribution if ready.
Some remarks:

Please update the license / disclaimer text.
Can you please replace tf1 with tf2?

moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.cpp

moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.h

moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.cpp

rhaschke · 2019-11-21T19:01:16Z

moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.cpp

+
+    for (auto obj = world->begin(); obj != world->end(); ++obj)
+    {
+      tf::poseEigenToTF(obj->second->shape_poses_[0], transform);


It's a pity that all transforms are published w.r.t. planning_frame only. I guess the original information is lost.
What about sub frames?

Also, what about CollisionObjects that are attached to the robot? I don't see them published in this file and I don't believe the robot state publishes them either.

Seconding the question about subframes, and agree that parent link information would be interesting. Nothing we can do about it for now I suppose.

Attached objects are not yet addressed, are they?

Not that I can see

v4hn · 2019-11-21T20:19:20Z

i guess this is still work-in-progress, although it's not marked as such.
thanks for the review, I did not actually look at it before.

moveit_ros/move_group/default_capabilities_plugin_description.xml

moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.cpp

felixvd · 2019-11-22T15:29:14Z

Thanks for tackling this!

moveit_ros/move_group/default_capabilities_plugin_description.xml

moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.cpp

v4hn · 2019-11-25T10:38:09Z

I could definitly do that. But I think renaming the other capabilities should go in another PR.

I did not ask you to rename the *other* capabilities. I asked you to remove the prefix from the C++ class.

JonasTietz · 2019-11-25T11:16:09Z

I could definitly do that. But I think renaming the other capabilities should go in another PR.
I did not ask you to rename the other capabilities. I asked you to remove the prefix from the C++ class.

Ahh. Ok I misunderstood that.

rhaschke

This is still using tf1, while we switched (with rather large effort) to tf2 a while ago. Please change.

moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.cpp

rhaschke

Thanks a lot for iterating on this. We are getting close 😄
One minor point: Please re-add ros::ok(): If ROS dies for any reason, the thread needs to be finished as well.

v4hn · 2019-11-26T15:29:41Z

Please re-add `ros::ok()`: If ROS dies for any reason, the thread needs to be finished as well.

I'm not sure I get your reasoning here. I thought you explicitly want to get rid of this exit condition, as it adds an outside flag for an internally-managed thread. If ROS dies, the `move_group` node should clean up and delete the capability, thus setting the flag and joining the thread. I do not see why you would want to add the check back.

rhaschke

I don't mind.

… scene objects to the tf system.

…mespace

…_publisher_capability object

welcome · 2019-11-27T15:12:42Z

Congrats on getting your first MoveIt pull request merged and improving open source robotics!

v4hn · 2019-11-27T15:13:02Z

Congrats @JonasTietz and thank you for this nice contribution!

v4hn · 2019-11-27T15:20:37Z

Attached objects are not yet addressed, are they?

Very true! Sorry, I merged after I got CI to accept it and did not see your comment. @JonasTietz Could you have a look at attached objects in a follow-up request? :) Attached objects are stored in the `RobotState` of the `PlanningScene` and can be accessed via `RobotState::getAttachedBodies`.

felixvd · 2019-11-27T16:02:48Z

Please also add subframes while you're at it. They are part of the CollisionObject.

rhaschke · 2019-11-27T16:04:19Z

subframes are already published for CollisionObjects, but I think attached collision objects also have subframes.

JonasTietz · 2019-11-27T16:18:22Z

@JonasTietz Could you have a look at attached objects in a follow-up request? :) Attached objects are stored in the RobotState of the PlanningScene and can be accessed via RobotState::getAttachedBodies.

I will take a look at it. :)

JonasTietz requested review from IanTheEngineer and rhaschke as code owners November 21, 2019 14:56

rhaschke reviewed Nov 21, 2019

View reviewed changes

v4hn reviewed Nov 21, 2019

View reviewed changes

v4hn reviewed Nov 23, 2019

View reviewed changes

rhaschke force-pushed the master branch from 68982c0 to 11c82ec Compare November 24, 2019 14:20

rhaschke requested changes Nov 26, 2019

View reviewed changes

moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.cpp Show resolved Hide resolved

rhaschke reviewed Nov 26, 2019

View reviewed changes

rhaschke approved these changes Nov 26, 2019

View reviewed changes

Jonas Tietz added 12 commits November 26, 2019 17:28

Added a move_group capability, which publishes the frames of planning…

bf85c1d

… scene objects to the tf system.

Lock gets now released before the sleep

0ae7ff0

Added rosparam to control the publishing rate

a9b9736

Removed unnecessary prefix

20abd8a

the namespace of the move_group node is now the prefix of the frame

21dd6ec

Fixed license

1ffe080

added subframe publishing

dd215d9

The tf prefix is now configurable with the default being the nodes na…

d1a0315

…mespace

Removed MoveGroup prefix from the TfPublisher class

0ee7d78

instead of detaching the thread it now joins on destruction of the tf…

ae18169

…_publisher_capability object

replaced tf1 with tf2

0708216

replaced ros::ok with a flag

053c90f

v4hn force-pushed the move-group-tf-publisher-capability branch from d5166be to 053c90f Compare November 26, 2019 16:28

address clang-format changes

7e8880f

v4hn merged commit e16436c into moveit:master Nov 27, 2019

v4hn mentioned this pull request Dec 1, 2019

MoveGroupCapability to publish frames to TF #1735

Closed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

move_group capability for publishing planning scene frames to the tf system #1761

move_group capability for publishing planning scene frames to the tf system #1761

JonasTietz commented Nov 21, 2019

welcome bot commented Nov 21, 2019

rhaschke left a comment

rhaschke Nov 21, 2019

felixvd Nov 22, 2019

rhaschke Nov 27, 2019

felixvd Nov 27, 2019

v4hn commented Nov 21, 2019

felixvd commented Nov 22, 2019

v4hn commented Nov 25, 2019 via email

JonasTietz commented Nov 25, 2019

rhaschke left a comment

rhaschke left a comment

v4hn commented Nov 26, 2019 via email

rhaschke left a comment

welcome bot commented Nov 27, 2019

v4hn commented Nov 27, 2019

v4hn commented Nov 27, 2019 via email

felixvd commented Nov 27, 2019

rhaschke commented Nov 27, 2019

JonasTietz commented Nov 27, 2019

move_group capability for publishing planning scene frames to the tf system #1761

move_group capability for publishing planning scene frames to the tf system #1761

Conversation

JonasTietz commented Nov 21, 2019

Description

welcome bot commented Nov 21, 2019

rhaschke left a comment

Choose a reason for hiding this comment

rhaschke Nov 21, 2019

Choose a reason for hiding this comment

felixvd Nov 22, 2019

Choose a reason for hiding this comment

rhaschke Nov 27, 2019

Choose a reason for hiding this comment

felixvd Nov 27, 2019

Choose a reason for hiding this comment

v4hn commented Nov 21, 2019

felixvd commented Nov 22, 2019

v4hn commented Nov 25, 2019 via email

JonasTietz commented Nov 25, 2019

rhaschke left a comment

Choose a reason for hiding this comment

rhaschke left a comment

Choose a reason for hiding this comment

v4hn commented Nov 26, 2019 via email

rhaschke left a comment

Choose a reason for hiding this comment

welcome bot commented Nov 27, 2019

v4hn commented Nov 27, 2019

v4hn commented Nov 27, 2019 via email

felixvd commented Nov 27, 2019

rhaschke commented Nov 27, 2019

JonasTietz commented Nov 27, 2019