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