From 9473a0d1085673deee26cce4407e4bc3d39aeb96 Mon Sep 17 00:00:00 2001 From: Connor Brew Date: Wed, 5 Aug 2015 18:08:05 -0700 Subject: [PATCH 1/2] 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 1f3a6f5e..13f37a65 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 23290621..8f6bca6b 100644 --- a/planning_scene/src/planning_scene.cpp +++ b/planning_scene/src/planning_scene.cpp @@ -1239,60 +1239,54 @@ void planning_scene::PlanningScene::usePlanningSceneMsg(const moveit_msgs::Plann 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 ocomap 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 ocomap 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 21894d1935d740691a19a559988492c041846ed2 Mon Sep 17 00:00:00 2001 From: Connor Brew Date: Wed, 12 Aug 2015 14:03:34 -0700 Subject: [PATCH 2/2] 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 13f37a65..f1189eec 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 8f6bca6b..d9279988 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_; @@ -1247,46 +1298,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)