From 3d949a630a5bca6d1f63b193fc23b4672a73b344 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Fri, 11 Mar 2022 17:41:32 +0000 Subject: [PATCH 1/2] Fix copyright notice in README script --- moveit/scripts/create_readme_table.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/moveit/scripts/create_readme_table.py b/moveit/scripts/create_readme_table.py index b1fc2954c9..6dd624dcb1 100644 --- a/moveit/scripts/create_readme_table.py +++ b/moveit/scripts/create_readme_table.py @@ -1,7 +1,5 @@ #!/usr/bin/env python # -*- coding: utf-8 -*- -# Usage: python moveit/moveit/scripts/create_readme_table.py > /tmp/table.md -# First update supported_distro_ubuntu_dict below! # Copyright 2021 PickNik Inc. # @@ -31,6 +29,9 @@ # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. +# Usage: python moveit/moveit/scripts/create_readme_table.py > /tmp/table.md +# First update supported_distro_ubuntu_dict below! + from __future__ import print_function From 5ee971806fadd4c0f80be6df3d81bcdd1be46bdf Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Fri, 11 Mar 2022 21:15:54 +0000 Subject: [PATCH 2/2] Remove exceptions from PlanningScene --- .../moveit/planning_scene/planning_scene.h | 43 ++++++++++++------- .../planning_scene/src/planning_scene.cpp | 37 +++++----------- .../src/planning_scene_monitor.cpp | 8 ++-- 3 files changed, 43 insertions(+), 45 deletions(-) diff --git a/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h b/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h index 5b3c06099a..98c88f267b 100644 --- a/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h +++ b/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h @@ -55,6 +55,7 @@ #include #include #include +#include #include "rclcpp/rclcpp.hpp" #include "moveit_planning_scene_export.h" @@ -87,24 +88,36 @@ using ObjectTypeMap = std::map +class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_from_this { public: - /** \brief construct using an existing RobotModel */ - PlanningScene(const moveit::core::RobotModelConstPtr& robot_model, - const collision_detection::WorldPtr& world = std::make_shared()); - - /** \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()); - 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()); + + /** \brief Static planningScene factory. */ + static std::optional + create(const moveit::core::RobotModelConstPtr& robot_model, + const collision_detection::WorldPtr& world = std::make_shared()); + /** \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()); + /* 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); diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index ec1848afae..4e44137b4e 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -42,7 +42,6 @@ #include #include #include -#include #include #include #include @@ -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 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 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() + "+"; diff --git a/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp b/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp index e43a00c9a3..808a13e78b 100644 --- a/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp +++ b/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp @@ -37,7 +37,6 @@ #include #include #include -#include #include #include @@ -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(rm_loader_->getModel()); + scene_ = std::make_shared(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();