Remove exceptions from PlanningScene

moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h

@@ -55,6 +55,7 @@
 #include <boost/function.hpp>
 #include <boost/concept_check.hpp>
 #include <memory>
+#include <optional>
 #include "rclcpp/rclcpp.hpp"
 
 #include "moveit_planning_scene_export.h"
@@ -87,24 +88,36 @@ using ObjectTypeMap = std::map<std::string, object_recognition_msgs::msg::Object
 /** \brief This class maintains the representation of the
     environment as seen by a planning instance. The environment
     geometry, the robot geometry and state are maintained. */
-class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : private boost::noncopyable,
-                                                   public std::enable_shared_from_this<PlanningScene>
+class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_from_this<PlanningScene>
 {
 public:
-  /** \brief construct using an existing RobotModel */
-  PlanningScene(const moveit::core::RobotModelConstPtr& robot_model,
-                const collision_detection::WorldPtr& world = std::make_shared<collision_detection::World>());
-
-  /** \brief construct using a urdf and srdf.
-   * A RobotModel for the PlanningScene will be created using the urdf and srdf. */
-  PlanningScene(const urdf::ModelInterfaceSharedPtr& urdf_model, const srdf::ModelConstSharedPtr& srdf_model,
-                const collision_detection::WorldPtr& world = std::make_shared<collision_detection::World>());
-
   static const std::string OCTOMAP_NS;
   static const std::string DEFAULT_SCENE_NAME;
 
+  // Constructor needs at least a RobotModel, use create() instead
+  PlanningScene() = delete;
+  // PlanningScene is non-copyable
+  PlanningScene(const PlanningScene&) = delete;
+  PlanningScene& operator=(const PlanningScene&) = delete;
+
+  // Moving is allowed
+  PlanningScene(PlanningScene&&) = default;
+  PlanningScene& operator=(const PlanningScene&&) = default;
+
+  // Custom destructor
   ~PlanningScene();
 
+  /** \brief construct using a urdf and srdf.
+   * A RobotModel for the PlanningScene will be created using the urdf and srdf. */
+  [[deprecated("Use PlanningScene::create() instead.")]] PlanningScene(
+      const urdf::ModelInterfaceSharedPtr& urdf_model, const srdf::ModelConstSharedPtr& srdf_model,
+      const collision_detection::WorldPtr& world = std::make_shared<collision_detection::World>());
+
+  /** \brief Static planningScene factory. */
+  static std::optional<PlanningScene>
+  create(const moveit::core::RobotModelConstPtr& robot_model,
+         const collision_detection::WorldPtr& world = std::make_shared<collision_detection::World>());
+
   /** \brief Get the name of the planning scene. This is empty by default */
   const std::string& getName() const
   {
@@ -946,14 +959,14 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : private boost::noncopyable,
   /* Private constructor used by the diff() methods. */
   PlanningScene(const PlanningSceneConstPtr& parent);
 
+  /** Private constructor used by create() */
+  PlanningScene(const moveit::core::RobotModelConstPtr& robot_model,
+                const collision_detection::WorldPtr& world = std::make_shared<collision_detection::World>());
+
   /* Initialize the scene.  This should only be called by the constructors.
    * Requires a valid robot_model_ */
   void initialize();
 
-  /* helper function to create a RobotModel from a urdf/srdf. */
-  static moveit::core::RobotModelPtr createRobotModel(const urdf::ModelInterfaceSharedPtr& urdf_model,
-                                                      const srdf::ModelConstSharedPtr& srdf_model);
-
   /* Helper functions for processing collision objects */
   bool processCollisionObjectAdd(const moveit_msgs::msg::CollisionObject& object);
   bool processCollisionObjectRemove(const moveit_msgs::msg::CollisionObject& object);

moveit_core/planning_scene/src/planning_scene.cpp

@@ -42,7 +42,6 @@
 #include <moveit/collision_detection/collision_tools.h>
 #include <moveit/trajectory_processing/trajectory_tools.h>
 #include <moveit/robot_state/conversions.h>
-#include <moveit/exceptions/exceptions.h>
 #include <moveit/robot_state/attached_body.h>
 #include <moveit/utils/message_checks.h>
 #include <octomap_msgs/conversions.h>
@@ -123,23 +122,6 @@ PlanningScene::PlanningScene(const moveit::core::RobotModelConstPtr& robot_model
   initialize();
 }
 
-PlanningScene::PlanningScene(const urdf::ModelInterfaceSharedPtr& urdf_model,
-                             const srdf::ModelConstSharedPtr& srdf_model, const collision_detection::WorldPtr& world)
-  : world_(world), world_const_(world)
-{
-  if (!urdf_model)
-    throw moveit::ConstructException("The URDF model cannot be nullptr");
-
-  if (!srdf_model)
-    throw moveit::ConstructException("The SRDF model cannot be nullptr");
-
-  robot_model_ = createRobotModel(urdf_model, srdf_model);
-  if (!robot_model_)
-    throw moveit::ConstructException("Could not create RobotModel");
-
-  initialize();
-}
-
 PlanningScene::~PlanningScene()
 {
   if (current_world_object_update_callback_)
@@ -169,21 +151,24 @@ void PlanningScene::initialize()
   allocateCollisionDetector(collision_detection::CollisionDetectorAllocatorFCL::create());
 }
 
-/* return nullptr on failure */
-moveit::core::RobotModelPtr PlanningScene::createRobotModel(const urdf::ModelInterfaceSharedPtr& urdf_model,
-                                                            const srdf::ModelConstSharedPtr& srdf_model)
+std::optional<PlanningScene> create(const moveit::core::RobotModelConstPtr& robot_model,
+                                    const collision_detection::WorldPtr& world)
 {
-  moveit::core::RobotModelPtr robot_model(new moveit::core::RobotModel(urdf_model, srdf_model));
   if (!robot_model || !robot_model->getRootJoint())
-    return moveit::core::RobotModelPtr();
+  {
+    RCLCPP_ERROR(LOGGER, "Cannot create PlanningScene with invalid RobotModel");
+    return {};
+  }
 
-  return robot_model;
+  std::optional<PlanningScene> scene_opt(std::in_place, robot_model, world);
+  return scene_opt;
 }
 
 PlanningScene::PlanningScene(const PlanningSceneConstPtr& parent) : parent_(parent)
 {
-  if (!parent_)
-    throw moveit::ConstructException("nullptr parent pointer for planning scene");
+  // This constructor is only called and accessible by the diff() functions which always pass a valid planning scene.
+  // The assert is really to catch invalid changes.
+  ROS_ASSERT_MSG(parent, "Private PlanningScene() constructor called with nullptr parent");
 
   if (!parent_->getName().empty())
     name_ = parent_->getName() + "+";

moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp

@@ -37,7 +37,6 @@
 #include <moveit/planning_scene_monitor/planning_scene_monitor.h>
 #include <moveit/robot_model_loader/robot_model_loader.h>
 #include <moveit/utils/message_checks.h>
-#include <moveit/exceptions/exceptions.h>
 #include <moveit_msgs/srv/get_planning_scene.hpp>
 
 #include <tf2/exceptions.h>
@@ -162,9 +161,10 @@ void PlanningSceneMonitor::initialize(const planning_scene::PlanningScenePtr& sc
     scene_ = scene;
     if (!scene_)
     {
-      try
+      auto scene_opt = planning_scene::PlanningScene::create(rm_loader_->getModel());
+      if (scene_opt)
       {
-        scene_ = std::make_shared<planning_scene::PlanningScene>(rm_loader_->getModel());
+        scene_ = std::make_shared<planning_scene::PlanningScene>(std::move(scene_opt.value()));
         configureCollisionMatrix(scene_);
         configureDefaultPadding();
 
@@ -179,7 +179,7 @@ void PlanningSceneMonitor::initialize(const planning_scene::PlanningScenePtr& sc
           scene_->getCollisionEnvNonConst()->setLinkScale(it.first, it.second);
         }
       }
-      catch (moveit::ConstructException& e)
+      else
       {
         RCLCPP_ERROR(LOGGER, "Configuration of planning scene failed");
         scene_.reset();