44 tf_buffer_ = std::make_shared<tf2_ros::Buffer>();
46 psm_ = std::make_shared<planning_scene_monitor::PlanningSceneMonitor>(
"robot_description",
tf_buffer_);
47 psm_->startStateMonitor();
70 throw std::runtime_error(
"Can not add handles if TransformProvider is running");
83 context_it.second->mutex_.lock();
84 context_it.second->transformation_.matrix().setZero();
85 context_it.second->mutex_.unlock();
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();
108 throw std::runtime_error(
"TransformProvider is listening to empty list of frames!");
132 static moveit::core::RobotStatePtr robot_state;
133 robot_state =
psm_->getStateMonitor()->getCurrentState();
136 geometry_msgs::TransformStamped common_tf =
138 input_transform.
stamp_ = common_tf.header.stamp;
145 input_transform.
frame_id_ =
psm_->getPlanningScene()->getPlanningFrame();
152 input_transform.
setData(robot_state->getGlobalLinkTransform(context_it.second->frame_id_));
162 catch (std::exception& ex)
164 ROS_ERROR(
"Caught %s while updating transforms", ex.what());
167 handle2context_[context_it.first]->transformation_ =
static_cast<Eigen::Isometry3d
>(output_transform);