38 #include <tf2_ros/transform_listener.h>
39 #include <tf2_ros/buffer.h>
40 #include <tf2_eigen/tf2_eigen.hpp>
44 tf_buffer_ = std::make_shared<tf2_ros::Buffer>();
45 tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
46 psm_ = std::make_shared<planning_scene_monitor::PlanningSceneMonitor>(
"robot_description", tf_buffer_);
47 psm_->startStateMonitor();
58 thread_ = std::thread(&TransformProvider::run,
this);
70 throw std::runtime_error(
"Can not add handles if TransformProvider is running");
72 handle2context_[handle] = std::make_shared<TransformContext>(
name);
77 if (frame_id_ != frame)
80 for (
auto& context_it : handle2context_)
83 context_it.second->mutex_.lock();
84 context_it.second->transformation_.matrix().setZero();
85 context_it.second->mutex_.unlock();
92 auto context_it = handle2context_.find(handle);
94 if (context_it == handle2context_.end())
96 ROS_ERROR(
"Unable to find mesh with handle %d", handle);
99 context_it->second->mutex_.lock();
100 transform = context_it->second->transformation_;
101 context_it->second->mutex_.unlock();
102 return !(transform.matrix().isZero(0));
105 void TransformProvider::run()
107 if (handle2context_.empty())
108 throw std::runtime_error(
"TransformProvider is listening to empty list of frames!");
110 while (ros::ok() && !stop_)
113 update_rate_.sleep();
119 update_rate_ = ros::Rate(update_rate);
122 void TransformProvider::updateTransforms()
125 if (frame_id_.empty())
127 ROS_DEBUG_THROTTLE(2.,
"Not updating transforms because frame_id_ is empty.");
131 static tf2::Stamped<Eigen::Isometry3d> input_transform, output_transform;
132 static moveit::core::RobotStatePtr robot_state;
133 robot_state = psm_->getStateMonitor()->getCurrentState();
136 geometry_msgs::TransformStamped common_tf =
137 tf_buffer_->lookupTransform(frame_id_, psm_->getPlanningScene()->getPlanningFrame(), ros::Time(0.0));
138 input_transform.stamp_ = common_tf.header.stamp;
140 catch (tf2::TransformException& ex)
142 ROS_ERROR(
"TF Problem: %s", ex.what());
145 input_transform.frame_id_ = psm_->getPlanningScene()->getPlanningFrame();
147 for (
auto& context_it : handle2context_)
152 input_transform.setData(robot_state->getGlobalLinkTransform(context_it.second->frame_id_));
153 tf_buffer_->transform(input_transform, output_transform, frame_id_);
155 catch (
const tf2::TransformException& ex)
157 handle2context_[context_it.first]->mutex_.lock();
158 handle2context_[context_it.first]->transformation_.matrix().setZero();
159 handle2context_[context_it.first]->mutex_.unlock();
162 catch (std::exception& ex)
164 ROS_ERROR(
"Caught %s while updating transforms", ex.what());
166 handle2context_[context_it.first]->mutex_.lock();
167 handle2context_[context_it.first]->transformation_ =
static_cast<Eigen::Isometry3d
>(output_transform);
168 handle2context_[context_it.first]->mutex_.unlock();