planning_scene_monitor.hpp

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 <rclcpp/rclcpp.hpp>
#include <rclcpp/version.h>
// For Rolling, Kilted, and newer
#if RCLCPP_VERSION_GTE(29, 6, 0)
#include <tf2_ros/buffer.hpp>
#include <tf2_ros/transform_listener.hpp>
// For Jazzy and older
#else
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#endif
#include <moveit/macros/class_forward.hpp>
#include <moveit/planning_scene/planning_scene.hpp>
#include <moveit/robot_model_loader/robot_model_loader.hpp>
#include <moveit/occupancy_map_monitor/occupancy_map_monitor.hpp>
#include <moveit/planning_scene_monitor/current_state_monitor.hpp>
#include <moveit/collision_plugin_loader/collision_plugin_loader.hpp>
#include <moveit_msgs/srv/get_planning_scene.hpp>
#include <memory>
#include <thread>
#include <shared_mutex>
#include <mutex>
#include <thread>
 
#include <moveit_planning_scene_monitor_export.h>
 
namespace planning_scene_monitor
{
MOVEIT_CLASS_FORWARD(PlanningSceneMonitor);  // Defines PlanningSceneMonitorPtr, ConstPtr, WeakPtr... etc
 
class MOVEIT_PLANNING_SCENE_MONITOR_EXPORT PlanningSceneMonitor
{
public:
  PlanningSceneMonitor(const PlanningSceneMonitor&) = delete;
 
  PlanningSceneMonitor& operator=(const PlanningSceneMonitor&) = delete;
 
  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 rclcpp::Node::SharedPtr& node, const std::string& robot_description,
                       const std::string& name = "");
 
  PlanningSceneMonitor(const rclcpp::Node::SharedPtr& node, const robot_model_loader::RobotModelLoaderPtr& rml,
                       const std::string& name = "");
 
  PlanningSceneMonitor(const rclcpp::Node::SharedPtr& node, const planning_scene::PlanningScenePtr& scene,
                       const std::string& robot_description, const std::string& name = "");
 
  PlanningSceneMonitor(const rclcpp::Node::SharedPtr& node, const planning_scene::PlanningScenePtr& scene,
                       const robot_model_loader::RobotModelLoaderPtr& rml, 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::msg::PlanningScene& diff = moveit_msgs::msg::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_.count() > 0.0)
    {
      return 1.0 / dt_state_update_.count();
    }
    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 std::function<void(SceneUpdateType)>& fn);
 
  void clearUpdateCallbacks();
 
  void getMonitoredTopics(std::vector<std::string>& topics) const;
 
  const rclcpp::Time& getLastUpdateTime() const
  {
    return last_update_time_;
  }
 
  void publishDebugInformation(bool flag);
 
  void triggerSceneUpdateEvent(SceneUpdateType update_type);
 
  bool waitForCurrentRobotState(const rclcpp::Time& t, double wait_time = 1.);
 
  void clearOctomap();
 
  // Called to update the planning scene with a new message.
  bool newPlanningSceneMessage(const moveit_msgs::msg::PlanningScene& scene);
 
  // Called to update a collision object in the planning scene.
  bool processCollisionObjectMsg(const moveit_msgs::msg::CollisionObject::ConstSharedPtr& collision_object_msg,
                                 const std::optional<moveit_msgs::msg::ObjectColor>& color_msg = std::nullopt);
 
  // Called to update an attached collision object in the planning scene.
  bool processAttachedCollisionObjectMsg(
      const moveit_msgs::msg::AttachedCollisionObject::ConstSharedPtr& attached_collision_object_msg);
 
