common_objects.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2012, 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 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 
00035 /* Author: Ioan Sucan */
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   // More specifically, class_loader's static variables might be already destroyed
00066   // while being accessed again in the destructor of the class_loader-based kinematics plugin.
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 }  // namespace planning_interface
00129 }  // namespace moveit


planning_interface
Author(s): Ioan Sucan
autogenerated on Wed Jun 19 2019 19:24:53