From ad4abfb62d660f92aa1c05f51ff1c761b7d09507 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Mon, 12 Aug 2024 11:24:50 +0200 Subject: [PATCH 01/12] Never return nullptr silently --- .../contact_checker_common.cpp | 2 +- moveit_core/robot_model/src/robot_model.cpp | 10 +++---- moveit_core/robot_state/src/robot_state.cpp | 29 ++++++++++++++++++- .../src/trajectory_generator_circ.cpp | 1 + 4 files changed, 35 insertions(+), 7 deletions(-) diff --git a/moveit_core/collision_detection_bullet/src/bullet_integration/contact_checker_common.cpp b/moveit_core/collision_detection_bullet/src/bullet_integration/contact_checker_common.cpp index 4da803358e..3d99b7187c 100644 --- a/moveit_core/collision_detection_bullet/src/bullet_integration/contact_checker_common.cpp +++ b/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; } diff --git a/moveit_core/robot_model/src/robot_model.cpp b/moveit_core/robot_model/src/robot_model.cpp index f65ff86a11..9c5c461649 100644 --- a/moveit_core/robot_model/src/robot_model.cpp +++ b/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(); diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index b16373da04..090706c640 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/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); } diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp index c512e21578..9354234853 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp @@ -268,6 +268,7 @@ std::unique_ptr TrajectoryGeneratorCIRC::setPathCIRC(const MotionPlan throw CenterPointDifferentRadius(os.str()); } + RCLCPP_ERROR(getLogger(), "Result cannot set path CIRC. Returning nullptr, this should never happen!"); return nullptr; } From 5d5332a85f3c81fc2aa8caa39ebc1d230a584263 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Mon, 12 Aug 2024 11:45:15 +0200 Subject: [PATCH 02/12] Fix typo --- moveit_core/robot_model/src/robot_model.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_core/robot_model/src/robot_model.cpp b/moveit_core/robot_model/src/robot_model.cpp index 9c5c461649..80d2e8384d 100644 --- a/moveit_core/robot_model/src/robot_model.cpp +++ b/moveit_core/robot_model/src/robot_model.cpp @@ -1365,7 +1365,7 @@ const LinkModel* RobotModel::getRigidlyConnectedParentLinkModel(const LinkModel* { if (!link) { - RCLCPP_ERROR(get_logger(), "Cannot determine rigidly connected parent link because input link is nullptr"); + RCLCPP_ERROR(getLogger(), "Cannot determine rigidly connected parent link because input link is nullptr"); return link; } const moveit::core::LinkModel* parent_link = link->getParentLinkModel(); From c2a3f52aca2bddb4f8921be70d7ed50cf21d1cf4 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Mon, 12 Aug 2024 14:14:08 +0200 Subject: [PATCH 03/12] Remove obvious comments --- moveit_core/robot_state/src/robot_state.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index 090706c640..626385bc5b 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -926,7 +926,6 @@ const LinkModel* RobotState::getRigidlyConnectedParentLinkModel(const std::strin 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()); @@ -935,7 +934,6 @@ const LinkModel* RobotState::getRigidlyConnectedParentLinkModel(const std::strin 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.", From 7a6b3e9fc7750b78680a8f52293b6cefb9198ce8 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Mon, 12 Aug 2024 14:32:22 +0200 Subject: [PATCH 04/12] A valid link always has a parent joint. --- moveit_core/robot_model/src/robot_model.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_core/robot_model/src/robot_model.cpp b/moveit_core/robot_model/src/robot_model.cpp index 80d2e8384d..22adc54a3c 100644 --- a/moveit_core/robot_model/src/robot_model.cpp +++ b/moveit_core/robot_model/src/robot_model.cpp @@ -1371,7 +1371,7 @@ const LinkModel* RobotModel::getRigidlyConnectedParentLinkModel(const LinkModel* const moveit::core::LinkModel* parent_link = link->getParentLinkModel(); const moveit::core::JointModel* joint = link->getParentJointModel(); - while (parent_link && joint && joint->getType() == moveit::core::JointModel::FIXED) + while (parent_link && joint->getType() == moveit::core::JointModel::FIXED) { link = parent_link; joint = link->getParentJointModel(); From 57a85c88fd02728c18e24c282940cb7bce1ac823 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Fri, 23 Aug 2024 18:17:01 +0200 Subject: [PATCH 05/12] Deprecate ugly function signature --- .../include/moveit/robot_model/robot_model.h | 8 ++++++-- moveit_core/robot_model/src/robot_model.cpp | 17 ++++++++++------- moveit_core/robot_state/src/robot_state.cpp | 3 ++- .../ikfast61_moveit_plugin_template.cpp | 5 ++--- 4 files changed, 20 insertions(+), 13 deletions(-) diff --git a/moveit_core/robot_model/include/moveit/robot_model/robot_model.h b/moveit_core/robot_model/include/moveit/robot_model/robot_model.h index 6c4d1aad1b..a3cf91da12 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/robot_model.h +++ b/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 * diff --git a/moveit_core/robot_model/src/robot_model.cpp b/moveit_core/robot_model/src/robot_model.cpp index 22adc54a3c..490c8f0115 100644 --- a/moveit_core/robot_model/src/robot_model.cpp +++ b/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(this)->getLinkModel(name, has_link); + *has_link = false; + return const_cast(this)->getLinkModel(name); } const LinkModel* RobotModel::getLinkModel(size_t index) const @@ -1347,20 +1348,22 @@ 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 + 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) diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index 626385bc5b..80e0c6fe82 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -1344,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()]; } diff --git a/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp b/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp index fdba5f1f5b..99f41927e4 100644 --- a/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp +++ b/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(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; From 598cc0c3512336c14a9f9bfeb9f85c662125e6fd Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Mon, 26 Aug 2024 10:40:58 +0200 Subject: [PATCH 06/12] Revert "Deprecate ugly function signature" This reverts commit 57a85c88fd02728c18e24c282940cb7bce1ac823. --- .../include/moveit/robot_model/robot_model.h | 8 ++------ moveit_core/robot_model/src/robot_model.cpp | 17 +++++++---------- moveit_core/robot_state/src/robot_state.cpp | 3 +-- .../ikfast61_moveit_plugin_template.cpp | 5 +++-- 4 files changed, 13 insertions(+), 20 deletions(-) diff --git a/moveit_core/robot_model/include/moveit/robot_model/robot_model.h b/moveit_core/robot_model/include/moveit/robot_model/robot_model.h index a3cf91da12..6c4d1aad1b 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/robot_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/robot_model.h @@ -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 * diff --git a/moveit_core/robot_model/src/robot_model.cpp b/moveit_core/robot_model/src/robot_model.cpp index 490c8f0115..22adc54a3c 100644 --- a/moveit_core/robot_model/src/robot_model.cpp +++ b/moveit_core/robot_model/src/robot_model.cpp @@ -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(this)->getLinkModel(name); + return const_cast(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) diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index 80e0c6fe82..626385bc5b 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -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()]; } diff --git a/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp b/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp index 99f41927e4..fdba5f1f5b 100644 --- a/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp +++ b/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp @@ -389,8 +389,9 @@ bool IKFastKinematicsPlugin::computeRelativeTransform(const std::string& from, c robot_state = std::make_shared(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; From 28a4975bdc45f4a9b99f17b25264e4ca7f1007e9 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Mon, 26 Aug 2024 10:56:03 +0200 Subject: [PATCH 07/12] Reduce log level to debug --- moveit_core/robot_model/src/robot_model.cpp | 5 ++++- moveit_core/robot_state/src/robot_state.cpp | 12 ++++++------ 2 files changed, 10 insertions(+), 7 deletions(-) diff --git a/moveit_core/robot_model/src/robot_model.cpp b/moveit_core/robot_model/src/robot_model.cpp index 22adc54a3c..4c7651cf87 100644 --- a/moveit_core/robot_model/src/robot_model.cpp +++ b/moveit_core/robot_model/src/robot_model.cpp @@ -1357,7 +1357,10 @@ LinkModel* RobotModel::getLinkModel(const std::string& name, bool* 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()); + else + { // Otherwise print error + RCLCPP_ERROR(getLogger(), "Link '%s' not found in model '%s'", name.c_str(), model_name_.c_str()); + } return nullptr; } diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index 626385bc5b..7d0fdb7ace 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -519,7 +519,7 @@ 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", + RCLCPP_DEBUG(getLogger(), "Cannot get joint positions for joint '%s' because it has no variables", joint->getName().c_str()); return nullptr; } @@ -530,7 +530,7 @@ 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", + RCLCPP_DEBUG(getLogger(), "Cannot get joint velocities for joint '%s' because it has no variables", joint->getName().c_str()); return nullptr; } @@ -541,7 +541,7 @@ 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", + RCLCPP_DEBUG(getLogger(), "Cannot get joint accelerations for joint '%s' because it has no variables", joint->getName().c_str()); return nullptr; } @@ -552,7 +552,7 @@ 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", + RCLCPP_DEBUG(getLogger(), "Cannot get joint effort for joint '%s' because it has no variables", joint->getName().c_str()); return nullptr; } @@ -926,7 +926,7 @@ const LinkModel* RobotState::getRigidlyConnectedParentLinkModel(const std::strin std::string object{ frame.substr(0, idx) }; if (!hasAttachedBody(object)) { - RCLCPP_ERROR(getLogger(), + RCLCPP_DEBUG(getLogger(), "Cannot find rigidly connected parent link for frame '%s' because there is no attached body.", frame.c_str()); return nullptr; @@ -934,7 +934,7 @@ const LinkModel* RobotState::getRigidlyConnectedParentLinkModel(const std::strin auto body{ getAttachedBody(object) }; if (!body->hasSubframeTransform(frame)) { - RCLCPP_ERROR(getLogger(), + RCLCPP_DEBUG(getLogger(), "Cannot find rigidly connected parent link for frame '%s' because the transformation to the parent " "link is unknown.", frame.c_str()); From 4b173e7e549b3fc1e1d1e97544727f7648c6dc6e Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Mon, 26 Aug 2024 14:00:52 +0200 Subject: [PATCH 08/12] Apply suggestions from code review Co-authored-by: Robert Haschke --- .../src/bullet_integration/contact_checker_common.cpp | 2 +- moveit_core/robot_model/src/robot_model.cpp | 2 +- moveit_core/robot_state/src/robot_state.cpp | 11 ++++------- 3 files changed, 6 insertions(+), 9 deletions(-) diff --git a/moveit_core/collision_detection_bullet/src/bullet_integration/contact_checker_common.cpp b/moveit_core/collision_detection_bullet/src/bullet_integration/contact_checker_common.cpp index 3d99b7187c..0a505458bd 100644 --- a/moveit_core/collision_detection_bullet/src/bullet_integration/contact_checker_common.cpp +++ b/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!"); + RCLCPP_DEBUG(getLogger(), "Result cannot be processed. Returning nullptr, this should never happen!"); return nullptr; } diff --git a/moveit_core/robot_model/src/robot_model.cpp b/moveit_core/robot_model/src/robot_model.cpp index 4c7651cf87..04c244204e 100644 --- a/moveit_core/robot_model/src/robot_model.cpp +++ b/moveit_core/robot_model/src/robot_model.cpp @@ -1368,7 +1368,7 @@ const LinkModel* RobotModel::getRigidlyConnectedParentLinkModel(const LinkModel* { if (!link) { - RCLCPP_ERROR(getLogger(), "Cannot determine rigidly connected parent link because input link is nullptr"); + RCLCPP_DEBUG(getLogger().get_child("getRigidlyConnectedParentLink", "Link is NULL"); return link; } const moveit::core::LinkModel* parent_link = link->getParentLinkModel(); diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index 7d0fdb7ace..f52d6c71a8 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -926,18 +926,15 @@ const LinkModel* RobotState::getRigidlyConnectedParentLinkModel(const std::strin std::string object{ frame.substr(0, idx) }; if (!hasAttachedBody(object)) { - RCLCPP_DEBUG(getLogger(), - "Cannot find rigidly connected parent link for frame '%s' because there is no attached body.", - frame.c_str()); + RCLCPP_DEBUG(getLogger().get_child("getRigidlyConnectedParentLink", + "Attached object '%s' for frame '%s' does not exist.", object.c_str(), frame.c_str()); return nullptr; } auto body{ getAttachedBody(object) }; if (!body->hasSubframeTransform(frame)) { - RCLCPP_DEBUG(getLogger(), - "Cannot find rigidly connected parent link for frame '%s' because the transformation to the parent " - "link is unknown.", - frame.c_str()); + RCLCPP_DEBUG(getLogger().get_child("getRigidlyConnectedParentLink", + "Body '%s' does not have subframe '%s', object.c_str(), frame.c_str()); return nullptr; } link = body->getAttachedLink(); From f9ffff2848daf681e762d57ae676c46d82af12d1 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Mon, 26 Aug 2024 15:28:16 +0200 Subject: [PATCH 09/12] Fix build --- moveit_core/robot_model/src/robot_model.cpp | 2 +- moveit_core/robot_state/src/robot_state.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/moveit_core/robot_model/src/robot_model.cpp b/moveit_core/robot_model/src/robot_model.cpp index 04c244204e..dffb678300 100644 --- a/moveit_core/robot_model/src/robot_model.cpp +++ b/moveit_core/robot_model/src/robot_model.cpp @@ -1368,7 +1368,7 @@ const LinkModel* RobotModel::getRigidlyConnectedParentLinkModel(const LinkModel* { if (!link) { - RCLCPP_DEBUG(getLogger().get_child("getRigidlyConnectedParentLink", "Link is NULL"); + RCLCPP_DEBUG(getLogger().get_child("getRigidlyConnectedParentLink", "Link is NULL")); return link; } const moveit::core::LinkModel* parent_link = link->getParentLinkModel(); diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index f52d6c71a8..f112f0eda9 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -934,7 +934,7 @@ const LinkModel* RobotState::getRigidlyConnectedParentLinkModel(const std::strin if (!body->hasSubframeTransform(frame)) { RCLCPP_DEBUG(getLogger().get_child("getRigidlyConnectedParentLink", - "Body '%s' does not have subframe '%s', object.c_str(), frame.c_str()); + "Body '%s' does not have subframe '%s'", object.c_str(), frame.c_str()); return nullptr; } link = body->getAttachedLink(); From ede820c98d9f4c43673e6941cda0e2a711060f10 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Mon, 26 Aug 2024 15:46:48 +0200 Subject: [PATCH 10/12] Apply suggestions from code review Co-authored-by: Robert Haschke --- moveit_core/robot_model/src/robot_model.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_core/robot_model/src/robot_model.cpp b/moveit_core/robot_model/src/robot_model.cpp index dffb678300..e9abee860f 100644 --- a/moveit_core/robot_model/src/robot_model.cpp +++ b/moveit_core/robot_model/src/robot_model.cpp @@ -1368,7 +1368,7 @@ const LinkModel* RobotModel::getRigidlyConnectedParentLinkModel(const LinkModel* { if (!link) { - RCLCPP_DEBUG(getLogger().get_child("getRigidlyConnectedParentLink", "Link is NULL")); + RCLCPP_DEBUG(getLogger().get_child("getRigidlyConnectedParentLink"), "Link is NULL"); return link; } const moveit::core::LinkModel* parent_link = link->getParentLinkModel(); From 32d2fc045e7be402130870b8dc870329cde9c16e Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Mon, 26 Aug 2024 15:47:06 +0200 Subject: [PATCH 11/12] Apply suggestions from code review Co-authored-by: Robert Haschke --- moveit_core/robot_state/src/robot_state.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index f112f0eda9..ec1bccbec2 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -926,14 +926,14 @@ const LinkModel* RobotState::getRigidlyConnectedParentLinkModel(const std::strin std::string object{ frame.substr(0, idx) }; if (!hasAttachedBody(object)) { - RCLCPP_DEBUG(getLogger().get_child("getRigidlyConnectedParentLink", + RCLCPP_DEBUG(getLogger().get_child("getRigidlyConnectedParentLink"), "Attached object '%s' for frame '%s' does not exist.", object.c_str(), frame.c_str()); return nullptr; } auto body{ getAttachedBody(object) }; if (!body->hasSubframeTransform(frame)) { - RCLCPP_DEBUG(getLogger().get_child("getRigidlyConnectedParentLink", + RCLCPP_DEBUG(getLogger().get_child("getRigidlyConnectedParentLink"), "Body '%s' does not have subframe '%s'", object.c_str(), frame.c_str()); return nullptr; } From 5b21e45c3f3ab359e95caa7175abe0e35ebf3d45 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Mon, 26 Aug 2024 15:50:21 +0200 Subject: [PATCH 12/12] Format --- moveit_core/robot_state/src/robot_state.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index ec1bccbec2..e977dc1fef 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -933,8 +933,8 @@ const LinkModel* RobotState::getRigidlyConnectedParentLinkModel(const std::strin 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()); + RCLCPP_DEBUG(getLogger().get_child("getRigidlyConnectedParentLink"), "Body '%s' does not have subframe '%s'", + object.c_str(), frame.c_str()); return nullptr; } link = body->getAttachedLink();