From d583c0403b4a6c5c93b3ddceb17b5899854ed5d5 Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@users.noreply.github.com>
Date: Fri, 2 Aug 2024 16:47:13 +0200
Subject: [PATCH] PILZ: Throw if IK solver doesn't exist (#2082) (#2921)

---
 .../src/trajectory_generator_lin.cpp                          | 4 +++-
 .../test/unit_tests/src/unittest_trajectory_functions.cpp     | 4 ++++
 2 files changed, 7 insertions(+), 1 deletion(-)

diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp
index a5282a05eab..0a7cb7340c0 100644
--- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp
+++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp
@@ -34,6 +34,8 @@
 
 #include <pilz_industrial_motion_planner/trajectory_generator_lin.h>
 
+#include <pilz_industrial_motion_planner/tip_frame_getter.h>
+
 #include <cassert>
 #include <sstream>
 #include <time.h>
@@ -75,7 +77,7 @@ void TrajectoryGeneratorLIN::extractMotionPlanInfo(const planning_scene::Plannin
   // goal given in joint space
   if (!req.goal_constraints.front().joint_constraints.empty())
   {
-    info.link_name = robot_model_->getJointModelGroup(req.group_name)->getSolverInstance()->getTipFrame();
+    info.link_name = getSolverTipFrame(robot_model_->getJointModelGroup(req.group_name));
 
     if (req.goal_constraints.front().joint_constraints.size() !=
         robot_model_->getJointModelGroup(req.group_name)->getActiveJointModelNames().size())
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_functions.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_functions.cpp
index 3ad29859c1c..b514e0afdef 100644
--- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_functions.cpp
+++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_functions.cpp
@@ -220,6 +220,10 @@ TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testIKSolver)
   const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(planning_group_);
   const kinematics::KinematicsBaseConstPtr& solver = jmg->getSolverInstance();
 
+  if (!solver)
+  {
+    throw("No IK solver configured for group '" + planning_group_ + "'");
+  }
   // robot state
   moveit::core::RobotState rstate(robot_model_);