planning_environment: planning_environment: kinematic_model_state

00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2008, Willow Garage, Inc.
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00037 #include "planning_environment/monitors/kinematic_model_state_monitor.h"
00038 #include "planning_environment/util/construct_object.h"
00039 #include "planning_environment/models/model_utils.h"
00040 #include <angles/angles.h>
00041 #include <sstream>
00042 
00043 void planning_environment::KinematicModelStateMonitor::setupRSM(void)
00044 {
00045   state_monitor_started_ = false;
00046   on_state_update_callback_ = NULL;
00047   have_pose_ = have_joint_state_ = false;
00048     
00049   printed_out_of_date_ = false;
00050   if (rm_->loadedModels())
00051   {
00052     kmodel_ = rm_->getKinematicModel();
00053     robot_frame_ = rm_->getRobotFrameId();
00054     ROS_INFO("Robot frame is '%s'", robot_frame_.c_str());
00055     startStateMonitor();
00056   } else {
00057     ROS_INFO("Can't start state monitor yet");
00058   }
00059     
00060   nh_.param<double>("joint_state_cache_time", joint_state_cache_time_, 2.0);
00061   nh_.param<double>("joint_state_cache_allowed_difference", joint_state_cache_allowed_difference_, .25);
00062 
00063 }
00064 
00065 void planning_environment::KinematicModelStateMonitor::startStateMonitor(void)
00066 {   
00067   if (state_monitor_started_)
00068     return;
00069     
00070   if (rm_->loadedModels())
00071   {
00072     joint_state_subscriber_ = root_handle_.subscribe("joint_states", 25, &KinematicModelStateMonitor::jointStateCallback, this);
00073     ROS_DEBUG("Listening to joint states");
00074         
00075   }
00076   state_monitor_started_ = true;
00077 }
00078 
00079 void planning_environment::KinematicModelStateMonitor::stopStateMonitor(void)
00080 {
00081   if (!state_monitor_started_)
00082     return;
00083     
00084   joint_state_subscriber_.shutdown();
00085 
00086   joint_state_map_cache_.clear();
00087     
00088   ROS_DEBUG("Kinematic state is no longer being monitored");
00089     
00090   state_monitor_started_ = false;
00091 }
00092 
00093 void planning_environment::KinematicModelStateMonitor::jointStateCallback(const sensor_msgs::JointStateConstPtr &joint_state)
00094 {
00095   //bool change = !have_joint_state_;
00096 
00097   planning_models::KinematicState state(kmodel_);
00098 
00099   current_joint_values_lock_.lock();
00100 
00101   state.setKinematicState(current_joint_state_map_);
00102 
00103   static bool first_time = true;
00104 
00105   unsigned int n = joint_state->name.size();
00106   if (joint_state->name.size() != joint_state->position.size())
00107   {
00108     ROS_ERROR("Planning environment received invalid joint state");
00109     current_joint_values_lock_.unlock();
00110     return;
00111   }
00112   
00113   std::map<std::string, double> joint_state_map;
00114   for (unsigned int i = 0 ; i < n ; ++i)
00115   {
00116     joint_state_map[joint_state->name[i]] = joint_state->position[i];
00117   }
00118 
00119   std::vector<planning_models::KinematicState::JointState*>& joint_state_vector = state.getJointStateVector();
00120   
00121   for(std::vector<planning_models::KinematicState::JointState*>::iterator it = joint_state_vector.begin();
00122       it != joint_state_vector.end();
00123       it++) {
00124     bool tfSets = false;
00125     bool joint_state_sets = false;
00126     //see if we need to update any transforms
00127     std::string parent_frame_id = (*it)->getParentFrameId();
00128     std::string child_frame_id = (*it)->getChildFrameId();
00129     if(!parent_frame_id.empty() && !child_frame_id.empty()) {
00130       std::string err;
00131       ros::Time tm;
00132       tf::StampedTransform transf;
00133       bool ok = false;
00134       if (tf_->getLatestCommonTime(parent_frame_id, child_frame_id, tm, &err) == tf::NO_ERROR) {
00135         ok = true;
00136         try
00137         {
00138           tf_->lookupTransform(parent_frame_id, child_frame_id, tm, transf);
00139         }
00140         catch(tf::TransformException& ex)
00141         {
00142           ROS_ERROR("Unable to lookup transform from %s to %s.  Exception: %s", parent_frame_id.c_str(), child_frame_id.c_str(), ex.what());
00143           ok = false;
00144         }
00145       } else {
00146         ROS_DEBUG("Unable to lookup transform from %s to %s: no common time.", parent_frame_id.c_str(), child_frame_id.c_str());
00147         ok = false;
00148       }
00149       if(ok) {
00150         tfSets = (*it)->setJointStateValues(transf);
00151         if(tfSets) {
00152           const std::vector<std::string>& joint_state_names = (*it)->getJointStateNameOrder();
00153           for(std::vector<std::string>::const_iterator it = joint_state_names.begin();
00154               it != joint_state_names.end();
00155               it++) {
00156             last_joint_update_[*it] = tm;
00157           }
00158         }
00159         // btTransform transf = getKinematicModel()->getRoot()->variable_transform;
00160         // ROS_INFO_STREAM("transform is to " << transf.getRotation().x() << " " 
00161         //                 << transf.getRotation().y() << " z " << transf.getRotation().z()
00162         //                 << " w " << transf.getRotation().w());
00163         have_pose_ = tfSets;
00164         last_pose_update_ = tm;
00165       }
00166     }
00167     //now we update from joint state
00168     joint_state_sets = (*it)->setJointStateValues(joint_state_map);
00169     if(joint_state_sets) {
00170       const std::vector<std::string>& joint_state_names = (*it)->getJointStateNameOrder();
00171       for(std::vector<std::string>::const_iterator it = joint_state_names.begin();
00172           it != joint_state_names.end();
00173           it++) {
00174         last_joint_update_[*it] = joint_state->header.stamp;
00175       }
00176     }
00177   }
00178   
00179   state.updateKinematicLinks();
00180   state.getKinematicStateValues(current_joint_state_map_);
00181 
00182   if(allJointsUpdated()) {
00183     have_joint_state_ = true;
00184     last_joint_state_update_ = joint_state->header.stamp;
00185     
00186     if(!joint_state_map_cache_.empty()) {
00187       if(joint_state->header.stamp-joint_state_map_cache_.back().first > ros::Duration(joint_state_cache_allowed_difference_)) {
00188         ROS_DEBUG_STREAM("Introducing joint state cache sparsity time of " << (joint_state->header.stamp-joint_state_map_cache_.back().first).toSec());
00189       }
00190     }
00191 
00192     joint_state_map_cache_.push_back(std::pair<ros::Time, std::map<std::string, double> >(joint_state->header.stamp,
00193                                                                                           current_joint_state_map_));
00194   } 
00195 
00196   if(have_joint_state_) {
00197     while(1) {
00198       if(joint_state_map_cache_.empty()) {
00199         ROS_WARN("Empty joint state map cache");
00200         break;
00201       }
00202       if((ros::Time::now()-joint_state_map_cache_.front().first) > ros::Duration(joint_state_cache_time_)) {
00203         joint_state_map_cache_.pop_front();
00204       } else {
00205         break;
00206       }
00207     }
00208   }
00209     
00210   first_time = false;
00211 
00212   if(!allJointsUpdated(ros::Duration(1.0))) {
00213     if(!printed_out_of_date_) {
00214       ROS_WARN_STREAM("Got joint state update but did not update some joints for more than 1 second.  Turn on DEBUG for more info");
00215       printed_out_of_date_ = true;
00216     }
00217   } else {
00218     printed_out_of_date_ = false;
00219   }
00220   if(on_state_update_callback_ != NULL) {
00221     on_state_update_callback_(joint_state);
00222   }
00223   current_joint_values_lock_.unlock();
00224 }
00225 
00226 bool planning_environment::KinematicModelStateMonitor::allJointsUpdated(ros::Duration allowed_dur) const {
00227   current_joint_values_lock_.lock();
00228   bool ret = true;
00229   for(std::map<std::string, double>::const_iterator it = current_joint_state_map_.begin();
00230       it != current_joint_state_map_.end();
00231       it++) {
00232     if(last_joint_update_.find(it->first) == last_joint_update_.end()) {
00233       ROS_DEBUG_STREAM("Joint " << it->first << " not yet updated");
00234       ret = false;
00235       continue;
00236     }
00237     if(allowed_dur != ros::Duration()) {
00238       ros::Duration dur = ros::Time::now()-last_joint_update_.find(it->first)->second; 
00239       if(dur > allowed_dur) {
00240         ROS_DEBUG_STREAM("Joint " << it->first << " last updated " << dur.toSec() << " where allowed duration is " << allowed_dur.toSec());
00241         ret = false;
00242         continue;
00243       }
00244     }
00245   }
00246   current_joint_values_lock_.unlock();
00247   return ret;
00248 }
00249 
00250 void planning_environment::KinematicModelStateMonitor::setStateValuesFromCurrentValues(planning_models::KinematicState& state) const
00251 {
00252   current_joint_values_lock_.lock();
00253   state.setKinematicState(current_joint_state_map_);
00254   current_joint_values_lock_.unlock();
00255 }
00256 
00257 
00258 bool planning_environment::KinematicModelStateMonitor::getCurrentRobotState(arm_navigation_msgs::RobotState& robot_state) const
00259 {
00260   planning_models::KinematicState state(kmodel_);
00261   setStateValuesFromCurrentValues(state);
00262   convertKinematicStateToRobotState(state, last_joint_state_update_, rm_->getWorldFrameId(), robot_state);
00263   return true;
00264 }
00265 
00266 bool planning_environment::KinematicModelStateMonitor::setKinematicStateToTime(const ros::Time& time,
00267                                                                                planning_models::KinematicState& state) const
00268 {
00269   std::map<std::string, double> joint_value_map;
00270   if(!getCachedJointStateValues(time, joint_value_map)) {
00271     return false;
00272   }
00273   state.setKinematicState(joint_value_map);
00274   return true;
00275 }
00276 
00277 bool planning_environment::KinematicModelStateMonitor::getCachedJointStateValues(const ros::Time& time, std::map<std::string, double>& ret_map) const {
00278   
00279   current_joint_values_lock_.lock();
00280 
00281   //first we check the front and backs of the cache versus the time for error states
00282   if(time-ros::Duration(joint_state_cache_allowed_difference_) > joint_state_map_cache_.back().first) {
00283     ROS_WARN("Asking for time substantially newer than that contained in cache");
00284     current_joint_values_lock_.unlock(); 
00285     return false;
00286   }
00287   if(time+ros::Duration(joint_state_cache_allowed_difference_) < joint_state_map_cache_.front().first) {
00288     ROS_WARN_STREAM("Asking for time substantially older than that contained in cache " << time.toSec() << " " << joint_state_map_cache_.front().first.toSec());
00289     current_joint_values_lock_.unlock();
00290     return false;
00291   }
00292   
00293   //then we check if oldest or newest is being requested
00294   if(time < joint_state_map_cache_.front().first) {
00295     ret_map = joint_state_map_cache_.front().second;
00296   } else if (time > joint_state_map_cache_.back().first) {
00297     ret_map = joint_state_map_cache_.back().second;
00298   } else {
00299     std::list<std::pair<ros::Time, std::map<std::string, double> > >::const_reverse_iterator next = ++joint_state_map_cache_.rbegin();
00300     std::list<std::pair<ros::Time, std::map<std::string, double> > >::const_reverse_iterator cur = joint_state_map_cache_.rbegin();
00301     while(next != joint_state_map_cache_.rbegin()) {
00302       if(time < cur->first && time > next->first) {
00303         ros::Duration ear_diff = time - next->first;
00304         ros::Duration lat_diff = cur->first - time;
00305         if(ear_diff > ros::Duration(joint_state_cache_allowed_difference_) &&
00306            lat_diff > ros::Duration(joint_state_cache_allowed_difference_)) {
00307           ROS_WARN("Asking for time in joint state area that's too sparse");
00308           current_joint_values_lock_.unlock();
00309           return false;
00310         } else {
00311           if(ear_diff > lat_diff) {
00312             ROS_DEBUG_STREAM("Got time later within " << lat_diff.toSec());
00313             ret_map = cur->second;
00314             break;
00315           } else {
00316             ROS_DEBUG_STREAM("Got time earlier within " << ear_diff.toSec());
00317             ret_map = next->second;
00318             break;
00319           }
00320         }
00321       }
00322       next++;
00323       cur++;
00324     }
00325   }
00326   current_joint_values_lock_.unlock();
00327   return true;
00328 }
00329 
00330 void planning_environment::KinematicModelStateMonitor::waitForState(void) const
00331 {
00332   int s = 0;
00333   while (nh_.ok() && !haveState())
00334   {
00335     if (s == 0)
00336     {
00337       ROS_INFO("Waiting for robot state ...");
00338       if (!have_joint_state_)
00339         ROS_INFO("Waiting for joint state ...");
00340     }
00341     s = (s + 1) % 40;
00342     ros::spinOnce();
00343     ros::Duration().fromSec(0.05).sleep();
00344   }
00345   if (haveState())
00346     ROS_INFO("Robot state received!");
00347 }
00348 
00349 bool planning_environment::KinematicModelStateMonitor::isJointStateUpdated(double sec) const
00350 {  
00351   //ROS_ERROR_STREAM("Here " << sec << " time " << last_joint_state_update_.toSec() << " now " << (ros::Time::now().toSec()));
00352 
00353   //three cases
00354   //1. interval to small - less than 10us is considered 0
00355   if(sec < 1e-5) 
00356   {
00357     return false;
00358   }
00359 
00360   //ROS_ERROR_STREAM("cond 1 " << (sec > 1e-5) << " cond 2 " << (last_joint_state_update_ > ros::TIME_MIN) << " cond 3 " << (ros::Time::now() < ros::Time(sec)));
00361 
00362   //2. it hasn't yet been a full second interval but we've updated
00363   if(sec > 1e-5 && last_joint_state_update_ > ros::TIME_MIN && ros::Time::now() < ros::Time(sec)) 
00364   {
00365     return true;
00366   }
00367 
00368   ROS_DEBUG("Last joint update %g interval begins %g", last_joint_state_update_.toSec(), (ros::Time::now()-ros::Duration(sec)).toSec());
00369 
00370   //3. Been longer than sec interval, so we check that the update has happened in the indicated interval
00371   if (last_joint_state_update_ < ros::Time::now()-ros::Duration(sec)) 
00372   {
00373     return false;
00374   }
00375 
00376   return true;
00377 }
00378 
00379 bool planning_environment::KinematicModelStateMonitor::isPoseUpdated(double sec) const
00380 {  
00381 
00382   if(sec < 1e-5) 
00383   {
00384     return false;
00385   }
00386 
00387   //2. it hasn't yet been a full second interval but we've updated
00388   if(last_pose_update_ > ros::TIME_MIN && ros::Time::now() < ros::Time(sec)) 
00389   {
00390     return true;
00391   }
00392 
00393   //3. Been longer than sec interval, so we check that the update has happened in the indicated interval
00394   if (last_pose_update_ < ros::Time::now()-ros::Duration(sec)) 
00395   {
00396     return false;
00397   }
00398 
00399   return true;
00400 }
kinematic_model_state_monitor.cpp
