planning_scene_monitor.h

Go to the documentation of this file.

/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2011, Willow Garage, Inc.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Willow Garage nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

/* Author: Ioan Sucan */

#ifndef MOVEIT_PLANNING_SCENE_MONITOR_PLANNING_SCENE_MONITOR_
#define MOVEIT_PLANNING_SCENE_MONITOR_PLANNING_SCENE_MONITOR_

#include <ros/ros.h>
#include <tf/tf.h>
#include <tf/message_filter.h>
#include <message_filters/subscriber.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/occupancy_map_monitor/occupancy_map_monitor.h>
#include <moveit/planning_scene_monitor/current_state_monitor.h>
#include <boost/noncopyable.hpp>
#include <boost/thread/shared_mutex.hpp>
#include <boost/thread/recursive_mutex.hpp>

namespace planning_scene_monitor
{

class PlanningSceneMonitor : private boost::noncopyable
{
public:

  enum SceneUpdateType
    {
      UPDATE_NONE = 0,

      UPDATE_STATE = 1,

      UPDATE_TRANSFORMS = 2,

      UPDATE_GEOMETRY = 4,

      UPDATE_SCENE = 8 + UPDATE_STATE + UPDATE_TRANSFORMS + UPDATE_GEOMETRY
    };


  PlanningSceneMonitor(const std::string &robot_description,
                       const boost::shared_ptr<tf::Transformer> &tf = boost::shared_ptr<tf::Transformer>(),
                       const std::string &name = "");

  PlanningSceneMonitor(const robot_model_loader::RobotModelLoaderPtr &rml,
                       const boost::shared_ptr<tf::Transformer> &tf = boost::shared_ptr<tf::Transformer>(),
                       const std::string &name = "");

  PlanningSceneMonitor(const planning_scene::PlanningScenePtr &scene, const std::string &robot_description,
                       const boost::shared_ptr<tf::Transformer> &tf = boost::shared_ptr<tf::Transformer>(),
                       const std::string &name = "");

  PlanningSceneMonitor(const planning_scene::PlanningScenePtr &scene,
                       const robot_model_loader::RobotModelLoaderPtr &rml,
                       const boost::shared_ptr<tf::Transformer> &tf = boost::shared_ptr<tf::Transformer>(),
                       const std::string &name = "");

  ~PlanningSceneMonitor();

  const std::string& getName() const
  {
    return monitor_name_;
  }

  const robot_model_loader::RobotModelLoaderPtr& getRobotModelLoader() const
  {
    return rm_loader_;
  }

  const robot_model::RobotModelConstPtr& getRobotModel() const
  {
    return robot_model_;
  }

  const planning_scene::PlanningScenePtr& getPlanningScene()
  {
    return scene_;
  }

  const planning_scene::PlanningSceneConstPtr& getPlanningScene() const
  {
    return scene_const_;
  }

  bool updatesScene(const planning_scene::PlanningSceneConstPtr &scene) const;

  bool updatesScene(const planning_scene::PlanningScenePtr &scene) const;

  const std::string& getRobotDescription() const
  {
    return robot_description_;
  }

  double getDefaultRobotPadding() const
  {
    return default_robot_padd_;
  }

  double getDefaultRobotScale() const
  {
    return default_robot_scale_;
  }

  double getDefaultObjectPadding() const
  {
    return default_object_padd_;
  }

  double getDefaultAttachedObjectPadding() const
  {
    return default_attached_padd_;
  }

  const boost::shared_ptr<tf::Transformer>& getTFClient() const
  {
    return tf_;
  }

  void monitorDiffs(bool flag);

  void startPublishingPlanningScene(SceneUpdateType event, const std::string &planning_scene_topic = "monitored_planning_scene");

  void stopPublishingPlanningScene();

  void setPlanningScenePublishingFrequency(double hz);

  double getPlanningScenePublishingFrequency() const
  {
    return publish_planning_scene_frequency_;
  }

  const CurrentStateMonitorPtr& getStateMonitor() const
  {
    return current_state_monitor_;
  }

  void updateFrameTransforms();

  void startStateMonitor(const std::string &joint_states_topic = "joint_states", const std::string &attached_objects_topic = "attached_collision_object");

  void stopStateMonitor();

  void updateSceneWithCurrentState();

  void setStateUpdateFrequency(double hz);

  double getStateUpdateFrequency();

  void startSceneMonitor(const std::string &scene_topic = "planning_scene");

  void stopSceneMonitor();

