Enhance pilz can service request

moveit_planners/pilz_industrial_motion_planner/src/pilz_industrial_motion_planner.cpp

@@ -156,7 +156,40 @@ CommandPlanner::getPlanningContext(const planning_scene::PlanningSceneConstPtr&
 
 bool CommandPlanner::canServiceRequest(const moveit_msgs::msg::MotionPlanRequest& req) const
 {
-  return context_loader_map_.find(req.planner_id) != context_loader_map_.end();
+  if (context_loader_map_.find(req.planner_id) == context_loader_map_.end())
+  {
+    RCLCPP_ERROR(LOGGER, "Cannot service planning request because planner ID '%s' does not exist.",
+                 req.planner_id.c_str());
+    return false;
+  }
+
+  if (req.group_name.empty())
+  {
+    RCLCPP_ERROR(LOGGER, "Cannot service planning request because group name is not specified.");
+    return false;
+  }
+
+  auto joint_mode_group_ptr = model_->getJointModelGroup(req.group_name);
+  if (joint_mode_group_ptr == nullptr)
+  {
+    RCLCPP_ERROR(LOGGER, "Cannot service planning request because group '%s' does not exist.", req.group_name.c_str());
+    return false;
+  }
+
+  if (joint_mode_group_ptr->getSolverInstance() == nullptr)
+  {
+    RCLCPP_ERROR(LOGGER, "Cannot service planning request because group '%s' does have an IK solver instance.",
+                 req.group_name.c_str());
+    return false;
+  }
+
+  if (!req.trajectory_constraints.constraints.empty())
+  {
+    RCLCPP_ERROR(LOGGER, "Cannot service planning request because PILZ does not support 'trajectory constraints'.");
+    return false;
+  }
+
+  return true;
 }
 
 void CommandPlanner::registerContextLoader(

moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_pilz_industrial_motion_planner.cpp

@@ -155,11 +155,43 @@ TEST_F(CommandPlannerTest, CheckValidAlgorithmsForServiceRequest)
   {
     planning_interface::MotionPlanRequest req;
     req.planner_id = alg;
+    req.group_name = "manipulator";
 
     EXPECT_TRUE(planner_instance_->canServiceRequest(req));
   }
 }
 
+TEST_F(CommandPlannerTest, CheckEmptyGroupNameForServiceRequest)
+{
+  // Check for the algorithms
+  std::vector<std::string> algs;
+  planner_instance_->getPlanningAlgorithms(algs);
+
+  for (const auto& alg : algs)
+  {
+    planning_interface::MotionPlanRequest req;
+    req.planner_id = alg;
+
+    EXPECT_FALSE(planner_instance_->canServiceRequest(req));
+  }
+}
+
+TEST_F(CommandPlannerTest, CheckInvalidGroupNameForServiceRequest)
+{
+  // Check for the algorithms
+  std::vector<std::string> algs;
+  planner_instance_->getPlanningAlgorithms(algs);
+
+  for (const auto& alg : algs)
+  {
+    planning_interface::MotionPlanRequest req;
+    req.planner_id = alg;
+    req.group_name = "1234manipulator";
+
+    EXPECT_FALSE(planner_instance_->canServiceRequest(req));
+  }
+}
+
 /**
  * @brief Check that canServiceRequest(req) returns false if planner_id is not
  * supported
@@ -168,6 +200,7 @@ TEST_F(CommandPlannerTest, CheckInvalidAlgorithmsForServiceRequest)
 {
   planning_interface::MotionPlanRequest req;
   req.planner_id = "NON_EXISTEND_ALGORITHM_HASH_da39a3ee5e6b4b0d3255bfef95601890afd80709";
+  req.group_name = "manipulator";
 
   EXPECT_FALSE(planner_instance_->canServiceRequest(req));
 }
@@ -179,6 +212,7 @@ TEST_F(CommandPlannerTest, CheckEmptyPlannerIdForServiceRequest)
 {
   planning_interface::MotionPlanRequest req;
   req.planner_id = "";
+  req.group_name = "manipulator";
 
   EXPECT_FALSE(planner_instance_->canServiceRequest(req));
 }
@@ -224,6 +258,46 @@ TEST_F(CommandPlannerTest, CheckPlanningContextDescriptionNotEmptyAndStable)
   EXPECT_GT(desc.length(), 0u);
 }
 
+/**
+ * @brief Check that getPlanningContext() fails if the underlying ContextLoader
+ * fails to load the context.
+ */
+TEST_F(CommandPlannerTest, FailOnLoadContext)
+{
+  pilz_industrial_motion_planner::CommandPlanner planner;
+  planner.initialize(robot_model_, node_, "");
+
+  // Mock of failing PlanningContextLoader
+  class TestPlanningContextLoader : public pilz_industrial_motion_planner::PlanningContextLoader
+  {
+  public:
+    std::string getAlgorithm() const override
+    {
+      return "Test_Algorithm";
+    }
+
+    bool loadContext(planning_interface::PlanningContextPtr& /* planning_context */, const std::string& /* name */,
+                     const std::string& /* group */) const override
+    {
+      // Mock behaviour: Cannot load planning context.
+      return false;
+    }
+  };
+
+  /// Registered a found loader
+  pilz_industrial_motion_planner::PlanningContextLoaderPtr planning_context_loader =
+      std::make_shared<TestPlanningContextLoader>();
+  planner.registerContextLoader(planning_context_loader);
+
+  moveit_msgs::msg::MotionPlanRequest req;
+  req.planner_id = "Test_Algorithm";
+  req.group_name = "manipulator";
+
+  moveit_msgs::msg::MoveItErrorCodes error_code;
+  EXPECT_FALSE(planner.getPlanningContext(nullptr, req, error_code));
+  EXPECT_EQ(moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED, error_code.val);
+}
+
 int main(int argc, char** argv)
 {
   rclcpp::init(argc, argv);

moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_pilz_industrial_motion_planner_direct.cpp

@@ -77,44 +77,6 @@ TEST(CommandPlannerTestDirect, CheckDoubleLoadingException)
                pilz_industrial_motion_planner::ContextLoaderRegistrationException);
 }
 
-/**
- * @brief Check that getPlanningContext() fails if the underlying ContextLoader
- * fails to load the context.
- */
-TEST(CommandPlannerTestDirect, FailOnLoadContext)
-{
-  pilz_industrial_motion_planner::CommandPlanner planner;
-
-  // Mock of failing PlanningContextLoader
-  class TestPlanningContextLoader : public pilz_industrial_motion_planner::PlanningContextLoader
-  {
-  public:
-    std::string getAlgorithm() const override
-    {
-      return "Test_Algorithm";
-    }
-
-    bool loadContext(planning_interface::PlanningContextPtr& /* planning_context */, const std::string& /* name */,
-                     const std::string& /* group */) const override
-    {
-      // Mock behaviour: Cannot load planning context.
-      return false;
-    }
-  };
-
-  /// Registered a found loader
-  pilz_industrial_motion_planner::PlanningContextLoaderPtr planning_context_loader =
-      std::make_shared<TestPlanningContextLoader>();
-  planner.registerContextLoader(planning_context_loader);
-
-  moveit_msgs::msg::MotionPlanRequest req;
-  req.planner_id = "Test_Algorithm";
-
-  moveit_msgs::msg::MoveItErrorCodes error_code;
-  EXPECT_FALSE(planner.getPlanningContext(nullptr, req, error_code));
-  EXPECT_EQ(moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED, error_code.val);
-}
-
 int main(int argc, char** argv)
 {
   testing::InitGoogleTest(&argc, argv);