Never return nullptr silently

moveit_core/collision_detection_bullet/src/bullet_integration/contact_checker_common.cpp

@@ -116,7 +116,7 @@ collision_detection::Contact* processResult(ContactTestData& cdata, collision_de
 
     return &(dr.back());
   }
-
+  RCLCPP_ERROR(getLogger(), "Result cannot be processed. Returning nullptr, this should never happen!");
   return nullptr;
 }
 

moveit_core/robot_model/src/robot_model.cpp

@@ -1357,21 +1357,21 @@ LinkModel* RobotModel::getLinkModel(const std::string& name, bool* has_link)
   {
     *has_link = false;  // Report failure via argument
   }
-  else
-  {  // Otherwise print error
-    RCLCPP_ERROR(getLogger(), "Link '%s' not found in model '%s'", name.c_str(), model_name_.c_str());
-  }
+  RCLCPP_ERROR(getLogger(), "Link '%s' not found in model '%s'", name.c_str(), model_name_.c_str());
   return nullptr;
 }
 
 const LinkModel* RobotModel::getRigidlyConnectedParentLinkModel(const LinkModel* link)
 {
   if (!link)
+  {
+    RCLCPP_ERROR(get_logger(), "Cannot determine rigidly connected parent link because input link is nullptr");
     return link;
+  }
   const moveit::core::LinkModel* parent_link = link->getParentLinkModel();
   const moveit::core::JointModel* joint = link->getParentJointModel();
 
-  while (parent_link && joint->getType() == moveit::core::JointModel::FIXED)
+  while (parent_link && joint && joint->getType() == moveit::core::JointModel::FIXED)
   {
     link = parent_link;
     joint = link->getParentJointModel();

moveit_core/robot_state/src/robot_state.cpp

@@ -519,6 +519,8 @@ const double* RobotState::getJointPositions(const JointModel* joint) const
 {
   if (joint->getVariableCount() == 0)
   {
+    RCLCPP_ERROR(getLogger(), "Cannot get joint positions for joint '%s' because it has no variables",
+                 joint->getName().c_str());
     return nullptr;
   }
   return &position_.at(joint->getFirstVariableIndex());
@@ -528,6 +530,8 @@ const double* RobotState::getJointVelocities(const JointModel* joint) const
 {
   if (joint->getVariableCount() == 0)
   {
+    RCLCPP_ERROR(getLogger(), "Cannot get joint velocities for joint '%s' because it has no variables",
+                 joint->getName().c_str());
     return nullptr;
   }
   return &velocity_.at(joint->getFirstVariableIndex());
@@ -537,6 +541,8 @@ const double* RobotState::getJointAccelerations(const JointModel* joint) const
 {
   if (joint->getVariableCount() == 0)
   {
+    RCLCPP_ERROR(getLogger(), "Cannot get joint accelerations for joint '%s' because it has no variables",
+                 joint->getName().c_str());
     return nullptr;
   }
   return &effort_or_acceleration_.at(joint->getFirstVariableIndex());
@@ -546,6 +552,8 @@ const double* RobotState::getJointEffort(const JointModel* joint) const
 {
   if (joint->getVariableCount() == 0)
   {
+    RCLCPP_ERROR(getLogger(), "Cannot get joint effort for joint '%s' because it has no variables",
+                 joint->getName().c_str());
     return nullptr;
   }
   return &effort_or_acceleration_.at(joint->getFirstVariableIndex());
@@ -911,24 +919,43 @@ const LinkModel* RobotState::getRigidlyConnectedParentLinkModel(const std::strin
 {
   const LinkModel* link{ nullptr };
 
+  // Resolve the rigidly connected parent link for the given frame
   size_t idx = 0;
   if ((idx = frame.find('/')) != std::string::npos)
-  {  // resolve sub frame
+  {  // Check if the frame has a subframe
     std::string object{ frame.substr(0, idx) };
     if (!hasAttachedBody(object))
+    {
+      // If the attached body is not found, log an error and return nullptr
+      RCLCPP_ERROR(getLogger(),
+                   "Cannot find rigidly connected parent link for frame '%s' because there is no attached body.",
+                   frame.c_str());
       return nullptr;
+    }
     auto body{ getAttachedBody(object) };
     if (!body->hasSubframeTransform(frame))
+    {
+      // If the subframe transform is not found, log an error and return nullptr
+      RCLCPP_ERROR(getLogger(),
+                   "Cannot find rigidly connected parent link for frame '%s' because the transformation to the parent "
+                   "link is unknown.",
+                   frame.c_str());
       return nullptr;
+    }
     link = body->getAttachedLink();
   }
   else if (hasAttachedBody(frame))
   {
+    // If the frame is an attached body, get the attached link
     link = getAttachedBody(frame)->getAttachedLink();
   }
   else if (getRobotModel()->hasLinkModel(frame))
+  {
+    // If the frame is a link in the robot model, get the link model
     link = getLinkModel(frame);
+  }
 
+  // Return the rigidly connected parent link model for the given frame
   return getRobotModel()->getRigidlyConnectedParentLinkModel(link);
 }
 

moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp

@@ -268,6 +268,7 @@ std::unique_ptr<KDL::Path> TrajectoryGeneratorCIRC::setPathCIRC(const MotionPlan
     throw CenterPointDifferentRadius(os.str());
   }
 
+  RCLCPP_ERROR(getLogger(), "Result cannot set path CIRC. Returning nullptr, this should never happen!");
   return nullptr;
 }