From bf001bd0938ad9556d4a03e4bd5fc7e32eaab9fd Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 13 Aug 2025 11:39:38 +0200 Subject: [PATCH] Improve code documentation --- core/include/moveit/task_constructor/utils.h | 3 ++- core/src/utils.cpp | 8 ++++---- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/core/include/moveit/task_constructor/utils.h b/core/include/moveit/task_constructor/utils.h index a2b30ec6..2e72a235 100644 --- a/core/include/moveit/task_constructor/utils.h +++ b/core/include/moveit/task_constructor/utils.h @@ -139,7 +139,8 @@ private: Int i; }; -bool getRobotTipForFrame(const Property& property, const planning_scene::PlanningScene& scene, +/** For a PoseStamped property, lookup the associated LinkModel* and yield the pose in global frame */ +bool getRobotTipForFrame(const Property& tip_pose, const planning_scene::PlanningScene& scene, const moveit::core::JointModelGroup* jmg, std::string& error_msg, const moveit::core::LinkModel*& robot_link, Eigen::Isometry3d& tip_in_global_frame); } // namespace utils diff --git a/core/src/utils.cpp b/core/src/utils.cpp index 122471e0..27a3b27f 100644 --- a/core/src/utils.cpp +++ b/core/src/utils.cpp @@ -47,7 +47,7 @@ namespace moveit { namespace task_constructor { namespace utils { -bool getRobotTipForFrame(const Property& property, const planning_scene::PlanningScene& scene, +bool getRobotTipForFrame(const Property& tip_pose, const planning_scene::PlanningScene& scene, const moveit::core::JointModelGroup* jmg, std::string& error_msg, const moveit::core::LinkModel*& robot_link, Eigen::Isometry3d& tip_in_global_frame) { auto get_tip = [&jmg]() -> const moveit::core::LinkModel* { @@ -62,7 +62,7 @@ bool getRobotTipForFrame(const Property& property, const planning_scene::Plannin error_msg = ""; - if (property.value().empty()) { // property undefined + if (tip_pose.value().empty()) { // property undefined robot_link = get_tip(); if (!robot_link) { error_msg = "missing ik_frame"; @@ -70,7 +70,7 @@ bool getRobotTipForFrame(const Property& property, const planning_scene::Plannin } tip_in_global_frame = scene.getCurrentState().getGlobalLinkTransform(robot_link); } else { - auto ik_pose_msg = boost::any_cast(property.value()); + auto ik_pose_msg = boost::any_cast(tip_pose.value()); tf2::fromMsg(ik_pose_msg.pose, tip_in_global_frame); robot_link = nullptr; @@ -82,7 +82,7 @@ bool getRobotTipForFrame(const Property& property, const planning_scene::Plannin error_msg = ss.str(); return false; } - if (!robot_link) + if (!robot_link) // if ik_pose_msg.header.frame_id was empty, use default tip frame robot_link = get_tip(); if (!robot_link) { error_msg = "ik_frame doesn't specify a link frame";