Improve code documentation
Some checks failed
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[CLANG_TIDY:true IMAGE:noble-ci-testing TARGET_CMAKE_… (push) Has been cancelled
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[DOCKER_RUN_OPTS:-e PRELOAD=libasan.so.8 -e LSAN_OPTI… (push) Has been cancelled
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[IMAGE:jammy-ci]) (push) Has been cancelled
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[IMAGE:noble-ci NAME:ccov TARGET_CMAKE_ARGS:-DCMAKE_B… (push) Has been cancelled
Format / pre-commit (push) Has been cancelled
CI / doc (push) Has been cancelled
CI / deploy (push) Has been cancelled

This commit is contained in:
Robert Haschke 2025-08-13 11:39:38 +02:00
parent 87b3701223
commit bf001bd093
2 changed files with 6 additions and 5 deletions

View File

@ -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

View File

@ -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<geometry_msgs::PoseStamped>(property.value());
auto ik_pose_msg = boost::any_cast<geometry_msgs::PoseStamped>(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";