00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <moveit/planning_scene_monitor/current_state_monitor.h>
00038 #include <tf_conversions/tf_eigen.h>
00039 #include <limits>
00040
00041 planning_scene_monitor::CurrentStateMonitor::CurrentStateMonitor(const robot_model::RobotModelConstPtr& robot_model,
00042 const boost::shared_ptr<tf::Transformer>& tf)
00043 : CurrentStateMonitor(robot_model, tf, ros::NodeHandle())
00044 {
00045 }
00046
00047 planning_scene_monitor::CurrentStateMonitor::CurrentStateMonitor(const robot_model::RobotModelConstPtr& robot_model,
00048 const boost::shared_ptr<tf::Transformer>& tf,
00049 ros::NodeHandle nh)
00050 : nh_(nh)
00051 , tf_(tf)
00052 , robot_model_(robot_model)
00053 , robot_state_(robot_model)
00054 , state_monitor_started_(false)
00055 , error_(std::numeric_limits<float>::epsilon())
00056 {
00057 robot_state_.setToDefaultValues();
00058 }
00059
00060 planning_scene_monitor::CurrentStateMonitor::~CurrentStateMonitor()
00061 {
00062 stopStateMonitor();
00063 }
00064
00065 robot_state::RobotStatePtr planning_scene_monitor::CurrentStateMonitor::getCurrentState() const
00066 {
00067 boost::mutex::scoped_lock slock(state_update_lock_);
00068 robot_state::RobotState* result = new robot_state::RobotState(robot_state_);
00069 return robot_state::RobotStatePtr(result);
00070 }
00071
00072 ros::Time planning_scene_monitor::CurrentStateMonitor::getCurrentStateTime() const
00073 {
00074 boost::mutex::scoped_lock slock(state_update_lock_);
00075 return current_state_time_;
00076 }
00077
00078 std::pair<robot_state::RobotStatePtr, ros::Time>
00079 planning_scene_monitor::CurrentStateMonitor::getCurrentStateAndTime() const
00080 {
00081 boost::mutex::scoped_lock slock(state_update_lock_);
00082 robot_state::RobotState* result = new robot_state::RobotState(robot_state_);
00083 return std::make_pair(robot_state::RobotStatePtr(result), current_state_time_);
00084 }
00085
00086 std::map<std::string, double> planning_scene_monitor::CurrentStateMonitor::getCurrentStateValues() const
00087 {
00088 std::map<std::string, double> m;
00089 boost::mutex::scoped_lock slock(state_update_lock_);
00090 const double* pos = robot_state_.getVariablePositions();
00091 const std::vector<std::string>& names = robot_state_.getVariableNames();
00092 for (std::size_t i = 0; i < names.size(); ++i)
00093 m[names[i]] = pos[i];
00094 return m;
00095 }
00096
00097 void planning_scene_monitor::CurrentStateMonitor::setToCurrentState(robot_state::RobotState& upd) const
00098 {
00099 boost::mutex::scoped_lock slock(state_update_lock_);
00100 const double* pos = robot_state_.getVariablePositions();
00101 upd.setVariablePositions(pos);
00102 }
00103
00104 void planning_scene_monitor::CurrentStateMonitor::addUpdateCallback(const JointStateUpdateCallback& fn)
00105 {
00106 if (fn)
00107 update_callbacks_.push_back(fn);
00108 }
00109
00110 void planning_scene_monitor::CurrentStateMonitor::clearUpdateCallbacks()
00111 {
00112 update_callbacks_.clear();
00113 }
00114
00115 void planning_scene_monitor::CurrentStateMonitor::startStateMonitor(const std::string& joint_states_topic)
00116 {
00117 if (!state_monitor_started_ && robot_model_)
00118 {
00119 joint_time_.clear();
00120 if (joint_states_topic.empty())
00121 ROS_ERROR("The joint states topic cannot be an empty string");
00122 else
00123 joint_state_subscriber_ = nh_.subscribe(joint_states_topic, 25, &CurrentStateMonitor::jointStateCallback, this);
00124 state_monitor_started_ = true;
00125 monitor_start_time_ = ros::Time::now();
00126 ROS_DEBUG("Listening to joint states on topic '%s'", nh_.resolveName(joint_states_topic).c_str());
00127 }
00128 }
00129
00130 bool planning_scene_monitor::CurrentStateMonitor::isActive() const
00131 {
00132 return state_monitor_started_;
00133 }
00134
00135 void planning_scene_monitor::CurrentStateMonitor::stopStateMonitor()
00136 {
00137 if (state_monitor_started_)
00138 {
00139 joint_state_subscriber_.shutdown();
00140 ROS_DEBUG("No longer listening for joint states");
00141 state_monitor_started_ = false;
00142 }
00143 }
00144
00145 std::string planning_scene_monitor::CurrentStateMonitor::getMonitoredTopic() const
00146 {
00147 if (joint_state_subscriber_)
00148 return joint_state_subscriber_.getTopic();
00149 else
00150 return "";
00151 }
00152
00153 bool planning_scene_monitor::CurrentStateMonitor::isPassiveOrMimicDOF(const std::string& dof) const
00154 {
00155 if (robot_model_->hasJointModel(dof))
00156 {
00157 if (robot_model_->getJointModel(dof)->isPassive() || robot_model_->getJointModel(dof)->getMimic())
00158 return true;
00159 }
00160 else
00161 {
00162
00163 std::size_t slash = dof.find_last_of("/");
00164 if (slash != std::string::npos)
00165 {
00166 std::string joint_name = dof.substr(0, slash);
00167 if (robot_model_->hasJointModel(joint_name))
00168 if (robot_model_->getJointModel(joint_name)->isPassive() || robot_model_->getJointModel(joint_name)->getMimic())
00169 return true;
00170 }
00171 }
00172 return false;
00173 }
00174
00175 bool planning_scene_monitor::CurrentStateMonitor::haveCompleteState() const
00176 {
00177 bool result = true;
00178 const std::vector<std::string>& dof = robot_model_->getVariableNames();
00179 boost::mutex::scoped_lock slock(state_update_lock_);
00180 for (std::size_t i = 0; i < dof.size(); ++i)
00181 if (joint_time_.find(dof[i]) == joint_time_.end())
00182 {
00183 if (!isPassiveOrMimicDOF(dof[i]))
00184 {
00185 ROS_DEBUG("Joint variable '%s' has never been updated", dof[i].c_str());
00186 result = false;
00187 }
00188 }
00189 return result;
00190 }
00191
00192 bool planning_scene_monitor::CurrentStateMonitor::haveCompleteState(std::vector<std::string>& missing_states) const
00193 {
00194 bool result = true;
00195 const std::vector<std::string>& dof = robot_model_->getVariableNames();
00196 boost::mutex::scoped_lock slock(state_update_lock_);
00197 for (std::size_t i = 0; i < dof.size(); ++i)
00198 if (joint_time_.find(dof[i]) == joint_time_.end())
00199 if (!isPassiveOrMimicDOF(dof[i]))
00200 {
00201 missing_states.push_back(dof[i]);
00202 result = false;
00203 }
00204 return result;
00205 }
00206
00207 bool planning_scene_monitor::CurrentStateMonitor::haveCompleteState(const ros::Duration& age) const
00208 {
00209 bool result = true;
00210 const std::vector<std::string>& dof = robot_model_->getVariableNames();
00211 ros::Time now = ros::Time::now();
00212 ros::Time old = now - age;
00213 boost::mutex::scoped_lock slock(state_update_lock_);
00214 for (std::size_t i = 0; i < dof.size(); ++i)
00215 {
00216 if (isPassiveOrMimicDOF(dof[i]))
00217 continue;
00218 std::map<std::string, ros::Time>::const_iterator it = joint_time_.find(dof[i]);
00219 if (it == joint_time_.end())
00220 {
00221 ROS_DEBUG("Joint variable '%s' has never been updated", dof[i].c_str());
00222 result = false;
00223 }
00224 else if (it->second < old)
00225 {
00226 ROS_DEBUG("Joint variable '%s' was last updated %0.3lf seconds ago (older than the allowed %0.3lf seconds)",
00227 dof[i].c_str(), (now - it->second).toSec(), age.toSec());
00228 result = false;
00229 }
00230 }
00231 return result;
00232 }
00233
00234 bool planning_scene_monitor::CurrentStateMonitor::haveCompleteState(const ros::Duration& age,
00235 std::vector<std::string>& missing_states) const
00236 {
00237 bool result = true;
00238 const std::vector<std::string>& dof = robot_model_->getVariableNames();
00239 ros::Time now = ros::Time::now();
00240 ros::Time old = now - age;
00241 boost::mutex::scoped_lock slock(state_update_lock_);
00242 for (std::size_t i = 0; i < dof.size(); ++i)
00243 {
00244 if (isPassiveOrMimicDOF(dof[i]))
00245 continue;
00246 std::map<std::string, ros::Time>::const_iterator it = joint_time_.find(dof[i]);
00247 if (it == joint_time_.end())
00248 {
00249 ROS_DEBUG("Joint variable '%s' has never been updated", dof[i].c_str());
00250 missing_states.push_back(dof[i]);
00251 result = false;
00252 }
00253 else if (it->second < old)
00254 {
00255 ROS_DEBUG("Joint variable '%s' was last updated %0.3lf seconds ago (older than the allowed %0.3lf seconds)",
00256 dof[i].c_str(), (now - it->second).toSec(), age.toSec());
00257 missing_states.push_back(dof[i]);
00258 result = false;
00259 }
00260 }
00261 return result;
00262 }
00263
00264 bool planning_scene_monitor::CurrentStateMonitor::waitForCurrentState(const ros::Time t, double wait_time) const
00265 {
00266 ros::WallTime timeout = ros::WallTime::now() + ros::WallDuration(wait_time);
00267 ros::WallDuration busywait(0.1);
00268
00269 boost::mutex::scoped_lock lock(state_update_lock_);
00270 while (current_state_time_ < t)
00271 {
00272 lock.unlock();
00273 busywait.sleep();
00274 if (ros::WallTime::now() >= timeout)
00275 return false;
00276 lock.lock();
00277 }
00278 return true;
00279 }
00280
00281 bool planning_scene_monitor::CurrentStateMonitor::waitForCurrentState(double wait_time) const
00282 {
00283 double slept_time = 0.0;
00284 double sleep_step_s = std::min(0.05, wait_time / 10.0);
00285 ros::Duration sleep_step(sleep_step_s);
00286 while (!haveCompleteState() && slept_time < wait_time)
00287 {
00288 sleep_step.sleep();
00289 slept_time += sleep_step_s;
00290 }
00291 return haveCompleteState();
00292 }
00293
00294 bool planning_scene_monitor::CurrentStateMonitor::waitForCurrentState(const std::string& group, double wait_time) const
00295 {
00296 if (waitForCurrentState(wait_time))
00297 return true;
00298 bool ok = true;
00299
00300
00301 std::vector<std::string> missing_joints;
00302 if (!haveCompleteState(missing_joints))
00303 {
00304 const robot_model::JointModelGroup* jmg = robot_model_->getJointModelGroup(group);
00305 if (jmg)
00306 {
00307 std::set<std::string> mj;
00308 mj.insert(missing_joints.begin(), missing_joints.end());
00309 const std::vector<std::string>& names = jmg->getJointModelNames();
00310 bool ok = true;
00311 for (std::size_t i = 0; ok && i < names.size(); ++i)
00312 if (mj.find(names[i]) != mj.end())
00313 ok = false;
00314 }
00315 else
00316 ok = false;
00317 }
00318 return ok;
00319 }
00320
00321 void planning_scene_monitor::CurrentStateMonitor::jointStateCallback(const sensor_msgs::JointStateConstPtr& joint_state)
00322 {
00323 if (joint_state->name.size() != joint_state->position.size())
00324 {
00325 ROS_ERROR_THROTTLE(1, "State monitor received invalid joint state (number of joint names does not match number of "
00326 "positions)");
00327 return;
00328 }
00329 bool update = false;
00330
00331 {
00332 boost::mutex::scoped_lock _(state_update_lock_);
00333
00334 std::size_t n = joint_state->name.size();
00335 current_state_time_ = joint_state->header.stamp;
00336 for (std::size_t i = 0; i < n; ++i)
00337 {
00338 const robot_model::JointModel* jm = robot_model_->getJointModel(joint_state->name[i]);
00339 if (!jm)
00340 continue;
00341
00342 if (jm->getVariableCount() != 1)
00343 continue;
00344
00345 joint_time_[joint_state->name[i]] = joint_state->header.stamp;
00346
00347 if (robot_state_.getJointPositions(jm)[0] != joint_state->position[i])
00348 {
00349 update = true;
00350 robot_state_.setJointPositions(jm, &(joint_state->position[i]));
00351
00352
00353 if (jm->getType() == robot_model::JointModel::REVOLUTE)
00354 if (static_cast<const robot_model::RevoluteJointModel*>(jm)->isContinuous())
00355 continue;
00356
00357 const robot_model::VariableBounds& b =
00358 jm->getVariableBounds()[0];
00359
00360
00361
00362 if (joint_state->position[i] < b.min_position_ && joint_state->position[i] >= b.min_position_ - error_)
00363 robot_state_.setJointPositions(jm, &b.min_position_);
00364 else if (joint_state->position[i] > b.max_position_ && joint_state->position[i] <= b.max_position_ + error_)
00365 robot_state_.setJointPositions(jm, &b.max_position_);
00366 }
00367 }
00368
00369
00370 if (tf_ && (robot_model_->getRootJoint()->getType() == robot_model::JointModel::PLANAR ||
00371 robot_model_->getRootJoint()->getType() == robot_model::JointModel::FLOATING))
00372 {
00373 const std::string& child_frame = robot_model_->getRootLink()->getName();
00374 const std::string& parent_frame = robot_model_->getModelFrame();
00375
00376 std::string err;
00377 ros::Time tm;
00378 tf::StampedTransform transf;
00379 bool ok = false;
00380 if (tf_->getLatestCommonTime(parent_frame, child_frame, tm, &err) == tf::NO_ERROR)
00381 {
00382 try
00383 {
00384 tf_->lookupTransform(parent_frame, child_frame, tm, transf);
00385 ok = true;
00386 }
00387 catch (tf::TransformException& ex)
00388 {
00389 ROS_ERROR_THROTTLE(1, "Unable to lookup transform from %s to %s. Exception: %s", parent_frame.c_str(),
00390 child_frame.c_str(), ex.what());
00391 }
00392 }
00393 else
00394 ROS_DEBUG_THROTTLE(1, "Unable to lookup transform from %s to %s: no common time.", parent_frame.c_str(),
00395 child_frame.c_str());
00396 if (ok && last_tf_update_ != tm)
00397 {
00398 update = true;
00399 last_tf_update_ = tm;
00400 const std::vector<std::string>& vars = robot_model_->getRootJoint()->getVariableNames();
00401 for (std::size_t j = 0; j < vars.size(); ++j)
00402 joint_time_[vars[j]] = tm;
00403 Eigen::Affine3d eigen_transf;
00404 tf::transformTFToEigen(transf, eigen_transf);
00405 robot_state_.setJointPositions(robot_model_->getRootJoint(), eigen_transf);
00406 }
00407 }
00408 }
00409
00410
00411 if (update)
00412 for (std::size_t i = 0; i < update_callbacks_.size(); ++i)
00413 update_callbacks_[i](joint_state);
00414 }