kinematic_model_state_monitor.h
Go to the documentation of this file.
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 #ifndef PLANNING_ENVIRONMENT_MONITORS_KINEMATIC_MODEL_STATE_MONITOR_
00038 #define PLANNING_ENVIRONMENT_MONITORS_KINEMATIC_MODEL_STATE_MONITOR_
00039 
00040 #include "planning_environment/models/robot_models.h"
00041 #include <tf/transform_datatypes.h>
00042 #include <tf/transform_listener.h>
00043 #ifndef Q_MOC_RUN
00044 #include <tf/message_filter.h>
00045 #include <message_filters/subscriber.h>
00046 #endif
00047 #include <sensor_msgs/JointState.h>
00048 #include <arm_navigation_msgs/RobotState.h>
00049 #include <boost/bind.hpp>
00050 #include <vector>
00051 #include <string>
00052 #include <map>
00053 #include <planning_models/kinematic_state.h>
00054 
00055 namespace planning_environment
00056 {
00057 
00062 class KinematicModelStateMonitor
00063 {
00064 public:
00065 
00066   KinematicModelStateMonitor(RobotModels *rm, tf::TransformListener *tf) : nh_("~")
00067   {
00068     rm_ = rm;
00069     tf_ = tf;
00070     setupRSM();
00071   }
00072 
00073   virtual ~KinematicModelStateMonitor(void)
00074   {
00075   }
00076 
00078   void startStateMonitor(void);
00079         
00081   void stopStateMonitor(void);
00082         
00084   bool isStateMonitorStarted(void) const
00085   {
00086     return state_monitor_started_;
00087   }     
00088   
00089   void addOnStateUpdateCallback(const boost::function<void(const sensor_msgs::JointStateConstPtr &joint_state)> &callback) {
00090     on_state_update_callback_ = callback;
00091   }
00092 
00094   const planning_models::KinematicModel* getKinematicModel(void) const
00095   {
00096     return kmodel_;
00097   }
00098 
00100   RobotModels* getRobotModels(void) const
00101   {
00102     return rm_;
00103   }
00104 
00106   tf::TransformListener *getTransformListener(void) const
00107   {
00108     return tf_;
00109   }
00110         
00112   bool haveState(void) const
00113   {
00114     return have_joint_state_ && have_pose_;
00115   }
00116         
00118   const ros::Time& lastJointStateUpdate(void) const
00119   {
00120     return last_joint_state_update_;
00121   }
00122 
00124   const ros::Time& lastPoseUpdate(void) const
00125   {
00126     return last_pose_update_;
00127   }
00128         
00130   void waitForState(void) const;
00131 
00133   bool isJointStateUpdated(double sec) const;
00134 
00138   bool isPoseUpdated(double sec) const;
00139         
00141   void printRobotState(void) const;
00142   
00144   void setStateValuesFromCurrentValues(planning_models::KinematicState& state) const;
00145 
00146   bool getCurrentRobotState(arm_navigation_msgs::RobotState &robot_state) const;
00147 
00148   bool setKinematicStateToTime(const ros::Time& time,
00149                                planning_models::KinematicState& state) const;
00150 
00151   bool getCachedJointStateValues(const ros::Time& time, std::map<std::string, double>& ret_map) const;
00152 
00153   bool allJointsUpdated(ros::Duration dur = ros::Duration()) const;
00154 
00155   //need to pass by value for thread safety
00156   std::map<std::string, double> getCurrentJointStateValues() const {
00157     current_joint_values_lock_.lock();
00158     std::map<std::string, double> ret_values = current_joint_state_map_;
00159     current_joint_values_lock_.unlock();
00160     return ret_values;
00161   }
00162 
00163 protected:
00164 
00165   void setupRSM(void);
00166   void jointStateCallback(const sensor_msgs::JointStateConstPtr &joint_state);
00167 
00168   std::list<std::pair<ros::Time, std::map<std::string, double> > >joint_state_map_cache_;
00169 
00170   std::map<std::string, ros::Time> last_joint_update_;
00171   std::map<std::string, double> current_joint_state_map_;
00172 
00173   mutable boost::recursive_mutex current_joint_values_lock_;
00174 
00175   double joint_state_cache_time_;
00176   double joint_state_cache_allowed_difference_;
00177 
00178   RobotModels *rm_;
00179       
00180   const planning_models::KinematicModel* kmodel_;
00181 
00182   bool state_monitor_started_;
00183   bool printed_out_of_date_;
00184         
00185   ros::NodeHandle nh_;
00186   ros::NodeHandle root_handle_;
00187   ros::Subscriber joint_state_subscriber_;
00188   tf::TransformListener *tf_;
00189 
00190   tf::Pose pose_;
00191   std::string robot_frame_;
00192 
00193   boost::function<void(const sensor_msgs::JointStateConstPtr &joint_state)> on_state_update_callback_;
00194 
00195   bool have_pose_;
00196   bool have_joint_state_;
00197   ros::Time last_joint_state_update_;
00198   ros::Time last_pose_update_;
00199 };
00200 
00201 }
00202 
00203 #endif


planning_environment
Author(s): Ioan Sucan
autogenerated on Mon Dec 2 2013 12:34:43