planning_scene_monitor.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011, 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/planning_scene_monitor/planning_scene_monitor.h>
00038 #include <moveit/robot_model_loader/robot_model_loader.h>
00039 #include <moveit/exceptions/exceptions.h>
00040 #include <moveit_msgs/GetPlanningScene.h>
00041 
00042 #include <dynamic_reconfigure/server.h>
00043 #include <moveit_ros_planning/PlanningSceneMonitorDynamicReconfigureConfig.h>
00044 #include <tf_conversions/tf_eigen.h>
00045 #include <moveit/profiler/profiler.h>
00046 
00047 namespace planning_scene_monitor
00048 {
00049 using namespace moveit_ros_planning;
00050 
00051 static const std::string LOGNAME = "planning_scene_monitor";
00052 
00053 class PlanningSceneMonitor::DynamicReconfigureImpl
00054 {
00055 public:
00056   DynamicReconfigureImpl(PlanningSceneMonitor* owner)
00057     : owner_(owner), dynamic_reconfigure_server_(ros::NodeHandle(decideNamespace(owner->getName())))
00058   {
00059     dynamic_reconfigure_server_.setCallback(
00060         boost::bind(&DynamicReconfigureImpl::dynamicReconfigureCallback, this, _1, _2));
00061   }
00062 
00063 private:
00064   // make sure we do not advertise the same service multiple times, in case we use multiple PlanningSceneMonitor
00065   // instances in a process
00066   static std::string decideNamespace(const std::string& name)
00067   {
00068     std::string ns = "~/" + name;
00069     std::replace(ns.begin(), ns.end(), ' ', '_');
00070     std::transform(ns.begin(), ns.end(), ns.begin(), ::tolower);
00071     if (ros::service::exists(ns + "/set_parameters", false))
00072     {
00073       unsigned int c = 1;
00074       while (ros::service::exists(ns + boost::lexical_cast<std::string>(c) + "/set_parameters", false))
00075         c++;
00076       ns += boost::lexical_cast<std::string>(c);
00077     }
00078     return ns;
00079   }
00080 
00081   void dynamicReconfigureCallback(PlanningSceneMonitorDynamicReconfigureConfig& config, uint32_t level)
00082   {
00083     PlanningSceneMonitor::SceneUpdateType event = PlanningSceneMonitor::UPDATE_NONE;
00084     if (config.publish_geometry_updates)
00085       event = (PlanningSceneMonitor::SceneUpdateType)((int)event | (int)PlanningSceneMonitor::UPDATE_GEOMETRY);
00086     if (config.publish_state_updates)
00087       event = (PlanningSceneMonitor::SceneUpdateType)((int)event | (int)PlanningSceneMonitor::UPDATE_STATE);
00088     if (config.publish_transforms_updates)
00089       event = (PlanningSceneMonitor::SceneUpdateType)((int)event | (int)PlanningSceneMonitor::UPDATE_TRANSFORMS);
00090     if (config.publish_planning_scene)
00091     {
00092       owner_->setPlanningScenePublishingFrequency(config.publish_planning_scene_hz);
00093       owner_->startPublishingPlanningScene(event);
00094     }
00095     else
00096       owner_->stopPublishingPlanningScene();
00097   }
00098 
00099   PlanningSceneMonitor* owner_;
00100   dynamic_reconfigure::Server<PlanningSceneMonitorDynamicReconfigureConfig> dynamic_reconfigure_server_;
00101 };
00102 }
00103 
00104 const std::string planning_scene_monitor::PlanningSceneMonitor::DEFAULT_JOINT_STATES_TOPIC = "joint_states";
00105 const std::string planning_scene_monitor::PlanningSceneMonitor::DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC =
00106     "attached_collision_object";
00107 const std::string planning_scene_monitor::PlanningSceneMonitor::DEFAULT_COLLISION_OBJECT_TOPIC = "collision_object";
00108 const std::string planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_WORLD_TOPIC = "planning_scene_"
00109                                                                                                      "world";
00110 const std::string planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_TOPIC = "planning_scene";
00111 const std::string planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_SERVICE = "get_planning_scene";
00112 const std::string planning_scene_monitor::PlanningSceneMonitor::MONITORED_PLANNING_SCENE_TOPIC = "monitored_planning_"
00113                                                                                                  "scene";
00114 
00115 planning_scene_monitor::PlanningSceneMonitor::PlanningSceneMonitor(const std::string& robot_description,
00116                                                                    const boost::shared_ptr<tf::Transformer>& tf,
00117                                                                    const std::string& name)
00118   : monitor_name_(name), nh_("~"), tf_(tf)
00119 {
00120   rm_loader_.reset(new robot_model_loader::RobotModelLoader(robot_description));
00121   initialize(planning_scene::PlanningScenePtr());
00122 }
00123 
00124 planning_scene_monitor::PlanningSceneMonitor::PlanningSceneMonitor(const planning_scene::PlanningScenePtr& scene,
00125                                                                    const std::string& robot_description,
00126                                                                    const boost::shared_ptr<tf::Transformer>& tf,
00127                                                                    const std::string& name)
00128   : monitor_name_(name), nh_("~"), tf_(tf)
00129 {
00130   rm_loader_.reset(new robot_model_loader::RobotModelLoader(robot_description));
00131   initialize(scene);
00132 }
00133 
00134 planning_scene_monitor::PlanningSceneMonitor::PlanningSceneMonitor(
00135     const robot_model_loader::RobotModelLoaderPtr& rm_loader, const boost::shared_ptr<tf::Transformer>& tf,
00136     const std::string& name)
00137   : monitor_name_(name), nh_("~"), tf_(tf), rm_loader_(rm_loader)
00138 {
00139   initialize(planning_scene::PlanningScenePtr());
00140 }
00141 
00142 planning_scene_monitor::PlanningSceneMonitor::PlanningSceneMonitor(
00143     const planning_scene::PlanningScenePtr& scene, const robot_model_loader::RobotModelLoaderPtr& rm_loader,
00144     const boost::shared_ptr<tf::Transformer>& tf, const std::string& name)
00145   : monitor_name_(name), nh_("~"), tf_(tf), rm_loader_(rm_loader)
00146 {
00147   initialize(scene);
00148 }
00149 
00150 planning_scene_monitor::PlanningSceneMonitor::~PlanningSceneMonitor()
00151 {
00152   if (scene_)
00153   {
00154     scene_->setCollisionObjectUpdateCallback(collision_detection::World::ObserverCallbackFn());
00155     scene_->setAttachedBodyUpdateCallback(robot_state::AttachedBodyCallback());
00156   }
00157   stopPublishingPlanningScene();
00158   stopStateMonitor();
00159   stopWorldGeometryMonitor();
00160   stopSceneMonitor();
00161   delete reconfigure_impl_;
00162   current_state_monitor_.reset();
00163   scene_const_.reset();
00164   scene_.reset();
00165   parent_scene_.reset();
00166   robot_model_.reset();
00167   rm_loader_.reset();
00168 }
00169 
00170 void planning_scene_monitor::PlanningSceneMonitor::initialize(const planning_scene::PlanningScenePtr& scene)
00171 {
00172   moveit::tools::Profiler::ScopedStart prof_start;
00173   moveit::tools::Profiler::ScopedBlock prof_block("PlanningSceneMonitor::initialize");
00174 
00175   if (monitor_name_.empty())
00176     monitor_name_ = "planning_scene_monitor";
00177   robot_description_ = rm_loader_->getRobotDescription();
00178   if (rm_loader_->getModel())
00179   {
00180     robot_model_ = rm_loader_->getModel();
00181     scene_ = scene;
00182     collision_loader_.setupScene(nh_, scene_);
00183     scene_const_ = scene_;
00184     if (!scene_)
00185     {
00186       try
00187       {
00188         scene_.reset(new planning_scene::PlanningScene(rm_loader_->getModel()));
00189         collision_loader_.setupScene(nh_, scene_);
00190         scene_const_ = scene_;
00191         configureCollisionMatrix(scene_);
00192         configureDefaultPadding();
00193 
00194         scene_->getCollisionRobotNonConst()->setPadding(default_robot_padd_);
00195         scene_->getCollisionRobotNonConst()->setScale(default_robot_scale_);
00196         for (std::map<std::string, double>::iterator it = default_robot_link_padd_.begin();
00197              it != default_robot_link_padd_.end(); ++it)
00198         {
00199           scene_->getCollisionRobotNonConst()->setLinkPadding(it->first, it->second);
00200         }
00201         for (std::map<std::string, double>::iterator it = default_robot_link_scale_.begin();
00202              it != default_robot_link_scale_.end(); ++it)
00203         {
00204           scene_->getCollisionRobotNonConst()->setLinkScale(it->first, it->second);
00205         }
00206         scene_->propogateRobotPadding();
00207       }
00208       catch (moveit::ConstructException& e)
00209       {
00210         ROS_ERROR_NAMED(LOGNAME, "Configuration of planning scene failed");
00211         scene_.reset();
00212         scene_const_ = scene_;
00213       }
00214     }
00215     if (scene_)
00216     {
00217       scene_->setAttachedBodyUpdateCallback(
00218           boost::bind(&PlanningSceneMonitor::currentStateAttachedBodyUpdateCallback, this, _1, _2));
00219       scene_->setCollisionObjectUpdateCallback(
00220           boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, _1, _2));
00221     }
00222   }
00223   else
00224   {
00225     ROS_ERROR_NAMED(LOGNAME, "Robot model not loaded");
00226   }
00227 
00228   publish_planning_scene_frequency_ = 2.0;
00229   new_scene_update_ = UPDATE_NONE;
00230 
00231   last_update_time_ = ros::Time::now();
00232   last_state_update_ = ros::WallTime::now();
00233   dt_state_update_ = ros::WallDuration(0.1);
00234 
00235   double temp_wait_time = 0.05;
00236 
00237   if (!robot_description_.empty())
00238     nh_.param(robot_description_ + "_planning/shape_transform_cache_lookup_wait_time", temp_wait_time, temp_wait_time);
00239 
00240   shape_transform_cache_lookup_wait_time_ = ros::Duration(temp_wait_time);
00241 
00242   state_update_pending_ = false;
00243   state_update_timer_ = nh_.createWallTimer(dt_state_update_, &PlanningSceneMonitor::stateUpdateTimerCallback, this,
00244                                             false,   // not a oneshot timer
00245                                             false);  // do not start the timer yet
00246 
00247   reconfigure_impl_ = new DynamicReconfigureImpl(this);
00248 }
00249 
00250 void planning_scene_monitor::PlanningSceneMonitor::monitorDiffs(bool flag)
00251 {
00252   if (scene_)
00253   {
00254     if (flag)
00255     {
00256       boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
00257       if (scene_)
00258       {
00259         scene_->setAttachedBodyUpdateCallback(robot_state::AttachedBodyCallback());
00260         scene_->setCollisionObjectUpdateCallback(collision_detection::World::ObserverCallbackFn());
00261         scene_->decoupleParent();
00262         parent_scene_ = scene_;
00263         scene_ = parent_scene_->diff();
00264         scene_const_ = scene_;
00265         scene_->setAttachedBodyUpdateCallback(
00266             boost::bind(&PlanningSceneMonitor::currentStateAttachedBodyUpdateCallback, this, _1, _2));
00267         scene_->setCollisionObjectUpdateCallback(
00268             boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, _1, _2));
00269       }
00270     }
00271     else
00272     {
00273       if (publish_planning_scene_)
00274       {
00275         ROS_WARN_NAMED(LOGNAME, "Diff monitoring was stopped while publishing planning scene diffs. "
00276                                 "Stopping planning scene diff publisher");
00277         stopPublishingPlanningScene();
00278       }
00279       {
00280         boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
00281         if (scene_)
00282         {
00283           scene_->decoupleParent();
00284           parent_scene_.reset();
00285           // remove the '+' added by .diff() at the end of the scene name
00286           if (!scene_->getName().empty())
00287           {
00288             if (scene_->getName()[scene_->getName().length() - 1] == '+')
00289               scene_->setName(scene_->getName().substr(0, scene_->getName().length() - 1));
00290           }
00291         }
00292       }
00293     }
00294   }
00295 }
00296 
00297 void planning_scene_monitor::PlanningSceneMonitor::stopPublishingPlanningScene()
00298 {
00299   if (publish_planning_scene_)
00300   {
00301     boost::scoped_ptr<boost::thread> copy;
00302     copy.swap(publish_planning_scene_);
00303     new_scene_update_condition_.notify_all();
00304     copy->join();
00305     monitorDiffs(false);
00306     planning_scene_publisher_.shutdown();
00307     ROS_INFO_NAMED(LOGNAME, "Stopped publishing maintained planning scene.");
00308   }
00309 }
00310 
00311 void planning_scene_monitor::PlanningSceneMonitor::startPublishingPlanningScene(SceneUpdateType update_type,
00312                                                                                 const std::string& planning_scene_topic)
00313 {
00314   publish_update_types_ = update_type;
00315   if (!publish_planning_scene_ && scene_)
00316   {
00317     planning_scene_publisher_ = nh_.advertise<moveit_msgs::PlanningScene>(planning_scene_topic, 100, false);
00318     ROS_INFO_NAMED(LOGNAME, "Publishing maintained planning scene on '%s'", planning_scene_topic.c_str());
00319     monitorDiffs(true);
00320     publish_planning_scene_.reset(new boost::thread(boost::bind(&PlanningSceneMonitor::scenePublishingThread, this)));
00321   }
00322 }
00323 
00324 void planning_scene_monitor::PlanningSceneMonitor::scenePublishingThread()
00325 {
00326   ROS_DEBUG_NAMED(LOGNAME, "Started scene publishing thread ...");
00327 
00328   // publish the full planning scene
00329   moveit_msgs::PlanningScene msg;
00330   {
00331     occupancy_map_monitor::OccMapTree::ReadLock lock;
00332     if (octomap_monitor_)
00333       lock = octomap_monitor_->getOcTreePtr()->reading();
00334     scene_->getPlanningSceneMsg(msg);
00335   }
00336   planning_scene_publisher_.publish(msg);
00337   ROS_DEBUG_NAMED(LOGNAME, "Published the full planning scene: '%s'", msg.name.c_str());
00338 
00339   do
00340   {
00341     bool publish_msg = false;
00342     bool is_full = false;
00343     ros::Rate rate(publish_planning_scene_frequency_);
00344     {
00345       boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
00346       while (new_scene_update_ == UPDATE_NONE && publish_planning_scene_)
00347         new_scene_update_condition_.wait(ulock);
00348       if (new_scene_update_ != UPDATE_NONE)
00349       {
00350         if ((publish_update_types_ & new_scene_update_) || new_scene_update_ == UPDATE_SCENE)
00351         {
00352           if (new_scene_update_ == UPDATE_SCENE)
00353             is_full = true;
00354           else
00355           {
00356             occupancy_map_monitor::OccMapTree::ReadLock lock;
00357             if (octomap_monitor_)
00358               lock = octomap_monitor_->getOcTreePtr()->reading();
00359             scene_->getPlanningSceneDiffMsg(msg);
00360           }
00361           boost::recursive_mutex::scoped_lock prevent_shape_cache_updates(shape_handles_lock_);  // we don't want the
00362                                                                                                  // transform cache to
00363                                                                                                  // update while we are
00364                                                                                                  // potentially changing
00365                                                                                                  // attached bodies
00366           scene_->setAttachedBodyUpdateCallback(robot_state::AttachedBodyCallback());
00367           scene_->setCollisionObjectUpdateCallback(collision_detection::World::ObserverCallbackFn());
00368           scene_->pushDiffs(parent_scene_);
00369           scene_->clearDiffs();
00370           scene_->setAttachedBodyUpdateCallback(
00371               boost::bind(&PlanningSceneMonitor::currentStateAttachedBodyUpdateCallback, this, _1, _2));
00372           scene_->setCollisionObjectUpdateCallback(
00373               boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, _1, _2));
00374           if (octomap_monitor_)
00375           {
00376             excludeAttachedBodiesFromOctree();  // in case updates have happened to the attached bodies, put them in
00377             excludeWorldObjectsFromOctree();    // in case updates have happened to the attached bodies, put them in
00378           }
00379           if (is_full)
00380           {
00381             occupancy_map_monitor::OccMapTree::ReadLock lock;
00382             if (octomap_monitor_)
00383               lock = octomap_monitor_->getOcTreePtr()->reading();
00384             scene_->getPlanningSceneMsg(msg);
00385           }
00386           publish_msg = true;
00387         }
00388         new_scene_update_ = UPDATE_NONE;
00389       }
00390     }
00391     if (publish_msg)
00392     {
00393       rate.reset();
00394       planning_scene_publisher_.publish(msg);
00395       if (is_full)
00396         ROS_DEBUG_NAMED(LOGNAME, "Published full planning scene: '%s'", msg.name.c_str());
00397       rate.sleep();
00398     }
00399   } while (publish_planning_scene_);
00400 }
00401 
00402 void planning_scene_monitor::PlanningSceneMonitor::getMonitoredTopics(std::vector<std::string>& topics) const
00403 {
00404   topics.clear();
00405   if (current_state_monitor_)
00406   {
00407     const std::string& t = current_state_monitor_->getMonitoredTopic();
00408     if (!t.empty())
00409       topics.push_back(t);
00410   }
00411   if (planning_scene_subscriber_)
00412     topics.push_back(planning_scene_subscriber_.getTopic());
00413   if (collision_object_subscriber_)
00414     topics.push_back(collision_object_subscriber_->getTopic());
00415   if (planning_scene_world_subscriber_)
00416     topics.push_back(planning_scene_world_subscriber_.getTopic());
00417 }
00418 
00419 namespace
00420 {
00421 bool sceneIsParentOf(const planning_scene::PlanningSceneConstPtr& scene,
00422                      const planning_scene::PlanningScene* possible_parent)
00423 {
00424   if (scene && scene.get() == possible_parent)
00425     return true;
00426   if (scene)
00427     return sceneIsParentOf(scene->getParent(), possible_parent);
00428   return false;
00429 }
00430 }
00431 
00432 bool planning_scene_monitor::PlanningSceneMonitor::updatesScene(const planning_scene::PlanningScenePtr& scene) const
00433 {
00434   return sceneIsParentOf(scene_const_, scene.get());
00435 }
00436 
00437 bool planning_scene_monitor::PlanningSceneMonitor::updatesScene(
00438     const planning_scene::PlanningSceneConstPtr& scene) const
00439 {
00440   return sceneIsParentOf(scene_const_, scene.get());
00441 }
00442 
00443 void planning_scene_monitor::PlanningSceneMonitor::triggerSceneUpdateEvent(SceneUpdateType update_type)
00444 {
00445   // do not modify update functions while we are calling them
00446   boost::recursive_mutex::scoped_lock lock(update_lock_);
00447 
00448   for (std::size_t i = 0; i < update_callbacks_.size(); ++i)
00449     update_callbacks_[i](update_type);
00450   new_scene_update_ = (SceneUpdateType)((int)new_scene_update_ | (int)update_type);
00451   new_scene_update_condition_.notify_all();
00452 }
00453 
00454 bool planning_scene_monitor::PlanningSceneMonitor::requestPlanningSceneState(const std::string& service_name)
00455 {
00456   // use global namespace for service
00457   ros::ServiceClient client = ros::NodeHandle().serviceClient<moveit_msgs::GetPlanningScene>(service_name);
00458   moveit_msgs::GetPlanningScene srv;
00459   srv.request.components.components =
00460       srv.request.components.SCENE_SETTINGS | srv.request.components.ROBOT_STATE |
00461       srv.request.components.ROBOT_STATE_ATTACHED_OBJECTS | srv.request.components.WORLD_OBJECT_NAMES |
00462       srv.request.components.WORLD_OBJECT_GEOMETRY | srv.request.components.OCTOMAP |
00463       srv.request.components.TRANSFORMS | srv.request.components.ALLOWED_COLLISION_MATRIX |
00464       srv.request.components.LINK_PADDING_AND_SCALING | srv.request.components.OBJECT_COLORS;
00465 
00466   // Make sure client is connected to server
00467   if (!client.exists())
00468   {
00469     ROS_DEBUG_STREAM_NAMED(LOGNAME, "Waiting for service `" << service_name << "` to exist.");
00470     client.waitForExistence(ros::Duration(5.0));
00471   }
00472 
00473   if (client.call(srv))
00474   {
00475     newPlanningSceneMessage(srv.response.scene);
00476   }
00477   else
00478   {
00479     ROS_INFO_NAMED(LOGNAME, "Failed to call service %s, have you launched move_group? at %s:%d", service_name.c_str(),
00480                    __FILE__, __LINE__);
00481     return false;
00482   }
00483   return true;
00484 }
00485 
00486 void planning_scene_monitor::PlanningSceneMonitor::newPlanningSceneCallback(
00487     const moveit_msgs::PlanningSceneConstPtr& scene)
00488 {
00489   newPlanningSceneMessage(*scene);
00490 }
00491 
00492 void planning_scene_monitor::PlanningSceneMonitor::clearOctomap()
00493 {
00494   octomap_monitor_->getOcTreePtr()->lockWrite();
00495   octomap_monitor_->getOcTreePtr()->clear();
00496   octomap_monitor_->getOcTreePtr()->unlockWrite();
00497 }
00498 
00499 bool planning_scene_monitor::PlanningSceneMonitor::newPlanningSceneMessage(const moveit_msgs::PlanningScene& scene)
00500 {
00501   if (!scene_)
00502     return false;
00503 
00504   bool result;
00505 
00506   SceneUpdateType upd = UPDATE_SCENE;
00507   std::string old_scene_name;
00508   {
00509     boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
00510     // we don't want the transform cache to update while we are potentially changing attached bodies
00511     boost::recursive_mutex::scoped_lock prevent_shape_cache_updates(shape_handles_lock_);
00512 
00513     last_update_time_ = ros::Time::now();
00514     old_scene_name = scene_->getName();
00515     result = scene_->usePlanningSceneMsg(scene);
00516     if (octomap_monitor_)
00517     {
00518       if (!scene.is_diff && scene.world.octomap.octomap.data.empty())
00519       {
00520         octomap_monitor_->getOcTreePtr()->lockWrite();
00521         octomap_monitor_->getOcTreePtr()->clear();
00522         octomap_monitor_->getOcTreePtr()->unlockWrite();
00523       }
00524     }
00525     robot_model_ = scene_->getRobotModel();
00526 
00527     // if we just reset the scene completely but we were maintaining diffs, we need to fix that
00528     if (!scene.is_diff && parent_scene_)
00529     {
00530       // the scene is now decoupled from the parent, since we just reset it
00531       scene_->setAttachedBodyUpdateCallback(robot_state::AttachedBodyCallback());
00532       scene_->setCollisionObjectUpdateCallback(collision_detection::World::ObserverCallbackFn());
00533       parent_scene_ = scene_;
00534       scene_ = parent_scene_->diff();
00535       scene_const_ = scene_;
00536       scene_->setAttachedBodyUpdateCallback(
00537           boost::bind(&PlanningSceneMonitor::currentStateAttachedBodyUpdateCallback, this, _1, _2));
00538       scene_->setCollisionObjectUpdateCallback(
00539           boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, _1, _2));
00540     }
00541     if (octomap_monitor_)
00542     {
00543       excludeAttachedBodiesFromOctree();  // in case updates have happened to the attached bodies, put them in
00544       excludeWorldObjectsFromOctree();    // in case updates have happened to the attached bodies, put them in
00545     }
00546   }
00547 
00548   // if we have a diff, try to more accuratelly determine the update type
00549   if (scene.is_diff)
00550   {
00551     bool no_other_scene_upd = (scene.name.empty() || scene.name == old_scene_name) &&
00552                               scene.allowed_collision_matrix.entry_names.empty() && scene.link_padding.empty() &&
00553                               scene.link_scale.empty();
00554     if (no_other_scene_upd)
00555     {
00556       upd = UPDATE_NONE;
00557       if (!planning_scene::PlanningScene::isEmpty(scene.world))
00558         upd = (SceneUpdateType)((int)upd | (int)UPDATE_GEOMETRY);
00559 
00560       if (!scene.fixed_frame_transforms.empty())
00561         upd = (SceneUpdateType)((int)upd | (int)UPDATE_TRANSFORMS);
00562 
00563       if (!planning_scene::PlanningScene::isEmpty(scene.robot_state))
00564       {
00565         upd = (SceneUpdateType)((int)upd | (int)UPDATE_STATE);
00566         if (!scene.robot_state.attached_collision_objects.empty() || scene.robot_state.is_diff == false)
00567           upd = (SceneUpdateType)((int)upd | (int)UPDATE_GEOMETRY);
00568       }
00569     }
00570   }
00571   triggerSceneUpdateEvent(upd);
00572   return result;
00573 }
00574 
00575 void planning_scene_monitor::PlanningSceneMonitor::newPlanningSceneWorldCallback(
00576     const moveit_msgs::PlanningSceneWorldConstPtr& world)
00577 {
00578   if (scene_)
00579   {
00580     updateFrameTransforms();
00581     {
00582       boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
00583       last_update_time_ = ros::Time::now();
00584       scene_->getWorldNonConst()->clearObjects();
00585       scene_->processPlanningSceneWorldMsg(*world);
00586       if (octomap_monitor_)
00587       {
00588         if (world->octomap.octomap.data.empty())
00589         {
00590           octomap_monitor_->getOcTreePtr()->lockWrite();
00591           octomap_monitor_->getOcTreePtr()->clear();
00592           octomap_monitor_->getOcTreePtr()->unlockWrite();
00593         }
00594       }
00595     }
00596     triggerSceneUpdateEvent(UPDATE_SCENE);
00597   }
00598 }
00599 
00600 void planning_scene_monitor::PlanningSceneMonitor::collisionObjectFailTFCallback(
00601     const moveit_msgs::CollisionObjectConstPtr& obj, tf::filter_failure_reasons::FilterFailureReason reason)
00602 {
00603   // if we just want to remove objects, the frame does not matter
00604   if (reason == tf::filter_failure_reasons::EmptyFrameID && obj->operation == moveit_msgs::CollisionObject::REMOVE)
00605     collisionObjectCallback(obj);
00606 }
00607 
00608 void planning_scene_monitor::PlanningSceneMonitor::collisionObjectCallback(
00609     const moveit_msgs::CollisionObjectConstPtr& obj)
00610 {
00611   if (scene_)
00612   {
00613     updateFrameTransforms();
00614     {
00615       boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
00616       last_update_time_ = ros::Time::now();
00617       scene_->processCollisionObjectMsg(*obj);
00618     }
00619     triggerSceneUpdateEvent(UPDATE_GEOMETRY);
00620   }
00621 }
00622 
00623 void planning_scene_monitor::PlanningSceneMonitor::attachObjectCallback(
00624     const moveit_msgs::AttachedCollisionObjectConstPtr& obj)
00625 {
00626   if (scene_)
00627   {
00628     updateFrameTransforms();
00629     {
00630       boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
00631       last_update_time_ = ros::Time::now();
00632       scene_->processAttachedCollisionObjectMsg(*obj);
00633     }
00634     triggerSceneUpdateEvent(UPDATE_GEOMETRY);
00635   }
00636 }
00637 
00638 void planning_scene_monitor::PlanningSceneMonitor::excludeRobotLinksFromOctree()
00639 {
00640   if (!octomap_monitor_)
00641     return;
00642 
00643   boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
00644 
00645   includeRobotLinksInOctree();
00646   const std::vector<const robot_model::LinkModel*>& links = getRobotModel()->getLinkModelsWithCollisionGeometry();
00647   ros::WallTime start = ros::WallTime::now();
00648   bool warned = false;
00649   for (std::size_t i = 0; i < links.size(); ++i)
00650   {
00651     std::vector<shapes::ShapeConstPtr> shapes = links[i]->getShapes();  // copy shared ptrs on purpuse
00652     for (std::size_t j = 0; j < shapes.size(); ++j)
00653     {
00654       // merge mesh vertices up to 0.1 mm apart
00655       if (shapes[j]->type == shapes::MESH)
00656       {
00657         shapes::Mesh* m = static_cast<shapes::Mesh*>(shapes[j]->clone());
00658         m->mergeVertices(1e-4);
00659         shapes[j].reset(m);
00660       }
00661 
00662       occupancy_map_monitor::ShapeHandle h = octomap_monitor_->excludeShape(shapes[j]);
00663       if (h)
00664         link_shape_handles_[links[i]].push_back(std::make_pair(h, j));
00665     }
00666     if (!warned && ((ros::WallTime::now() - start) > ros::WallDuration(30.0)))
00667     {
00668       ROS_WARN_STREAM_NAMED(LOGNAME, "It is likely there are too many vertices in collision geometry");
00669       warned = true;
00670     }
00671   }
00672 }
00673 
00674 void planning_scene_monitor::PlanningSceneMonitor::includeRobotLinksInOctree()
00675 {
00676   if (!octomap_monitor_)
00677     return;
00678 
00679   boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
00680 
00681   for (LinkShapeHandles::iterator it = link_shape_handles_.begin(); it != link_shape_handles_.end(); ++it)
00682     for (std::size_t i = 0; i < it->second.size(); ++i)
00683       octomap_monitor_->forgetShape(it->second[i].first);
00684   link_shape_handles_.clear();
00685 }
00686 
00687 void planning_scene_monitor::PlanningSceneMonitor::includeAttachedBodiesInOctree()
00688 {
00689   if (!octomap_monitor_)
00690     return;
00691 
00692   boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
00693 
00694   // clear information about any attached body, without refering to the AttachedBody* ptr (could be invalid)
00695   for (AttachedBodyShapeHandles::iterator it = attached_body_shape_handles_.begin();
00696        it != attached_body_shape_handles_.end(); ++it)
00697     for (std::size_t k = 0; k < it->second.size(); ++k)
00698       octomap_monitor_->forgetShape(it->second[k].first);
00699   attached_body_shape_handles_.clear();
00700 }
00701 
00702 void planning_scene_monitor::PlanningSceneMonitor::excludeAttachedBodiesFromOctree()
00703 {
00704   boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
00705 
00706   includeAttachedBodiesInOctree();
00707   // add attached objects again
00708   std::vector<const robot_state::AttachedBody*> ab;
00709   scene_->getCurrentState().getAttachedBodies(ab);
00710   for (std::size_t i = 0; i < ab.size(); ++i)
00711     excludeAttachedBodyFromOctree(ab[i]);
00712 }
00713 
00714 void planning_scene_monitor::PlanningSceneMonitor::includeWorldObjectsInOctree()
00715 {
00716   if (!octomap_monitor_)
00717     return;
00718 
00719   boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
00720 
00721   // clear information about any attached object
00722   for (CollisionBodyShapeHandles::iterator it = collision_body_shape_handles_.begin();
00723        it != collision_body_shape_handles_.end(); ++it)
00724     for (std::size_t k = 0; k < it->second.size(); ++k)
00725       octomap_monitor_->forgetShape(it->second[k].first);
00726   collision_body_shape_handles_.clear();
00727 }
00728 
00729 void planning_scene_monitor::PlanningSceneMonitor::excludeWorldObjectsFromOctree()
00730 {
00731   boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
00732 
00733   includeWorldObjectsInOctree();
00734   for (collision_detection::World::const_iterator it = scene_->getWorld()->begin(); it != scene_->getWorld()->end();
00735        ++it)
00736     excludeWorldObjectFromOctree(it->second);
00737 }
00738 
00739 void planning_scene_monitor::PlanningSceneMonitor::excludeAttachedBodyFromOctree(
00740     const robot_state::AttachedBody* attached_body)
00741 {
00742   if (!octomap_monitor_)
00743     return;
00744 
00745   boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
00746   bool found = false;
00747   for (std::size_t i = 0; i < attached_body->getShapes().size(); ++i)
00748   {
00749     if (attached_body->getShapes()[i]->type == shapes::PLANE || attached_body->getShapes()[i]->type == shapes::OCTREE)
00750       continue;
00751     occupancy_map_monitor::ShapeHandle h = octomap_monitor_->excludeShape(attached_body->getShapes()[i]);
00752     if (h)
00753     {
00754       found = true;
00755       attached_body_shape_handles_[attached_body].push_back(std::make_pair(h, i));
00756     }
00757   }
00758   if (found)
00759     ROS_DEBUG_NAMED(LOGNAME, "Excluding attached body '%s' from monitored octomap", attached_body->getName().c_str());
00760 }
00761 
00762 void planning_scene_monitor::PlanningSceneMonitor::includeAttachedBodyInOctree(
00763     const robot_state::AttachedBody* attached_body)
00764 {
00765   if (!octomap_monitor_)
00766     return;
00767 
00768   boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
00769 
00770   AttachedBodyShapeHandles::iterator it = attached_body_shape_handles_.find(attached_body);
00771   if (it != attached_body_shape_handles_.end())
00772   {
00773     for (std::size_t k = 0; k < it->second.size(); ++k)
00774       octomap_monitor_->forgetShape(it->second[k].first);
00775     ROS_DEBUG_NAMED(LOGNAME, "Including attached body '%s' in monitored octomap", attached_body->getName().c_str());
00776     attached_body_shape_handles_.erase(it);
00777   }
00778 }
00779 
00780 void planning_scene_monitor::PlanningSceneMonitor::excludeWorldObjectFromOctree(
00781     const collision_detection::World::ObjectConstPtr& obj)
00782 {
00783   if (!octomap_monitor_)
00784     return;
00785 
00786   boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
00787 
00788   bool found = false;
00789   for (std::size_t i = 0; i < obj->shapes_.size(); ++i)
00790   {
00791     if (obj->shapes_[i]->type == shapes::PLANE || obj->shapes_[i]->type == shapes::OCTREE)
00792       continue;
00793     occupancy_map_monitor::ShapeHandle h = octomap_monitor_->excludeShape(obj->shapes_[i]);
00794     if (h)
00795     {
00796       collision_body_shape_handles_[obj->id_].push_back(std::make_pair(h, &obj->shape_poses_[i]));
00797       found = true;
00798     }
00799   }
00800   if (found)
00801     ROS_DEBUG_NAMED(LOGNAME, "Excluding collision object '%s' from monitored octomap", obj->id_.c_str());
00802 }
00803 
00804 void planning_scene_monitor::PlanningSceneMonitor::includeWorldObjectInOctree(
00805     const collision_detection::World::ObjectConstPtr& obj)
00806 {
00807   if (!octomap_monitor_)
00808     return;
00809 
00810   boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
00811 
00812   CollisionBodyShapeHandles::iterator it = collision_body_shape_handles_.find(obj->id_);
00813   if (it != collision_body_shape_handles_.end())
00814   {
00815     for (std::size_t k = 0; k < it->second.size(); ++k)
00816       octomap_monitor_->forgetShape(it->second[k].first);
00817     ROS_DEBUG_NAMED(LOGNAME, "Including collision object '%s' in monitored octomap", obj->id_.c_str());
00818     collision_body_shape_handles_.erase(it);
00819   }
00820 }
00821 
00822 void planning_scene_monitor::PlanningSceneMonitor::currentStateAttachedBodyUpdateCallback(
00823     robot_state::AttachedBody* attached_body, bool just_attached)
00824 {
00825   if (!octomap_monitor_)
00826     return;
00827 
00828   if (just_attached)
00829     excludeAttachedBodyFromOctree(attached_body);
00830   else
00831     includeAttachedBodyInOctree(attached_body);
00832 }
00833 
00834 void planning_scene_monitor::PlanningSceneMonitor::currentWorldObjectUpdateCallback(
00835     const collision_detection::World::ObjectConstPtr& obj, collision_detection::World::Action action)
00836 {
00837   if (!octomap_monitor_)
00838     return;
00839   if (obj->id_ == planning_scene::PlanningScene::OCTOMAP_NS)
00840     return;
00841 
00842   if (action & collision_detection::World::CREATE)
00843     excludeWorldObjectFromOctree(obj);
00844   else if (action & collision_detection::World::DESTROY)
00845     includeWorldObjectInOctree(obj);
00846   else
00847   {
00848     excludeWorldObjectFromOctree(obj);
00849     includeWorldObjectInOctree(obj);
00850   }
00851 }
00852 
00853 void planning_scene_monitor::PlanningSceneMonitor::lockSceneRead()
00854 {
00855   scene_update_mutex_.lock_shared();
00856   if (octomap_monitor_)
00857     octomap_monitor_->getOcTreePtr()->lockRead();
00858 }
00859 
00860 void planning_scene_monitor::PlanningSceneMonitor::unlockSceneRead()
00861 {
00862   if (octomap_monitor_)
00863     octomap_monitor_->getOcTreePtr()->unlockRead();
00864   scene_update_mutex_.unlock_shared();
00865 }
00866 
00867 void planning_scene_monitor::PlanningSceneMonitor::lockSceneWrite()
00868 {
00869   scene_update_mutex_.lock();
00870   if (octomap_monitor_)
00871     octomap_monitor_->getOcTreePtr()->lockWrite();
00872 }
00873 
00874 void planning_scene_monitor::PlanningSceneMonitor::unlockSceneWrite()
00875 {
00876   if (octomap_monitor_)
00877     octomap_monitor_->getOcTreePtr()->unlockWrite();
00878   scene_update_mutex_.unlock();
00879 }
00880 
00881 void planning_scene_monitor::PlanningSceneMonitor::startSceneMonitor(const std::string& scene_topic)
00882 {
00883   stopSceneMonitor();
00884 
00885   ROS_INFO_NAMED(LOGNAME, "Starting scene monitor");
00886   // listen for planning scene updates; these messages include transforms, so no need for filters
00887   if (!scene_topic.empty())
00888   {
00889     planning_scene_subscriber_ =
00890         root_nh_.subscribe(scene_topic, 100, &PlanningSceneMonitor::newPlanningSceneCallback, this);
00891     ROS_INFO_NAMED(LOGNAME, "Listening to '%s'", root_nh_.resolveName(scene_topic).c_str());
00892   }
00893 }
00894 
00895 void planning_scene_monitor::PlanningSceneMonitor::stopSceneMonitor()
00896 {
00897   if (planning_scene_subscriber_)
00898   {
00899     ROS_INFO_NAMED(LOGNAME, "Stopping scene monitor");
00900     planning_scene_subscriber_.shutdown();
00901   }
00902 }
00903 
00904 bool planning_scene_monitor::PlanningSceneMonitor::getShapeTransformCache(
00905     const std::string& target_frame, const ros::Time& target_time,
00906     occupancy_map_monitor::ShapeTransformCache& cache) const
00907 {
00908   if (!tf_)
00909     return false;
00910   try
00911   {
00912     boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
00913 
00914     for (LinkShapeHandles::const_iterator it = link_shape_handles_.begin(); it != link_shape_handles_.end(); ++it)
00915     {
00916       tf::StampedTransform tr;
00917       tf_->waitForTransform(target_frame, it->first->getName(), target_time, shape_transform_cache_lookup_wait_time_);
00918       tf_->lookupTransform(target_frame, it->first->getName(), target_time, tr);
00919       Eigen::Affine3d ttr;
00920       tf::transformTFToEigen(tr, ttr);
00921       for (std::size_t j = 0; j < it->second.size(); ++j)
00922         cache[it->second[j].first] = ttr * it->first->getCollisionOriginTransforms()[it->second[j].second];
00923     }
00924     for (AttachedBodyShapeHandles::const_iterator it = attached_body_shape_handles_.begin();
00925          it != attached_body_shape_handles_.end(); ++it)
00926     {
00927       tf::StampedTransform tr;
00928       tf_->waitForTransform(target_frame, it->first->getAttachedLinkName(), target_time,
00929                             shape_transform_cache_lookup_wait_time_);
00930       tf_->lookupTransform(target_frame, it->first->getAttachedLinkName(), target_time, tr);
00931       Eigen::Affine3d transform;
00932       tf::transformTFToEigen(tr, transform);
00933       for (std::size_t k = 0; k < it->second.size(); ++k)
00934         cache[it->second[k].first] = transform * it->first->getFixedTransforms()[it->second[k].second];
00935     }
00936     {
00937       tf::StampedTransform tr;
00938       tf_->waitForTransform(target_frame, scene_->getPlanningFrame(), target_time,
00939                             shape_transform_cache_lookup_wait_time_);
00940       tf_->lookupTransform(target_frame, scene_->getPlanningFrame(), target_time, tr);
00941       Eigen::Affine3d transform;
00942       tf::transformTFToEigen(tr, transform);
00943       for (CollisionBodyShapeHandles::const_iterator it = collision_body_shape_handles_.begin();
00944            it != collision_body_shape_handles_.end(); ++it)
00945         for (std::size_t k = 0; k < it->second.size(); ++k)
00946           cache[it->second[k].first] = transform * (*it->second[k].second);
00947     }
00948   }
00949   catch (tf::TransformException& ex)
00950   {
00951     ROS_ERROR_THROTTLE_NAMED(1, LOGNAME, "Transform error: %s", ex.what());
00952     return false;
00953   }
00954   return true;
00955 }
00956 
00957 void planning_scene_monitor::PlanningSceneMonitor::startWorldGeometryMonitor(
00958     const std::string& collision_objects_topic, const std::string& planning_scene_world_topic,
00959     const bool load_octomap_monitor)
00960 {
00961   stopWorldGeometryMonitor();
00962   ROS_INFO_NAMED(LOGNAME, "Starting world geometry monitor");
00963 
00964   // listen for world geometry updates using message filters
00965   if (!collision_objects_topic.empty())
00966   {
00967     collision_object_subscriber_.reset(
00968         new message_filters::Subscriber<moveit_msgs::CollisionObject>(root_nh_, collision_objects_topic, 1024));
00969     if (tf_)
00970     {
00971       collision_object_filter_.reset(new tf::MessageFilter<moveit_msgs::CollisionObject>(
00972           *collision_object_subscriber_, *tf_, scene_->getPlanningFrame(), 1024));
00973       collision_object_filter_->registerCallback(boost::bind(&PlanningSceneMonitor::collisionObjectCallback, this, _1));
00974       collision_object_filter_->registerFailureCallback(
00975           boost::bind(&PlanningSceneMonitor::collisionObjectFailTFCallback, this, _1, _2));
00976       ROS_INFO_NAMED(LOGNAME, "Listening to '%s' using message notifier with target frame '%s'",
00977                      root_nh_.resolveName(collision_objects_topic).c_str(),
00978                      collision_object_filter_->getTargetFramesString().c_str());
00979     }
00980     else
00981     {
00982       collision_object_subscriber_->registerCallback(
00983           boost::bind(&PlanningSceneMonitor::collisionObjectCallback, this, _1));
00984       ROS_INFO_NAMED(LOGNAME, "Listening to '%s'", root_nh_.resolveName(collision_objects_topic).c_str());
00985     }
00986   }
00987 
00988   if (!planning_scene_world_topic.empty())
00989   {
00990     planning_scene_world_subscriber_ =
00991         root_nh_.subscribe(planning_scene_world_topic, 1, &PlanningSceneMonitor::newPlanningSceneWorldCallback, this);
00992     ROS_INFO_NAMED(LOGNAME, "Listening to '%s' for planning scene world geometry",
00993                    root_nh_.resolveName(planning_scene_world_topic).c_str());
00994   }
00995 
00996   // Ocotomap monitor is optional
00997   if (load_octomap_monitor)
00998   {
00999     if (!octomap_monitor_)
01000     {
01001       octomap_monitor_.reset(new occupancy_map_monitor::OccupancyMapMonitor(tf_, scene_->getPlanningFrame()));
01002       excludeRobotLinksFromOctree();
01003       excludeAttachedBodiesFromOctree();
01004       excludeWorldObjectsFromOctree();
01005 
01006       octomap_monitor_->setTransformCacheCallback(
01007           boost::bind(&PlanningSceneMonitor::getShapeTransformCache, this, _1, _2, _3));
01008       octomap_monitor_->setUpdateCallback(boost::bind(&PlanningSceneMonitor::octomapUpdateCallback, this));
01009     }
01010     octomap_monitor_->startMonitor();
01011   }
01012 }
01013 
01014 void planning_scene_monitor::PlanningSceneMonitor::stopWorldGeometryMonitor()
01015 {
01016   if (collision_object_subscriber_ || collision_object_filter_)
01017   {
01018     ROS_INFO_NAMED(LOGNAME, "Stopping world geometry monitor");
01019     collision_object_filter_.reset();
01020     collision_object_subscriber_.reset();
01021     planning_scene_world_subscriber_.shutdown();
01022   }
01023   else if (planning_scene_world_subscriber_)
01024   {
01025     ROS_INFO_NAMED(LOGNAME, "Stopping world geometry monitor");
01026     planning_scene_world_subscriber_.shutdown();
01027   }
01028   if (octomap_monitor_)
01029     octomap_monitor_->stopMonitor();
01030 }
01031 
01032 void planning_scene_monitor::PlanningSceneMonitor::startStateMonitor(const std::string& joint_states_topic,
01033                                                                      const std::string& attached_objects_topic)
01034 {
01035   stopStateMonitor();
01036   if (scene_)
01037   {
01038     if (!current_state_monitor_)
01039       current_state_monitor_.reset(new CurrentStateMonitor(getRobotModel(), tf_));
01040     current_state_monitor_->addUpdateCallback(boost::bind(&PlanningSceneMonitor::onStateUpdate, this, _1));
01041     current_state_monitor_->startStateMonitor(joint_states_topic);
01042 
01043     {
01044       boost::mutex::scoped_lock lock(state_pending_mutex_);
01045       if (!dt_state_update_.isZero())
01046         state_update_timer_.start();
01047     }
01048 
01049     if (!attached_objects_topic.empty())
01050     {
01051       // using regular message filter as there's no header
01052       attached_collision_object_subscriber_ =
01053           root_nh_.subscribe(attached_objects_topic, 1024, &PlanningSceneMonitor::attachObjectCallback, this);
01054       ROS_INFO_NAMED(LOGNAME, "Listening to '%s' for attached collision objects",
01055                      root_nh_.resolveName(attached_objects_topic).c_str());
01056     }
01057   }
01058   else
01059     ROS_ERROR_NAMED(LOGNAME, "Cannot monitor robot state because planning scene is not configured");
01060 }
01061 
01062 void planning_scene_monitor::PlanningSceneMonitor::stopStateMonitor()
01063 {
01064   if (current_state_monitor_)
01065     current_state_monitor_->stopStateMonitor();
01066   if (attached_collision_object_subscriber_)
01067     attached_collision_object_subscriber_.shutdown();
01068 
01069   // stop must be called with state_pending_mutex_ unlocked to avoid deadlock
01070   state_update_timer_.stop();
01071   {
01072     boost::mutex::scoped_lock lock(state_pending_mutex_);
01073     state_update_pending_ = false;
01074   }
01075 }
01076 
01077 void planning_scene_monitor::PlanningSceneMonitor::onStateUpdate(
01078     const sensor_msgs::JointStateConstPtr& /* joint_state */)
01079 {
01080   const ros::WallTime& n = ros::WallTime::now();
01081   ros::WallDuration dt = n - last_state_update_;
01082 
01083   bool update = false;
01084   {
01085     boost::mutex::scoped_lock lock(state_pending_mutex_);
01086 
01087     if (dt < dt_state_update_)
01088     {
01089       state_update_pending_ = true;
01090     }
01091     else
01092     {
01093       state_update_pending_ = false;
01094       last_state_update_ = n;
01095       update = true;
01096     }
01097   }
01098 
01099   // run the state update with state_pending_mutex_ unlocked
01100   if (update)
01101     updateSceneWithCurrentState();
01102 }
01103 
01104 void planning_scene_monitor::PlanningSceneMonitor::stateUpdateTimerCallback(const ros::WallTimerEvent& event)
01105 {
01106   if (state_update_pending_)
01107   {
01108     bool update = false;
01109 
01110     const ros::WallTime& n = ros::WallTime::now();
01111     ros::WallDuration dt = n - last_state_update_;
01112 
01113     {
01114       // lock for access to dt_state_update_ and state_update_pending_
01115       boost::mutex::scoped_lock lock(state_pending_mutex_);
01116       if (state_update_pending_ && dt >= dt_state_update_)
01117       {
01118         state_update_pending_ = false;
01119         last_state_update_ = ros::WallTime::now();
01120         update = true;
01121       }
01122     }
01123 
01124     // run the state update with state_pending_mutex_ unlocked
01125     if (update)
01126       updateSceneWithCurrentState();
01127   }
01128 }
01129 
01130 void planning_scene_monitor::PlanningSceneMonitor::octomapUpdateCallback()
01131 {
01132   if (!octomap_monitor_)
01133     return;
01134 
01135   updateFrameTransforms();
01136   {
01137     boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
01138     last_update_time_ = ros::Time::now();
01139     octomap_monitor_->getOcTreePtr()->lockRead();
01140     try
01141     {
01142       scene_->processOctomapPtr(octomap_monitor_->getOcTreePtr(), Eigen::Affine3d::Identity());
01143       octomap_monitor_->getOcTreePtr()->unlockRead();
01144     }
01145     catch (...)
01146     {
01147       octomap_monitor_->getOcTreePtr()->unlockRead();  // unlock and rethrow
01148       throw;
01149     }
01150   }
01151   triggerSceneUpdateEvent(UPDATE_GEOMETRY);
01152 }
01153 
01154 void planning_scene_monitor::PlanningSceneMonitor::setStateUpdateFrequency(double hz)
01155 {
01156   bool update = false;
01157   if (hz > std::numeric_limits<double>::epsilon())
01158   {
01159     boost::mutex::scoped_lock lock(state_pending_mutex_);
01160     dt_state_update_.fromSec(1.0 / hz);
01161     state_update_timer_.setPeriod(dt_state_update_);
01162     state_update_timer_.start();
01163   }
01164   else
01165   {
01166     // stop must be called with state_pending_mutex_ unlocked to avoid deadlock
01167     state_update_timer_.stop();
01168     boost::mutex::scoped_lock lock(state_pending_mutex_);
01169     dt_state_update_ = ros::WallDuration(0, 0);
01170     if (state_update_pending_)
01171       update = true;
01172   }
01173   ROS_INFO_NAMED(LOGNAME, "Updating internal planning scene state at most every %lf seconds", dt_state_update_.toSec());
01174 
01175   if (update)
01176     updateSceneWithCurrentState();
01177 }
01178 
01179 void planning_scene_monitor::PlanningSceneMonitor::updateSceneWithCurrentState()
01180 {
01181   if (current_state_monitor_)
01182   {
01183     std::vector<std::string> missing;
01184     if (!current_state_monitor_->haveCompleteState(missing) &&
01185         (ros::Time::now() - current_state_monitor_->getMonitorStartTime()).toSec() > 1.0)
01186     {
01187       std::string missing_str = boost::algorithm::join(missing, ", ");
01188       ROS_WARN_THROTTLE_NAMED(1, LOGNAME, "The complete state of the robot is not yet known.  Missing %s",
01189                               missing_str.c_str());
01190     }
01191 
01192     {
01193       boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
01194       current_state_monitor_->setToCurrentState(scene_->getCurrentStateNonConst());
01195       last_update_time_ = ros::Time::now();
01196       scene_->getCurrentStateNonConst().update();  // compute all transforms
01197     }
01198     triggerSceneUpdateEvent(UPDATE_STATE);
01199   }
01200   else
01201     ROS_ERROR_THROTTLE_NAMED(1, LOGNAME, "State monitor is not active. Unable to set the planning scene state");
01202 }
01203 
01204 void planning_scene_monitor::PlanningSceneMonitor::addUpdateCallback(const boost::function<void(SceneUpdateType)>& fn)
01205 {
01206   boost::recursive_mutex::scoped_lock lock(update_lock_);
01207   if (fn)
01208     update_callbacks_.push_back(fn);
01209 }
01210 
01211 void planning_scene_monitor::PlanningSceneMonitor::clearUpdateCallbacks()
01212 {
01213   boost::recursive_mutex::scoped_lock lock(update_lock_);
01214   update_callbacks_.clear();
01215 }
01216 
01217 void planning_scene_monitor::PlanningSceneMonitor::setPlanningScenePublishingFrequency(double hz)
01218 {
01219   publish_planning_scene_frequency_ = hz;
01220   ROS_DEBUG_NAMED(LOGNAME, "Maximum frquency for publishing a planning scene is now %lf Hz",
01221                   publish_planning_scene_frequency_);
01222 }
01223 
01224 void planning_scene_monitor::PlanningSceneMonitor::getUpdatedFrameTransforms(
01225     std::vector<geometry_msgs::TransformStamped>& transforms)
01226 {
01227   const std::string& target = getRobotModel()->getModelFrame();
01228 
01229   std::vector<std::string> all_frame_names;
01230   tf_->getFrameStrings(all_frame_names);
01231   for (std::size_t i = 0; i < all_frame_names.size(); ++i)
01232   {
01233     const std::string& frame_no_slash = (!all_frame_names[i].empty() && all_frame_names[i][0] == '/') ?
01234                                             all_frame_names[i].substr(1) :
01235                                             all_frame_names[i];
01236     const std::string& frame_with_slash =
01237         (!all_frame_names[i].empty() && all_frame_names[i][0] != '/') ? '/' + all_frame_names[i] : all_frame_names[i];
01238 
01239     if (frame_with_slash == target || getRobotModel()->hasLinkModel(frame_no_slash))
01240       continue;
01241 
01242     ros::Time stamp(0);
01243     std::string err_string;
01244     if (tf_->getLatestCommonTime(target, all_frame_names[i], stamp, &err_string) != tf::NO_ERROR)
01245     {
01246       ROS_WARN_STREAM_NAMED(LOGNAME, "No transform available between frame '"
01247                                          << all_frame_names[i] << "' and planning frame '" << target << "' ("
01248                                          << err_string << ")");
01249       continue;
01250     }
01251 
01252     tf::StampedTransform t;
01253     try
01254     {
01255       tf_->lookupTransform(target, all_frame_names[i], stamp, t);
01256     }
01257     catch (tf::TransformException& ex)
01258     {
01259       ROS_WARN_STREAM_NAMED(LOGNAME, "Unable to transform object from frame '"
01260                                          << all_frame_names[i] << "' to planning frame '" << target << "' ("
01261                                          << ex.what() << ")");
01262       continue;
01263     }
01264     geometry_msgs::TransformStamped f;
01265     f.header.frame_id = frame_with_slash;
01266     f.child_frame_id = target;
01267     f.transform.translation.x = t.getOrigin().x();
01268     f.transform.translation.y = t.getOrigin().y();
01269     f.transform.translation.z = t.getOrigin().z();
01270     const tf::Quaternion& q = t.getRotation();
01271     f.transform.rotation.x = q.x();
01272     f.transform.rotation.y = q.y();
01273     f.transform.rotation.z = q.z();
01274     f.transform.rotation.w = q.w();
01275     transforms.push_back(f);
01276   }
01277 }
01278 
01279 void planning_scene_monitor::PlanningSceneMonitor::updateFrameTransforms()
01280 {
01281   if (!tf_)
01282     return;
01283 
01284   if (scene_)
01285   {
01286     std::vector<geometry_msgs::TransformStamped> transforms;
01287     getUpdatedFrameTransforms(transforms);
01288     {
01289       boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
01290       scene_->getTransformsNonConst().setTransforms(transforms);
01291       last_update_time_ = ros::Time::now();
01292     }
01293     triggerSceneUpdateEvent(UPDATE_TRANSFORMS);
01294   }
01295 }
01296 
01297 void planning_scene_monitor::PlanningSceneMonitor::publishDebugInformation(bool flag)
01298 {
01299   if (octomap_monitor_)
01300     octomap_monitor_->publishDebugInformation(flag);
01301 }
01302 
01303 void planning_scene_monitor::PlanningSceneMonitor::configureCollisionMatrix(
01304     const planning_scene::PlanningScenePtr& scene)
01305 {
01306   if (!scene || robot_description_.empty())
01307     return;
01308   collision_detection::AllowedCollisionMatrix& acm = scene->getAllowedCollisionMatrixNonConst();
01309 
01310   // read overriding values from the param server
01311 
01312   // first we do default collision operations
01313   if (!nh_.hasParam(robot_description_ + "_planning/default_collision_operations"))
01314     ROS_DEBUG_NAMED(LOGNAME, "No additional default collision operations specified");
01315   else
01316   {
01317     ROS_DEBUG_NAMED(LOGNAME, "Reading additional default collision operations");
01318 
01319     XmlRpc::XmlRpcValue coll_ops;
01320     nh_.getParam(robot_description_ + "_planning/default_collision_operations", coll_ops);
01321 
01322     if (coll_ops.getType() != XmlRpc::XmlRpcValue::TypeArray)
01323     {
01324       ROS_WARN_NAMED(LOGNAME, "default_collision_operations is not an array");
01325       return;
01326     }
01327 
01328     if (coll_ops.size() == 0)
01329     {
01330       ROS_WARN_NAMED(LOGNAME, "No collision operations in default collision operations");
01331       return;
01332     }
01333 
01334     for (int i = 0; i < coll_ops.size(); ++i)
01335     {
01336       if (!coll_ops[i].hasMember("object1") || !coll_ops[i].hasMember("object2") || !coll_ops[i].hasMember("operation"))
01337       {
01338         ROS_WARN_NAMED(LOGNAME, "All collision operations must have two objects and an operation");
01339         continue;
01340       }
01341       acm.setEntry(std::string(coll_ops[i]["object1"]), std::string(coll_ops[i]["object2"]),
01342                    std::string(coll_ops[i]["operation"]) == "disable");
01343     }
01344   }
01345 }
01346 
01347 void planning_scene_monitor::PlanningSceneMonitor::configureDefaultPadding()
01348 {
01349   if (robot_description_.empty())
01350   {
01351     default_robot_padd_ = 0.0;
01352     default_robot_scale_ = 1.0;
01353     default_object_padd_ = 0.0;
01354     default_attached_padd_ = 0.0;
01355     return;
01356   }
01357 
01358   // Ensure no leading slash creates a bad param server address
01359   static const std::string robot_description =
01360       (robot_description_[0] == '/') ? robot_description_.substr(1) : robot_description_;
01361 
01362   nh_.param(robot_description + "_planning/default_robot_padding", default_robot_padd_, 0.0);
01363   nh_.param(robot_description + "_planning/default_robot_scale", default_robot_scale_, 1.0);
01364   nh_.param(robot_description + "_planning/default_object_padding", default_object_padd_, 0.0);
01365   nh_.param(robot_description + "_planning/default_attached_padding", default_attached_padd_, 0.0);
01366   nh_.param(robot_description + "_planning/default_robot_link_padding", default_robot_link_padd_,
01367             std::map<std::string, double>());
01368   nh_.param(robot_description + "_planning/default_robot_link_scale", default_robot_link_scale_,
01369             std::map<std::string, double>());
01370 
01371   ROS_DEBUG_STREAM_NAMED(LOGNAME, "Loaded " << default_robot_link_padd_.size() << " default link paddings");
01372   ROS_DEBUG_STREAM_NAMED(LOGNAME, "Loaded " << default_robot_link_scale_.size() << " default link scales");
01373 }


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Tue Dec 26 2017 03:33:56