  void startWorldGeometryMonitor(const std::string &collision_objects_topic = "collision_object",
                                 const std::string &collision_map_topic = "collision_map",
                                 const std::string &planning_scene_world_topic = "planning_scene_world");

  void stopWorldGeometryMonitor();

  void addUpdateCallback(const boost::function<void(SceneUpdateType)> &fn);

  void clearUpdateCallbacks();

  void getMonitoredTopics(std::vector<std::string> &topics) const;

  const ros::Time& getLastUpdateTime() const
  {
    return last_update_time_;
  }

  void publishDebugInformation(bool flag);

  void triggerSceneUpdateEvent(SceneUpdateType update_type);

  void lockSceneRead();

  void unlockSceneRead();

  void lockSceneWrite();

  void unlockSceneWrite();

protected:

  void initialize(const planning_scene::PlanningScenePtr &scene);

  void configureCollisionMatrix(const planning_scene::PlanningScenePtr &scene);

  void configureDefaultPadding();

  void newPlanningSceneCallback(const moveit_msgs::PlanningSceneConstPtr &scene);

  void collisionObjectCallback(const moveit_msgs::CollisionObjectConstPtr &obj);

  void collisionObjectFailTFCallback(const moveit_msgs::CollisionObjectConstPtr &obj, tf::filter_failure_reasons::FilterFailureReason reason);

  void newPlanningSceneWorldCallback(const moveit_msgs::PlanningSceneWorldConstPtr &world);

  void collisionMapCallback(const moveit_msgs::CollisionMapConstPtr &map);

  void octomapUpdateCallback();

  void attachObjectCallback(const moveit_msgs::AttachedCollisionObjectConstPtr &obj);

  void currentStateAttachedBodyUpdateCallback(robot_state::AttachedBody *attached_body, bool just_attached);

  void currentWorldObjectUpdateCallback(const collision_detection::World::ObjectConstPtr &object, collision_detection::World::Action action);

  void includeRobotLinksInOctree();
  void excludeRobotLinksFromOctree();

  void excludeWorldObjectsFromOctree();
  void includeWorldObjectsInOctree();
  void excludeWorldObjectFromOctree(const collision_detection::World::ObjectConstPtr &obj);
  void includeWorldObjectInOctree(const collision_detection::World::ObjectConstPtr &obj);

  void excludeAttachedBodiesFromOctree();
  void includeAttachedBodiesInOctree();
  void excludeAttachedBodyFromOctree(const robot_state::AttachedBody *attached_body);
  void includeAttachedBodyInOctree(const robot_state::AttachedBody *attached_body);

  bool getShapeTransformCache(const std::string &target_frame, const ros::Time &target_time, occupancy_map_monitor::ShapeTransformCache &cache) const;

  std::string                           monitor_name_;

  planning_scene::PlanningScenePtr      scene_;
  planning_scene::PlanningSceneConstPtr scene_const_;
  planning_scene::PlanningScenePtr      parent_scene_; 
  boost::shared_mutex                   scene_update_mutex_; 

  ros::NodeHandle                       nh_;
  ros::NodeHandle                       root_nh_;
  boost::shared_ptr<tf::Transformer>    tf_;
  std::string                           robot_description_;

  double                                default_robot_padd_;
  double                                default_robot_scale_;
  double                                default_object_padd_;
  double                                default_attached_padd_;

  // variables for planning scene publishing
  ros::Publisher                        planning_scene_publisher_;
  boost::scoped_ptr<boost::thread>      publish_planning_scene_;
  double                                publish_planning_scene_frequency_;
  SceneUpdateType                       publish_update_types_;
  SceneUpdateType                       new_scene_update_;
  boost::condition_variable_any         new_scene_update_condition_;

  // subscribe to various sources of data
  ros::Subscriber                       planning_scene_subscriber_;
  ros::Subscriber                       planning_scene_world_subscriber_;

  ros::Subscriber                       attached_collision_object_subscriber_;

  boost::scoped_ptr<message_filters::Subscriber<moveit_msgs::CollisionObject> > collision_object_subscriber_;
  boost::scoped_ptr<tf::MessageFilter<moveit_msgs::CollisionObject> > collision_object_filter_;

  boost::scoped_ptr<message_filters::Subscriber<moveit_msgs::CollisionMap> > collision_map_subscriber_;
  boost::scoped_ptr<tf::MessageFilter<moveit_msgs::CollisionMap> > collision_map_filter_;

