Fix test - WIP

moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp

@@ -358,7 +358,7 @@ PlanningContextManager::getPlanningContext(const planning_interface::PlannerConf
 
     if (factory->getType() == ConstrainedPlanningStateSpace::PARAMETERIZATION_TYPE)
     {
-      RCLCPP_DEBUG_STREAM(LOGGER, "planning_context_manager: Using OMPL's constrained state space for planning.");
+      RCLCPP_ERROR_STREAM(LOGGER, "planning_context_manager: Using OMPL's constrained state space for planning.");
 
       // Select the correct type of constraints based on the path constraints in the planning request.
       ompl::base::ConstraintPtr ompl_constraint =
@@ -381,7 +381,7 @@ PlanningContextManager::getPlanningContext(const planning_interface::PlannerConf
       context_spec.ompl_simple_setup_ = std::make_shared<ompl::geometric::SimpleSetup>(context_spec.state_space_);
     }
 
-    RCLCPP_DEBUG(LOGGER, "Creating new planning context");
+    RCLCPP_ERROR(LOGGER, "Creating new planning context");
     context = std::make_shared<ModelBasedPlanningContext>(config.name, context_spec);
 
     // Do not cache a constrained planning context, as the constraints could be changed
@@ -453,7 +453,8 @@ PlanningContextManager::getStateSpaceFactory(const std::string& group,
   }
   else
   {
-    RCLCPP_DEBUG(LOGGER, "Using '%s' parameterization for solving problem", best->first.c_str());
+    RCLCPP_ERROR(LOGGER, "Using '%s' parameterization for solving problem, priority %i", best->first.c_str(),
+                 prev_priority);
     return best->second;
   }
 }
@@ -544,6 +545,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))
@@ -578,7 +580,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,16 @@ 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] + ")" } };
 
     planning_interface::PlannerConfigurationMap pconfig_map{ { pconfig_settings.name, pconfig_settings } };
     moveit_msgs::msg::MoveItErrorCodes error_code;
@@ -145,8 +152,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 +188,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;