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 */
 
#pragma once
 
#include <ros/ros.h>
#include <ros/callback_queue.h>
#include <tf2_ros/buffer.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 <moveit_msgs/GetPlanningScene.h>
#include <boost/noncopyable.hpp>
#include <boost/thread/shared_mutex.hpp>
#include <boost/thread/recursive_mutex.hpp>
#include <memory>
 
namespace planning_scene_monitor
{
MOVEIT_CLASS_FORWARD(PlanningSceneMonitor);  // Defines PlanningSceneMonitorPtr, ConstPtr, WeakPtr... etc
 
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 std::shared_ptr<tf2_ros::Buffer>& tf_buffer = std::shared_ptr<tf2_ros::Buffer>(),
                       const std::string& name = "");
 
  PlanningSceneMonitor(const robot_model_loader::RobotModelLoaderPtr& rml,
                       const std::shared_ptr<tf2_ros::Buffer>& tf_buffer = std::shared_ptr<tf2_ros::Buffer>(),
                       const std::string& name = "");
 
  PlanningSceneMonitor(const planning_scene::PlanningScenePtr& scene, const std::string& robot_description,
                       const std::shared_ptr<tf2_ros::Buffer>& tf_buffer = std::shared_ptr<tf2_ros::Buffer>(),
                       const std::string& name = "");
 
  PlanningSceneMonitor(const planning_scene::PlanningScenePtr& scene,
                       const robot_model_loader::RobotModelLoaderPtr& rml,
                       const std::shared_ptr<tf2_ros::Buffer>& tf_buffer = std::shared_ptr<tf2_ros::Buffer>(),
                       const std::string& name = "");
 
  PlanningSceneMonitor(const planning_scene::PlanningScenePtr& scene,
                       const robot_model_loader::RobotModelLoaderPtr& rml, const ros::NodeHandle& nh,
                       const std::shared_ptr<tf2_ros::Buffer>& tf_buffer = std::shared_ptr<tf2_ros::Buffer>(),
                       const std::string& name = "");
 
  ~PlanningSceneMonitor();
 
  const std::string& getName() const
  {
    return monitor_name_;
  }
 
  const robot_model_loader::RobotModelLoaderPtr& getRobotModelLoader() const
  {
    return rm_loader_;
  }
 
  const moveit::core::RobotModelConstPtr& getRobotModel() const
  {
    return robot_model_;
  }
 
  const planning_scene::PlanningScenePtr& getPlanningScene()
  {
    return scene_;
  }
 
  const planning_scene::PlanningSceneConstPtr& getPlanningScene() const
  {
    return scene_const_;
  }
 
  planning_scene::PlanningScenePtr
  copyPlanningScene(const moveit_msgs::PlanningScene& diff = moveit_msgs::PlanningScene());
 
  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 std::shared_ptr<tf2_ros::Buffer>& getTFClient() const
  {
    return tf_buffer_;
  }
 
  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_;
  }
 
  CurrentStateMonitorPtr& getStateMonitorNonConst()
  {
    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(bool skip_update_if_locked = false);
 
  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 providePlanningSceneService(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 newPlanningSceneWorldCallback(const moveit_msgs::PlanningSceneWorldConstPtr& world);
 
  void octomapUpdateCallback();
 
  void attachObjectCallback(const moveit_msgs::AttachedCollisionObjectConstPtr& obj);
 
  void currentStateAttachedBodyUpdateCallback(moveit::core::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 moveit::core::AttachedBody* attached_body);
  void includeAttachedBodyInOctree(const moveit::core::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::Time last_update_time_;                     
  ros::Time last_robot_motion_time_;               
 
  ros::NodeHandle nh_;
  ros::NodeHandle root_nh_;
  ros::CallbackQueue queue_;
  std::shared_ptr<ros::AsyncSpinner> spinner_;
 
  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
 
  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_;
  std::unique_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_;
  ros::Subscriber collision_object_subscriber_;
 
  // provide an optional service to get the full planning scene state
  // this is used by MoveGroup and related application nodes
  ros::ServiceServer get_scene_service_;
 
  // include a octomap monitor
  std::unique_ptr<occupancy_map_monitor::OccupancyMapMonitor> octomap_monitor_;
 
  // include a current state monitor
  CurrentStateMonitorPtr current_state_monitor_;
 
  typedef std::map<const moveit::core::LinkModel*,
                   std::vector<std::pair<occupancy_map_monitor::ShapeHandle, std::size_t> > >
      LinkShapeHandles;
  using AttachedBodyShapeHandles = std::map<const moveit::core::AttachedBody*,
                                            std::vector<std::pair<occupancy_map_monitor::ShapeHandle, std::size_t> > >;
  using CollisionBodyShapeHandles =
      std::map<std::string, std::vector<std::pair<occupancy_map_monitor::ShapeHandle, const Eigen::Isometry3d*> > >;
 
  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_;  
 
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);
 
  // Callback for requesting the full planning scene via service
  bool getPlanningSceneServiceCallback(moveit_msgs::GetPlanningScene::Request& req,
                                       moveit_msgs::GetPlanningScene::Response& res);
 
  std::atomic<bool> state_update_pending_;
 
  // Lock for writing last_robot_state_update_wall_time_ and dt_state_update_
  boost::mutex state_update_mutex_;
 
  // Only access this from callback functions (and constructor)
  // This field is protected by state_update_mutex_
  ros::WallTime last_robot_state_update_wall_time_;
 
  // This field is protected by state_update_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_;
 
  // If state_update_pending_ is true, call updateSceneWithCurrentState()
  // Not safe to access from callback functions.
  ros::WallTimer state_update_timer_;
 
  robot_model_loader::RobotModelLoaderPtr rm_loader_;
  moveit::core::RobotModelConstPtr robot_model_;
 
  collision_detection::CollisionPluginLoader collision_loader_;
 
  class DynamicReconfigureImpl;
  DynamicReconfigureImpl* reconfigure_impl_;
 
  std::set<std::string> ignored_frames_;
  bool checkFrameIgnored(const std::string& frame);
};
 
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 std::const_pointer_cast<const PlanningSceneMonitor>(planning_scene_monitor_)->getPlanningScene();
  }
 
  const planning_scene::PlanningSceneConstPtr& operator->() const
  {
    return std::const_pointer_cast<const PlanningSceneMonitor>(planning_scene_monitor_)->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_ = std::make_shared<SingleUnlock>(planning_scene_monitor_.get(), read_only);
  }
 
  MOVEIT_STRUCT_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();
  }
};
}  // namespace planning_scene_monitor