42 using namespace boost;
43 using namespace Eigen;
50 psm_.reset(
new planning_scene_monitor::PlanningSceneMonitor(
"robot_description",
tf_));
51 psm_->startStateMonitor();
74 throw runtime_error(
"Can not add handles if TransformProvider is running");
88 contextIt->second->mutex_.lock();
89 contextIt->second->transformation_.matrix().setZero();
90 contextIt->second->mutex_.unlock();
97 map<MeshHandle, shared_ptr<TransformContext> >::const_iterator contextIt =
handle2context_.find(handle);
101 ROS_ERROR(
"Unable to find mesh with handle %d", handle);
104 contextIt->second->mutex_.lock();
105 transform = contextIt->second->transformation_;
106 contextIt->second->mutex_.unlock();
107 return !(transform.matrix().isZero(0));
113 throw runtime_error(
"TransformProvider is listening to empty list of frames!");
129 static Affine3d transformation;
130 static robot_state::RobotStatePtr robot_state;
131 robot_state =
psm_->getStateMonitor()->getCurrentState();
135 tf_->getLatestCommonTime(
frame_id_,
psm_->getPlanningScene()->getPlanningFrame(), input_pose.
stamp_, &error);
136 input_pose.
frame_id_ =
psm_->getPlanningScene()->getPlanningFrame();
141 transformation = robot_state->getLinkState(contextIt->second->frame_id_)->getGlobalCollisionBodyTransform();
154 catch (std::exception& ex)
156 ROS_ERROR(
"Caught %s while updating transforms", ex.what());
void poseEigenToTF(const Eigen::Affine3d &e, tf::Pose &t)
void poseTFToEigen(const tf::Pose &t, Eigen::Affine3d &e)
Context Object for registered frames.