Add test for PlanningContextManager in ompl interface #2248

JeroenDM · 2020-08-11T14:15:23Z

Description

(This pull request could use some extra work, but early feedback would be really valuable.)

This test goes through the main steps in solving a planning problem with the interface;

Create a PlanningContextManager.
Call it's getPlanningContext with a specific request to get a ModelBasedPlanningContext.
Call the solve method on the planning context.

The main intent is to check if the ModelBasedPlanningContext has the correct type of ModelBasedStateSpace assigned for planning. (For a detailed explanation of this process, see here.)
Calling the solve method is just the cherry on top of the cake.

The load_test_robot_class.h is exactly the same as in #2247, so I assume that change will not be part of this pull request once rebased.

TODO / issues

Loading a (dummy) IK solver for a planning group
The selection of the state space depends on the availability of an IK solver. I found an example of how to load a solver in a test class in test_constrained_sampler.cpp, which basically adds a complete implementation of an IK solver (KinematicsBase class) to the test directory.

rostest dependency
Ideally, this test could be completely ROS independent if I had a way to create a "dummy" ros::NodeHandle without actually starting a roscore. The only method that needs this ROS handle is loadConstraintApproximations, which I'm not testing here.

Why are these useful?

I'm currently using this test in the context of #2092, where they are extremely helpful.

Checklist

Required by CI: Code is auto formatted using clang-format
Extend the tutorials / documentation reference
Document API changes relevant to the user in the MIGRATION.md notes
Create tests, which fail without this PR reference
Include a screenshot if changing a GUI
While waiting for someone to review your request, please help review another open pull request to support the maintainers

JeroenDM · 2020-08-11T14:16:12Z

@felixvd @ommmid @mamoll Maybe something to discuss in the meeting?

JeroenDM · 2020-08-11T14:55:07Z

Travis fails because I did not add rostest and eigen_conversions to the test dependencies in the package.xml yet.

mamoll

Looks good to me. Just make sure copyright is assigned to KU Leuven, not Willow Garage.

moveit_planners/ompl/ompl_interface/test/load_test_robot.h

JeroenDM · 2020-09-08T18:06:25Z

I will fix the package.xml file tomorrow and remove the WIP tag.

codecov · 2020-09-10T13:48:50Z

Codecov Report

Merging #2248 into master will decrease coverage by 1.52%.
The diff coverage is n/a.

@@            Coverage Diff             @@
##           master    #2248      +/-   ##
==========================================
- Coverage   57.94%   56.42%   -1.51%     
==========================================
  Files         327      287      -40     
  Lines       25645    24451    -1194     
==========================================
- Hits        14858    13795    -1063     
+ Misses      10787    10656     -131

Impacted Files	Coverage Δ
...include/moveit/robot_model/prismatic_joint_model.h	`0.00% <0.00%> (-50.00%)`	⬇️
...sh_filter/include/moveit/mesh_filter/mesh_filter.h	`66.67% <0.00%> (-33.33%)`	⬇️
...clude/moveit/occupancy_map_monitor/occupancy_map.h	`16.67% <0.00%> (-20.83%)`	⬇️
..._capabilities/clear_octomap_service_capability.cpp	`30.00% <0.00%> (-20.00%)`	⬇️
...de/moveit/constraint_samplers/constraint_sampler.h	`20.00% <0.00%> (-17.50%)`	⬇️
..._core/collision_detection/src/collision_common.cpp	`33.34% <0.00%> (-16.66%)`	⬇️
...e_controller_manager/src/moveit_fake_controllers.h	`33.34% <0.00%> (-16.66%)`	⬇️
moveit_core/exceptions/src/exceptions.cpp	`0.00% <0.00%> (-16.66%)`	⬇️
...ccupancy_map_monitor/src/occupancy_map_updater.cpp	`58.83% <0.00%> (-13.90%)`	⬇️
...tection/include/moveit/collision_detection/world.h	`75.00% <0.00%> (-13.88%)`	⬇️
... and 289 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 8771af6...da55c87. Read the comment docs.

JeroenDM · 2020-09-10T14:07:55Z

Woohoo, 0.06% coverage increase!

I don't think moveit_planners_ompl is low hanging fruit in terms of increasing code coverage. Most of it is tested by moveit_ros_planning_interface tests. But the tests added here are still really useful when working on improving the ompl interface and as documentation on how the internals work.

