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 static SharedStorage storage;
00065 return storage;
00066 }
00067 }
00068
00069 namespace moveit
00070 {
00071 namespace planning_interface
00072 {
00073
00074 boost::shared_ptr<tf::Transformer> getSharedTF()
00075 {
00076 SharedStorage &s = getSharedStorage();
00077 boost::mutex::scoped_lock slock(s.lock_);
00078 if (!s.tf_)
00079 s.tf_.reset(new tf::TransformListener());
00080 return s.tf_;
00081 }
00082
00083 robot_model::RobotModelConstPtr getSharedRobotModel(const std::string &robot_description)
00084 {
00085 SharedStorage &s = getSharedStorage();
00086 boost::mutex::scoped_lock slock(s.lock_);
00087 if (s.model_loaders_.find(robot_description) != s.model_loaders_.end())
00088 return s.model_loaders_[robot_description]->getModel();
00089 else
00090 {
00091 robot_model_loader::RobotModelLoader::Options opt(robot_description);
00092 opt.load_kinematics_solvers_ = true;
00093 robot_model_loader::RobotModelLoaderPtr loader(new robot_model_loader::RobotModelLoader(opt));
00094 s.model_loaders_[robot_description] = loader;
00095 return loader->getModel();
00096 }
00097 }
00098
00099 planning_scene_monitor::CurrentStateMonitorPtr getSharedStateMonitor(const robot_model::RobotModelConstPtr &kmodel, const boost::shared_ptr<tf::Transformer> &tf)
00100 {
00101 SharedStorage &s = getSharedStorage();
00102 boost::mutex::scoped_lock slock(s.lock_);
00103 if (s.state_monitors_.find(kmodel->getName()) != s.state_monitors_.end())
00104 return s.state_monitors_[kmodel->getName()];
00105 else
00106 {
00107 planning_scene_monitor::CurrentStateMonitorPtr monitor(new planning_scene_monitor::CurrentStateMonitor(kmodel, tf));
00108 s.state_monitors_[kmodel->getName()] = monitor;
00109 return monitor;
00110 }
00111 }
00112
00113 }
00114 }