Skip to content

Commit

Permalink
Use id="empty" to differentiate from a not set octomap field
Browse files Browse the repository at this point in the history
  • Loading branch information
FSund committed May 11, 2022
1 parent 5e5c3e1 commit 78e0f71
Showing 1 changed file with 7 additions and 1 deletion.
8 changes: 7 additions & 1 deletion moveit_core/planning_scene/src/planning_scene.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -842,6 +842,12 @@ bool PlanningScene::getOctomapMsg(octomap_msgs::OctomapWithPose& octomap) const
ROS_ERROR_NAMED(LOGNAME, "Unexpected number of shapes in octomap collision object. Not including '%s' object",
OCTOMAP_NS.c_str());
}
else
{
// indicate empty octomap, which is different from a not set octomap field
octomap.octomap.id = "empty";
return true;
}
return false;
}

Expand Down Expand Up @@ -1289,7 +1295,7 @@ bool PlanningScene::setPlanningSceneDiffMsg(const moveit_msgs::PlanningScene& sc
result &= processCollisionObjectMsg(collision_object);

// if an octomap was specified, replace the one we have with that one
if (!scene_msg.world.octomap.octomap.data.empty())
if (!scene_msg.world.octomap.octomap.id.empty())
processOctomapMsg(scene_msg.world.octomap);

return result;
Expand Down

0 comments on commit 78e0f71

Please sign in to comment.