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           // also publish timestamp of this robot_state
00387           if (current_state_monitor_)
00388             msg.robot_state.joint_state.header.stamp = current_state_monitor_->getCurrentStateTime();
00389           publish_msg = true;
00390         }
00391         new_scene_update_ = UPDATE_NONE;
00392       }
00393     }
00394     if (publish_msg)
00395     {
00396       rate.reset();
00397       planning_scene_publisher_.publish(msg);
00398       if (is_full)
00399         ROS_DEBUG_NAMED(LOGNAME, "Published full planning scene: '%s'", msg.name.c_str());
00400       rate.sleep();
00401     }
00402   } while (publish_planning_scene_);
00403 }
00404 
00405 void planning_scene_monitor::PlanningSceneMonitor::getMonitoredTopics(std::vector<std::string>& topics) const
00406 {
00407   topics.clear();
00408   if (current_state_monitor_)
00409   {
00410     const std::string& t = current_state_monitor_->getMonitoredTopic();
00411     if (!t.empty())
00412       topics.push_back(t);
00413   }
00414   if (planning_scene_subscriber_)
00415     topics.push_back(planning_scene_subscriber_.getTopic());
00416   if (collision_object_subscriber_)
00417     topics.push_back(collision_object_subscriber_->getTopic());
00418   if (planning_scene_world_subscriber_)
00419     topics.push_back(planning_scene_world_subscriber_.getTopic());
00420 }
00421 
00422 namespace
00423 {
00424 bool sceneIsParentOf(const planning_scene::PlanningSceneConstPtr& scene,
00425                      const planning_scene::PlanningScene* possible_parent)
00426 {
00427   if (scene && scene.get() == possible_parent)
00428     return true;
00429   if (scene)
00430     return sceneIsParentOf(scene->getParent(), possible_parent);
00431   return false;
00432 }
00433 }
00434 
00435 bool planning_scene_monitor::PlanningSceneMonitor::updatesScene(const planning_scene::PlanningScenePtr& scene) const
00436 {
00437   return sceneIsParentOf(scene_const_, scene.get());
00438 }
00439 
00440 bool planning_scene_monitor::PlanningSceneMonitor::updatesScene(
00441     const planning_scene::PlanningSceneConstPtr& scene) const
00442 {
00443   return sceneIsParentOf(scene_const_, scene.get());
00444 }
00445 
00446 void planning_scene_monitor::PlanningSceneMonitor::triggerSceneUpdateEvent(SceneUpdateType update_type)
00447 {
00448   // do not modify update functions while we are calling them
00449   boost::recursive_mutex::scoped_lock lock(update_lock_);
00450 
00451   for (std::size_t i = 0; i < update_callbacks_.size(); ++i)
00452     update_callbacks_[i](update_type);
00453   new_scene_update_ = (SceneUpdateType)((int)new_scene_update_ | (int)update_type);
00454   new_scene_update_condition_.notify_all();
00455 }
00456 
00457 bool planning_scene_monitor::PlanningSceneMonitor::requestPlanningSceneState(const std::string& service_name)
00458 {
00459   // use global namespace for service
00460   ros::ServiceClient client = ros::NodeHandle().serviceClient<moveit_msgs::GetPlanningScene>(service_name);
00461   moveit_msgs::GetPlanningScene srv;
00462   srv.request.components.components =
00463       srv.request.components.SCENE_SETTINGS | srv.request.components.ROBOT_STATE |
00464       srv.request.components.ROBOT_STATE_ATTACHED_OBJECTS | srv.request.components.WORLD_OBJECT_NAMES |
00465       srv.request.components.WORLD_OBJECT_GEOMETRY | srv.request.components.OCTOMAP |
00466       srv.request.components.TRANSFORMS | srv.request.components.ALLOWED_COLLISION_MATRIX |
00467       srv.request.components.LINK_PADDING_AND_SCALING | srv.request.components.OBJECT_COLORS;
00468 
00469   // Make sure client is connected to server
00470   if (!client.exists())
00471   {
00472     ROS_DEBUG_STREAM_NAMED(LOGNAME, "Waiting for service `" << service_name << "` to exist.");
00473     client.waitForExistence(ros::Duration(5.0));
00474   }
00475 
00476   if (client.call(srv))
00477   {
00478     newPlanningSceneMessage(srv.response.scene);
00479   }
00480   else
00481   {
00482     ROS_INFO_NAMED(LOGNAME, "Failed to call service %s, have you launched move_group? at %s:%d", service_name.c_str(),
00483                    __FILE__, __LINE__);
00484     return false;
00485   }
00486   return true;
00487 }
00488 
00489 void planning_scene_monitor::PlanningSceneMonitor::newPlanningSceneCallback(
00490     const moveit_msgs::PlanningSceneConstPtr& scene)
00491 {
00492   newPlanningSceneMessage(*scene);
00493 }
00494 
00495 void planning_scene_monitor::PlanningSceneMonitor::clearOctomap()
00496 {
00497   octomap_monitor_->getOcTreePtr()->lockWrite();
00498   octomap_monitor_->getOcTreePtr()->clear();
00499   octomap_monitor_->getOcTreePtr()->unlockWrite();
00500 }
00501 
00502 bool planning_scene_monitor::PlanningSceneMonitor::newPlanningSceneMessage(const moveit_msgs::PlanningScene& scene)
00503 {
00504   if (!scene_)
00505     return false;
00506 
00507   bool result;
00508 
00509   SceneUpdateType upd = UPDATE_SCENE;
00510   std::string old_scene_name;
00511   {
00512     boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
00513     // we don't want the transform cache to update while we are potentially changing attached bodies
00514     boost::recursive_mutex::scoped_lock prevent_shape_cache_updates(shape_handles_lock_);
00515 
00516     last_update_time_ = ros::Time::now();
00517     old_scene_name = scene_->getName();
00518     result = scene_->usePlanningSceneMsg(scene);
00519     if (octomap_monitor_)
00520     {
00521       if (!scene.is_diff && scene.world.octomap.octomap.data.empty())
00522       {
00523         octomap_monitor_->getOcTreePtr()->lockWrite();
00524         octomap_monitor_->getOcTreePtr()->clear();
00525         octomap_monitor_->getOcTreePtr()->unlockWrite();
00526       }
00527     }
00528     robot_model_ = scene_->getRobotModel();
00529 
00530     // if we just reset the scene completely but we were maintaining diffs, we need to fix that
00531     if (!scene.is_diff && parent_scene_)
00532     {
00533       // the scene is now decoupled from the parent, since we just reset it
00534       scene_->setAttachedBodyUpdateCallback(robot_state::AttachedBodyCallback());
00535       scene_->setCollisionObjectUpdateCallback(collision_detection::World::ObserverCallbackFn());
00536       parent_scene_ = scene_;
00537       scene_ = parent_scene_->diff();
00538       scene_const_ = scene_;
00539       scene_->setAttachedBodyUpdateCallback(
00540           boost::bind(&PlanningSceneMonitor::currentStateAttachedBodyUpdateCallback, this, _1, _2));
00541       scene_->setCollisionObjectUpdateCallback(
00542           boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, _1, _2));
00543     }
00544     if (octomap_monitor_)
00545     {
00546       excludeAttachedBodiesFromOctree();  // in case updates have happened to the attached bodies, put them in
00547       excludeWorldObjectsFromOctree();    // in case updates have happened to the attached bodies, put them in
00548     }
00549   }
00550 
00551   // if we have a diff, try to more accuratelly determine the update type
00552   if (scene.is_diff)
00553   {
00554     bool no_other_scene_upd = (scene.name.empty() || scene.name == old_scene_name) &&
00555                               scene.allowed_collision_matrix.entry_names.empty() && scene.link_padding.empty() &&
00556                               scene.link_scale.empty();
00557     if (no_other_scene_upd)
00558     {
00559       upd = UPDATE_NONE;
00560       if (!planning_scene::PlanningScene::isEmpty(scene.world))
00561         upd = (SceneUpdateType)((int)upd | (int)UPDATE_GEOMETRY);
00562 
00563       if (!scene.fixed_frame_transforms.empty())
00564         upd = (SceneUpdateType)((int)upd | (int)UPDATE_TRANSFORMS);
00565 
00566       if (!planning_scene::PlanningScene::isEmpty(scene.robot_state))
00567       {
00568         upd = (SceneUpdateType)((int)upd | (int)UPDATE_STATE);
00569         if (!scene.robot_state.attached_collision_objects.empty() || scene.robot_state.is_diff == false)
00570           upd = (SceneUpdateType)((int)upd | (int)UPDATE_GEOMETRY);
00571       }
00572     }
00573   }
00574   triggerSceneUpdateEvent(upd);
00575   return result;
00576 }
00577 
00578 void planning_scene_monitor::PlanningSceneMonitor::newPlanningSceneWorldCallback(
00579     const moveit_msgs::PlanningSceneWorldConstPtr& world)
00580 {
00581   if (scene_)
00582   {
00583     updateFrameTransforms();
00584     {
00585       boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
00586       last_update_time_ = ros::Time::now();
00587       scene_->getWorldNonConst()->clearObjects();
00588       scene_->processPlanningSceneWorldMsg(*world);
00589       if (octomap_monitor_)
00590       {
00591         if (world->octomap.octomap.data.empty())
00592         {
00593           octomap_monitor_->getOcTreePtr()->lockWrite();
00594           octomap_monitor_->getOcTreePtr()->clear();
00595           octomap_monitor_->getOcTreePtr()->unlockWrite();
00596         }
00597       }
00598     }
00599     triggerSceneUpdateEvent(UPDATE_SCENE);
00600   }
00601 }
00602 
00603 void planning_scene_monitor::PlanningSceneMonitor::collisionObjectFailTFCallback(
00604     const moveit_msgs::CollisionObjectConstPtr& obj, tf::filter_failure_reasons::FilterFailureReason reason)
00605 {
00606   // if we just want to remove objects, the frame does not matter
00607   if (reason == tf::filter_failure_reasons::EmptyFrameID && obj->operation == moveit_msgs::CollisionObject::REMOVE)
00608     collisionObjectCallback(obj);
00609 }
00610 
00611 void planning_scene_monitor::PlanningSceneMonitor::collisionObjectCallback(
00612     const moveit_msgs::CollisionObjectConstPtr& obj)
00613 {
00614   if (scene_)
00615   {
00616     updateFrameTransforms();
00617     {
00618       boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
00619       last_update_time_ = ros::Time::now();
00620       scene_->processCollisionObjectMsg(*obj);
00621     }
00622     triggerSceneUpdateEvent(UPDATE_GEOMETRY);
00623   }
00624 }
00625 
00626 void planning_scene_monitor::PlanningSceneMonitor::attachObjectCallback(
00627     const moveit_msgs::AttachedCollisionObjectConstPtr& obj)
00628 {
00629   if (scene_)
00630   {
00631     updateFrameTransforms();
00632     {
00633       boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
00634       last_update_time_ = ros::Time::now();
00635       scene_->processAttachedCollisionObjectMsg(*obj);
00636     }
00637     triggerSceneUpdateEvent(UPDATE_GEOMETRY);
00638   }
00639 }
00640 
00641 void planning_scene_monitor::PlanningSceneMonitor::excludeRobotLinksFromOctree()
00642 {
00643   if (!octomap_monitor_)
00644     return;
00645 
00646   boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
00647 
00648   includeRobotLinksInOctree();
00649   const std::vector<const robot_model::LinkModel*>& links = getRobotModel()->getLinkModelsWithCollisionGeometry();
00650   ros::WallTime start = ros::WallTime::now();
00651   bool warned = false;
00652   for (std::size_t i = 0; i < links.size(); ++i)
00653   {
00654     std::vector<shapes::ShapeConstPtr> shapes = links[i]->getShapes();  // copy shared ptrs on purpuse
00655     for (std::size_t j = 0; j < shapes.size(); ++j)
00656     {
00657       // merge mesh vertices up to 0.1 mm apart
00658       if (shapes[j]->type == shapes::MESH)
00659       {
00660         shapes::Mesh* m = static_cast<shapes::Mesh*>(shapes[j]->clone());
00661         m->mergeVertices(1e-4);
00662         shapes[j].reset(m);
00663       }
00664 
00665       occupancy_map_monitor::ShapeHandle h = octomap_monitor_->excludeShape(shapes[j]);
00666       if (h)
00667         link_shape_handles_[links[i]].push_back(std::make_pair(h, j));
00668     }
00669     if (!warned && ((ros::WallTime::now() - start) > ros::WallDuration(30.0)))
00670     {
00671       ROS_WARN_STREAM_NAMED(LOGNAME, "It is likely there are too many vertices in collision geometry");
00672       warned = true;
00673     }
00674   }
00675 }
00676 
00677 void planning_scene_monitor::PlanningSceneMonitor::includeRobotLinksInOctree()
00678 {
00679   if (!octomap_monitor_)
00680     return;
00681 
00682   boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
00683 
00684   for (LinkShapeHandles::iterator it = link_shape_handles_.begin(); it != link_shape_handles_.end(); ++it)
00685     for (std::size_t i = 0; i < it->second.size(); ++i)
00686       octomap_monitor_->forgetShape(it->second[i].first);
00687   link_shape_handles_.clear();
00688 }
00689 
00690 void planning_scene_monitor::PlanningSceneMonitor::includeAttachedBodiesInOctree()
00691 {
00692   if (!octomap_monitor_)
00693     return;
00694 
00695   boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
00696 
00697   // clear information about any attached body, without refering to the AttachedBody* ptr (could be invalid)
00698   for (AttachedBodyShapeHandles::iterator it = attached_body_shape_handles_.begin();
00699        it != attached_body_shape_handles_.end(); ++it)
00700     for (std::size_t k = 0; k < it->second.size(); ++k)
00701       octomap_monitor_->forgetShape(it->second[k].first);
00702   attached_body_shape_handles_.clear();
00703 }
00704 
00705 void planning_scene_monitor::PlanningSceneMonitor::excludeAttachedBodiesFromOctree()
00706 {
00707   boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
00708 
00709   includeAttachedBodiesInOctree();
00710   // add attached objects again
00711   std::vector<const robot_state::AttachedBody*> ab;
00712   scene_->getCurrentState().getAttachedBodies(ab);
00713   for (std::size_t i = 0; i < ab.size(); ++i)
00714     excludeAttachedBodyFromOctree(ab[i]);
00715 }
00716 
00717 void planning_scene_monitor::PlanningSceneMonitor::includeWorldObjectsInOctree()
00718 {
00719   if (!octomap_monitor_)
00720     return;
00721 
00722   boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
00723 
00724   // clear information about any attached object
00725   for (CollisionBodyShapeHandles::iterator it = collision_body_shape_handles_.begin();
00726        it != collision_body_shape_handles_.end(); ++it)
00727     for (std::size_t k = 0; k < it->second.size(); ++k)
00728       octomap_monitor_->forgetShape(it->second[k].first);
00729   collision_body_shape_handles_.clear();
00730 }
00731 
00732 void planning_scene_monitor::PlanningSceneMonitor::excludeWorldObjectsFromOctree()
00733 {
00734   boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
00735 
00736   includeWorldObjectsInOctree();
00737   for (collision_detection::World::const_iterator it = scene_->getWorld()->begin(); it != scene_->getWorld()->end();
00738        ++it)
00739     excludeWorldObjectFromOctree(it->second);
00740 }
00741 
00742 void planning_scene_monitor::PlanningSceneMonitor::excludeAttachedBodyFromOctree(
00743     const robot_state::AttachedBody* attached_body)
00744 {
00745   if (!octomap_monitor_)
00746     return;
00747 
00748   boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
00749   bool found = false;
00750   for (std::size_t i = 0; i < attached_body->getShapes().size(); ++i)
00751   {
00752     if (attached_body->getShapes()[i]->type == shapes::PLANE || attached_body->getShapes()[i]->type == shapes::OCTREE)
00753       continue;
00754     occupancy_map_monitor::ShapeHandle h = octomap_monitor_->excludeShape(attached_body->getShapes()[i]);
00755     if (h)
00756     {
00757       found = true;
00758       attached_body_shape_handles_[attached_body].push_back(std::make_pair(h, i));
00759     }
00760   }
00761   if (found)
00762     ROS_DEBUG_NAMED(LOGNAME, "Excluding attached body '%s' from monitored octomap", attached_body->getName().c_str());
00763 }
00764 
00765 void planning_scene_monitor::PlanningSceneMonitor::includeAttachedBodyInOctree(
00766     const robot_state::AttachedBody* attached_body)
00767 {
00768   if (!octomap_monitor_)
00769     return;
00770 
00771   boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
00772 
00773   AttachedBodyShapeHandles::iterator it = attached_body_shape_handles_.find(attached_body);
00774   if (it != attached_body_shape_handles_.end())
00775   {
00776     for (std::size_t k = 0; k < it->second.size(); ++k)
00777       octomap_monitor_->forgetShape(it->second[k].first);
00778     ROS_DEBUG_NAMED(LOGNAME, "Including attached body '%s' in monitored octomap", attached_body->getName().c_str());
00779     attached_body_shape_handles_.erase(it);
00780   }
00781 }
00782 
00783 void planning_scene_monitor::PlanningSceneMonitor::excludeWorldObjectFromOctree(
00784     const collision_detection::World::ObjectConstPtr& obj)
00785 {
00786   if (!octomap_monitor_)
00787     return;
00788 
00789   boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
00790 
00791   bool found = false;
00792   for (std::size_t i = 0; i < obj->shapes_.size(); ++i)
00793   {
00794     if (obj->shapes_[i]->type == shapes::PLANE || obj->shapes_[i]->type == shapes::OCTREE)
00795       continue;
00796     occupancy_map_monitor::ShapeHandle h = octomap_monitor_->excludeShape(obj->shapes_[i]);
00797     if (h)
00798     {
00799       collision_body_shape_handles_[obj->id_].push_back(std::make_pair(h, &obj->shape_poses_[i]));
00800       found = true;
00801     }
00802   }
00803   if (found)
00804     ROS_DEBUG_NAMED(LOGNAME, "Excluding collision object '%s' from monitored octomap", obj->id_.c_str());
00805 }
00806 
00807 void planning_scene_monitor::PlanningSceneMonitor::includeWorldObjectInOctree(
00808     const collision_detection::World::ObjectConstPtr& obj)
00809 {
00810   if (!octomap_monitor_)
00811     return;
00812 
00813   boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
00814 
00815   CollisionBodyShapeHandles::iterator it = collision_body_shape_handles_.find(obj->id_);
00816   if (it != collision_body_shape_handles_.end())
00817   {
00818     for (std::size_t k = 0; k < it->second.size(); ++k)
00819       octomap_monitor_->forgetShape(it->second[k].first);
00820     ROS_DEBUG_NAMED(LOGNAME, "Including collision object '%s' in monitored octomap", obj->id_.c_str());
00821     collision_body_shape_handles_.erase(it);
00822   }
00823 }
00824 
00825 void planning_scene_monitor::PlanningSceneMonitor::currentStateAttachedBodyUpdateCallback(
00826     robot_state::AttachedBody* attached_body, bool just_attached)
00827 {
00828   if (!octomap_monitor_)
00829     return;
00830 
00831   if (just_attached)
00832     excludeAttachedBodyFromOctree(attached_body);
00833   else
00834     includeAttachedBodyInOctree(attached_body);
00835 }
00836 
00837 void planning_scene_monitor::PlanningSceneMonitor::currentWorldObjectUpdateCallback(
00838     const collision_detection::World::ObjectConstPtr& obj, collision_detection::World::Action action)
00839 {
00840   if (!octomap_monitor_)
00841     return;
00842   if (obj->id_ == planning_scene::PlanningScene::OCTOMAP_NS)
00843     return;
00844 
00845   if (action & collision_detection::World::CREATE)
00846     excludeWorldObjectFromOctree(obj);
00847   else if (action & collision_detection::World::DESTROY)
00848     includeWorldObjectInOctree(obj);
00849   else
00850   {
00851     excludeWorldObjectFromOctree(obj);
00852     includeWorldObjectInOctree(obj);
00853   }
00854 }
00855 
00856 bool planning_scene_monitor::PlanningSceneMonitor::waitForCurrentRobotState(const ros::Time& t, double wait_time)
00857 {
00858   if (t.isZero())
00859     return false;
00860 
00861   if (current_state_monitor_)
00862   {
00863     // Wait for next robot update in state monitor.
00864     bool success = current_state_monitor_->waitForCurrentState(t, wait_time);
00865 
00866     /* As robot updates are passed to the planning scene only in throttled fashion, there might
00867        be still an update pending. If so, explicitly update the planning scene here.
00868        If waitForCurrentState failed, we didn't get any new state updates within wait_time. */
00869     if (success)
00870     {
00871       boost::mutex::scoped_lock lock(state_pending_mutex_);
00872       if (state_update_pending_)  // enforce state update
00873       {
00874         state_update_pending_ = false;
00875         lock.unlock();
00876         updateSceneWithCurrentState();
00877       }
00878       return true;
00879     }
00880 
00881     ROS_WARN_NAMED(LOGNAME, "Failed to fetch current robot state.");
00882     return false;
00883   }
00884 
00885   // Sometimes there is no state monitor. In this case state updates are received as part of scene updates only.
00886   // However, scene updates are only published if the robot actually moves. Hence we need a timeout!
00887   // As publishing planning scene updates is throttled (2Hz by default), a 1s timeout is a suitable default.
00888   ros::WallTime timeout = ros::WallTime::now() + ros::WallDuration(wait_time);
00889   ros::WallDuration busywait(0.1);
00890   boost::shared_lock<boost::shared_mutex> lock(scene_update_mutex_);
00891   ros::Time prev_update_time = last_update_time_;
00892   while (last_update_time_ < t &&  // Wait until the state update actually reaches the scene.
00893          ros::WallTime::now() < timeout)
00894   {
00895     lock.unlock();
00896     busywait.sleep();
00897     lock.lock();
00898   }
00899   bool success = last_update_time_ >= t;
00900   // suppress warning if we received an update at all
00901   if (!success && prev_update_time != last_update_time_)
00902     ROS_WARN_NAMED(LOGNAME, "Maybe failed to update robot state, time diff: %.3fs", (t - last_update_time_).toSec());
00903 
00904   return success;
00905 }
00906 
00907 void planning_scene_monitor::PlanningSceneMonitor::lockSceneRead()
00908 {
00909   scene_update_mutex_.lock_shared();
00910   if (octomap_monitor_)
00911     octomap_monitor_->getOcTreePtr()->lockRead();
00912 }
00913 
00914 void planning_scene_monitor::PlanningSceneMonitor::unlockSceneRead()
00915 {
00916   if (octomap_monitor_)
00917     octomap_monitor_->getOcTreePtr()->unlockRead();
00918   scene_update_mutex_.unlock_shared();
00919 }
00920 
00921 void planning_scene_monitor::PlanningSceneMonitor::lockSceneWrite()
00922 {
00923   scene_update_mutex_.lock();
00924   if (octomap_monitor_)
00925     octomap_monitor_->getOcTreePtr()->lockWrite();
00926 }
00927 
00928 void planning_scene_monitor::PlanningSceneMonitor::unlockSceneWrite()
00929 {
00930   if (octomap_monitor_)
00931     octomap_monitor_->getOcTreePtr()->unlockWrite();
00932   scene_update_mutex_.unlock();
00933 }
00934 
00935 void planning_scene_monitor::PlanningSceneMonitor::startSceneMonitor(const std::string& scene_topic)
00936 {
00937   stopSceneMonitor();
00938 
00939   ROS_INFO_NAMED(LOGNAME, "Starting scene monitor");
00940   // listen for planning scene updates; these messages include transforms, so no need for filters
00941   if (!scene_topic.empty())
00942   {
00943     planning_scene_subscriber_ =
00944         root_nh_.subscribe(scene_topic, 100, &PlanningSceneMonitor::newPlanningSceneCallback, this);
00945     ROS_INFO_NAMED(LOGNAME, "Listening to '%s'", root_nh_.resolveName(scene_topic).c_str());
00946   }
00947 }
00948 
00949 void planning_scene_monitor::PlanningSceneMonitor::stopSceneMonitor()
00950 {
00951   if (planning_scene_subscriber_)
00952   {
00953     ROS_INFO_NAMED(LOGNAME, "Stopping scene monitor");
00954     planning_scene_subscriber_.shutdown();
00955   }
00956 }
00957 
00958 bool planning_scene_monitor::PlanningSceneMonitor::getShapeTransformCache(
00959     const std::string& target_frame, const ros::Time& target_time,
00960     occupancy_map_monitor::ShapeTransformCache& cache) const
00961 {
00962   if (!tf_)
00963     return false;
00964   try
00965   {
00966     boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
00967 
00968     for (LinkShapeHandles::const_iterator it = link_shape_handles_.begin(); it != link_shape_handles_.end(); ++it)
00969     {
00970       tf::StampedTransform tr;
00971       tf_->waitForTransform(target_frame, it->first->getName(), target_time, shape_transform_cache_lookup_wait_time_);
00972       tf_->lookupTransform(target_frame, it->first->getName(), target_time, tr);
00973       Eigen::Affine3d ttr;
00974       tf::transformTFToEigen(tr, ttr);
00975       for (std::size_t j = 0; j < it->second.size(); ++j)
00976         cache[it->second[j].first] = ttr * it->first->getCollisionOriginTransforms()[it->second[j].second];
00977     }
00978     for (AttachedBodyShapeHandles::const_iterator it = attached_body_shape_handles_.begin();
00979          it != attached_body_shape_handles_.end(); ++it)
00980     {
00981       tf::StampedTransform tr;
00982       tf_->waitForTransform(target_frame, it->first->getAttachedLinkName(), target_time,
00983                             shape_transform_cache_lookup_wait_time_);
00984       tf_->lookupTransform(target_frame, it->first->getAttachedLinkName(), target_time, tr);
00985       Eigen::Affine3d transform;
00986       tf::transformTFToEigen(tr, transform);
00987       for (std::size_t k = 0; k < it->second.size(); ++k)
00988         cache[it->second[k].first] = transform * it->first->getFixedTransforms()[it->second[k].second];
00989     }
00990     {
00991       tf::StampedTransform tr;
00992       tf_->waitForTransform(target_frame, scene_->getPlanningFrame(), target_time,
00993                             shape_transform_cache_lookup_wait_time_);
00994       tf_->lookupTransform(target_frame, scene_->getPlanningFrame(), target_time, tr);
00995       Eigen::Affine3d transform;
00996       tf::transformTFToEigen(tr, transform);
00997       for (CollisionBodyShapeHandles::const_iterator it = collision_body_shape_handles_.begin();
00998            it != collision_body_shape_handles_.end(); ++it)
00999         for (std::size_t k = 0; k < it->second.size(); ++k)
01000           cache[it->second[k].first] = transform * (*it->second[k].second);
01001     }
01002   }
01003   catch (tf::TransformException& ex)
01004   {
01005     ROS_ERROR_THROTTLE_NAMED(1, LOGNAME, "Transform error: %s", ex.what());
01006     return false;
01007   }
01008   return true;
01009 }
01010 
01011 void planning_scene_monitor::PlanningSceneMonitor::startWorldGeometryMonitor(
01012     const std::string& collision_objects_topic, const std::string& planning_scene_world_topic,
01013     const bool load_octomap_monitor)
01014 {
01015   stopWorldGeometryMonitor();
01016   ROS_INFO_NAMED(LOGNAME, "Starting world geometry monitor");
01017 
01018   // listen for world geometry updates using message filters
01019   if (!collision_objects_topic.empty())
01020   {
01021     collision_object_subscriber_.reset(
01022         new message_filters::Subscriber<moveit_msgs::CollisionObject>(root_nh_, collision_objects_topic, 1024));
01023     if (tf_)
01024     {
01025       collision_object_filter_.reset(new tf::MessageFilter<moveit_msgs::CollisionObject>(
01026           *collision_object_subscriber_, *tf_, scene_->getPlanningFrame(), 1024));
01027       collision_object_filter_->registerCallback(boost::bind(&PlanningSceneMonitor::collisionObjectCallback, this, _1));
01028       collision_object_filter_->registerFailureCallback(
01029           boost::bind(&PlanningSceneMonitor::collisionObjectFailTFCallback, this, _1, _2));
01030       ROS_INFO_NAMED(LOGNAME, "Listening to '%s' using message notifier with target frame '%s'",
01031                      root_nh_.resolveName(collision_objects_topic).c_str(),
01032                      collision_object_filter_->getTargetFramesString().c_str());
01033     }
01034     else
01035     {
01036       collision_object_subscriber_->registerCallback(
01037           boost::bind(&PlanningSceneMonitor::collisionObjectCallback, this, _1));
01038       ROS_INFO_NAMED(LOGNAME, "Listening to '%s'", root_nh_.resolveName(collision_objects_topic).c_str());
01039     }
01040   }
01041 
01042   if (!planning_scene_world_topic.empty())
01043   {
01044     planning_scene_world_subscriber_ =
01045         root_nh_.subscribe(planning_scene_world_topic, 1, &PlanningSceneMonitor::newPlanningSceneWorldCallback, this);
01046     ROS_INFO_NAMED(LOGNAME, "Listening to '%s' for planning scene world geometry",
01047                    root_nh_.resolveName(planning_scene_world_topic).c_str());
01048   }
01049 
01050   // Ocotomap monitor is optional
01051   if (load_octomap_monitor)
01052   {
01053     if (!octomap_monitor_)
01054     {
01055       octomap_monitor_.reset(new occupancy_map_monitor::OccupancyMapMonitor(tf_, scene_->getPlanningFrame()));
01056       excludeRobotLinksFromOctree();
01057       excludeAttachedBodiesFromOctree();
01058       excludeWorldObjectsFromOctree();
01059 
01060       octomap_monitor_->setTransformCacheCallback(
01061           boost::bind(&PlanningSceneMonitor::getShapeTransformCache, this, _1, _2, _3));
01062       octomap_monitor_->setUpdateCallback(boost::bind(&PlanningSceneMonitor::octomapUpdateCallback, this));
01063     }
01064     octomap_monitor_->startMonitor();
01065   }
01066 }
01067 
01068 void planning_scene_monitor::PlanningSceneMonitor::stopWorldGeometryMonitor()
01069 {
01070   if (collision_object_subscriber_ || collision_object_filter_)
01071   {
01072     ROS_INFO_NAMED(LOGNAME, "Stopping world geometry monitor");
01073     collision_object_filter_.reset();
01074     collision_object_subscriber_.reset();
01075     planning_scene_world_subscriber_.shutdown();
01076   }
01077   else if (planning_scene_world_subscriber_)
01078   {
01079     ROS_INFO_NAMED(LOGNAME, "Stopping world geometry monitor");
01080     planning_scene_world_subscriber_.shutdown();
01081   }
01082   if (octomap_monitor_)
01083     octomap_monitor_->stopMonitor();
01084 }
01085 
01086 void planning_scene_monitor::PlanningSceneMonitor::startStateMonitor(const std::string& joint_states_topic,
01087                                                                      const std::string& attached_objects_topic)
01088 {
01089   stopStateMonitor();
01090   if (scene_)
01091   {
01092     if (!current_state_monitor_)
01093       current_state_monitor_.reset(new CurrentStateMonitor(getRobotModel(), tf_));
01094     current_state_monitor_->addUpdateCallback(boost::bind(&PlanningSceneMonitor::onStateUpdate, this, _1));
01095     current_state_monitor_->startStateMonitor(joint_states_topic);
01096 
01097     {
01098       boost::mutex::scoped_lock lock(state_pending_mutex_);
01099       if (!dt_state_update_.isZero())
01100         state_update_timer_.start();
01101     }
01102 
01103     if (!attached_objects_topic.empty())
01104     {
01105       // using regular message filter as there's no header
01106       attached_collision_object_subscriber_ =
01107           root_nh_.subscribe(attached_objects_topic, 1024, &PlanningSceneMonitor::attachObjectCallback, this);
01108       ROS_INFO_NAMED(LOGNAME, "Listening to '%s' for attached collision objects",
01109                      root_nh_.resolveName(attached_objects_topic).c_str());
01110     }
01111   }
01112   else
01113     ROS_ERROR_NAMED(LOGNAME, "Cannot monitor robot state because planning scene is not configured");
01114 }
01115 
01116 void planning_scene_monitor::PlanningSceneMonitor::stopStateMonitor()
01117 {
01118   if (current_state_monitor_)
01119     current_state_monitor_->stopStateMonitor();
01120   if (attached_collision_object_subscriber_)
01121     attached_collision_object_subscriber_.shutdown();
01122 
01123   // stop must be called with state_pending_mutex_ unlocked to avoid deadlock
01124   state_update_timer_.stop();
01125   {
01126     boost::mutex::scoped_lock lock(state_pending_mutex_);
01127     state_update_pending_ = false;
01128   }
01129 }
01130 
01131 void planning_scene_monitor::PlanningSceneMonitor::onStateUpdate(
01132     const sensor_msgs::JointStateConstPtr& /* joint_state */)
01133 {
01134   const ros::WallTime& n = ros::WallTime::now();
01135   ros::WallDuration dt = n - last_state_update_;
01136 
01137   bool update = false;
01138   {
01139     boost::mutex::scoped_lock lock(state_pending_mutex_);
01140 
01141     if (dt < dt_state_update_)
01142     {
01143       state_update_pending_ = true;
01144     }
01145     else
01146     {
01147       state_update_pending_ = false;
01148       last_state_update_ = n;
01149       update = true;
01150     }
01151   }
01152 
01153   // run the state update with state_pending_mutex_ unlocked
01154   if (update)
01155     updateSceneWithCurrentState();
01156 }
01157 
01158 void planning_scene_monitor::PlanningSceneMonitor::stateUpdateTimerCallback(const ros::WallTimerEvent& event)
01159 {
01160   if (state_update_pending_)
01161   {
01162     bool update = false;
01163 
01164     const ros::WallTime& n = ros::WallTime::now();
01165     ros::WallDuration dt = n - last_state_update_;
01166 
01167     {
01168       // lock for access to dt_state_update_ and state_update_pending_
01169       boost::mutex::scoped_lock lock(state_pending_mutex_);
01170       if (state_update_pending_ && dt >= dt_state_update_)
01171       {
01172         state_update_pending_ = false;
01173         last_state_update_ = ros::WallTime::now();
01174         update = true;
01175       }
01176     }
01177 
01178     // run the state update with state_pending_mutex_ unlocked
01179     if (update)
01180       updateSceneWithCurrentState();
01181   }
01182 }
01183 
01184 void planning_scene_monitor::PlanningSceneMonitor::octomapUpdateCallback()
01185 {
01186   if (!octomap_monitor_)
01187     return;
01188 
01189   updateFrameTransforms();
01190   {
01191     boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
01192     last_update_time_ = ros::Time::now();
01193     octomap_monitor_->getOcTreePtr()->lockRead();
01194     try
01195     {
01196       scene_->processOctomapPtr(octomap_monitor_->getOcTreePtr(), Eigen::Affine3d::Identity());
01197       octomap_monitor_->getOcTreePtr()->unlockRead();
01198     }
01199     catch (...)
01200     {
01201       octomap_monitor_->getOcTreePtr()->unlockRead();  // unlock and rethrow
01202       throw;
01203     }
01204   }
01205   triggerSceneUpdateEvent(UPDATE_GEOMETRY);
01206 }
01207 
01208 void planning_scene_monitor::PlanningSceneMonitor::setStateUpdateFrequency(double hz)
01209 {
01210   bool update = false;
01211   if (hz > std::numeric_limits<double>::epsilon())
01212   {
01213     boost::mutex::scoped_lock lock(state_pending_mutex_);
01214     dt_state_update_.fromSec(1.0 / hz);
01215     state_update_timer_.setPeriod(dt_state_update_);
01216     state_update_timer_.start();
01217   }
01218   else
01219   {
01220     // stop must be called with state_pending_mutex_ unlocked to avoid deadlock
01221     state_update_timer_.stop();
01222     boost::mutex::scoped_lock lock(state_pending_mutex_);
01223     dt_state_update_ = ros::WallDuration(0, 0);
01224     if (state_update_pending_)
01225       update = true;
01226   }
01227   ROS_INFO_NAMED(LOGNAME, "Updating internal planning scene state at most every %lf seconds", dt_state_update_.toSec());
01228 
01229   if (update)
01230     updateSceneWithCurrentState();
01231 }
01232 
01233 void planning_scene_monitor::PlanningSceneMonitor::updateSceneWithCurrentState()
01234 {
01235   if (current_state_monitor_)
01236   {
01237     std::vector<std::string> missing;
01238     if (!current_state_monitor_->haveCompleteState(missing) &&
01239         (ros::Time::now() - current_state_monitor_->getMonitorStartTime()).toSec() > 1.0)
01240     {
01241       std::string missing_str = boost::algorithm::join(missing, ", ");
01242       ROS_WARN_THROTTLE_NAMED(1, LOGNAME, "The complete state of the robot is not yet known.  Missing %s",
01243                               missing_str.c_str());
01244     }
01245 
01246     {
01247       boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
01248       current_state_monitor_->setToCurrentState(scene_->getCurrentStateNonConst());
01249       last_update_time_ = current_state_monitor_->getCurrentStateTime();
01250       scene_->getCurrentStateNonConst().update();  // compute all transforms
01251     }
01252     triggerSceneUpdateEvent(UPDATE_STATE);
01253   }
01254   else
01255     ROS_ERROR_THROTTLE_NAMED(1, LOGNAME, "State monitor is not active. Unable to set the planning scene state");
01256 }
01257 
01258 void planning_scene_monitor::PlanningSceneMonitor::addUpdateCallback(const boost::function<void(SceneUpdateType)>& fn)
01259 {
01260   boost::recursive_mutex::scoped_lock lock(update_lock_);
01261   if (fn)
01262     update_callbacks_.push_back(fn);
01263 }
01264 
01265 void planning_scene_monitor::PlanningSceneMonitor::clearUpdateCallbacks()
01266 {
01267   boost::recursive_mutex::scoped_lock lock(update_lock_);
01268   update_callbacks_.clear();
01269 }
01270 
01271 void planning_scene_monitor::PlanningSceneMonitor::setPlanningScenePublishingFrequency(double hz)
01272 {
01273   publish_planning_scene_frequency_ = hz;
01274   ROS_DEBUG_NAMED(LOGNAME, "Maximum frquency for publishing a planning scene is now %lf Hz",
01275                   publish_planning_scene_frequency_);
01276 }
01277 
01278 void planning_scene_monitor::PlanningSceneMonitor::getUpdatedFrameTransforms(
01279     std::vector<geometry_msgs::TransformStamped>& transforms)
01280 {
01281   const std::string& target = getRobotModel()->getModelFrame();
01282 
01283   std::vector<std::string> all_frame_names;
01284   tf_->getFrameStrings(all_frame_names);
01285   for (std::size_t i = 0; i < all_frame_names.size(); ++i)
01286   {
01287     const std::string& frame_no_slash = (!all_frame_names[i].empty() && all_frame_names[i][0] == '/') ?
01288                                             all_frame_names[i].substr(1) :
01289                                             all_frame_names[i];
01290     const std::string& frame_with_slash =
01291         (!all_frame_names[i].empty() && all_frame_names[i][0] != '/') ? '/' + all_frame_names[i] : all_frame_names[i];
01292 
01293     if (frame_with_slash == target || getRobotModel()->hasLinkModel(frame_no_slash))
01294       continue;
01295 
01296     ros::Time stamp(0);
01297     std::string err_string;
01298     if (tf_->getLatestCommonTime(target, all_frame_names[i], stamp, &err_string) != tf::NO_ERROR)
01299     {
01300       ROS_WARN_STREAM_NAMED(LOGNAME, "No transform available between frame '"
01301                                          << all_frame_names[i] << "' and planning frame '" << target << "' ("
01302                                          << err_string << ")");
01303       continue;
01304     }
01305 
01306     tf::StampedTransform t;
01307     try
01308     {
01309       tf_->lookupTransform(target, all_frame_names[i], stamp, t);
01310     }
01311     catch (tf::TransformException& ex)
01312     {
01313       ROS_WARN_STREAM_NAMED(LOGNAME, "Unable to transform object from frame '"
01314                                          << all_frame_names[i] << "' to planning frame '" << target << "' ("
01315                                          << ex.what() << ")");
01316       continue;
01317     }
01318     geometry_msgs::TransformStamped f;
01319     f.header.frame_id = frame_with_slash;
01320     f.child_frame_id = target;
01321     f.transform.translation.x = t.getOrigin().x();
01322     f.transform.translation.y = t.getOrigin().y();
01323     f.transform.translation.z = t.getOrigin().z();
01324     const tf::Quaternion& q = t.getRotation();
01325     f.transform.rotation.x = q.x();
01326     f.transform.rotation.y = q.y();
01327     f.transform.rotation.z = q.z();
01328     f.transform.rotation.w = q.w();
01329     transforms.push_back(f);
01330   }
01331 }
01332 
01333 void planning_scene_monitor::PlanningSceneMonitor::updateFrameTransforms()
01334 {
01335   if (!tf_)
01336     return;
01337 
01338   if (scene_)
01339   {
01340     std::vector<geometry_msgs::TransformStamped> transforms;
01341     getUpdatedFrameTransforms(transforms);
01342     {
01343       boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
01344       scene_->getTransformsNonConst().setTransforms(transforms);
01345       last_update_time_ = ros::Time::now();
01346     }
01347     triggerSceneUpdateEvent(UPDATE_TRANSFORMS);
01348   }
01349 }
01350 
01351 void planning_scene_monitor::PlanningSceneMonitor::publishDebugInformation(bool flag)
01352 {
01353   if (octomap_monitor_)
01354     octomap_monitor_->publishDebugInformation(flag);
01355 }
01356 
01357 void planning_scene_monitor::PlanningSceneMonitor::configureCollisionMatrix(
01358     const planning_scene::PlanningScenePtr& scene)
01359 {
01360   if (!scene || robot_description_.empty())
01361     return;
01362   collision_detection::AllowedCollisionMatrix& acm = scene->getAllowedCollisionMatrixNonConst();
01363 
01364   // read overriding values from the param server
01365 
01366   // first we do default collision operations
01367   if (!nh_.hasParam(robot_description_ + "_planning/default_collision_operations"))
01368     ROS_DEBUG_NAMED(LOGNAME, "No additional default collision operations specified");
01369   else
01370   {
01371     ROS_DEBUG_NAMED(LOGNAME, "Reading additional default collision operations");
01372 
01373     XmlRpc::XmlRpcValue coll_ops;
01374     nh_.getParam(robot_description_ + "_planning/default_collision_operations", coll_ops);
01375 
01376     if (coll_ops.getType() != XmlRpc::XmlRpcValue::TypeArray)
01377     {
01378       ROS_WARN_NAMED(LOGNAME, "default_collision_operations is not an array");
01379       return;
01380     }
01381 
01382     if (coll_ops.size() == 0)
01383     {
01384       ROS_WARN_NAMED(LOGNAME, "No collision operations in default collision operations");
01385       return;
01386     }
01387 
01388     for (int i = 0; i < coll_ops.size(); ++i)
01389     {
01390       if (!coll_ops[i].hasMember("object1") || !coll_ops[i].hasMember("object2") || !coll_ops[i].hasMember("operation"))
01391       {
01392         ROS_WARN_NAMED(LOGNAME, "All collision operations must have two objects and an operation");
01393         continue;
01394       }
01395       acm.setEntry(std::string(coll_ops[i]["object1"]), std::string(coll_ops[i]["object2"]),
01396                    std::string(coll_ops[i]["operation"]) == "disable");
01397     }
01398   }
01399 }
01400 
01401 void planning_scene_monitor::PlanningSceneMonitor::configureDefaultPadding()
01402 {
01403   if (robot_description_.empty())
01404   {
01405     default_robot_padd_ = 0.0;
01406     default_robot_scale_ = 1.0;
01407     default_object_padd_ = 0.0;
01408     default_attached_padd_ = 0.0;
01409     return;
01410   }
01411 
01412   // Ensure no leading slash creates a bad param server address
01413   static const std::string robot_description =
01414       (robot_description_[0] == '/') ? robot_description_.substr(1) : robot_description_;
01415 
01416   nh_.param(robot_description + "_planning/default_robot_padding", default_robot_padd_, 0.0);
01417   nh_.param(robot_description + "_planning/default_robot_scale", default_robot_scale_, 1.0);
01418   nh_.param(robot_description + "_planning/default_object_padding", default_object_padd_, 0.0);
01419   nh_.param(robot_description + "_planning/default_attached_padding", default_attached_padd_, 0.0);
01420   nh_.param(robot_description + "_planning/default_robot_link_padding", default_robot_link_padd_,
01421             std::map<std::string, double>());
01422   nh_.param(robot_description + "_planning/default_robot_link_scale", default_robot_link_scale_,
01423             std::map<std::string, double>());
01424 
01425   ROS_DEBUG_STREAM_NAMED(LOGNAME, "Loaded " << default_robot_link_padd_.size() << " default link paddings");
01426   ROS_DEBUG_STREAM_NAMED(LOGNAME, "Loaded " << default_robot_link_scale_.size() << " default link scales");
01427 }


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Mon Apr 23 2018 10:25:14