00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <moveit/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
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
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
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_);
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();
00311 excludeWorldObjectsFromOctree();
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_);
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();
00331 excludeWorldObjectsFromOctree();
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_);
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
00419 if (!scene->is_diff && parent_scene_)
00420 {
00421
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();
00433 excludeWorldObjectsFromOctree();
00434 }
00435 }
00436
00437
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
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
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
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
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
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
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
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
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
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 & )
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();
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
01064
01065
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 }