Fix constrained planning tests

moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp

@@ -543,6 +543,7 @@ ModelBasedPlanningContextPtr PlanningContextManager::getPlanningContext(
        (req.path_constraints.orientation_constraints.size() == 1)))
   {
     factory = getStateSpaceFactory(ConstrainedPlanningStateSpace::PARAMETERIZATION_TYPE);
+    RCLCPP_ERROR_STREAM(LOGGER, "OMPL Using constrainedPlanningStateSpace as it was user-enforced");
   }
   else if (joint_space_planning_iterator != pc->second.config.end() &&
            boost::lexical_cast<bool>(joint_space_planning_iterator->second))
@@ -577,7 +578,6 @@ ModelBasedPlanningContextPtr PlanningContextManager::getPlanningContext(
     {
       return ModelBasedPlanningContextPtr();
     }
-
     try
     {
       context->configure(node, use_constraints_approximation);

moveit_planners/ompl/ompl_interface/test/test_planning_context_manager.cpp

@@ -63,6 +63,7 @@
 #include <moveit/kinematic_constraints/utils.h>
 #include <moveit/constraint_samplers/constraint_sampler_manager.h>
 #include <moveit/ompl_interface/parameterization/joint_space/joint_model_state_space.h>
+#include <moveit/ompl_interface/parameterization/joint_space/constrained_planning_state_space.h>
 
 // static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ompl_planning.test.test_planning_context_manager");
 
@@ -115,10 +116,17 @@ class TestPlanningContext : public ompl_interface_testing::LoadTestRobot, public
     SCOPED_TRACE("testPathConstraints");
 
     // create all the test specific input necessary to make the getPlanningContext call possible
+    auto joint_names = joint_model_group_->getJointModelNames();
+    for (auto joint : joint_names)
+    {
+      std::cout << joint << std::endl;
+    }
     planning_interface::PlannerConfigurationSettings pconfig_settings;
     pconfig_settings.group = group_name_;
     pconfig_settings.name = group_name_;
-    pconfig_settings.config = { { "enforce_joint_model_state_space", "0" } };
+    pconfig_settings.config = { { "enforce_joint_model_state_space", "0" },
+                                { "projection_evaluator", "joints(" + joint_names[0] + "," + joint_names[1] + ")" },
+                                { "type", "geometric::RRTConnect" } };
 
     planning_interface::PlannerConfigurationMap pconfig_map{ { pconfig_settings.name, pconfig_settings } };
     moveit_msgs::msg::MoveItErrorCodes error_code;
@@ -145,8 +153,8 @@ class TestPlanningContext : public ompl_interface_testing::LoadTestRobot, public
 
     EXPECT_NE(pc->getOMPLSimpleSetup(), nullptr);
 
-    // As the joint_model_group_ has no IK solver initialized, we expect a joint model state space
-    EXPECT_NE(dynamic_cast<ompl_interface::JointModelStateSpace*>(pc->getOMPLStateSpace().get()), nullptr);
+    // As the joint_model_group_ has exactly one constraint, we expect a constrained planning state space
+    EXPECT_NE(dynamic_cast<ompl_interface::ConstrainedPlanningStateSpace*>(pc->getOMPLStateSpace().get()), nullptr);
 
     planning_interface::MotionPlanDetailedResponse response;
     ASSERT_TRUE(pc->solve(response));
@@ -181,8 +189,8 @@ class TestPlanningContext : public ompl_interface_testing::LoadTestRobot, public
 
     EXPECT_NE(pc->getOMPLSimpleSetup(), nullptr);
 
-    // As the joint_model_group_ has no IK solver initialized, we expect a joint model state space
-    EXPECT_NE(dynamic_cast<ompl_interface::JointModelStateSpace*>(pc->getOMPLStateSpace().get()), nullptr);
+    // As the joint_model_group_ has exactly one constraint, we expect a constrained planning state space
+    EXPECT_NE(dynamic_cast<ompl_interface::ConstrainedPlanningStateSpace*>(pc->getOMPLStateSpace().get()), nullptr);
 
     // Create a new response, because the solve method does not clear the given respone
     planning_interface::MotionPlanDetailedResponse response2;