mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
MoveTo Cartesian: create marker frame at current and goal pose
This commit is contained in:
parent
cb38a87a73
commit
a4fed6a18a
@ -38,6 +38,7 @@
|
||||
|
||||
#include <moveit/task_constructor/stages/move_to.h>
|
||||
#include <moveit/planning_scene/planning_scene.h>
|
||||
#include <rviz_marker_tools/marker_creation.h>
|
||||
#include <eigen_conversions/eigen_msg.h>
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user