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
Prev Previous commit
Next Next commit
Revert "Deprecate ugly function signature"
This reverts commit 57a85c8.
sjahr committed Aug 26, 2024
commit 598cc0c3512336c14a9f9bfeb9f85c662125e6fd
Original file line number Diff line number Diff line change
@@ -246,17 +246,13 @@ 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) const;
[[deprecated("Use getLinkModel(const std::string& name) instead.")]] const LinkModel*
getLinkModel(const std::string& link, bool* has_link) const;
const LinkModel* getLinkModel(const std::string& link, bool* has_link = nullptr) 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);
[[deprecated("Use getLinkModel(const std::string& name) instead.")]] LinkModel*
getLinkModel(const std::string& link, bool* has_link = nullptr);
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
*
17 changes: 7 additions & 10 deletions moveit_core/robot_model/src/robot_model.cpp
Original file line number Diff line number Diff line change
@@ -1331,8 +1331,7 @@ JointModel* RobotModel::getJointModel(const std::string& name)

const LinkModel* RobotModel::getLinkModel(const std::string& name, bool* has_link) const
{
*has_link = false;
return const_cast<RobotModel*>(this)->getLinkModel(name);
return const_cast<RobotModel*>(this)->getLinkModel(name, has_link);
}

const LinkModel* RobotModel::getLinkModel(size_t index) const
@@ -1348,22 +1347,20 @@ 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
}
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)
3 changes: 1 addition & 2 deletions moveit_core/robot_state/src/robot_state.cpp
Original file line number Diff line number Diff line change
@@ -1344,9 +1344,8 @@ 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)))
if ((robot_link = robot_model_->getLinkModel(frame_id, &frame_found)))
{
frame_found = true;
assert(checkLinkTransforms());
return global_link_transforms_[robot_link->getLinkIndex()];
}
Original file line number Diff line number Diff line change
@@ -389,8 +389,9 @@ bool IKFastKinematicsPlugin::computeRelativeTransform(const std::string& from, c
robot_state = std::make_shared<RobotState>(robot_model_);
robot_state->setToDefaultValues();

auto* from_link = robot_model_->getLinkModel(from);
auto* to_link = robot_model_->getLinkModel(to);
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);
if (!from_link || !to_link)
return false;