From a4fed6a18ad000affe4dbb8655c79031beeaadeb Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 13 Mar 2018 02:47:44 +0100 Subject: [PATCH] MoveTo Cartesian: create marker frame at current and goal pose --- core/src/stages/move_to.cpp | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/core/src/stages/move_to.cpp b/core/src/stages/move_to.cpp index 79662370..46f3f0af 100644 --- a/core/src/stages/move_to.cpp +++ b/core/src/stages/move_to.cpp @@ -38,6 +38,7 @@ #include #include +#include #include namespace moveit { namespace task_constructor { namespace stages { @@ -156,6 +157,16 @@ bool MoveTo::compute(const InterfaceState &state, planning_scene::PlanningSceneP target_eigen.translation() = target_point; } + // frame at current link pose + geometry_msgs::PoseStamped pose_msg; + pose_msg.header.frame_id = scene->getPlanningFrame(); + tf::poseEigenToMsg(scene->getCurrentState().getGlobalLinkTransform(link_name), pose_msg.pose); + rviz_marker_tools::appendFrame(solution.markers(), pose_msg, 0.1, "ik frame"); + + // frame at target pose + tf::poseEigenToMsg(target_eigen, pose_msg.pose); + rviz_marker_tools::appendFrame(solution.markers(), pose_msg, 0.1, "ik frame"); + success = planner_->plan(state.scene(), *link, target_eigen, jmg, timeout, robot_trajectory); }