Go to the documentation of this file.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/common_planning_interface_objects/common_objects.h>
00038 #include <moveit/robot_model_loader/robot_model_loader.h>
00039 #include <tf/transform_listener.h>
00040
00041 namespace
00042 {
00043 struct SharedStorage
00044 {
00045 SharedStorage()
00046 {
00047 }
00048
00049 ~SharedStorage()
00050 {
00051 tf_.reset();
00052 state_monitors_.clear();
00053 model_loaders_.clear();
00054 }
00055
00056 boost::mutex lock_;
00057 boost::shared_ptr<tf::Transformer> tf_;
00058 std::map<std::string, robot_model_loader::RobotModelLoaderPtr> model_loaders_;
00059 std::map<std::string, planning_scene_monitor::CurrentStateMonitorPtr> state_monitors_;
00060 };
00061
00062 SharedStorage& getSharedStorage()
00063 {
00064 #if 0 // destruction of static storage interferes with static destruction in class_loader
00065
00066
00067 static SharedStorage storage;
00068 return storage;
00069 #else // thus avoid destruction at all (until class_loader is fixed)
00070 static SharedStorage* storage = new SharedStorage;
00071 return *storage;
00072 #endif
00073 }
00074 }
00075
00076 namespace moveit
00077 {
00078 namespace planning_interface
00079 {
00080 boost::shared_ptr<tf::Transformer> getSharedTF()
00081 {
00082 SharedStorage& s = getSharedStorage();
00083 boost::mutex::scoped_lock slock(s.lock_);
00084 if (!s.tf_)
00085 s.tf_.reset(new tf::TransformListener());
00086 return s.tf_;
00087 }
00088
00089 robot_model::RobotModelConstPtr getSharedRobotModel(const std::string& robot_description)
00090 {
00091 SharedStorage& s = getSharedStorage();
00092 boost::mutex::scoped_lock slock(s.lock_);
00093 if (s.model_loaders_.find(robot_description) != s.model_loaders_.end())
00094 return s.model_loaders_[robot_description]->getModel();
00095 else
00096 {
00097 robot_model_loader::RobotModelLoader::Options opt(robot_description);
00098 opt.load_kinematics_solvers_ = true;
00099 robot_model_loader::RobotModelLoaderPtr loader(new robot_model_loader::RobotModelLoader(opt));
00100 s.model_loaders_[robot_description] = loader;
00101 return loader->getModel();
00102 }
00103 }
00104
00105 planning_scene_monitor::CurrentStateMonitorPtr getSharedStateMonitor(const robot_model::RobotModelConstPtr& kmodel,
00106 const boost::shared_ptr<tf::Transformer>& tf)
00107 {
00108 return getSharedStateMonitor(kmodel, tf, ros::NodeHandle());
00109 }
00110
00111 planning_scene_monitor::CurrentStateMonitorPtr getSharedStateMonitor(const robot_model::RobotModelConstPtr& kmodel,
00112 const boost::shared_ptr<tf::Transformer>& tf,
00113 ros::NodeHandle nh)
00114 {
00115 SharedStorage& s = getSharedStorage();
00116 boost::mutex::scoped_lock slock(s.lock_);
00117 if (s.state_monitors_.find(kmodel->getName()) != s.state_monitors_.end())
00118 return s.state_monitors_[kmodel->getName()];
00119 else
00120 {
00121 planning_scene_monitor::CurrentStateMonitorPtr monitor(
00122 new planning_scene_monitor::CurrentStateMonitor(kmodel, tf, nh));
00123 s.state_monitors_[kmodel->getName()] = monitor;
00124 return monitor;
00125 }
00126 }
00127
00128 }
00129 }