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 the 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 
00041 #include <dynamic_reconfigure/server.h>
00042 #include <moveit_ros_planning/PlanningSceneMonitorDynamicReconfigureConfig.h>
00043 #include <tf_conversions/tf_eigen.h>
00044 #include <moveit/profiler/profiler.h>
00045 
00046 namespace planning_scene_monitor
00047 {
00048 
00049 using namespace moveit_ros_planning;
00050 
00051 class PlanningSceneMonitor::DynamicReconfigureImpl
00052 {
00053 public:
00054 
00055   DynamicReconfigureImpl(PlanningSceneMonitor *owner) : owner_(owner),
00056                             dynamic_reconfigure_server_(ros::NodeHandle(decideNamespace(owner->getName())))
00057   {
00058     dynamic_reconfigure_server_.setCallback(boost::bind(&DynamicReconfigureImpl::dynamicReconfigureCallback, this, _1, _2));
00059   }
00060 
00061 private:
00062 
00063   // make sure we do not advertise the same service multiple times, in case we use multiple PlanningSceneMonitor instances in a process
00064   static std::string decideNamespace(const std::string &name)
00065   {
00066     std::string ns = "~/" + name;
00067     std::replace(ns.begin(), ns.end(), ' ', '_');
00068     std::transform(ns.begin(), ns.end(), ns.begin(), ::tolower);
00069     if (ros::service::exists(ns + "/set_parameters", false))
00070     {
00071       unsigned int c = 1;
00072       while (ros::service::exists(ns + boost::lexical_cast<std::string>(c) + "/set_parameters", false))
00073         c++;
00074       ns += boost::lexical_cast<std::string>(c);
00075     }
00076     return ns;
00077   }
00078 
00079   void dynamicReconfigureCallback(PlanningSceneMonitorDynamicReconfigureConfig &config, uint32_t level)
00080   {
00081     PlanningSceneMonitor::SceneUpdateType event = PlanningSceneMonitor::UPDATE_NONE;
00082     if (config.publish_geometry_updates)
00083       event = (PlanningSceneMonitor::SceneUpdateType) ((int)event | (int)PlanningSceneMonitor::UPDATE_GEOMETRY);
00084     if (config.publish_state_updates)
00085       event = (PlanningSceneMonitor::SceneUpdateType) ((int)event | (int)PlanningSceneMonitor::UPDATE_STATE);
00086     if (config.publish_transforms_updates)
00087       event = (PlanningSceneMonitor::SceneUpdateType) ((int)event | (int)PlanningSceneMonitor::UPDATE_TRANSFORMS);
00088     if (config.publish_planning_scene)
00089     {
00090       owner_->setPlanningScenePublishingFrequency(config.publish_planning_scene_hz);
00091       owner_->startPublishingPlanningScene(event);
00092     }
00093     else
00094       owner_->stopPublishingPlanningScene();
00095   }
00096 
00097   PlanningSceneMonitor *owner_;
00098   dynamic_reconfigure::Server<PlanningSceneMonitorDynamicReconfigureConfig> dynamic_reconfigure_server_;
00099 };
00100 
00101 }
00102 
00103 planning_scene_monitor::PlanningSceneMonitor::PlanningSceneMonitor(const std::string &robot_description, const boost::shared_ptr<tf::Transformer> &tf, const std::string &name) :
00104   nh_("~"), tf_(tf), monitor_name_(name)
00105 {
00106   rm_loader_.reset(new robot_model_loader::RobotModelLoader(robot_description));
00107   initialize(planning_scene::PlanningScenePtr());
00108 }
00109 
00110 planning_scene_monitor::PlanningSceneMonitor::PlanningSceneMonitor(const planning_scene::PlanningScenePtr &scene, const std::string &robot_description,
00111                                                                    const boost::shared_ptr<tf::Transformer> &tf, const std::string &name) :
00112   nh_("~"), tf_(tf), monitor_name_(name)
00113 {
00114   rm_loader_.reset(new robot_model_loader::RobotModelLoader(robot_description));
00115   initialize(scene);
00116 }
00117 
00118 planning_scene_monitor::PlanningSceneMonitor::PlanningSceneMonitor(const robot_model_loader::RobotModelLoaderPtr &rm_loader,
00119                                                                    const boost::shared_ptr<tf::Transformer> &tf, const std::string &name) :
00120   nh_("~"), tf_(tf), rm_loader_(rm_loader), monitor_name_(name)
00121 {
00122   initialize(planning_scene::PlanningScenePtr());
00123 }
00124 
00125 planning_scene_monitor::PlanningSceneMonitor::PlanningSceneMonitor(const planning_scene::PlanningScenePtr &scene, const robot_model_loader::RobotModelLoaderPtr &rm_loader,
00126                                                                    const boost::shared_ptr<tf::Transformer> &tf, const std::string &name) :
00127   nh_("~"), tf_(tf), rm_loader_(rm_loader), monitor_name_(name)
00128 {
00129   initialize(scene);
00130 }
00131 
00132 planning_scene_monitor::PlanningSceneMonitor::~PlanningSceneMonitor()
00133 {
00134   if (scene_)
00135   {
00136     scene_->setCollisionObjectUpdateCallback(collision_detection::World::ObserverCallbackFn());
00137     scene_->setAttachedBodyUpdateCallback(robot_state::AttachedBodyCallback());
00138   }
00139   stopPublishingPlanningScene();
00140   stopStateMonitor();
00141   stopWorldGeometryMonitor();
00142   stopSceneMonitor();
00143   delete reconfigure_impl_;
00144   current_state_monitor_.reset();
00145   scene_const_.reset();
00146   scene_.reset();
00147   parent_scene_.reset();
00148   robot_model_.reset();
00149   rm_loader_.reset();
00150 }
00151 
00152 void planning_scene_monitor::PlanningSceneMonitor::initialize(const planning_scene::PlanningScenePtr &scene)
00153 {
00154   moveit::Profiler::ScopedStart prof_start;
00155   moveit::Profiler::ScopedBlock prof_block("PlanningSceneMonitor::initialize");
00156 
00157   if (monitor_name_.empty())
00158     monitor_name_ = "planning_scene_monitor";
00159   robot_description_ = rm_loader_->getRobotDescription();
00160   if (rm_loader_->getModel())
00161   {
00162     robot_model_ = rm_loader_->getModel();
00163     scene_ = scene;
00164     if (!scene_)
00165     {
00166       try
00167       {
00168         scene_.reset(new planning_scene::PlanningScene(rm_loader_->getModel()));
00169         scene_const_ = scene_;
00170         configureCollisionMatrix(scene_);
00171         configureDefaultPadding();
00172 
00173         scene_->getCollisionRobotNonConst()->setPadding(default_robot_padd_);
00174         scene_->getCollisionRobotNonConst()->setScale(default_robot_scale_);
00175         scene_->propogateRobotPadding();
00176       }
00177       catch (moveit::ConstructException &e)
00178       {
00179         ROS_ERROR("Configuration of planning scene failed");
00180         scene_.reset();
00181         scene_const_ = scene_;
00182       }
00183     }
00184     if (scene_)
00185     {
00186       scene_->setAttachedBodyUpdateCallback(boost::bind(&PlanningSceneMonitor::currentStateAttachedBodyUpdateCallback, this, _1, _2));
00187       scene_->setCollisionObjectUpdateCallback(boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, _1, _2));
00188     }
00189   }
00190   else
00191   {
00192     ROS_ERROR("Robot model not loaded");
00193   }
00194 
00195   publish_planning_scene_frequency_ = 2.0;
00196   new_scene_update_ = UPDATE_NONE;
00197 
00198   last_update_time_ = ros::Time::now();
00199   last_state_update_ = ros::WallTime::now();
00200   dt_state_update_ = 0.1;
00201 
00202   reconfigure_impl_ = new DynamicReconfigureImpl(this);
00203 }
00204 
00205 void planning_scene_monitor::PlanningSceneMonitor::monitorDiffs(bool flag)
00206 {
00207   if (scene_)
00208   {
00209     if (flag)
00210     {
00211       boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
00212       if (scene_)
00213       {
00214         scene_->setAttachedBodyUpdateCallback(robot_state::AttachedBodyCallback());
00215         scene_->setCollisionObjectUpdateCallback(collision_detection::World::ObserverCallbackFn());
00216         scene_->decoupleParent();
00217         parent_scene_ = scene_;
00218         scene_ = parent_scene_->diff();
00219         scene_const_ = scene_;
00220         scene_->setAttachedBodyUpdateCallback(boost::bind(&PlanningSceneMonitor::currentStateAttachedBodyUpdateCallback, this, _1, _2));
00221         scene_->setCollisionObjectUpdateCallback(boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, _1, _2));
00222       }
00223     }
00224     else
00225     {
00226       if (publish_planning_scene_)
00227       {
00228         ROS_WARN("Diff monitoring was stopped while publishing planning scene diffs. Stopping planning scene diff publisher");
00229         stopPublishingPlanningScene();
00230       }
00231       {
00232         boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
00233         if (scene_)
00234         {
00235           scene_->decoupleParent();
00236           parent_scene_.reset();
00237           // remove the '+' added by .diff() at the end of the scene name
00238           if (!scene_->getName().empty())
00239           {
00240             if (scene_->getName()[scene_->getName().length() - 1] == '+')
00241               scene_->setName(scene_->getName().substr(0, scene_->getName().length() - 1));
00242           }
00243         }
00244       }
00245     }
00246   }
00247 }
00248 
00249 void planning_scene_monitor::PlanningSceneMonitor::stopPublishingPlanningScene()
00250 {
00251   if (publish_planning_scene_)
00252   {
00253     boost::scoped_ptr<boost::thread> copy;
00254     copy.swap(publish_planning_scene_);
00255     new_scene_update_condition_.notify_all();
00256     copy->join();
00257     monitorDiffs(false);
00258     planning_scene_publisher_.shutdown();
00259     ROS_INFO("Stopped publishing maintained planning scene.");
00260   }
00261 }
00262 
00263 void planning_scene_monitor::PlanningSceneMonitor::startPublishingPlanningScene(SceneUpdateType update_type, const std::string &planning_scene_topic)
00264 {
00265   publish_update_types_ = update_type;
00266   if (!publish_planning_scene_ && scene_)
00267   {
00268     planning_scene_publisher_ = nh_.advertise<moveit_msgs::PlanningScene>(planning_scene_topic, 100, false);
00269     ROS_INFO("Publishing maintained planning scene on '%s'", planning_scene_topic.c_str());
00270     monitorDiffs(true);
00271     publish_planning_scene_.reset(new boost::thread(boost::bind(&PlanningSceneMonitor::scenePublishingThread, this)));
00272   }
00273 }
00274 
00275 void planning_scene_monitor::PlanningSceneMonitor::scenePublishingThread()
00276 {
00277   ROS_DEBUG("Started scene publishing thread ...");
00278 
00279   // publish the full planning scene
00280   moveit_msgs::PlanningScene msg;
00281   scene_->getPlanningSceneMsg(msg);
00282   planning_scene_publisher_.publish(msg);
00283   ROS_DEBUG("Published the full planning scene: '%s'", msg.name.c_str());
00284 
00285   bool have_diff = false;
00286   bool have_full = false;
00287   do
00288   {
00289     have_diff = false;
00290     have_full = false;
00291     ros::Rate rate(publish_planning_scene_frequency_);
00292     {
00293       boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
00294       while (new_scene_update_ == UPDATE_NONE && publish_planning_scene_)
00295         new_scene_update_condition_.wait(ulock);
00296       if (new_scene_update_ != UPDATE_NONE)
00297       {
00298         if (new_scene_update_ == UPDATE_SCENE)
00299         {
00300           rate.reset();
00301       boost::recursive_mutex::scoped_lock prevent_shape_cache_updates(shape_handles_lock_); // we don't want the transform cache to update while we are potentially changing attached bodies
00302       scene_->setAttachedBodyUpdateCallback(robot_state::AttachedBodyCallback());
00303           scene_->setCollisionObjectUpdateCallback(collision_detection::World::ObserverCallbackFn());
00304           scene_->pushDiffs(parent_scene_);
00305           scene_->clearDiffs();
00306       scene_->setAttachedBodyUpdateCallback(boost::bind(&PlanningSceneMonitor::currentStateAttachedBodyUpdateCallback, this, _1, _2));
00307           scene_->setCollisionObjectUpdateCallback(boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, _1, _2));
00308           if (octomap_monitor_)
00309           {
00310             excludeAttachedBodiesFromOctree(); // in case updates have happened to the attached bodies, put them in
00311             excludeWorldObjectsFromOctree(); // in case updates have happened to the attached bodies, put them in
00312           }
00313           scene_->getPlanningSceneMsg(msg);
00314           have_full = true;
00315         }
00316         else
00317           if (publish_update_types_ & new_scene_update_)
00318           {
00319             rate.reset();
00320             scene_->getPlanningSceneDiffMsg(msg);
00321         boost::recursive_mutex::scoped_lock prevent_shape_cache_updates(shape_handles_lock_); // we don't want the transform cache to update while we are potentially changing attached bodies
00322         scene_->setAttachedBodyUpdateCallback(robot_state::AttachedBodyCallback());
00323             scene_->setCollisionObjectUpdateCallback(collision_detection::World::ObserverCallbackFn());
00324             scene_->pushDiffs(parent_scene_);
00325             scene_->clearDiffs();
00326         scene_->setAttachedBodyUpdateCallback(boost::bind(&PlanningSceneMonitor::currentStateAttachedBodyUpdateCallback, this, _1, _2));
00327             scene_->setCollisionObjectUpdateCallback(boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, _1, _2));
00328             if (octomap_monitor_)
00329             {
00330               excludeAttachedBodiesFromOctree(); // in case updates have happened to the attached bodies, put them in
00331               excludeWorldObjectsFromOctree(); // in case updates have happened to the attached bodies, put them in
00332             }
00333             have_diff = true;
00334           }
00335         new_scene_update_ = UPDATE_NONE;
00336       }
00337     }
00338     if (have_diff)
00339     {
00340       planning_scene_publisher_.publish(msg);
00341       rate.sleep();
00342     }
00343     else
00344       if (have_full)
00345       {
00346         planning_scene_publisher_.publish(msg);
00347         ROS_DEBUG("Published complete planning scene: '%s'", msg.name.c_str());
00348         rate.sleep();
00349       }
00350   }
00351   while (publish_planning_scene_);
00352 }
00353 
00354 void planning_scene_monitor::PlanningSceneMonitor::getMonitoredTopics(std::vector<std::string> &topics) const
00355 {
00356   topics.clear();
00357   if (current_state_monitor_)
00358   {
00359     const std::string &t = current_state_monitor_->getMonitoredTopic();
00360     if (!t.empty())
00361       topics.push_back(t);
00362   }
00363   if (planning_scene_subscriber_)
00364     topics.push_back(planning_scene_subscriber_.getTopic());
00365   if (collision_object_subscriber_)
00366     topics.push_back(collision_object_subscriber_->getTopic());
00367   if (collision_map_subscriber_)
00368     topics.push_back(collision_map_subscriber_->getTopic());
00369   if (planning_scene_world_subscriber_)
00370     topics.push_back(planning_scene_world_subscriber_.getTopic());
00371 }
00372 
00373 namespace
00374 {
00375 bool sceneIsParentOf(const planning_scene::PlanningSceneConstPtr &scene, const planning_scene::PlanningScene *possible_parent)
00376 {
00377   if (scene && scene.get() == possible_parent)
00378     return true;
00379   if (scene)
00380     return sceneIsParentOf(scene->getParent(), possible_parent);
00381   return false;
00382 }
00383 }
00384 
00385 bool planning_scene_monitor::PlanningSceneMonitor::updatesScene(const planning_scene::PlanningScenePtr &scene) const
00386 {
00387   return sceneIsParentOf(scene_const_, scene.get());
00388 }
00389 
00390 bool planning_scene_monitor::PlanningSceneMonitor::updatesScene(const planning_scene::PlanningSceneConstPtr &scene) const
00391 {
00392   return sceneIsParentOf(scene_const_, scene.get());
00393 }
00394 
00395 void planning_scene_monitor::PlanningSceneMonitor::triggerSceneUpdateEvent(SceneUpdateType update_type)
00396 {
00397   for (std::size_t i = 0 ; i < update_callbacks_.size() ; ++i)
00398     update_callbacks_[i](update_type);
00399   new_scene_update_ = (SceneUpdateType) ((int)new_scene_update_ | (int)update_type);
00400   new_scene_update_condition_.notify_all();
00401 }
00402 
00403 void planning_scene_monitor::PlanningSceneMonitor::newPlanningSceneCallback(const moveit_msgs::PlanningSceneConstPtr &scene)
00404 {
00405   if (scene_)
00406   {
00407     SceneUpdateType upd = UPDATE_SCENE;
00408     std::string old_scene_name;
00409     {
00410       boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
00411       boost::recursive_mutex::scoped_lock prevent_shape_cache_updates(shape_handles_lock_); // we don't want the transform cache to update while we are potentially changing attached bodies
00412 
00413       last_update_time_ = ros::Time::now();
00414       old_scene_name = scene_->getName();
00415       scene_->usePlanningSceneMsg(*scene);
00416       robot_model_ = scene_->getRobotModel();
00417 
00418       // if we just reset the scene completely but we were maintaining diffs, we need to fix that
00419       if (!scene->is_diff && parent_scene_)
00420       {
00421         // the scene is now decoupled from the parent, since we just reset it
00422         scene_->setAttachedBodyUpdateCallback(robot_state::AttachedBodyCallback());
00423         scene_->setCollisionObjectUpdateCallback(collision_detection::World::ObserverCallbackFn());
00424         parent_scene_ = scene_;
00425         scene_ = parent_scene_->diff();
00426         scene_const_ = scene_;
00427         scene_->setAttachedBodyUpdateCallback(boost::bind(&PlanningSceneMonitor::currentStateAttachedBodyUpdateCallback, this, _1, _2));
00428         scene_->setCollisionObjectUpdateCallback(boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, _1, _2));
00429       }
00430       if (octomap_monitor_)
00431       {
00432         excludeAttachedBodiesFromOctree(); // in case updates have happened to the attached bodies, put them in
00433         excludeWorldObjectsFromOctree(); // in case updates have happened to the attached bodies, put them in
00434       }
00435     }
00436 
00437     // if we have a diff, try to more accuratelly determine the update type
00438     if (scene->is_diff)
00439     {
00440       bool no_other_scene_upd = (scene->name.empty() || scene->name == old_scene_name) &&
00441         scene->allowed_collision_matrix.entry_names.empty() && scene->link_padding.empty() && scene->link_scale.empty();
00442       if (no_other_scene_upd)
00443       {
00444         upd = UPDATE_NONE;
00445         if (!planning_scene::PlanningScene::isEmpty(scene->world))
00446           upd = (SceneUpdateType) ((int)upd | (int)UPDATE_GEOMETRY);
00447 
00448         if (!scene->fixed_frame_transforms.empty())
00449           upd = (SceneUpdateType) ((int)upd | (int)UPDATE_TRANSFORMS);
00450 
00451         if (!planning_scene::PlanningScene::isEmpty(scene->robot_state))
00452           upd = (SceneUpdateType) ((int)upd | (int)UPDATE_STATE);
00453       }
00454     }
00455     triggerSceneUpdateEvent(upd);
00456   }
00457 }
00458 
00459 void planning_scene_monitor::PlanningSceneMonitor::newPlanningSceneWorldCallback(const moveit_msgs::PlanningSceneWorldConstPtr &world)
00460 {
00461   if (scene_)
00462   {
00463     updateFrameTransforms();
00464     {
00465       boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
00466       last_update_time_ = ros::Time::now();
00467       scene_->getWorldNonConst()->clearObjects();
00468       scene_->processPlanningSceneWorldMsg(*world);
00469     }
00470     triggerSceneUpdateEvent(UPDATE_SCENE);
00471   }
00472 }
00473 
00474 void planning_scene_monitor::PlanningSceneMonitor::collisionObjectFailTFCallback(const moveit_msgs::CollisionObjectConstPtr &obj, tf::filter_failure_reasons::FilterFailureReason reason)
00475 {
00476   // if we just want to remove objects, the frame does not matter
00477   if (reason == tf::filter_failure_reasons::EmptyFrameID && obj->operation == moveit_msgs::CollisionObject::REMOVE)
00478     collisionObjectCallback(obj);
00479 }
00480 
00481 void planning_scene_monitor::PlanningSceneMonitor::collisionObjectCallback(const moveit_msgs::CollisionObjectConstPtr &obj)
00482 {
00483   if (scene_)
00484   {
00485     updateFrameTransforms();
00486     {
00487       boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
00488       last_update_time_ = ros::Time::now();
00489       scene_->processCollisionObjectMsg(*obj);
00490     }
00491     triggerSceneUpdateEvent(UPDATE_GEOMETRY);
00492   }
00493 }
00494 
00495 void planning_scene_monitor::PlanningSceneMonitor::attachObjectCallback(const moveit_msgs::AttachedCollisionObjectConstPtr &obj)
00496 {
00497   if (scene_)
00498   {
00499     updateFrameTransforms();
00500     {
00501       boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
00502       last_update_time_ = ros::Time::now();
00503       scene_->processAttachedCollisionObjectMsg(*obj);
00504     }
00505     triggerSceneUpdateEvent(UPDATE_GEOMETRY);
00506   }
00507 }
00508 
00509 void planning_scene_monitor::PlanningSceneMonitor::collisionMapCallback(const moveit_msgs::CollisionMapConstPtr &map)
00510 {
00511   if (scene_)
00512   {
00513     updateFrameTransforms();
00514     {
00515       boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
00516       last_update_time_ = ros::Time::now();
00517       scene_->processCollisionMapMsg(*map);
00518     }
00519     triggerSceneUpdateEvent(UPDATE_GEOMETRY);
00520   }
00521 }
00522 
00523 void planning_scene_monitor::PlanningSceneMonitor::excludeRobotLinksFromOctree()
00524 {
00525   boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
00526 
00527   includeRobotLinksInOctree();
00528   const std::vector<robot_model::LinkModel*> &links = getRobotModel()->getLinkModelsWithCollisionGeometry();
00529   for (std::size_t i = 0 ; i < links.size() ; ++i)
00530   {
00531     shapes::ShapeConstPtr shape = links[i]->getShape();
00532     // merge mesh vertices up to 0.1 mm apart
00533     if (links[i]->getShape()->type == shapes::MESH)
00534     {
00535       shapes::Mesh *m = static_cast<shapes::Mesh*>(links[i]->getShape()->clone());
00536       m->mergeVertices(1e-4);
00537       shape.reset(m);
00538     }
00539 
00540     occupancy_map_monitor::ShapeHandle h = octomap_monitor_->excludeShape(shape);
00541     if (h)
00542       link_shape_handles_[links[i]->getName()] = h;
00543   }
00544 }
00545 
00546 void planning_scene_monitor::PlanningSceneMonitor::includeRobotLinksInOctree()
00547 {
00548   if (!octomap_monitor_)
00549     return;
00550 
00551   boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
00552 
00553   for (LinkShapeHandles::iterator it = link_shape_handles_.begin() ; it != link_shape_handles_.end() ; ++it)
00554     octomap_monitor_->forgetShape(it->second);
00555   link_shape_handles_.clear();
00556 }
00557 
00558 void planning_scene_monitor::PlanningSceneMonitor::includeAttachedBodiesInOctree()
00559 {
00560   boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
00561 
00562   // clear information about any attached body, without refering to the AttachedBody* ptr (could be invalid)
00563   for (AttachedBodyShapeHandles::iterator it = attached_body_shape_handles_.begin() ; it != attached_body_shape_handles_.end() ; ++it)
00564     for (std::size_t k = 0 ; k < it->second.size() ; ++k)
00565       octomap_monitor_->forgetShape(it->second[k].first);
00566   attached_body_shape_handles_.clear();
00567 }
00568 
00569 void planning_scene_monitor::PlanningSceneMonitor::excludeAttachedBodiesFromOctree()
00570 {
00571   boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
00572 
00573   includeAttachedBodiesInOctree();
00574   // add attached objects again
00575   std::vector<const robot_state::AttachedBody*> ab;
00576   scene_->getCurrentState().getAttachedBodies(ab);
00577   for (std::size_t i = 0 ; i < ab.size() ; ++i)
00578     excludeAttachedBodyFromOctree(ab[i]);
00579 }
00580 
00581 void planning_scene_monitor::PlanningSceneMonitor::includeWorldObjectsInOctree()
00582 {
00583   boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
00584 
00585   // clear information about any attached object
00586   for (CollisionBodyShapeHandles::iterator it = collision_body_shape_handles_.begin() ; it != collision_body_shape_handles_.end() ; ++it)
00587     for (std::size_t k = 0 ; k < it->second.size() ; ++k)
00588       octomap_monitor_->forgetShape(it->second[k].first);
00589   collision_body_shape_handles_.clear();
00590 }
00591 
00592 void planning_scene_monitor::PlanningSceneMonitor::excludeWorldObjectsFromOctree()
00593 {
00594   boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
00595 
00596   includeWorldObjectsInOctree();
00597   for (collision_detection::World::const_iterator it = scene_->getWorld()->begin(); it != scene_->getWorld()->end() ; ++it)
00598     excludeWorldObjectFromOctree(it->second);
00599 }
00600 
00601 void planning_scene_monitor::PlanningSceneMonitor::excludeAttachedBodyFromOctree(const robot_state::AttachedBody *attached_body)
00602 {
00603   boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
00604   bool found = false;
00605   for (std::size_t i = 0 ; i < attached_body->getShapes().size() ; ++i)
00606   {
00607     if (attached_body->getShapes()[i]->type == shapes::PLANE || attached_body->getShapes()[i]->type == shapes::OCTREE)
00608       continue;
00609     occupancy_map_monitor::ShapeHandle h = octomap_monitor_->excludeShape(attached_body->getShapes()[i]);
00610     if (h)
00611     {
00612       found = true;
00613       attached_body_shape_handles_[attached_body].push_back(std::make_pair(h, i));
00614     }
00615   }
00616   if (found)
00617     ROS_DEBUG("Excluding attached body '%s' from monitored octomap", attached_body->getName().c_str());
00618 }
00619 
00620 void planning_scene_monitor::PlanningSceneMonitor::includeAttachedBodyInOctree(const robot_state::AttachedBody *attached_body)
00621 {
00622   boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
00623 
00624   AttachedBodyShapeHandles::iterator it = attached_body_shape_handles_.find(attached_body);
00625   if (it != attached_body_shape_handles_.end())
00626   {
00627     for (std::size_t k = 0 ; k < it->second.size() ; ++k)
00628       octomap_monitor_->forgetShape(it->second[k].first);
00629     ROS_DEBUG("Including attached body '%s' in monitored octomap", attached_body->getName().c_str());
00630     attached_body_shape_handles_.erase(it);
00631   }
00632 }
00633 
00634 void planning_scene_monitor::PlanningSceneMonitor::excludeWorldObjectFromOctree(const collision_detection::World::ObjectConstPtr &obj)
00635 {
00636   boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
00637 
00638   bool found = false;
00639   for (std::size_t i = 0 ; i < obj->shapes_.size() ; ++i)
00640   {
00641     if (obj->shapes_[i]->type == shapes::PLANE || obj->shapes_[i]->type == shapes::OCTREE)
00642       continue;
00643     occupancy_map_monitor::ShapeHandle h = octomap_monitor_->excludeShape(obj->shapes_[i]);
00644     if (h)
00645     {
00646       collision_body_shape_handles_[obj->id_].push_back(std::make_pair(h, &obj->shape_poses_[i]));
00647       found = true;
00648     }
00649   }
00650   if (found)
00651     ROS_DEBUG("Excluding collision object '%s' from monitored octomap", obj->id_.c_str());
00652 }
00653 
00654 void planning_scene_monitor::PlanningSceneMonitor::includeWorldObjectInOctree(const collision_detection::World::ObjectConstPtr &obj)
00655 {
00656   boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
00657 
00658   CollisionBodyShapeHandles::iterator it = collision_body_shape_handles_.find(obj->id_);
00659   if (it != collision_body_shape_handles_.end())
00660   {
00661     for (std::size_t k = 0 ; k < it->second.size() ; ++k)
00662       octomap_monitor_->forgetShape(it->second[k].first);
00663     ROS_DEBUG("Including collision object '%s' in monitored octomap", obj->id_.c_str());
00664     collision_body_shape_handles_.erase(it);
00665   }
00666 }
00667 
00668 void planning_scene_monitor::PlanningSceneMonitor::currentStateAttachedBodyUpdateCallback(robot_state::AttachedBody *attached_body, bool just_attached)
00669 {
00670   if (!octomap_monitor_)
00671     return;
00672 
00673   if (just_attached)
00674     excludeAttachedBodyFromOctree(attached_body);
00675   else
00676     includeAttachedBodyInOctree(attached_body);
00677 }
00678 
00679 void planning_scene_monitor::PlanningSceneMonitor::currentWorldObjectUpdateCallback(const collision_detection::World::ObjectConstPtr &obj, collision_detection::World::Action action)
00680 {
00681   if (!octomap_monitor_)
00682     return;
00683   if (obj->id_ == planning_scene::PlanningScene::OCTOMAP_NS)
00684     return;
00685 
00686   if (action & collision_detection::World::CREATE)
00687     excludeWorldObjectFromOctree(obj);
00688   else
00689     if (action & collision_detection::World::DESTROY)
00690       includeWorldObjectInOctree(obj);
00691     else
00692     {
00693       excludeWorldObjectFromOctree(obj);
00694       includeWorldObjectInOctree(obj);
00695     }
00696 }
00697 
00698 void planning_scene_monitor::PlanningSceneMonitor::lockSceneRead()
00699 {
00700   scene_update_mutex_.lock_shared();
00701   if (octomap_monitor_)
00702     octomap_monitor_->getOcTreePtr()->lockRead();
00703 }
00704 
00705 void planning_scene_monitor::PlanningSceneMonitor::unlockSceneRead()
00706 {
00707   scene_update_mutex_.unlock_shared();
00708   if (octomap_monitor_)
00709     octomap_monitor_->getOcTreePtr()->unlockRead();
00710 }
00711 
00712 void planning_scene_monitor::PlanningSceneMonitor::lockSceneWrite()
00713 {
00714   scene_update_mutex_.lock();
00715   if (octomap_monitor_)
00716     octomap_monitor_->getOcTreePtr()->lockWrite();
00717 }
00718 
00719 void planning_scene_monitor::PlanningSceneMonitor::unlockSceneWrite()
00720 {
00721   scene_update_mutex_.unlock();
00722   if (octomap_monitor_)
00723     octomap_monitor_->getOcTreePtr()->unlockWrite();
00724 }
00725 
00726 void planning_scene_monitor::PlanningSceneMonitor::startSceneMonitor(const std::string &scene_topic)
00727 {
00728   stopSceneMonitor();
00729 
00730   ROS_INFO("Starting scene monitor");
00731   // listen for planning scene updates; these messages include transforms, so no need for filters
00732   if (!scene_topic.empty())
00733   {
00734     planning_scene_subscriber_ = root_nh_.subscribe(scene_topic, 100, &PlanningSceneMonitor::newPlanningSceneCallback, this);
00735     ROS_INFO("Listening to '%s'", scene_topic.c_str());
00736   }
00737 }
00738 
00739 void planning_scene_monitor::PlanningSceneMonitor::stopSceneMonitor()
00740 {
00741   if (planning_scene_subscriber_)
00742   {
00743     ROS_INFO("Stopping scene monitor");
00744     planning_scene_subscriber_.shutdown();
00745   }
00746 }
00747 
00748 bool planning_scene_monitor::PlanningSceneMonitor::getShapeTransformCache(const std::string &target_frame, const ros::Time &target_time,
00749                                                                           occupancy_map_monitor::ShapeTransformCache &cache) const
00750 {
00751   if (!tf_)
00752     return false;
00753   try
00754   {
00755     boost::recursive_mutex::scoped_lock _(shape_handles_lock_);
00756 
00757     for (LinkShapeHandles::const_iterator it = link_shape_handles_.begin() ; it != link_shape_handles_.end() ; ++it)
00758     {
00759       tf::StampedTransform tr;
00760       tf_->lookupTransform(target_frame, it->first, target_time, tr);
00761       Eigen::Affine3d &transform = cache[it->second];
00762       tf::transformTFToEigen(tr, transform);
00763       transform = transform * getRobotModel()->getLinkModel(it->first)->getCollisionOriginTransform();
00764     }
00765     for (AttachedBodyShapeHandles::const_iterator it = attached_body_shape_handles_.begin() ; it != attached_body_shape_handles_.end() ; ++it)
00766     {
00767       tf::StampedTransform tr;
00768       tf_->lookupTransform(target_frame, it->first->getAttachedLinkName(), target_time, tr);
00769       Eigen::Affine3d transform;
00770       tf::transformTFToEigen(tr, transform);
00771       for (std::size_t k = 0 ; k < it->second.size() ; ++k)
00772         cache[it->second[k].first] = transform * it->first->getFixedTransforms()[it->second[k].second];
00773     }
00774     {
00775       tf::StampedTransform tr;
00776       tf_->lookupTransform(target_frame, scene_->getPlanningFrame(), target_time, tr);
00777       Eigen::Affine3d transform;
00778       tf::transformTFToEigen(tr, transform);
00779       for (CollisionBodyShapeHandles::const_iterator it = collision_body_shape_handles_.begin() ; it != collision_body_shape_handles_.end() ; ++it)
00780         for (std::size_t k = 0 ; k < it->second.size() ; ++k)
00781           cache[it->second[k].first] = transform * (*it->second[k].second);
00782     }
00783   }
00784   catch (tf::TransformException& ex)
00785   {
00786     ROS_ERROR_THROTTLE(1, "Transform error: %s", ex.what());
00787     return false;
00788   }
00789   return true;
00790 }
00791 
00792 void planning_scene_monitor::PlanningSceneMonitor::startWorldGeometryMonitor(const std::string &collision_objects_topic,
00793                                                                              const std::string &collision_map_topic,
00794                                                                              const std::string &planning_scene_world_topic)
00795 {
00796   stopWorldGeometryMonitor();
00797   ROS_INFO("Starting world geometry monitor");
00798 
00799   // listen for world geometry updates using message filters
00800   if (!collision_objects_topic.empty())
00801   {
00802     collision_object_subscriber_.reset(new message_filters::Subscriber<moveit_msgs::CollisionObject>(root_nh_, collision_objects_topic, 1024));
00803     if (tf_)
00804     {
00805       collision_object_filter_ .reset(new tf::MessageFilter<moveit_msgs::CollisionObject>(*collision_object_subscriber_, *tf_, scene_->getPlanningFrame(), 1024));
00806       collision_object_filter_->registerCallback(boost::bind(&PlanningSceneMonitor::collisionObjectCallback, this, _1));
00807       collision_object_filter_->registerFailureCallback(boost::bind(&PlanningSceneMonitor::collisionObjectFailTFCallback, this, _1, _2));
00808       ROS_INFO("Listening to '%s' using message notifier with target frame '%s'", collision_objects_topic.c_str(), collision_object_filter_->getTargetFramesString().c_str());
00809     }
00810     else
00811     {
00812       collision_object_subscriber_->registerCallback(boost::bind(&PlanningSceneMonitor::collisionObjectCallback, this, _1));
00813       ROS_INFO("Listening to '%s'", collision_objects_topic.c_str());
00814     }
00815   }
00816 
00817   if (!collision_map_topic.empty())
00818   {
00819     // listen to collision map using filters
00820     collision_map_subscriber_.reset(new message_filters::Subscriber<moveit_msgs::CollisionMap>(root_nh_, collision_map_topic, 2));
00821     if (tf_)
00822     {
00823       collision_map_filter_.reset(new tf::MessageFilter<moveit_msgs::CollisionMap>(*collision_map_subscriber_, *tf_, scene_->getPlanningFrame(), 2));
00824       collision_map_filter_->registerCallback(boost::bind(&PlanningSceneMonitor::collisionMapCallback, this, _1));
00825       ROS_INFO("Listening to '%s' using message notifier with target frame '%s'", collision_map_topic.c_str(), collision_map_filter_->getTargetFramesString().c_str());
00826     }
00827     else
00828     {
00829       collision_map_subscriber_->registerCallback(boost::bind(&PlanningSceneMonitor::collisionMapCallback, this, _1));
00830       ROS_INFO("Listening to '%s'", collision_map_topic.c_str());
00831     }
00832   }
00833 
00834   if (!planning_scene_world_topic.empty())
00835   {
00836     planning_scene_world_subscriber_ = root_nh_.subscribe(planning_scene_world_topic, 1, &PlanningSceneMonitor::newPlanningSceneWorldCallback, this);
00837     ROS_INFO("Listening to '%s' for planning scene world geometry", planning_scene_world_topic.c_str());
00838   }
00839   if (!octomap_monitor_)
00840   {
00841     octomap_monitor_.reset(new occupancy_map_monitor::OccupancyMapMonitor(tf_, scene_->getPlanningFrame()));
00842     excludeRobotLinksFromOctree();
00843     excludeAttachedBodiesFromOctree();
00844     excludeWorldObjectsFromOctree();
00845 
00846     octomap_monitor_->setTransformCacheCallback(boost::bind(&PlanningSceneMonitor::getShapeTransformCache, this, _1, _2, _3));
00847     octomap_monitor_->setUpdateCallback(boost::bind(&PlanningSceneMonitor::octomapUpdateCallback, this));
00848   }
00849   octomap_monitor_->startMonitor();
00850 }
00851 
00852 void planning_scene_monitor::PlanningSceneMonitor::stopWorldGeometryMonitor()
00853 {
00854   if (collision_object_subscriber_ || collision_object_filter_ ||
00855       collision_map_subscriber_ || collision_map_filter_)
00856   {
00857     ROS_INFO("Stopping world geometry monitor");
00858     collision_object_filter_.reset();
00859     collision_object_subscriber_.reset();
00860     collision_map_filter_.reset();
00861     collision_map_subscriber_.reset();
00862     planning_scene_world_subscriber_.shutdown();
00863   }
00864   else
00865     if (planning_scene_world_subscriber_)
00866     {
00867       ROS_INFO("Stopping world geometry monitor");
00868       planning_scene_world_subscriber_.shutdown();
00869     }
00870   if (octomap_monitor_)
00871     octomap_monitor_->stopMonitor();
00872 }
00873 
00874 void planning_scene_monitor::PlanningSceneMonitor::startStateMonitor(const std::string &joint_states_topic, const std::string &attached_objects_topic)
00875 {
00876   stopStateMonitor();
00877   if (scene_)
00878   {
00879     if (!current_state_monitor_)
00880       current_state_monitor_.reset(new CurrentStateMonitor(getRobotModel(), tf_));
00881     current_state_monitor_->addUpdateCallback(boost::bind(&PlanningSceneMonitor::onStateUpdate, this, _1));
00882     current_state_monitor_->startStateMonitor(joint_states_topic);
00883 
00884     if (!attached_objects_topic.empty())
00885     {
00886       // using regular message filter as there's no header
00887       attached_collision_object_subscriber_ = root_nh_.subscribe(attached_objects_topic, 1024, &PlanningSceneMonitor::attachObjectCallback, this);
00888       ROS_INFO("Listening to '%s' for attached collision objects", attached_objects_topic.c_str());
00889     }
00890   }
00891   else
00892     ROS_ERROR("Cannot monitor robot state because planning scene is not configured");
00893 }
00894 
00895 void planning_scene_monitor::PlanningSceneMonitor::stopStateMonitor()
00896 {
00897   if (current_state_monitor_)
00898     current_state_monitor_->stopStateMonitor();
00899   if (attached_collision_object_subscriber_)
00900     attached_collision_object_subscriber_.shutdown();
00901 }
00902 
00903 void planning_scene_monitor::PlanningSceneMonitor::onStateUpdate(const sensor_msgs::JointStateConstPtr & /* joint_state */ )
00904 {
00905   const ros::WallTime &n = ros::WallTime::now();
00906   const double t = (n - last_state_update_).toSec();
00907   if (t >= dt_state_update_ && dt_state_update_ > std::numeric_limits<double>::epsilon())
00908   {
00909     last_state_update_ = n;
00910     updateSceneWithCurrentState();
00911   }
00912 }
00913 
00914 void planning_scene_monitor::PlanningSceneMonitor::octomapUpdateCallback()
00915 {
00916   updateFrameTransforms();
00917   {
00918     boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
00919     last_update_time_ = ros::Time::now();
00920     octomap_monitor_->getOcTreePtr()->lockRead();
00921     try
00922     {
00923       scene_->processOctomapPtr(octomap_monitor_->getOcTreePtr(), Eigen::Affine3d::Identity());
00924       octomap_monitor_->getOcTreePtr()->unlockRead();
00925     }
00926     catch(...)
00927     {
00928       octomap_monitor_->getOcTreePtr()->unlockRead(); // unlock and rethrow
00929       throw;
00930     }
00931   }
00932   triggerSceneUpdateEvent(UPDATE_GEOMETRY);
00933 }
00934 
00935 void planning_scene_monitor::PlanningSceneMonitor::setStateUpdateFrequency(double hz)
00936 {
00937   if (hz > std::numeric_limits<double>::epsilon())
00938     dt_state_update_ = 1.0 / hz;
00939   else
00940     dt_state_update_ = 0.0;
00941   ROS_INFO("Updating internal planning scene state at most every %lf seconds", dt_state_update_);
00942 }
00943 
00944 void planning_scene_monitor::PlanningSceneMonitor::updateSceneWithCurrentState()
00945 {
00946   if (current_state_monitor_)
00947   {
00948     std::vector<std::string> missing;
00949     if (!current_state_monitor_->haveCompleteState(missing) && (ros::Time::now() - current_state_monitor_->getMonitorStartTime()).toSec() > 1.0)
00950     {
00951       std::string missing_str = boost::algorithm::join(missing, ", ");
00952       ROS_WARN_THROTTLE(1, "The complete state of the robot is not yet known.  Missing %s", missing_str.c_str());
00953     }
00954 
00955     {
00956       boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
00957       const std::map<std::string, double> &v = current_state_monitor_->getCurrentStateValues();
00958       scene_->getCurrentStateNonConst().setStateValues(v);
00959       last_update_time_ = ros::Time::now();
00960     }
00961     triggerSceneUpdateEvent(UPDATE_STATE);
00962   }
00963   else
00964     ROS_ERROR_THROTTLE(1, "State monitor is not active. Unable to set the planning scene state");
00965 }
00966 
00967 void planning_scene_monitor::PlanningSceneMonitor::addUpdateCallback(const boost::function<void(SceneUpdateType)> &fn)
00968 {
00969   if (fn)
00970     update_callbacks_.push_back(fn);
00971 }
00972 
00973 void planning_scene_monitor::PlanningSceneMonitor::clearUpdateCallbacks()
00974 {
00975   update_callbacks_.clear();
00976 }
00977 
00978 void planning_scene_monitor::PlanningSceneMonitor::setPlanningScenePublishingFrequency(double hz)
00979 {
00980   publish_planning_scene_frequency_ = hz;
00981   ROS_DEBUG("Maximum frquency for publishing a planning scene is now %lf Hz", publish_planning_scene_frequency_);
00982 }
00983 
00984 void planning_scene_monitor::PlanningSceneMonitor::getUpdatedFrameTransforms(std::vector<geometry_msgs::TransformStamped> &transforms)
00985 {
00986   const std::string &target = getRobotModel()->getModelFrame();
00987 
00988   std::vector<std::string> all_frame_names;
00989   tf_->getFrameStrings(all_frame_names);
00990   for (std::size_t i = 0 ; i < all_frame_names.size() ; ++i)
00991   {
00992     const std::string &frame_no_slash = (!all_frame_names[i].empty() && all_frame_names[i][0] == '/') ? all_frame_names[i].substr(1) : all_frame_names[i];
00993     const std::string &frame_with_slash = (!all_frame_names[i].empty() && all_frame_names[i][0] != '/') ? '/' + all_frame_names[i] : all_frame_names[i];
00994 
00995     if (frame_with_slash == target || getRobotModel()->hasLinkModel(frame_no_slash))
00996       continue;
00997 
00998     ros::Time stamp(0);
00999     std::string err_string;
01000     if (tf_->getLatestCommonTime(target, all_frame_names[i], stamp, &err_string) != tf::NO_ERROR)
01001     {
01002       ROS_WARN_STREAM("No transform available between frame '" << all_frame_names[i] << "' and planning frame '" <<
01003                       target << "' (" << err_string << ")");
01004       continue;
01005     }
01006 
01007     tf::StampedTransform t;
01008     try
01009     {
01010       tf_->lookupTransform(target, all_frame_names[i], stamp, t);
01011     }
01012     catch (tf::TransformException& ex)
01013     {
01014       ROS_WARN_STREAM("Unable to transform object from frame '" << all_frame_names[i] << "' to planning frame '" <<
01015                       target << "' (" << ex.what() << ")");
01016       continue;
01017     }
01018     geometry_msgs::TransformStamped f;
01019     f.header.frame_id = frame_with_slash;
01020     f.child_frame_id = target;
01021     f.transform.translation.x = t.getOrigin().x();
01022     f.transform.translation.y = t.getOrigin().y();
01023     f.transform.translation.z = t.getOrigin().z();
01024     const tf::Quaternion &q = t.getRotation();
01025     f.transform.rotation.x = q.x();
01026     f.transform.rotation.y = q.y();
01027     f.transform.rotation.z = q.z();
01028     f.transform.rotation.w = q.w();
01029     transforms.push_back(f);
01030   }
01031 }
01032 
01033 void planning_scene_monitor::PlanningSceneMonitor::updateFrameTransforms()
01034 {
01035   if (!tf_)
01036     return;
01037 
01038   if (scene_)
01039   {
01040     std::vector<geometry_msgs::TransformStamped> transforms;
01041     getUpdatedFrameTransforms(transforms);
01042     {
01043       boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
01044       scene_->getTransformsNonConst().setTransforms(transforms);
01045       last_update_time_ = ros::Time::now();
01046     }
01047     triggerSceneUpdateEvent(UPDATE_TRANSFORMS);
01048   }
01049 }
01050 
01051 void planning_scene_monitor::PlanningSceneMonitor::publishDebugInformation(bool flag)
01052 {
01053   if (octomap_monitor_)
01054     octomap_monitor_->publishDebugInformation(flag);
01055 }
01056 
01057 void planning_scene_monitor::PlanningSceneMonitor::configureCollisionMatrix(const planning_scene::PlanningScenePtr &scene)
01058 {
01059   if (!scene || robot_description_.empty())
01060     return;
01061   collision_detection::AllowedCollisionMatrix &acm = scene->getAllowedCollisionMatrixNonConst();
01062 
01063   // read overriding values from the param server
01064 
01065   // first we do default collision operations
01066   if (!nh_.hasParam(robot_description_ + "_planning/default_collision_operations"))
01067     ROS_DEBUG("No additional default collision operations specified");
01068   else
01069   {
01070     ROS_DEBUG("Reading additional default collision operations");
01071 
01072     XmlRpc::XmlRpcValue coll_ops;
01073     nh_.getParam(robot_description_ + "_planning/default_collision_operations", coll_ops);
01074 
01075     if (coll_ops.getType() != XmlRpc::XmlRpcValue::TypeArray)
01076     {
01077       ROS_WARN("default_collision_operations is not an array");
01078       return;
01079     }
01080 
01081     if (coll_ops.size() == 0)
01082     {
01083       ROS_WARN("No collision operations in default collision operations");
01084       return;
01085     }
01086 
01087     for (int i = 0 ; i < coll_ops.size() ; ++i)
01088     {
01089       if (!coll_ops[i].hasMember("object1") || !coll_ops[i].hasMember("object2") || !coll_ops[i].hasMember("operation"))
01090       {
01091         ROS_WARN("All collision operations must have two objects and an operation");
01092         continue;
01093       }
01094       acm.setEntry(std::string(coll_ops[i]["object1"]), std::string(coll_ops[i]["object2"]), std::string(coll_ops[i]["operation"]) == "disable");
01095     }
01096   }
01097 }
01098 
01099 void planning_scene_monitor::PlanningSceneMonitor::configureDefaultPadding()
01100 {
01101   if (robot_description_.empty())
01102   {
01103     default_robot_padd_ = 0.0;
01104     default_robot_scale_ = 1.0;
01105     default_object_padd_ = 0.0;
01106     default_attached_padd_ = 0.0;
01107     return;
01108   }
01109   nh_.param(robot_description_ + "_planning/default_robot_padding", default_robot_padd_, 0.0);
01110   nh_.param(robot_description_ + "_planning/default_robot_scale", default_robot_scale_, 1.0);
01111   nh_.param(robot_description_ + "_planning/default_object_padding", default_object_padd_, 0.0);
01112   nh_.param(robot_description_ + "_planning/default_attached_padding", default_attached_padd_, 0.0);
01113 }


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Mon Oct 6 2014 02:31:40