Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Never return nullptr silently #2969

Open
wants to merge 14 commits into
base: main
Choose a base branch
from
Original file line number Diff line number Diff line change
Expand Up @@ -116,7 +116,7 @@ collision_detection::Contact* processResult(ContactTestData& cdata, collision_de

return &(dr.back());
}

RCLCPP_DEBUG(getLogger(), "Result cannot be processed. Returning nullptr, this should never happen!");
return nullptr;
}

Expand Down
3 changes: 3 additions & 0 deletions moveit_core/robot_model/src/robot_model.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -1367,7 +1367,10 @@ LinkModel* RobotModel::getLinkModel(const std::string& name, bool* has_link)
const LinkModel* RobotModel::getRigidlyConnectedParentLinkModel(const LinkModel* link)
{
if (!link)
{
RCLCPP_DEBUG(getLogger().get_child("getRigidlyConnectedParentLink", "Link is NULL");
sjahr marked this conversation as resolved.
Show resolved Hide resolved
return link;
}
const moveit::core::LinkModel* parent_link = link->getParentLinkModel();
const moveit::core::JointModel* joint = link->getParentJointModel();
sjahr marked this conversation as resolved.
Show resolved Hide resolved

Expand Down
24 changes: 23 additions & 1 deletion moveit_core/robot_state/src/robot_state.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -519,6 +519,8 @@ const double* RobotState::getJointPositions(const JointModel* joint) const
{
if (joint->getVariableCount() == 0)
{
RCLCPP_DEBUG(getLogger(), "Cannot get joint positions for joint '%s' because it has no variables",
joint->getName().c_str());
return nullptr;
}
return &position_.at(joint->getFirstVariableIndex());
Expand All @@ -528,6 +530,8 @@ const double* RobotState::getJointVelocities(const JointModel* joint) const
{
if (joint->getVariableCount() == 0)
{
RCLCPP_DEBUG(getLogger(), "Cannot get joint velocities for joint '%s' because it has no variables",
joint->getName().c_str());
return nullptr;
}
return &velocity_.at(joint->getFirstVariableIndex());
Expand All @@ -537,6 +541,8 @@ const double* RobotState::getJointAccelerations(const JointModel* joint) const
{
if (joint->getVariableCount() == 0)
{
RCLCPP_DEBUG(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());
Expand All @@ -546,6 +552,8 @@ const double* RobotState::getJointEffort(const JointModel* joint) const
{
if (joint->getVariableCount() == 0)
{
RCLCPP_DEBUG(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());
Expand Down Expand Up @@ -911,24 +919,38 @@ 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_DEBUG(getLogger().get_child("getRigidlyConnectedParentLink",
"Attached object '%s' for frame '%s' does not exist.", object.c_str(), frame.c_str());
sjahr marked this conversation as resolved.
Show resolved Hide resolved
return nullptr;
}
auto body{ getAttachedBody(object) };
if (!body->hasSubframeTransform(frame))
{
RCLCPP_DEBUG(getLogger().get_child("getRigidlyConnectedParentLink",
"Body '%s' does not have subframe '%s', object.c_str(), frame.c_str());
sjahr marked this conversation as resolved.
Show resolved Hide resolved
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);
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}

Expand Down
Loading