  // include a octomap monitor
  boost::scoped_ptr<occupancy_map_monitor::OccupancyMapMonitor> octomap_monitor_;

  // include a current state monitor
  CurrentStateMonitorPtr current_state_monitor_;


  typedef std::map<std::string, occupancy_map_monitor::ShapeHandle> LinkShapeHandles;
  typedef std::map<const robot_state::AttachedBody*, std::vector<std::pair<occupancy_map_monitor::ShapeHandle, std::size_t> > > AttachedBodyShapeHandles;
  typedef std::map<std::string, std::vector<std::pair<occupancy_map_monitor::ShapeHandle, Eigen::Affine3d*> > > CollisionBodyShapeHandles;

  LinkShapeHandles link_shape_handles_;
  AttachedBodyShapeHandles attached_body_shape_handles_;
  CollisionBodyShapeHandles collision_body_shape_handles_;
  mutable boost::recursive_mutex shape_handles_lock_;

  std::vector<boost::function<void(SceneUpdateType)> > update_callbacks_; 
  ros::Time last_update_time_; 

private:

  void getUpdatedFrameTransforms(std::vector<geometry_msgs::TransformStamped> &transforms);

  void scenePublishingThread();

  void onStateUpdate(const sensor_msgs::JointStateConstPtr &joint_state);

  double dt_state_update_;

  ros::WallTime last_state_update_;

  robot_model_loader::RobotModelLoaderPtr rm_loader_;
  robot_model::RobotModelConstPtr robot_model_;

  class DynamicReconfigureImpl;
  DynamicReconfigureImpl *reconfigure_impl_;
};

typedef boost::shared_ptr<PlanningSceneMonitor> PlanningSceneMonitorPtr;
typedef boost::shared_ptr<const PlanningSceneMonitor> PlanningSceneMonitorConstPtr;

class LockedPlanningSceneRO
{
public:

  LockedPlanningSceneRO(const PlanningSceneMonitorPtr &planning_scene_monitor) :
    planning_scene_monitor_(planning_scene_monitor)
  {
    initialize(true);
  }

  const PlanningSceneMonitorPtr& getPlanningSceneMonitor()
  {
    return planning_scene_monitor_;
  }

  operator bool() const
  {
    return planning_scene_monitor_ && planning_scene_monitor_->getPlanningScene();
  }

  operator const planning_scene::PlanningSceneConstPtr&() const
  {
    return const_cast<const PlanningSceneMonitor*>(planning_scene_monitor_.get())->getPlanningScene();
  }

  const planning_scene::PlanningSceneConstPtr& operator->() const
  {
    return const_cast<const PlanningSceneMonitor*>(planning_scene_monitor_.get())->getPlanningScene();
  }

protected:

  LockedPlanningSceneRO(const PlanningSceneMonitorPtr &planning_scene_monitor, bool read_only) :
    planning_scene_monitor_(planning_scene_monitor)
  {
    initialize(read_only);
  }

  void initialize(bool read_only)
  {
    if (planning_scene_monitor_)
      lock_.reset(new SingleUnlock(planning_scene_monitor_.get(), read_only));
  }

  // we use this struct so that lock/unlock are called only once
  // even if the LockedPlanningScene instance is copied around
  struct SingleUnlock
  {
    SingleUnlock(PlanningSceneMonitor *planning_scene_monitor, bool read_only) :
      planning_scene_monitor_(planning_scene_monitor), read_only_(read_only)
    {
      if (read_only)
        planning_scene_monitor_->lockSceneRead();
      else
        planning_scene_monitor_->lockSceneWrite();
    }
    ~SingleUnlock()
    {
      if (read_only_)
        planning_scene_monitor_->unlockSceneRead();
      else
        planning_scene_monitor_->unlockSceneWrite();
    }
    PlanningSceneMonitor *planning_scene_monitor_;
    bool read_only_;
  };

  PlanningSceneMonitorPtr planning_scene_monitor_;
  boost::shared_ptr<SingleUnlock> lock_;
};

class LockedPlanningSceneRW : public LockedPlanningSceneRO
{
public:

  LockedPlanningSceneRW(const PlanningSceneMonitorPtr &planning_scene_monitor) :
    LockedPlanningSceneRO(planning_scene_monitor, false)
  {
  }

  operator const planning_scene::PlanningScenePtr&()
  {
    return planning_scene_monitor_->getPlanningScene();
  }

  const planning_scene::PlanningScenePtr& operator->()
  {
    return planning_scene_monitor_->getPlanningScene();
  }
};

}

#endif