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/macros/class_forward.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 <moveit/collision_plugin_loader/collision_plugin_loader.h>
#include <boost/noncopyable.hpp>
#include <boost/thread/shared_mutex.hpp>
#include <boost/thread/recursive_mutex.hpp>

namespace planning_scene_monitor
{
MOVEIT_CLASS_FORWARD(PlanningSceneMonitor);

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() const
  {
    if (!dt_state_update_.isZero())
      return 1.0 / dt_state_update_.toSec();
    else
      return 0.0;
  }

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

  bool waitForCurrentRobotState(const ros::Time& t, double wait_time = 1.);

  void lockSceneRead();

  void unlockSceneRead();

  void lockSceneWrite();

  void unlockSceneWrite();

  void clearOctomap();

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

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

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

  collision_detection::CollisionPluginLoader collision_loader_;

  class DynamicReconfigureImpl;
  DynamicReconfigureImpl* reconfigure_impl_;
};

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 static_cast<const PlanningSceneMonitor*>(planning_scene_monitor_.get())->getPlanningScene();
  }

  const planning_scene::PlanningSceneConstPtr& operator->() const
  {
    return static_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));
  }

  MOVEIT_CLASS_FORWARD(SingleUnlock);

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