Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Remove MoveIt exceptions #1117

Closed
wants to merge 2 commits into from
Closed
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 3 additions & 2 deletions moveit/scripts/create_readme_table.py
Original file line number Diff line number Diff line change
@@ -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.
#
Expand Down Expand Up @@ -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

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down Expand Up @@ -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
{
Expand Down Expand Up @@ -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);
Expand Down
37 changes: 11 additions & 26 deletions moveit_core/planning_scene/src/planning_scene.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down Expand Up @@ -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_)
Expand Down Expand Up @@ -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() + "+";
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down Expand Up @@ -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();

Expand All @@ -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();
Expand Down