Skip to content

Commit

Permalink
simplify newPlanningSceneMessage
Browse files Browse the repository at this point in the history
  • Loading branch information
@v4hn
v4hn committed Jun 16, 2016
1 parent 2542d28 commit 2000333
Showing 1 changed file with 53 additions and 52 deletions.
105 changes: 53 additions & 52 deletions moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -478,71 +478,72 @@ void planning_scene_monitor::PlanningSceneMonitor::clearOctomap()

void planning_scene_monitor::PlanningSceneMonitor::newPlanningSceneMessage(const moveit_msgs::PlanningScene& scene)
{
if (scene_)
if (!scene_)
return;

SceneUpdateType upd = UPDATE_SCENE;
std::string old_scene_name;
{
SceneUpdateType upd = UPDATE_SCENE;
std::string old_scene_name;
{
boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
boost::recursive_mutex::scoped_lock prevent_shape_cache_updates(shape_handles_lock_); // we don't want the transform cache to update while we are potentially changing attached bodies
boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_);
// we don't want the transform cache to update while we are potentially changing attached bodies
boost::recursive_mutex::scoped_lock prevent_shape_cache_updates(shape_handles_lock_);

last_update_time_ = ros::Time::now();
old_scene_name = scene_->getName();
scene_->usePlanningSceneMsg(scene);
if (octomap_monitor_)
last_update_time_ = ros::Time::now();
old_scene_name = scene_->getName();
scene_->usePlanningSceneMsg(scene);
if (octomap_monitor_)
{
if (!scene.is_diff && scene.world.octomap.octomap.data.empty())
{
if (!scene.is_diff && scene.world.octomap.octomap.data.empty())
{
octomap_monitor_->getOcTreePtr()->lockWrite();
octomap_monitor_->getOcTreePtr()->clear();
octomap_monitor_->getOcTreePtr()->unlockWrite();
}
octomap_monitor_->getOcTreePtr()->lockWrite();
octomap_monitor_->getOcTreePtr()->clear();
octomap_monitor_->getOcTreePtr()->unlockWrite();
}
robot_model_ = scene_->getRobotModel();
}
robot_model_ = scene_->getRobotModel();

// if we just reset the scene completely but we were maintaining diffs, we need to fix that
if (!scene.is_diff && parent_scene_)
{
// the scene is now decoupled from the parent, since we just reset it
scene_->setAttachedBodyUpdateCallback(robot_state::AttachedBodyCallback());
scene_->setCollisionObjectUpdateCallback(collision_detection::World::ObserverCallbackFn());
parent_scene_ = scene_;
scene_ = parent_scene_->diff();
scene_const_ = scene_;
scene_->setAttachedBodyUpdateCallback(boost::bind(&PlanningSceneMonitor::currentStateAttachedBodyUpdateCallback, this, _1, _2));
scene_->setCollisionObjectUpdateCallback(boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, _1, _2));
}
if (octomap_monitor_)
{
excludeAttachedBodiesFromOctree(); // in case updates have happened to the attached bodies, put them in
excludeWorldObjectsFromOctree(); // in case updates have happened to the attached bodies, put them in
}
// if we just reset the scene completely but we were maintaining diffs, we need to fix that
if (!scene.is_diff && parent_scene_)
{
// the scene is now decoupled from the parent, since we just reset it
scene_->setAttachedBodyUpdateCallback(robot_state::AttachedBodyCallback());
scene_->setCollisionObjectUpdateCallback(collision_detection::World::ObserverCallbackFn());
parent_scene_ = scene_;
scene_ = parent_scene_->diff();
scene_const_ = scene_;
scene_->setAttachedBodyUpdateCallback(boost::bind(&PlanningSceneMonitor::currentStateAttachedBodyUpdateCallback, this, _1, _2));
scene_->setCollisionObjectUpdateCallback(boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, _1, _2));
}
if (octomap_monitor_)
{
excludeAttachedBodiesFromOctree(); // in case updates have happened to the attached bodies, put them in
excludeWorldObjectsFromOctree(); // in case updates have happened to the attached bodies, put them in
}
}

// if we have a diff, try to more accuratelly determine the update type
if (scene.is_diff)
// if we have a diff, try to more accuratelly determine the update type
if (scene.is_diff)
{
bool no_other_scene_upd = (scene.name.empty() || scene.name == old_scene_name) &&
scene.allowed_collision_matrix.entry_names.empty() && scene.link_padding.empty() && scene.link_scale.empty();
if (no_other_scene_upd)
{
bool no_other_scene_upd = (scene.name.empty() || scene.name == old_scene_name) &&
scene.allowed_collision_matrix.entry_names.empty() && scene.link_padding.empty() && scene.link_scale.empty();
if (no_other_scene_upd)
{
upd = UPDATE_NONE;
if (!planning_scene::PlanningScene::isEmpty(scene.world))
upd = (SceneUpdateType) ((int)upd | (int)UPDATE_GEOMETRY);
upd = UPDATE_NONE;
if (!planning_scene::PlanningScene::isEmpty(scene.world))
upd = (SceneUpdateType) ((int)upd | (int)UPDATE_GEOMETRY);

if (!scene.fixed_frame_transforms.empty())
upd = (SceneUpdateType) ((int)upd | (int)UPDATE_TRANSFORMS);
if (!scene.fixed_frame_transforms.empty())
upd = (SceneUpdateType) ((int)upd | (int)UPDATE_TRANSFORMS);

if (!planning_scene::PlanningScene::isEmpty(scene.robot_state))
{
upd = (SceneUpdateType) ((int)upd | (int)UPDATE_STATE);
if (!scene.robot_state.attached_collision_objects.empty() || scene.robot_state.is_diff == false)
upd = (SceneUpdateType) ((int)upd | (int)UPDATE_GEOMETRY);
}
if (!planning_scene::PlanningScene::isEmpty(scene.robot_state))
{
upd = (SceneUpdateType) ((int)upd | (int)UPDATE_STATE);
if (!scene.robot_state.attached_collision_objects.empty() || scene.robot_state.is_diff == false)
upd = (SceneUpdateType) ((int)upd | (int)UPDATE_GEOMETRY);
}
}
triggerSceneUpdateEvent(upd);
}
triggerSceneUpdateEvent(upd);
}

void planning_scene_monitor::PlanningSceneMonitor::newPlanningSceneWorldCallback(const moveit_msgs::PlanningSceneWorldConstPtr &world)
Expand Down

0 comments on commit 2000333

Please sign in to comment.