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 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
    };

  static const std::string DEFAULT_JOINT_STATES_TOPIC; // "/joint_states"

  static const std::string DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC; // "/attached_collision_object"
  
  static const std::string DEFAULT_COLLISION_OBJECT_TOPIC; // "/collision_object"

  static const std::string DEFAULT_PLANNING_SCENE_WORLD_TOPIC; // "/planning_scene_world"
  
  static const std::string DEFAULT_PLANNING_SCENE_TOPIC; // "/planning_scene"
  
  static const std::string DEFAULT_PLANNING_SCENE_SERVICE; // "/get_planning_scene"
  
  static const std::string MONITORED_PLANNING_SCENE_TOPIC; // "monitored_planning_scene"

  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_TOPIC);

  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 = DEFAULT_JOINT_STATES_TOPIC, const std::string &attached_objects_topic = DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC);

  void stopStateMonitor();

  void updateSceneWithCurrentState();

  void setStateUpdateFrequency(double hz);

  double getStateUpdateFrequency();

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

  bool requestPlanningSceneState(const std::string &service_name = DEFAULT_PLANNING_SCENE_SERVICE);

  void stopSceneMonitor();

  void startWorldGeometryMonitor(const std::string &collision_objects_topic = DEFAULT_COLLISION_OBJECT_TOPIC,
                                 const std::string &planning_scene_world_topic = DEFAULT_PLANNING_SCENE_WORLD_TOPIC,
                                 const bool load_octomap_monitor = true);

  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 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 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_;
  std::map<std::string, double>         default_robot_link_padd_;
  std::map<std::string, double>         default_robot_link_scale_;

  // 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_;

  // 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<const robot_model::LinkModel*, std::vector<std::pair<occupancy_map_monitor::ShapeHandle, std::size_t> > > 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_;

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

private:

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

  // publish planning scene update diffs (runs in its own thread)
  void scenePublishingThread();

  // called by current_state_monitor_ when robot state (as monitored on joint state topic) changes
  void onStateUpdate(const sensor_msgs::JointStateConstPtr &joint_state);

  // called by state_update_timer_ when a state update it pending
  void stateUpdateTimerCallback(const ros::WallTimerEvent& event);

  // Callback for a new planning scene msg
  void newPlanningSceneCallback(const moveit_msgs::PlanningSceneConstPtr &scene);

  // Called to update the planning scene with a new message.
  void newPlanningSceneMessage(const moveit_msgs::PlanningScene& scene);


  // Lock for state_update_pending_ and dt_state_update_
  boost::mutex state_pending_mutex_;

  // This field is protected by state_pending_mutex_
  volatile bool state_update_pending_;

  // This field is protected by state_pending_mutex_
  ros::WallDuration dt_state_update_;

  // Setting this to a non-zero value resolves issues when the sensor data is
  // arriving so fast that it is preceding the transform state.
  ros::Duration shape_transform_cache_lookup_wait_time_;

  // Check if last_state_update_ is true and if so call updateSceneWithCurrentState()
  // Not safe to access from callback functions.
  ros::WallTimer state_update_timer_;

  // Only access this from callback functions (and constructor)
  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