Add MoveTo tests & make them pass#304
Conversation
a381e38 to
458626a
Compare
Codecov Report
@@ Coverage Diff @@
## master #304 +/- ##
==========================================
+ Coverage 51.85% 52.97% +1.13%
==========================================
Files 101 102 +1
Lines 7561 7595 +34
==========================================
+ Hits 3920 4023 +103
+ Misses 3641 3572 -69
Continue to review full report at Codecov.
|
|
Sorry for the broken tests before. This is ready for merge from my perspective. Maintaining support for the melodic branch becomes more annoying over time and I had to create the first compatibility function to keep most of the functionality. @rhaschke I'm unsure which approach to melodic I want to support here these days. Do you have a preference?
|
458626a to
adaed81
Compare
|
I can confirm the support of subframes as IK frame is working as in my previous PR. Is it somehow related to moveit/moveit#2250 (and #191 )? |
rhaschke
left a comment
There was a problem hiding this comment.
Looks good. Sorry for the long delay in reviewing this. I focussed on the MoveIt release first.
Note the minor fixup commits. They should be squashed into their base commit before merging (I guess you want to keep history).
|
Sorry, I didn't test on Melodic... Please, just drop my latest commit (8408fba). |
|
Hm. moveit/moveit_resources#92 broke the pick+place test. Should be fixed now. |
core/src/stages/move_to.cpp
Outdated
| if (value.empty()) { // property undefined | ||
| // determine IK link from group | ||
| if (!(link = jmg->getOnlyOneEndEffectorTip())) { | ||
| if (!get_tip(ik_pose_msg.header.frame_id)) { | ||
| solution.markAsFailure("missing ik_frame"); | ||
| return false; | ||
| } | ||
| ik_pose_msg.header.frame_id = link->getName(); | ||
| ik_pose_msg.pose.orientation.w = 1.0; | ||
| } else { | ||
| ik_pose_msg = boost::any_cast<geometry_msgs::PoseStamped>(value); | ||
| if (!(link = robot_model->getLinkModel(ik_pose_msg.header.frame_id))) { | ||
| solution.markAsFailure("unknown link for ik_frame: " + ik_pose_msg.header.frame_id); | ||
| if (ik_pose_msg.header.frame_id.empty() && !get_tip(ik_pose_msg.header.frame_id)) { | ||
| solution.markAsFailure("frame_id of ik_frame is empty and no unique group tip was found"); | ||
| return false; | ||
| } else if (!scene->knowsFrameTransform(ik_pose_msg.header.frame_id)) { | ||
| std::stringstream ss; | ||
| ss << "ik_frame specified in unknown frame '" << ik_pose_msg << "'"; | ||
| solution.markAsFailure(ss.str()); | ||
| return false; | ||
| } | ||
| } |
There was a problem hiding this comment.
This block of code is required 1:1 for MoveRelative as well:
https://github.com/ros-planning/moveit_task_constructor/blob/7ec874572c3b35f1cdd7a71c5e6b94c749b3093b/core/src/stages/move_relative.cpp#L199-L213
We should factor that out into a function...
There was a problem hiding this comment.
please have another look. I'm happy with the code again (assuming CI is) and I addressed this too.
never unload the plugin loader before the plugins (IK plugins here). We don't have unrelated loaders in gtest executables, so the static should be fine.
(partially disabled because broken)
I just didn't know the syntax was allowed
previous oversight
leaves us a place to put free helper functions
90e0dab to
f1fc447
Compare
fallbacks and verification.
rhaschke
left a comment
There was a problem hiding this comment.
Thanks for the cleanup. LGTM. Note, that I didn't test myself - I assume you did that 😉
core/test/test_move_to.cpp
Outdated
|
|
||
| TEST_F(PandaMoveTo, stateTarget) { | ||
| move_to->setGoal([]() { | ||
| move_to->setGoal([] { |
There was a problem hiding this comment.
Interesting. Haven't seen this syntax before 😉
There was a problem hiding this comment.
Me neither, but it's nice. The downside is that for some reason [] -> Type {} is not supported syntax though and requires []() -> Type {} (which reads so much... better?)
JointInterpolationSolverCartesian goals