mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
postpone pose transformation
This commit is contained in:
parent
603696f294
commit
3640928a76
@ -37,12 +37,8 @@
|
|||||||
#include <moveit/task_constructor/stages/generate_pose.h>
|
#include <moveit/task_constructor/stages/generate_pose.h>
|
||||||
#include <moveit/task_constructor/storage.h>
|
#include <moveit/task_constructor/storage.h>
|
||||||
#include <moveit/task_constructor/marker_tools.h>
|
#include <moveit/task_constructor/marker_tools.h>
|
||||||
#include <rviz_marker_tools/marker_creation.h>
|
|
||||||
|
|
||||||
#include <moveit/planning_scene/planning_scene.h>
|
#include <moveit/planning_scene/planning_scene.h>
|
||||||
|
#include <rviz_marker_tools/marker_creation.h>
|
||||||
#include <Eigen/Geometry>
|
|
||||||
#include <eigen_conversions/eigen_msg.h>
|
|
||||||
|
|
||||||
namespace moveit { namespace task_constructor { namespace stages {
|
namespace moveit { namespace task_constructor { namespace stages {
|
||||||
|
|
||||||
@ -68,27 +64,18 @@ bool GeneratePose::canCompute() const {
|
|||||||
return scenes_.size() > 0;
|
return scenes_.size() > 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool GeneratePose::compute(){
|
bool GeneratePose::compute() {
|
||||||
const auto& props = properties();
|
if (scenes_.empty())
|
||||||
|
return false;
|
||||||
geometry_msgs::PoseStamped target_pose = props.get<geometry_msgs::PoseStamped>("pose");
|
|
||||||
|
|
||||||
if(scenes_.empty()) throw std::runtime_error("GeneratePose called without checking canCompute.");
|
|
||||||
|
|
||||||
planning_scene::PlanningSceneConstPtr scene = scenes_[0];
|
planning_scene::PlanningSceneConstPtr scene = scenes_[0];
|
||||||
scenes_.pop_front();
|
scenes_.pop_front();
|
||||||
|
|
||||||
// take care of frame transforms
|
geometry_msgs::PoseStamped target_pose = properties().get<geometry_msgs::PoseStamped>("pose");
|
||||||
const std::string& frame = target_pose.header.frame_id;
|
if (target_pose.header.frame_id.empty())
|
||||||
if( !frame.empty() && frame != scene->getPlanningFrame() ){
|
|
||||||
if ( !scene->knowsFrameTransform(frame) )
|
|
||||||
throw std::runtime_error("GeneratePose does not know frame '" + frame + "'");
|
|
||||||
|
|
||||||
Eigen::Affine3d pose;
|
|
||||||
tf::poseMsgToEigen(target_pose.pose, pose);
|
|
||||||
pose = scene->getFrameTransform(target_pose.header.frame_id) * pose;
|
|
||||||
tf::poseEigenToMsg(pose, target_pose.pose);
|
|
||||||
target_pose.header.frame_id = scene->getPlanningFrame();
|
target_pose.header.frame_id = scene->getPlanningFrame();
|
||||||
|
else if (!scene->knowsFrameTransform(target_pose.header.frame_id)) {
|
||||||
|
ROS_WARN_NAMED("GeneratePose", "Unknown frame: '%s'", target_pose.header.frame_id.c_str());
|
||||||
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
InterfaceState state(scene);
|
InterfaceState state(scene);
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user