Thanks for the second look!

2. Mixing the child frames into the top level object namespace makes a few parts of the API slightly more convenient for the user, but I think it's too likely to cause problems. I think it would be a lot cleaner API (and less code) if the named frames only makes sense in the context of a specific object. Thoughts?

@henningkayser proposed prefixing the named frames/subframes with the object name (which already needs to be unique), so they're namespaced. The named frame tip of screwdriver would be screwdriver_tip or screwdriver/tip in the scene. What do you think about that?

I don't know what you mean by "the named frames only make sense in the context of a specific object". Do you mean the user should get the frame transformation from the object to the object part, apply it to the pose they want to move the part to, and then plan with the new pose applied to the end effector? I do think that's too cumbersome to be useful.

@felixvd yes, i was proposing requiring the user to manually lookup the object and child-frame transforms separately and chain them together. I was imagining something like:

transform = world.getTransform("object_foo") * world.getObject("object_foo").getTransform("child_frame_bar")

Which didn't seem so bad; but I can see that it would be even more convenient to have a single function call in the API as you suggest. Prefixing with the object name is interesting, but it doesn't feel very machine-friendly (it's certainly user-friendly). How about having a function like:

transform = world.getChildFrameTransform(object_name, chlid_frame_name)

And then within the world we can do the lookups and multiply the two transforms, and raise a runtime error if one of them doesn't exist?

I can add the function, but it would make things a lot easier to just write "move top of cup under spout" instead of multiplying transforms. Normal users don't handle raw transforms in ROS either, and I don't think they should be expected to. It's something that notoriously goes wrong and that I don't miss doing at all.

I'll implement an attempt at namespacing this week though.

Sounds good - having the function that handles the namespacing for the user sounds like a good compromise.

I changed the naming from "named_frames" to "subframes". Subframes are always prefixed by their parent object's name, so the subframe "tip" of an object "cylinder" can be used as "cylinder_tip" in the planning scene. The namespacing is done in world.cpp and attached_body.h, in the respective getTransform function for collision::world::objects and AttachedBody objects.

In the current commit there is a bug that seems to cause a crash when the subframe name is shorter than the object's name (?), but I should be able to fix that soon. You can see it occurring for "box_top" in my test package, but not for the longer subframe names. Shortening them causes the error to occur for them, too.

Thanks @felixvd ! It is going to be a few days before i have time to do my next review of this, but I will get to it as soon as I can.

Just documenting that this branch contains essentially the same commit that seems to be causing me trouble, except this one is fine. Should be done before the maintainer meeting tomorrow.

Dear Felix, could you please clean up your commit history a little bit?
I suggest separating the core functional changes from variable renamings. All fixups should be squashed into corresponding commits. As of now, the PR is large and not very modular in its commits.

Just documenting that this branch contains essentially the same commit that seems to be causing me trouble, except this one is fine.

@felixvd, what problem(s) are you referring to? Travis failures?

Thanks for the review!

Dear Felix, could you please clean up your commit history a little bit?
I suggest separating the core functional changes from variable renamings. All fixups should be squashed into corresponding commits. As of now, the PR is large and not very modular in its commits.

I usually do squash/reorder before pushing, but I am not sure what the right way to do it is when commits were already pushed. Separate everything into multiple new PRs and abandon this one? Force push the branch to a clean version of commits? I assume that would interfere with the PR and review interface here on Github?
Please advise on the preferred method.

it feels strange to access those transforms in the same flat namespace as all other transforms. This makes it both, more costly (because you need to search all objects) and less transparent. I support Jon's proposal to use a function like
transform = world.getObjectSubFrame(object_name, chlid_frame_name)

I worry that this is too complicated for the common user. I am hoping for this functionality to be readily accessible, and end users should not need to multiply transforms. The world object is already on a deeper level than the average user who uses only geometry_msg objects with the move_group or planning_scene_interface.

Just to confirm: Is it the search through all the subframes that you find problematic, or the access through the top-level namespace?

If it is the latter, I don't know that this design decision would be consistent. We already let users specify a collision object name as a target for planning, although that requires a search operation through the scene as well. If we do not value this convenience, why not ask users to do transform = world.getFixedTransform(object_id)? (That function does not exist at the moment, but it could)

If it is the former, I am not sure that performance would be affected significantly. The search through object subframes always happens last, so robot links, CollisionObjects and AttachedBody objects are retrieved as fast as before. It will also only make a difference in scenes with many objects and frames. It might be premature optimization to favor this rather than an easier interface.

This is the current API. I hope we can agree that this is very readable, even for beginners, and that readability is important:

geometry_msgs::PoseStamped ps;
tf2::Quaternion quaternion;
moveit::planning_interface::MoveGroupInterface group("panda_arm");

ps.header.frame_id = "box/hole";
quaternion.setRPY(0, (180.0 / 180.0 * M_PI), 0);
ps.pose.orientation = tf2::toMsg(quaternion);
ps.pose.position.z = 0.01;
group.setEndEffectorLink("cylinder/tip");
group.setPoseTarget(ps);
group.go();

The only alternative I could imagine (to avoid the top-level namespacing) is something like this:
(ignore the slightly incorrect syntax)

moveit::planning_interface::PlanningSceneInterface planning_scene_interface;

//ps.header.frame_id = "box/hole"
quaternion.setRPY(0, (180.0 / 180.0 * M_PI), 0);
ps.pose.orientation = tf2::toMsg(quaternion);
ps.pose.position.z = 0.01;
ps = planning_scene_interface.transformPoseHeaderFromSubframe(ps, object="box", subframe="hole");
// Returns a transformed ps such that ps.header.frame_id = "box"
group.setEndEffectorLink("cylinder/tip");
group.setPoseTarget(ps);
group.go();

But I find this less elegant, and likely much harder to find for new users.

@felixvd, what problem(s) are you referring to? Travis failures?

On 8f31daa, planning to the subframe box/top fails, but on e8c5172 it is fine. When I added the changes from 8f31daa in this branch while adding only comments, I could not reproduce the issue.

For the protocol, @rhaschke and I had a quick Skype call to discuss this PR and some ideas.

• Robert raised the idea of creating a global transform map in the planning scene, rather than keeping two separate entities (robot state and collision::world), which now get even more complex with this PR. I agree that there is some acrobatics involved to make it work (like the validateConstraintFrames service), and that the separation into robot_state and collision::world is strange and likely not very performant. I did not dare to make any more drastic changes, but I think this kind of change makes a lot of sense. We agreed to make that discussion a separate issue and try to get this PR merge-ready first.
• We are both in favor of the API I proposed, but with a slash as a prefix separator instead of an underscore, so the example from my last post would read:

ps.header.frame_id = "box/hole"
group.setEndEffectorLink("cylinder/tip");

• The above actually already works, because the lookup ignores the separator (it just jumps one character to find the subframe name)
• I will condense the commit history of this PR and force push a few prettier commits by next week.

As a detail that is open to comments, I wondered in the OP if the string copy in getFrameInfo here is wasteful, because getFrameTransform does not need the string. Robert proposed returning the pointer to the robot link instead of the name, so the overhead would only be an assignment and not a string copy. However, because the link model is const, a std::pair object would have to be created, and I am not sure how much overhead that is. The resulting function declaration would be:

std::pair<const Eigen::Isometry3d&, const LinkModel*> RobotState::getFrameInfo(const std::string& frame_id, bool& frame_found) const;

However, because the link model is const, a std::pair object would have to be created.

Can't you just "return" (via arguments) a const pointer to the LinkModel, like so:
const Eigen::Isometry3d& getFrameInfo(const std::string &frame_id, LinkModel* const &link, bool &frame_found) const;
Note the order of const in the type specification.
https://stackoverflow.com/questions/11514688/passing-const-pointer-by-reference

Sounds great, just what I was looking for. I'll do that if it works.

As it turns out, with the order const LinkModel* &link everything compiles and works fine, but with the order LinkModel* const &link, the compiler would give this error:

/home/catkin_ws/src/moveit/moveit_core/robot_state/src/robot_state.cpp:1019:44: error: assignment of read-only reference ‘robot_link’
     robot_link = robot_model_->getRootLink();
                                            ^
/home/catkin_ws/src/moveit/moveit_core/robot_state/src/robot_state.cpp:1019:43: error: invalid conversion from ‘const moveit::core::LinkModel*’ to ‘moveit::core::LinkModel*’ [-fpermissive]
     robot_link = robot_model_->getRootLink();

Second, I fixed the confusing issue I had earlier where only std::cout messages changed the program outcome. I had accidentally left the return value undefined after catching an exception in hasSubframeTransform.

Finally, as requested by @rhaschke, I cleaned up the commit history into hopefully readable chunks. I recommend looking at each of those separately instead of all the changed files together. There are more detailed explanations in the commit messages, as well. Each commit probably does not compile on its own, they are just grouped semantically for easier reading. Thanks in advance!

I updated each of the commits as suggested by Bryce, and started changing from std::map to Transforms in the WIP commit, but it looks like it's not that easy. It seems that the ensureUnique function in world.cpp tries to copy the object, but Transforms is set to boost::noncopyable.

Compiler error for the WIP commit:

Errors     << moveit_core:make /home/catkin_ws/logs/moveit_core/build.make.049.log
/home/catkin_ws/src/moveit/moveit_core/collision_detection/src/world.cpp: In member function ‘void collision_detection::World::ensureUnique(collision_detection::World::ObjectPtr&)’:
/home/catkin_ws/src/moveit/moveit_core/collision_detection/src/world.cpp:131:30: error: use of deleted function ‘collision_detection::World::Object::Object(const collision_detection::World::Object&)’
     obj.reset(new Object(*obj));
                              ^
In file included from /home/catkin_ws/src/moveit/moveit_core/collision_detection/src/world.cpp:37:0:
/home/catkin_ws/src/moveit/moveit_core/collision_detection/include/moveit/collision_detection/world.h:82:10: note: ‘collision_detection::World::Object::Object(const collision_detection::World::Object&)’ is implicitly deleted because the default definition would be ill-formed:
   struct Object
          ^~~~~~
/home/catkin_ws/src/moveit/moveit_core/collision_detection/include/moveit/collision_detection/world.h:82:10: error: use of deleted function ‘moveit::core::Transforms::Transforms(const moveit::core::Transforms&)’
In file included from /home/catkin_ws/src/moveit/moveit_core/collision_detection/include/moveit/collision_detection/world.h:41:0,
                 from /home/catkin_ws/src/moveit/moveit_core/collision_detection/src/world.cpp:37:
/home/catkin_ws/src/moveit/moveit_core/transforms/include/moveit/transforms/transforms.h:61:7: note: ‘moveit::core::Transforms::Transforms(const moveit::core::Transforms&)’ is implicitly deleted because the default definition would be ill-formed:
 class Transforms : private boost::noncopyable
       ^~~~~~~~~~
/home/catkin_ws/src/moveit/moveit_core/transforms/include/moveit/transforms/transforms.h:61:7: error: use of deleted function ‘boost::noncopyable_::noncopyable::noncopyable(const boost::noncopyable_::noncopyable&)’
In file included from /usr/include/boost/noncopyable.hpp:15:0,
                 from /home/catkin_ws/src/moveit/moveit_core/transforms/include/moveit/transforms/transforms.h:43,
                 from /home/catkin_ws/src/moveit/moveit_core/collision_detection/include/moveit/collision_detection/world.h:41,
                 from /home/catkin_ws/src/moveit/moveit_core/collision_detection/src/world.cpp:37:
/usr/include/boost/core/noncopyable.hpp:34:7: note: declared here
       noncopyable( const noncopyable& ) = delete;
       ^~~~~~~~~~~

I wouldn't know how to fix this (I assume ensureUnique is there for a reason), but I know that the subframe transforms are only used when changing collision objects in the scene and once in validateConstraintFrames when the planning request is constructed. Since they are not used in collision checking and other high-iteration procedures, there might not be a performance gain to be had here, but I don't know much about memory issues.

Looks good to me just reading the code, but can you explain the change in this commit? This seems to change behavior from the original version, and is also not in line with the behavior of getTransform in the same file.

The original code considered object frames to be known only if there is a unique shape associated with it. You changed this semantic in the first place:
SceneTransforms::knowsObject
PlanningScene::knowsFrameTransform
PlanningScene::getFrameTransform

True. I was thinking of the getTransform logic which just gave a warning. My bad.

I force-pushed your branch to let Travis run, but this line displays an error message when using subframes. I would keep using 'hasLinkModel'/'getLinkModel' because there are never that many robot links and creating the motion planning request is not a frequent operation, so this should not be costly.

Also do you want the fixup commits combined before merging?

this line displays an error message when using subframes.

You are right. I wasn't aware of the verboseness of this method anymore...
Please revert to the hasLink/getLink pair.

@felixvd, can you file your tutorial examples as a PR, please?

@felixvd, I just noticed that I already filed the tutorial PR: moveit/moveit_tutorials#326
Please review there.

So glad this major new feature was merged in!!