diff --git a/moveit_planners/pilz_industrial_motion_planner/src/pilz_industrial_motion_planner.cpp b/moveit_planners/pilz_industrial_motion_planner/src/pilz_industrial_motion_planner.cpp index 56476ddb01..9ddfc1baf9 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/pilz_industrial_motion_planner.cpp +++ b/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( diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_pilz_industrial_motion_planner.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_pilz_industrial_motion_planner.cpp index e1ea4fbedc..209b77aa1b 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_pilz_industrial_motion_planner.cpp +++ b/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 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 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(); + 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); diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_pilz_industrial_motion_planner_direct.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_pilz_industrial_motion_planner_direct.cpp index 16d8d3c6d7..cb22d36d50 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_pilz_industrial_motion_planner_direct.cpp +++ b/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(); - 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);