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

Check for nullptr on getGlobalLinkTransform #611

Merged
Merged
Changes from 2 commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
10 changes: 10 additions & 0 deletions moveit_core/robot_state/include/moveit/robot_state/robot_state.h
Expand Up @@ -1291,6 +1291,8 @@ class RobotState
* A related, more comprehensive function is |getFrameTransform|, which additionally to link frames
* also searches for attached object frames and their subframes.
*
* This will throw a std::runtime_error if the passed link is not found
AndyZe marked this conversation as resolved.
Show resolved Hide resolved
*
* The returned transformation is always a valid isometry.
*/
const Eigen::Isometry3d& getGlobalLinkTransform(const std::string& link_name)
Expand All @@ -1300,6 +1302,10 @@ class RobotState

const Eigen::Isometry3d& getGlobalLinkTransform(const LinkModel* link)
{
if (!link)
{
throw Exception("Invalid link");
}
updateLinkTransforms();
return global_link_transforms_[link->getLinkIndex()];
}
Expand All @@ -1311,6 +1317,10 @@ class RobotState

const Eigen::Isometry3d& getGlobalLinkTransform(const LinkModel* link) const
{
if (!link)
{
throw Exception("Invalid link");
}
BOOST_VERIFY(checkLinkTransforms());
return global_link_transforms_[link->getLinkIndex()];
}
Expand Down