From 61afd978498291e5113115766d2505c1172bf651 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 19 Jul 2024 13:40:04 +0200 Subject: [PATCH] Simpler fix to Pilz/LIN segfault --- .../src/trajectory_generator_lin.cpp | 9 ++------- 1 file changed, 2 insertions(+), 7 deletions(-) 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 527ec5029e..4054d669a2 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 @@ -33,6 +33,7 @@ *********************************************************************/ #include "pilz_industrial_motion_planner/trajectory_generator_lin.h" +#include #include #include @@ -74,13 +75,7 @@ void TrajectoryGeneratorLIN::extractMotionPlanInfo(const planning_scene::Plannin // goal given in joint space if (!req.goal_constraints.front().joint_constraints.empty()) { - auto solver = robot_model_->getJointModelGroup(req.group_name)->getSolverInstance(); - if (!solver) - { - ROS_ERROR_STREAM("No IK solver defined for group '" << req.group_name << "'"); - return; - } - info.link_name = solver->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())