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/include/moveit/robot_model/robot_model.h

@@ -246,13 +246,17 @@ class RobotModel
   bool hasLinkModel(const std::string& name) const;
 
   /** \brief Get a link by its name. Output error and return nullptr when the link is missing. */
-  const LinkModel* getLinkModel(const std::string& link, bool* has_link = nullptr) const;
+  const LinkModel* getLinkModel(const std::string& link) const;
+  [[deprecated("Use getLinkModel(const std::string& name) instead.")]] const LinkModel*
+  getLinkModel(const std::string& link, bool* has_link) const;
 
   /** \brief Get a link by its index. Output error and return nullptr when the link is missing. */
   const LinkModel* getLinkModel(size_t index) const;
 
   /** \brief Get a link by its name. Output error and return nullptr when the link is missing. */
-  LinkModel* getLinkModel(const std::string& link, bool* has_link = nullptr);
+  LinkModel* getLinkModel(const std::string& link);
+  [[deprecated("Use getLinkModel(const std::string& name) instead.")]] LinkModel*
+  getLinkModel(const std::string& link, bool* has_link = nullptr);
 
   /** \brief Get the latest link upwards the kinematic tree, which is only connected via fixed joints
    *

moveit_core/robot_model/src/robot_model.cpp

@@ -1331,7 +1331,8 @@ JointModel* RobotModel::getJointModel(const std::string& name)
 
 const LinkModel* RobotModel::getLinkModel(const std::string& name, bool* has_link) const
 {
-  return const_cast<RobotModel*>(this)->getLinkModel(name, has_link);
+  *has_link = false;
+  return const_cast<RobotModel*>(this)->getLinkModel(name);
 }
 
 const LinkModel* RobotModel::getLinkModel(size_t index) const
@@ -1347,27 +1348,29 @@ const LinkModel* RobotModel::getLinkModel(size_t index) const
 
 LinkModel* RobotModel::getLinkModel(const std::string& name, bool* has_link)
 {
-  if (has_link)
-    *has_link = true;  // Start out optimistic
   LinkModelMap::const_iterator it = link_model_map_.find(name);
   if (it != link_model_map_.end())
-    return it->second;
-
-  if (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());
+    return it->second;
   }
+  RCLCPP_ERROR(getLogger(), "Link '%s' not found in model '%s'", name.c_str(), model_name_.c_str());
   return nullptr;
 }
 
+const LinkModel* getLinkModel(const std::string& link, bool* has_link) const
+{
+  RCLCPP_ERROR(getLogger(), "'has_link' argument is deprecated. Use getLinkModel(const std::string& name) instead.");
+  *has_link = false;
+  return getLinkModel(link);
+}
+
 const LinkModel* RobotModel::getRigidlyConnectedParentLinkModel(const LinkModel* link)
 {
   if (!link)
+  {
+    RCLCPP_ERROR(getLogger(), "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();
 

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,41 @@ 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))
+    {
+      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))
+    {
+      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);
 }
 
@@ -1319,8 +1344,9 @@ const Eigen::Isometry3d& RobotState::getFrameInfo(const std::string& frame_id, c
     frame_found = true;
     return IDENTITY_TRANSFORM;
   }
-  if ((robot_link = robot_model_->getLinkModel(frame_id, &frame_found)))
+  if ((robot_link = robot_model_->getLinkModel(frame_id)))
   {
+    frame_found = true;
     assert(checkLinkTransforms());
     return global_link_transforms_[robot_link->getLinkIndex()];
   }

moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp

@@ -389,9 +389,8 @@ bool IKFastKinematicsPlugin::computeRelativeTransform(const std::string& from, c
   robot_state = std::make_shared<RobotState>(robot_model_);
   robot_state->setToDefaultValues();
 
-  bool has_link;  // to suppress RCLCPP_ERRORs for non-existent frames
-  auto* from_link = robot_model_->getLinkModel(from, &has_link);
-  auto* to_link = robot_model_->getLinkModel(to, &has_link);
+  auto* from_link = robot_model_->getLinkModel(from);
+  auto* to_link = robot_model_->getLinkModel(to);
   if (!from_link || !to_link)
     return false;
 

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;
 }