mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
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
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:
parent
87b3701223
commit
bf001bd093
@ -139,7 +139,8 @@ private:
|
|||||||
Int i;
|
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::JointModelGroup* jmg, std::string& error_msg,
|
||||||
const moveit::core::LinkModel*& robot_link, Eigen::Isometry3d& tip_in_global_frame);
|
const moveit::core::LinkModel*& robot_link, Eigen::Isometry3d& tip_in_global_frame);
|
||||||
} // namespace utils
|
} // namespace utils
|
||||||
|
|||||||
@ -47,7 +47,7 @@ namespace moveit {
|
|||||||
namespace task_constructor {
|
namespace task_constructor {
|
||||||
namespace utils {
|
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::JointModelGroup* jmg, std::string& error_msg,
|
||||||
const moveit::core::LinkModel*& robot_link, Eigen::Isometry3d& tip_in_global_frame) {
|
const moveit::core::LinkModel*& robot_link, Eigen::Isometry3d& tip_in_global_frame) {
|
||||||
auto get_tip = [&jmg]() -> const moveit::core::LinkModel* {
|
auto get_tip = [&jmg]() -> const moveit::core::LinkModel* {
|
||||||
@ -62,7 +62,7 @@ bool getRobotTipForFrame(const Property& property, const planning_scene::Plannin
|
|||||||
|
|
||||||
error_msg = "";
|
error_msg = "";
|
||||||
|
|
||||||
if (property.value().empty()) { // property undefined
|
if (tip_pose.value().empty()) { // property undefined
|
||||||
robot_link = get_tip();
|
robot_link = get_tip();
|
||||||
if (!robot_link) {
|
if (!robot_link) {
|
||||||
error_msg = "missing ik_frame";
|
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);
|
tip_in_global_frame = scene.getCurrentState().getGlobalLinkTransform(robot_link);
|
||||||
} else {
|
} 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);
|
tf2::fromMsg(ik_pose_msg.pose, tip_in_global_frame);
|
||||||
|
|
||||||
robot_link = nullptr;
|
robot_link = nullptr;
|
||||||
@ -82,7 +82,7 @@ bool getRobotTipForFrame(const Property& property, const planning_scene::Plannin
|
|||||||
error_msg = ss.str();
|
error_msg = ss.str();
|
||||||
return false;
|
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();
|
robot_link = get_tip();
|
||||||
if (!robot_link) {
|
if (!robot_link) {
|
||||||
error_msg = "ik_frame doesn't specify a link frame";
|
error_msg = "ik_frame doesn't specify a link frame";
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user