mamoll

Looks good to me!

rhaschke

I'm not sure the tests are very specific. Other than some nitpicks, I'm fine with it.

moveit_planners/ompl/ompl_interface/test/load_test_robot.h

moveit_planners/ompl/ompl_interface/test/test_planning_context_manager.cpp

rhaschke · 2020-10-15T16:52:27Z

moveit_planners/ompl/ompl_interface/test/test_planning_context_manager.cpp

+    // TODO(jeroendm) As the joint_model_group_ has not IK solver initialized, we still get a joint model state space
+    // here, it would be nice if we could load an IK solver for the robot to test the PoseModelStateSpace.
+    auto ss = dynamic_cast<ompl_interface::JointModelStateSpace*>(pc->getOMPLStateSpace().get());
+    EXPECT_NE(ss, nullptr);


If I'm not mistaken, you validate that you don't get a JointModelStateSpace here. This contradicts the comment.

I updated the comment to:

// As the joint_model_group_ has no IK solver initialized, we expect a joint model state space

I tried this summer, but I did not manage to figure out how to manually load an IK solver for the joint model group.

rhaschke · 2020-10-15T16:54:41Z

moveit_planners/ompl/ompl_interface/test/test_planning_context_manager.cpp

+    EXPECT_TRUE(success);
+  }
+
+  void testPathConstraints(const std::vector<double>& start, const std::vector<double>& goal)


The test here doesn't differ from testSimpleRequest() except that path constraints are created. Particularly, the test doesn't verify that the constraints are correctly created and used.

I added tests to see if the path constraints are added to the planning context and properly used by the planner. (The specific start and goal state used maybe a bit too trivial, but I'll leave this as future work if that's ok. Otherwise, I fear I might never finish this PR and continue working on the constrained planning PR.)

// Are the path constraints created in the planning context? auto path_constraints = pc->getPathConstraints(); EXPECT_FALSE(path_constraints->empty()); EXPECT_EQ(path_constraints->getOrientationConstraints().size(), 1); EXPECT_TRUE(path_constraints->getPositionConstraints().empty()); EXPECT_TRUE(path_constraints->getJointConstraints().empty()); EXPECT_TRUE(path_constraints->getVisibilityConstraints().empty()); // Check if all the states in the solution satisfy the path constraints. // A detailed response returns 3 solutions: the ompl solution, the simplified solution and the interpolated // solution. We test all of them here. for (const robot_trajectory::RobotTrajectoryPtr& trajectory : res.trajectory_) { for (std::size_t pt_index = { 0 }; pt_index < trajectory->getWayPointCount(); ++pt_index) { EXPECT_TRUE(path_constraints->decide(trajectory->getWayPoint(pt_index)).satisfied); } }

rhaschke · 2020-10-15T17:05:08Z

moveit_planners/ompl/ompl_interface/test/test_planning_context_manager.test

+<launch>
+    <test pkg="moveit_planners_ompl" type="test_planning_context_manager" test-name="test_planning_context_manager" />
+</launch>


There is no ROS environment loaded here. Do you need a rostest with a launch file after all?
Can the test run as a plain gtest as well?

I need a node handle to call getPlanningRequest, but the node handle is never used. As mentioned in the pr description:

Ideally, this test could be completely ROS independent if I had a way to create a "dummy" ros::NodeHandle without actually starting a roscore. The only method that needs this ROS handle is loadConstraintApproximations, which I'm not testing here.

I wish I could come up with a better solution. It is unfortunate that a ROS dependency got introduced in a non-ROS specific part of MoveIt. Technically, the code for the constraint approximations could be refactored to avoid this ros node handle dependency, but that would introduce breaking API changes for users, I think.

JeroenDM · 2020-10-15T19:56:00Z

@rhaschke thank you for the feedback!

I'm not sure the tests are very specific.

I think they become really useful if the new constrained-state-space gets merged, where an extra test is added to test the configurations options that the user can specify.
I wrote these tests originally more as documentation for future me when working on the new state space.

Co-authored-by: Robert Haschke <rhaschke@users.noreply.github.com>

JeroenDM · 2020-10-18T13:43:25Z

Note that I'll have to rebase this if #2247 gets merged first, because of the load_robot_model.h that is used by both tests.

JeroenDM requested a review from mamoll as a code owner August 11, 2020 14:15

JeroenDM mentioned this pull request Aug 21, 2020

