From cfa9218c1cd519b60f5cffb59a480ed0299d15d8 Mon Sep 17 00:00:00 2001 From: Connor Brew Date: Wed, 5 Aug 2015 18:08:05 -0700 Subject: [PATCH 1/5] Cleanup of msg functions --- .../moveit/planning_scene/planning_scene.h | 1 + planning_scene/src/planning_scene.cpp | 72 +++++++++---------- 2 files changed, 34 insertions(+), 39 deletions(-) diff --git a/planning_scene/include/moveit/planning_scene/planning_scene.h b/planning_scene/include/moveit/planning_scene/planning_scene.h index b4158a7a..0b46c2df 100644 --- a/planning_scene/include/moveit/planning_scene/planning_scene.h +++ b/planning_scene/include/moveit/planning_scene/planning_scene.h @@ -692,6 +692,7 @@ 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 processOctomapPtr(const boost::shared_ptr &octree, const Eigen::Affine3d &t); /** diff --git a/planning_scene/src/planning_scene.cpp b/planning_scene/src/planning_scene.cpp index b5078539..6bdfa87b 100644 --- a/planning_scene/src/planning_scene.cpp +++ b/planning_scene/src/planning_scene.cpp @@ -1245,60 +1245,54 @@ 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); - - if (map.data.empty()) - return; + 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.id != "OcTree") - { - logError("Received octomap is of type '%s' but type 'OcTree' is expected.", map.id.c_str()); - return; - } +void planning_scene::PlanningScene::processOctomapMsg(const octomap_msgs::OctomapWithPose &map) +{ + const Eigen::Affine3d &t = getTransforms().getTransform(map.header.frame_id); + Eigen::Affine3d p; + tf::poseMsgToEigen(map.origin, p); + p = t * p; + processOctomapMsg(map.octomap, p); +} - boost::shared_ptr om(static_cast(octomap_msgs::msgToMap(map))); +void planning_scene::PlanningScene::processOctomapMsg(const octomap_msgs::Octomap &map) +{ if (!map.header.frame_id.empty()) { const Eigen::Affine3d &t = getTransforms().getTransform(map.header.frame_id); - world_->addToObject(OCTOMAP_NS, shapes::ShapeConstPtr(new shapes::OcTree(om)), t); + processOctomapMsg(map, t); } else { - world_->addToObject(OCTOMAP_NS, shapes::ShapeConstPtr(new shapes::OcTree(om)), Eigen::Affine3d::Identity()); + processOctomapMsg(map, Eigen::Affine3d::Identity()); } } -void planning_scene::PlanningScene::removeAllCollisionObjects() +void planning_scene::PlanningScene::processOctomapMsg(const octomap_msgs::Octomap &map, const Eigen::Affine3d &t) { - 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); - - if (map.octomap.data.empty()) - return; - - if (map.octomap.id != "OcTree") + if (map.id.empty()) { - logError("Received octomap is of type '%s' but type 'OcTree' is expected.", map.octomap.id.c_str()); - return; + world_->removeObject(OCTOMAP_NS); + } + else if (map.id == "OcTree") + { + world_->removeObject(OCTOMAP_NS); // Octomap replaces any previous one + if (map.data.empty()) + return; + boost::shared_ptr om(static_cast(octomap_msgs::msgToMap(map))); + world_->addToObject(OCTOMAP_NS, shapes::ShapeConstPtr(new shapes::OcTree(om)), t); + } + else + { + logError("Received Octomap is of unknown type '%s'", map.id.c_str()); } - - 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); } void planning_scene::PlanningScene::processOctomapPtr(const boost::shared_ptr &octree, const Eigen::Affine3d &t) From cb5725b508235535deb544fa400d389394f3dc1f Mon Sep 17 00:00:00 2001 From: Connor Brew Date: Wed, 12 Aug 2015 14:03:34 -0700 Subject: [PATCH 2/5] Added Octomap diffing for both send and recive --- .../moveit/planning_scene/planning_scene.h | 4 + planning_scene/src/planning_scene.cpp | 127 +++++++++++++++--- 2 files changed, 115 insertions(+), 16 deletions(-) diff --git a/planning_scene/include/moveit/planning_scene/planning_scene.h b/planning_scene/include/moveit/planning_scene/planning_scene.h index 0b46c2df..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(); @@ -693,6 +695,7 @@ 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); /** @@ -891,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 6bdfa87b..43dda110 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,23 @@ 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 expected_size_diff = sizeof(int)+octree->numChangesDetected()*((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, octree->numChangesDetected()); + octomap_msgs::fullMapToMsg(*octree, scene_msg.world.octomap.octomap); + } + else + { + getPlanningSceneMsgOctomapDiff(octree, scene_msg.world.octomap.octomap); + } + } + else + octomap_msgs::fullMapToMsg(*octree, scene_msg.world.octomap.octomap); tf::poseEigenToMsg(map->shape_poses_[0], scene_msg.world.octomap.origin); } else @@ -861,6 +879,39 @@ 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); + + for (octomap::KeyBoolMap::const_iterator it = octree->changedKeysBegin(); + it != octree->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(int)); + } + + 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_; @@ -1253,46 +1304,90 @@ void planning_scene::PlanningScene::removeAllCollisionObjects() world_->removeObject(object_ids[i]); } -void planning_scene::PlanningScene::processOctomapMsg(const octomap_msgs::OctomapWithPose &map) +void planning_scene::PlanningScene::processOctomapMsg(const octomap_msgs::OctomapWithPose &msg) { - const Eigen::Affine3d &t = getTransforms().getTransform(map.header.frame_id); + const Eigen::Affine3d &t = getTransforms().getTransform(msg.header.frame_id); Eigen::Affine3d p; - tf::poseMsgToEigen(map.origin, p); + tf::poseMsgToEigen(msg.origin, p); p = t * p; - processOctomapMsg(map.octomap, p); + processOctomapMsg(msg.octomap, p); } -void planning_scene::PlanningScene::processOctomapMsg(const octomap_msgs::Octomap &map) +void planning_scene::PlanningScene::processOctomapMsg(const octomap_msgs::Octomap &msg) { - if (!map.header.frame_id.empty()) + if (!msg.header.frame_id.empty()) { - const Eigen::Affine3d &t = getTransforms().getTransform(map.header.frame_id); - processOctomapMsg(map, t); + const Eigen::Affine3d &t = getTransforms().getTransform(msg.header.frame_id); + processOctomapMsg(msg, t); } else { - processOctomapMsg(map, Eigen::Affine3d::Identity()); + processOctomapMsg(msg, Eigen::Affine3d::Identity()); } } -void planning_scene::PlanningScene::processOctomapMsg(const octomap_msgs::Octomap &map, const Eigen::Affine3d &t) +void planning_scene::PlanningScene::processOctomapMsg(const octomap_msgs::Octomap &msg, const Eigen::Affine3d &t) { - if (map.id.empty()) + if (msg.id.empty()) { world_->removeObject(OCTOMAP_NS); } - else if (map.id == "OcTree") + 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 (map.data.empty()) + if (msg.data.empty()) return; - boost::shared_ptr om(static_cast(octomap_msgs::msgToMap(map))); + boost::shared_ptr om(static_cast(octomap_msgs::msgToMap(msg))); world_->addToObject(OCTOMAP_NS, shapes::ShapeConstPtr(new shapes::OcTree(om)), t); } else { - logError("Received Octomap is of unknown type '%s'", map.id.c_str()); + logError("Received Octomap is of unknown type '%s'", msg.id.c_str()); + } +} + +void planning_scene::PlanningScene::processOctomapMsgDiff(const octomap_msgs::Octomap &msg, boost::shared_ptr octree) +{ + std::stringstream datastream; + datastream.write((const char*) &msg.data[0], msg.data.size()); + + 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("Did not receive enough data for specified diff size: %i bytes expected, %i received", expected_size, msg.data.size()); + return; + } + + 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) From 854e136bf7680c3b44572111807be9f1d10435f5 Mon Sep 17 00:00:00 2001 From: Christian Dornhege Date: Thu, 14 Jan 2016 17:40:14 +0100 Subject: [PATCH 3/5] Use correct size for storing float. --- planning_scene/src/planning_scene.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning_scene/src/planning_scene.cpp b/planning_scene/src/planning_scene.cpp index 43dda110..508338d9 100644 --- a/planning_scene/src/planning_scene.cpp +++ b/planning_scene/src/planning_scene.cpp @@ -899,7 +899,7 @@ bool planning_scene::PlanningScene::getPlanningSceneMsgOctomapDiff(boost::shared datastream.write((const char*) &key[j], sizeof(unsigned short int)); } float value = octree->search(key)->getLogOdds(); - datastream.write((const char*) &value, sizeof(int)); + datastream.write((const char*) &value, sizeof(float)); } if (!datastream) From 8f46a4f3f6bfcda7945eb2dcf3a0470f8033cd9a Mon Sep 17 00:00:00 2001 From: Christian Dornhege Date: Thu, 14 Jan 2016 18:00:22 +0100 Subject: [PATCH 4/5] Added warnings for octomap diffs. --- planning_scene/src/planning_scene.cpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/planning_scene/src/planning_scene.cpp b/planning_scene/src/planning_scene.cpp index 508338d9..4cbafab0 100644 --- a/planning_scene/src/planning_scene.cpp +++ b/planning_scene/src/planning_scene.cpp @@ -864,6 +864,10 @@ void planning_scene::PlanningScene::getPlanningSceneMsgOctomap(moveit_msgs::Plan { logInform("Cheaper to send tree instead of diff by %i bytes with %i changes", expected_size_diff-expected_size_tree, octree->numChangesDetected()); 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 { @@ -871,7 +875,13 @@ void planning_scene::PlanningScene::getPlanningSceneMsgOctomap(moveit_msgs::Plan } } 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 @@ -1374,6 +1384,9 @@ void planning_scene::PlanningScene::processOctomapMsgDiff(const octomap_msgs::Oc 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); + } for (int i=0; i Date: Thu, 14 Jan 2016 18:04:56 +0100 Subject: [PATCH 5/5] Ensure compatibility with the current OctoMap indigo release. --- planning_scene/src/planning_scene.cpp | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/planning_scene/src/planning_scene.cpp b/planning_scene/src/planning_scene.cpp index 4cbafab0..8218470a 100644 --- a/planning_scene/src/planning_scene.cpp +++ b/planning_scene/src/planning_scene.cpp @@ -858,11 +858,12 @@ void planning_scene::PlanningScene::getPlanningSceneMsgOctomap(moveit_msgs::Plan boost::shared_ptr octree = o->octree; if (scene_msg.is_diff == true && octree->isChangeDetectionEnabled()) { - int expected_size_diff = sizeof(int)+octree->numChangesDetected()*((3*sizeof(unsigned short int))+sizeof(float)); + 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, octree->numChangesDetected()); + 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", @@ -900,8 +901,10 @@ bool planning_scene::PlanningScene::getPlanningSceneMsgOctomapDiff(boost::shared datastream.write((const char*) &num_changes, sizeof(int)); //logInform("Octomap diff has %i changes", num_changes); - for (octomap::KeyBoolMap::const_iterator it = octree->changedKeysBegin(); - it != octree->changedKeysEnd() && !(!datastream); ++it) + // 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++)