Skip to content
This repository has been archived by the owner on Nov 13, 2017. It is now read-only.

[reopen #274] Send Octomap diff instead of whole tree #302

Closed
wants to merge 5 commits into from
Closed

[reopen #274] Send Octomap diff instead of whole tree #302

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
cfa9218
Cleanup of msg functions
TheBrewCrew Aug 6, 2015
cb5725b
Added Octomap diffing for both send and recive
TheBrewCrew Aug 12, 2015
854e136
Use correct size for storing float.
dornhege Jan 14, 2016
8f46a4f
Added warnings for octomap diffs.
dornhege Jan 14, 2016
f3eb655
Ensure compatibility with the current OctoMap indigo release.
dornhege Jan 14, 2016
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 5 additions & 0 deletions planning_scene/include/moveit/planning_scene/planning_scene.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand Down Expand Up @@ -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<octomap::OcTree> octree);
void processOctomapPtr(const boost::shared_ptr<const octomap::OcTree> &octree, const Eigen::Affine3d &t);

/**
Expand Down Expand Up @@ -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<const octomap::OcTree> octree, octomap_msgs::Octomap &msg) const;
void getPlanningSceneMsgObjectColors(moveit_msgs::PlanningScene &scene_msg) const;

struct CollisionDetector;
Expand Down
175 changes: 140 additions & 35 deletions planning_scene/src/planning_scene.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -49,6 +49,8 @@
namespace planning_scene
{
const std::string PlanningScene::OCTOMAP_NS = "<octomap>";
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
Expand Down Expand Up @@ -853,14 +855,76 @@ void planning_scene::PlanningScene::getPlanningSceneMsgOctomap(moveit_msgs::Plan
if (map->shapes_.size() == 1)
{
const shapes::OcTree *o = static_cast<const shapes::OcTree*>(map->shapes_[0].get());
octomap_msgs::fullMapToMsg(*o->octree, scene_msg.world.octomap.octomap);
boost::shared_ptr<const octomap::OcTree> 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);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

should this really be an inform? I would prefer debug.

octomap_msgs::fullMapToMsg(*octree, scene_msg.world.octomap.octomap);
if(scene_msg.world.octomap.octomap.id != OCTOMAP_MSG_TYPE) {
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

curly brackets on new line

logWarn("fullMapToMsg produced unexpected octomap type: %s",
scene_msg.world.octomap.octomap.id.c_str());
}
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

see the comment below

}
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());
}
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This looks like something that was needed during development, but bloats the final result.
Is this still needed? In my opinion, this check should be removed.

}
tf::poseEigenToMsg(map->shape_poses_[0], scene_msg.world.octomap.origin);
}
else
logError("Unexpected number of shapes in octomap collision object. Not including '%s' object", OCTOMAP_NS.c_str());
}
}

bool planning_scene::PlanningScene::getPlanningSceneMsgOctomapDiff(boost::shared_ptr<const octomap::OcTree> octree, octomap_msgs::Octomap &msg) const
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

line too long

Copy link
Contributor

@v4hn v4hn Jul 21, 2016 via email

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The ROS standard is 120 per style guidelines

{
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<octomap::OcTree*>(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++)
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

spaces between symbols

{
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<int8_t>(datastring.begin(), datastring.end());
return true;
}

void planning_scene::PlanningScene::getPlanningSceneMsg(moveit_msgs::PlanningScene &scene_msg) const
{
scene_msg.name = name_;
Expand Down Expand Up @@ -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<std::string> &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);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This does not check if frame_id.empty() and could therefore print an error message.
Please check for that here and use Identity if the id is not set.
Until now it seems to be valid to send an empty octomap without a frame_id to remove it from the scene.

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<octomap::OcTree> om(static_cast<octomap::OcTree*>(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;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Please move this check to the beginning of the surrounding block, so that the first thing that happens for empty diffs is check-and-return. There's no reason not to do that iif map->shapes_.size() != 1 :)

const shapes::OcTree *o = static_cast<const shapes::OcTree*>(map->shapes_[0].get());
boost::shared_ptr<const octomap::OcTree> octree = o->octree;
boost::shared_ptr<octomap::OcTree> nc_octree = boost::const_pointer_cast<octomap::OcTree>(octree);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

👎
removing const-ness in a context that is this complex invites disaster.
If you wish to be able to modify the octree, keep a non-const pointer in the planning scene.
This would also allow for some further simplifications...
However, this would break ABI and therefore would only be possible for kinetic-devel (I see dave jumping happily up and down when reading this :P).
As an alternative, you could simply copy the old octomap and drop it. I would still prefer the other solution.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This should be safe, as it is happening in the same function that - without the diff function - would just replace the complete OctoMap.
I would like to avoid keeping a non-const pointer in the planning scene as that would have to be managed to. The only cleaner solution that I can see is to copy the old one, apply the diff on that and set that as the new octomap. This is less efficient, but will still be a win as the network overhead is avoided by the diff.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This should be safe, as it is happening in the same function that - without the diff function - would just replace the complete OctoMap.

I don't see why this makes it safe to unconst a pointer out of a list of const elements from a class instance that has been extracted from a static_cast'ed const pointer from a map of objects that is from outside the world only const-visible.

Also I believe this does not involve much management to have a separate shared_ptr for the latest octree.
If the current message is a diff -> apply it to the maintained octomap::OcTree (and maybe still build a new CollisionWorld::Object with it?), otherwise generate a new octree, overwrite the current pointer and hand it down to the scene.

If you want to have this in indigo, then I would propose to have the copy in indigo and jade and add the pointer-version in kinetic.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I see what you mean. The clean way would be to remove the object from the scene, do the operations and re-insert the changed one. Currently the order is perform operations (with const-cast), remove, add. However, I don't see a way in the moveit API to extract something from a shape, while retaining it exclusively.

Having a second OcTree sounds hard as the actual OcTree lives in the CollisionWorld. There might be changes from different components in there (e.g. world updates and filtering by other objects) that would need to be managed.

The only somewhat simple and safe way that I see right now is to clone the OcTree and apply changes to that.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

On Mon, Aug 01, 2016 at 07:01:07AM -0700, Christian Dornhege wrote:

I see what you mean. The clean way would be to remove the object from the scene,
do the operations and re-insert the changed one.

Sadly, this is not possible without const-cast with the current API...

There might be changes from different components in there (e.g. world updates and filtering by other objects) that would need to be managed.

Crap, I didn't think about that, you're right.

The only somewhat simple and safe way that I see right now is to clone the OcTree and apply changes to that.

+1 please do that for now. If you could also address the other changes I'll +1 this for indigo.
It would be nice to have that in before Friday, I agree.

If you want to have an optimized version in MoveIt! (that would be really nice),
this will need some API revision in the World to access the non-const CollisionObject.

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<octomap::OcTree> om(static_cast<octomap::OcTree*>(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<octomap::OcTree> octree)
{
const std::vector<std::string> &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()) {
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

bracket break

logWarn("Got more data than expected (%zu > %d)", msg.data.size(), expected_size);
}
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Please simplify this into
if(expected_size != msg.data.size()){ ...complain... }.
MoveIt is way to verbose already.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@v4hn consider tweaking your rosconsole output to hide logging you are not interested in. i'm a big fan of good warnings like this so that when something goes wrong, we understand. there are too many instances and reports from users of moveit saying "failed to plan" with no other feedback. with a complex system like this we need as good tools to debug issues. see my rosconsole article for tips

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Thanks for the link! It's in my TODO queue.

I fully agree that MoveIt needs informative warnings and debug messages.
In cases like this one I would even prefer fatal errors that abort the system,
just to enforce the user actually setup a reasonable system that satisfies our assumptions
about the input.

On the other hand one always has to look at the amount of code bloat added because of log output
and this should really be minimal. It's hard enough already to read through existing functions
in MoveIt of 100+ lines where 90 of them are related to sanity checks and do not provide any
functionality.

In this particular case I believe it is perfectly fine to have one warning
"Octomap diff contains unexpected amount of data: %i bytes expected, %i bytes received".
Do you disagree?

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Actually that seems reasonable. If the data size is not what is expected (either less or more) something is probably wrong.

I would not go for anything that aborts without the option to only enable this globally. This could destroy a working system because of "something".

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It's hard enough already to read through existing functions in MoveIt of 100+ lines where 90 of them are related to sanity checks and do not provide any functionality.

Yes its hard to read, but seeing that many sanity checks has not been my experience with the code base


boost::shared_ptr<octomap::OcTree> om(static_cast<octomap::OcTree*>(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; i<num_changes && !(!datastream); ++i)
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

spaces

{
octomap::OcTreeKey key;
for (int j=0; j<3; j++)
{
datastream.read((char*) &key[j], sizeof(unsigned short int));
}
float value;
datastream.read((char*) &value, sizeof(float));
octree->setNodeValue(key, value);
}
if (!datastream)
logError("Error while reading Octomap diff message");
}

void planning_scene::PlanningScene::processOctomapPtr(const boost::shared_ptr<const octomap::OcTree> &octree, const Eigen::Affine3d &t)
Expand Down