protected:
  void initialize(const planning_scene::PlanningScenePtr& scene);
 
  void lockSceneRead();
 
  void unlockSceneRead();
 
  void lockSceneWrite();
 
  void unlockSceneWrite();
 
  void configureCollisionMatrix(const planning_scene::PlanningScenePtr& scene);
 
  void configureDefaultPadding();
 
  void newPlanningSceneWorldCallback(const moveit_msgs::msg::PlanningSceneWorld::ConstSharedPtr& world);
 
  void octomapUpdateCallback();
 
  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 rclcpp::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_;  
  std::shared_mutex scene_update_mutex_;           
  rclcpp::Time last_update_time_;                  
  rclcpp::Time last_robot_motion_time_;            
 
  std::shared_ptr<rclcpp::Node> node_;
 
  // TODO: (anasarrak) callbacks on ROS2?
  // https://answers.ros.org/question/300874/how-do-you-use-callbackgroups-as-a-replacement-for-callbackqueues-in-ros2/
  // ros::CallbackQueue queue_;
  std::shared_ptr<rclcpp::Node> pnode_;
  std::shared_ptr<rclcpp::executors::SingleThreadedExecutor> private_executor_;
  std::thread private_executor_thread_;
 
  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
 
  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
  rclcpp::Publisher<moveit_msgs::msg::PlanningScene>::SharedPtr planning_scene_publisher_;
  std::unique_ptr<std::thread> publish_planning_scene_;
  double publish_planning_scene_frequency_;
  SceneUpdateType publish_update_types_;
  std::atomic<SceneUpdateType> new_scene_update_;
  std::condition_variable_any new_scene_update_condition_;
 
  // subscribe to various sources of data
  rclcpp::Subscription<moveit_msgs::msg::PlanningScene>::SharedPtr planning_scene_subscriber_;
  rclcpp::Subscription<moveit_msgs::msg::PlanningSceneWorld>::SharedPtr planning_scene_world_subscriber_;
 
  rclcpp::Subscription<moveit_msgs::msg::AttachedCollisionObject>::SharedPtr attached_collision_object_subscriber_;
  rclcpp::Subscription<moveit_msgs::msg::CollisionObject>::SharedPtr collision_object_subscriber_;
 
  // provide an optional service to get the full planning scene state
  // this is used by MoveGroup and related application nodes
  rclcpp::Service<moveit_msgs::srv::GetPlanningScene>::SharedPtr 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 std::recursive_mutex shape_handles_lock_;
 
  std::recursive_mutex update_lock_;
  std::vector<std::function<void(SceneUpdateType)> > update_callbacks_;  
 
private:
  void getUpdatedFrameTransforms(std::vector<geometry_msgs::msg::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::msg::JointState::ConstSharedPtr& joint_state);
 
  // called by state_update_timer_ when a state update it pending
  void stateUpdateTimerCallback();
 
  // Callback for a new planning scene msg
  void newPlanningSceneCallback(const moveit_msgs::msg::PlanningScene::ConstSharedPtr& scene);
 
  // Callback for requesting the full planning scene via service
  void getPlanningSceneServiceCallback(const moveit_msgs::srv::GetPlanningScene::Request::SharedPtr& req,
                                       const moveit_msgs::srv::GetPlanningScene::Response::SharedPtr& res);
 
  void updatePublishSettings(bool publish_geom_updates, bool publish_state_updates, bool publish_transform_updates,
                             bool publish_planning_scene, double publish_planning_scene_hz);
 
  std::atomic<bool> state_update_pending_;
 
  // Lock for writing last_robot_state_update_wall_time_ and dt_state_update_
  std::mutex state_update_mutex_;
 
  // Only access this from callback functions (and constructor)
  // This field is protected by state_update_mutex_
  std::chrono::system_clock::time_point last_robot_state_update_wall_time_;
 
  // This field is protected by state_update_mutex_
  std::chrono::duration<double> dt_state_update_;  // 1hz
 
  // Setting this to a non-zero value resolves issues when the sensor data is
  // arriving so fast that it is preceding the transform state.
  rclcpp::Duration shape_transform_cache_lookup_wait_time_;
 
  // If state_update_pending_ is true, call updateSceneWithCurrentState()
  // Not safe to access from callback functions.
  rclcpp::TimerBase::SharedPtr state_update_timer_;
 
  robot_model_loader::RobotModelLoaderPtr rm_loader_;
  moveit::core::RobotModelConstPtr robot_model_;
 
  collision_detection::CollisionPluginLoader collision_loader_;
 
  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr callback_handler_;
 
  bool use_sim_time_;
 
  friend class LockedPlanningSceneRO;
  friend class LockedPlanningSceneRW;
 
  rclcpp::Logger logger_;
};
 
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_ = 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