OMPL Constrained State Space Planning #2273

Open

6 tasks

JeroenDM mentioned this pull request Sep 8, 2020

Add test to ompl interface for StateValidityChecker #2247

Merged

6 tasks

mamoll requested changes Sep 8, 2020

View reviewed changes

moveit_planners/ompl/ompl_interface/test/load_test_robot.h Outdated Show resolved Hide resolved

JeroenDM added 3 commits September 9, 2020 16:42

add test for PlanningContextManager in ompl interface

8d53aba

Fix CMakeLists for planning context manager test in ompl interface

92fd2b1

change license to KU Leuven

51ca003

JeroenDM force-pushed the add-planning-context-test branch from d6762af to 51ca003 Compare September 9, 2020 14:42

JeroenDM added 2 commits September 10, 2020 15:02

fix issues in CMakeLists of ompl interface

6ca6b3b

add new test dependencies to ompl interface

9a95793

JeroenDM changed the title ~~WIP: add test for PlanningContextManager in ompl interface~~ Add test for PlanningContextManager in ompl interface Sep 10, 2020

mamoll approved these changes Oct 14, 2020

View reviewed changes

mamoll requested a review from rhaschke October 14, 2020 15:30

rhaschke reviewed Oct 15, 2020

View reviewed changes

JeroenDM and others added 5 commits October 15, 2020 21:57

Apply suggestions from code review (spelling errors).

42cbab3

Co-authored-by: Robert Haschke <rhaschke@users.noreply.github.com>

fix spelling error + improve assert statements in test

235ee4d

check that the path constraints are created and used by the planner

d02b686

add test for position constraints

1624428

update comment in test

333c51f

JeroenDM requested a review from rhaschke October 18, 2020 13:43

Fix comparison between signed and unsigned integer

da55c87

rhaschke approved these changes Oct 18, 2020

View reviewed changes

rhaschke merged commit 144adb7 into moveit:master Oct 18, 2020

tylerjw mentioned this pull request Oct 23, 2020

Melodic backports #2387

Closed

57 tasks

JeroenDM deleted the add-planning-context-test branch January 9, 2021 09:07

tylerjw mentioned this pull request Apr 9, 2021

Noetic Release 1.1.2 #2593

Closed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Add test for PlanningContextManager in ompl interface #2248

Add test for PlanningContextManager in ompl interface #2248

JeroenDM commented Aug 11, 2020

JeroenDM commented Aug 11, 2020

JeroenDM commented Aug 11, 2020

mamoll left a comment

JeroenDM commented Sep 8, 2020

codecov bot commented Sep 10, 2020 •

edited

JeroenDM commented Sep 10, 2020

mamoll left a comment

rhaschke left a comment

rhaschke Oct 15, 2020

JeroenDM Oct 18, 2020

rhaschke Oct 15, 2020

JeroenDM Oct 18, 2020 •

edited

rhaschke Oct 15, 2020

JeroenDM Oct 15, 2020

JeroenDM commented Oct 15, 2020

JeroenDM commented Oct 18, 2020

Add test for PlanningContextManager in ompl interface #2248

Add test for PlanningContextManager in ompl interface #2248

Conversation

JeroenDM commented Aug 11, 2020

Description

TODO / issues

Why are these useful?

Checklist

JeroenDM commented Aug 11, 2020

JeroenDM commented Aug 11, 2020

mamoll left a comment

Choose a reason for hiding this comment

JeroenDM commented Sep 8, 2020

codecov bot commented Sep 10, 2020 • edited

Codecov Report

JeroenDM commented Sep 10, 2020

mamoll left a comment

Choose a reason for hiding this comment

rhaschke left a comment

Choose a reason for hiding this comment

rhaschke Oct 15, 2020

Choose a reason for hiding this comment

JeroenDM Oct 18, 2020

Choose a reason for hiding this comment

rhaschke Oct 15, 2020

Choose a reason for hiding this comment

JeroenDM Oct 18, 2020 • edited

Choose a reason for hiding this comment

rhaschke Oct 15, 2020

Choose a reason for hiding this comment

JeroenDM Oct 15, 2020

Choose a reason for hiding this comment

JeroenDM commented Oct 15, 2020

JeroenDM commented Oct 18, 2020

codecov bot commented Sep 10, 2020 •

edited

JeroenDM Oct 18, 2020 •

edited