diff --git a/planning_scene/include/moveit/planning_scene/planning_scene.h b/planning_scene/include/moveit/planning_scene/planning_scene.h index b4158a7a..b913d7d2 100644 --- a/planning_scene/include/moveit/planning_scene/planning_scene.h +++ b/planning_scene/include/moveit/planning_scene/planning_scene.h @@ -98,6 +98,8 @@ class PlanningScene : private boost::noncopyable, collision_detection::WorldPtr world = collision_detection::WorldPtr(new collision_detection::World())); static const std::string OCTOMAP_NS; + static const std::string OCTOMAP_MSG_TYPE; + static const std::string OCTOMAP_DIFF_MSG_TYPE; static const std::string DEFAULT_SCENE_NAME; ~PlanningScene(); @@ -692,6 +694,8 @@ class PlanningScene : private boost::noncopyable, void processOctomapMsg(const octomap_msgs::OctomapWithPose &map); void processOctomapMsg(const octomap_msgs::Octomap &map); + void processOctomapMsg(const octomap_msgs::Octomap &map, const Eigen::Affine3d &t); + void processOctomapMsgDiff(const octomap_msgs::Octomap &map, boost::shared_ptr octree); void processOctomapPtr(const boost::shared_ptr &octree, const Eigen::Affine3d &t); /** @@ -890,6 +894,7 @@ class PlanningScene : private boost::noncopyable, void getPlanningSceneMsgCollisionObject(moveit_msgs::PlanningScene &scene, const std::string &ns) const; void getPlanningSceneMsgCollisionObjects(moveit_msgs::PlanningScene &scene) const; void getPlanningSceneMsgOctomap(moveit_msgs::PlanningScene &scene) const; + bool getPlanningSceneMsgOctomapDiff(boost::shared_ptr octree, octomap_msgs::Octomap &msg) const; void getPlanningSceneMsgObjectColors(moveit_msgs::PlanningScene &scene_msg) const; struct CollisionDetector; diff --git a/planning_scene/src/planning_scene.cpp b/planning_scene/src/planning_scene.cpp index b5078539..8218470a 100644 --- a/planning_scene/src/planning_scene.cpp +++ b/planning_scene/src/planning_scene.cpp @@ -49,6 +49,8 @@ namespace planning_scene { const std::string PlanningScene::OCTOMAP_NS = ""; +const std::string PlanningScene::OCTOMAP_MSG_TYPE = "OcTree"; +const std::string PlanningScene::OCTOMAP_DIFF_MSG_TYPE = "diff(OcTree)"; const std::string PlanningScene::DEFAULT_SCENE_NAME = "(noname)"; class SceneTransforms : public robot_state::Transforms @@ -853,7 +855,34 @@ void planning_scene::PlanningScene::getPlanningSceneMsgOctomap(moveit_msgs::Plan if (map->shapes_.size() == 1) { const shapes::OcTree *o = static_cast(map->shapes_[0].get()); - octomap_msgs::fullMapToMsg(*o->octree, scene_msg.world.octomap.octomap); + boost::shared_ptr octree = o->octree; + if (scene_msg.is_diff == true && octree->isChangeDetectionEnabled()) + { + int num_changes = octree->numChangesDetected(); + int expected_size_diff = sizeof(int)+num_changes*((3*sizeof(unsigned short int))+sizeof(float)); + int expected_size_tree = octree->size()*(sizeof(float)+sizeof(char)); + if (expected_size_diff > expected_size_tree) + { + logInform("Cheaper to send tree instead of diff by %i bytes with %i changes", expected_size_diff-expected_size_tree, num_changes); + octomap_msgs::fullMapToMsg(*octree, scene_msg.world.octomap.octomap); + if(scene_msg.world.octomap.octomap.id != OCTOMAP_MSG_TYPE) { + logWarn("fullMapToMsg produced unexpected octomap type: %s", + scene_msg.world.octomap.octomap.id.c_str()); + } + } + else + { + getPlanningSceneMsgOctomapDiff(octree, scene_msg.world.octomap.octomap); + } + } + else + { + octomap_msgs::fullMapToMsg(*octree, scene_msg.world.octomap.octomap); + if(scene_msg.world.octomap.octomap.id != OCTOMAP_MSG_TYPE) { + logWarn("fullMapToMsg produced unexpected octomap type: %s", + scene_msg.world.octomap.octomap.id.c_str()); + } + } tf::poseEigenToMsg(map->shape_poses_[0], scene_msg.world.octomap.origin); } else @@ -861,6 +890,41 @@ void planning_scene::PlanningScene::getPlanningSceneMsgOctomap(moveit_msgs::Plan } } +bool planning_scene::PlanningScene::getPlanningSceneMsgOctomapDiff(boost::shared_ptr octree, octomap_msgs::Octomap &msg) const +{ + msg.id = OCTOMAP_DIFF_MSG_TYPE; + msg.resolution = octree->getResolution(); + msg.binary = false; + std::stringstream datastream; + + int num_changes = octree->numChangesDetected(); + datastream.write((const char*) &num_changes, sizeof(int)); + //logInform("Octomap diff has %i changes", num_changes); + + // this is safe as long as we only use the const_iterators for changedKeys + octomap::OcTree* octreeNonConst = const_cast(octree.get()); + for (octomap::KeyBoolMap::const_iterator it = octreeNonConst->changedKeysBegin(); + it != octreeNonConst->changedKeysEnd() && !(!datastream); ++it) + { + octomap::OcTreeKey key = it->first; + for (int j=0; j<3; j++) + { + datastream.write((const char*) &key[j], sizeof(unsigned short int)); + } + float value = octree->search(key)->getLogOdds(); + datastream.write((const char*) &value, sizeof(float)); + } + + if (!datastream) + { + logError("Error while writing Octomap diff message"); + return false; + } + std::string datastring = datastream.str(); + msg.data = std::vector(datastring.begin(), datastring.end()); + return true; +} + void planning_scene::PlanningScene::getPlanningSceneMsg(moveit_msgs::PlanningScene &scene_msg) const { scene_msg.name = name_; @@ -1245,60 +1309,101 @@ bool planning_scene::PlanningScene::usePlanningSceneMsg(const moveit_msgs::Plann return setPlanningSceneMsg(scene_msg); } -void planning_scene::PlanningScene::processOctomapMsg(const octomap_msgs::Octomap &map) +void planning_scene::PlanningScene::removeAllCollisionObjects() { - // each octomap replaces any previous one - world_->removeObject(OCTOMAP_NS); + const std::vector &object_ids = world_->getObjectIds(); + for (std::size_t i = 0; i < object_ids.size(); ++i) + if (object_ids[i] != OCTOMAP_NS) + world_->removeObject(object_ids[i]); +} - if (map.data.empty()) - return; +void planning_scene::PlanningScene::processOctomapMsg(const octomap_msgs::OctomapWithPose &msg) +{ + const Eigen::Affine3d &t = getTransforms().getTransform(msg.header.frame_id); + Eigen::Affine3d p; + tf::poseMsgToEigen(msg.origin, p); + p = t * p; + processOctomapMsg(msg.octomap, p); +} - if (map.id != "OcTree") +void planning_scene::PlanningScene::processOctomapMsg(const octomap_msgs::Octomap &msg) +{ + if (!msg.header.frame_id.empty()) { - logError("Received octomap is of type '%s' but type 'OcTree' is expected.", map.id.c_str()); - return; + const Eigen::Affine3d &t = getTransforms().getTransform(msg.header.frame_id); + processOctomapMsg(msg, t); } + else + { + processOctomapMsg(msg, Eigen::Affine3d::Identity()); + } +} - boost::shared_ptr om(static_cast(octomap_msgs::msgToMap(map))); - if (!map.header.frame_id.empty()) +void planning_scene::PlanningScene::processOctomapMsg(const octomap_msgs::Octomap &msg, const Eigen::Affine3d &t) +{ + if (msg.id.empty()) { - const Eigen::Affine3d &t = getTransforms().getTransform(map.header.frame_id); + world_->removeObject(OCTOMAP_NS); + } + else if (msg.id == OCTOMAP_DIFF_MSG_TYPE) + { + collision_detection::CollisionWorld::ObjectConstPtr map = world_->getObject(OCTOMAP_NS); + if (map && map->shapes_.size() == 1) + { + if (msg.data.empty()) + return; + const shapes::OcTree *o = static_cast(map->shapes_[0].get()); + boost::shared_ptr octree = o->octree; + boost::shared_ptr nc_octree = boost::const_pointer_cast(octree); + processOctomapMsgDiff(msg, nc_octree); + processOctomapPtr(octree, t); + } + } + else if (msg.id == OCTOMAP_MSG_TYPE) + { + world_->removeObject(OCTOMAP_NS); // Octomap replaces any previous one + if (msg.data.empty()) + return; + boost::shared_ptr om(static_cast(octomap_msgs::msgToMap(msg))); world_->addToObject(OCTOMAP_NS, shapes::ShapeConstPtr(new shapes::OcTree(om)), t); } else { - world_->addToObject(OCTOMAP_NS, shapes::ShapeConstPtr(new shapes::OcTree(om)), Eigen::Affine3d::Identity()); + logError("Received Octomap is of unknown type '%s'", msg.id.c_str()); } } -void planning_scene::PlanningScene::removeAllCollisionObjects() +void planning_scene::PlanningScene::processOctomapMsgDiff(const octomap_msgs::Octomap &msg, boost::shared_ptr octree) { - const std::vector &object_ids = world_->getObjectIds(); - for (std::size_t i = 0; i < object_ids.size(); ++i) - if (object_ids[i] != OCTOMAP_NS) - world_->removeObject(object_ids[i]); -} - -void planning_scene::PlanningScene::processOctomapMsg(const octomap_msgs::OctomapWithPose &map) -{ - // each octomap replaces any previous one - world_->removeObject(OCTOMAP_NS); + std::stringstream datastream; + datastream.write((const char*) &msg.data[0], msg.data.size()); - if (map.octomap.data.empty()) - return; - - if (map.octomap.id != "OcTree") + int num_changes; + datastream.read((char*) &num_changes, sizeof(int)); + //logInform("Octomap diff has %i changes", num_changes); + int expected_size = sizeof(int)+num_changes*((3*sizeof(unsigned short int))+sizeof(float)); + if (expected_size > msg.data.size()) { - logError("Received octomap is of type '%s' but type 'OcTree' is expected.", map.octomap.id.c_str()); + logError("Did not receive enough data for specified diff size: %i bytes expected, %i received", expected_size, msg.data.size()); return; } + if(expected_size < msg.data.size()) { + logWarn("Got more data than expected (%zu > %d)", msg.data.size(), expected_size); + } - boost::shared_ptr om(static_cast(octomap_msgs::msgToMap(map.octomap))); - const Eigen::Affine3d &t = getTransforms().getTransform(map.header.frame_id); - Eigen::Affine3d p; - tf::poseMsgToEigen(map.origin, p); - p = t * p; - world_->addToObject(OCTOMAP_NS, shapes::ShapeConstPtr(new shapes::OcTree(om)), p); + for (int i=0; isetNodeValue(key, value); + } + if (!datastream) + logError("Error while reading Octomap diff message"); } void planning_scene::PlanningScene::processOctomapPtr(const boost::shared_ptr &octree, const Eigen::Affine3d &t)