Skip to content

Commit

Permalink
Getter for name of CollisionDetectorAllocator
Browse files Browse the repository at this point in the history
  • Loading branch information
tylerjw authored and wolfv committed Apr 27, 2021
1 parent a10e5e3 commit d2eecfa
Show file tree
Hide file tree
Showing 14 changed files with 118 additions and 52 deletions.
1 change: 1 addition & 0 deletions MIGRATION.md
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@
API changes in MoveIt releases

## ROS Noetic
- Static member variable interface of the CollisionDetectorAllocatorTemplate for the string NAME was replaced with a virtual method `getName`.
- RobotModel no longer overrides empty URDF collision geometry by matching the visual geometry of the link.
- Planned trajectories are *slow* by default.
The default values of the `velocity_scaling_factor` and `acceleration_scaling_factor` can now be customized and default to 0.1 instead of 1.0.
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -46,6 +46,6 @@ class CollisionDetectorAllocatorAllValid
: public CollisionDetectorAllocatorTemplate<CollisionEnvAllValid, CollisionDetectorAllocatorAllValid>
{
public:
static const std::string NAME; // defined in collision_env_allvalid.cpp
const std::string& getName() const override;
};
} // namespace collision_detection
Original file line number Diff line number Diff line change
Expand Up @@ -72,10 +72,7 @@ template <class CollisionEnvType, class CollisionDetectorAllocatorType>
class CollisionDetectorAllocatorTemplate : public CollisionDetectorAllocator
{
public:
const std::string& getName() const override
{
return CollisionDetectorAllocatorType::NAME;
}
virtual const std::string& getName() const = 0;

CollisionEnvPtr allocateEnv(const WorldPtr& world, const moveit::core::RobotModelConstPtr& robot_model) const override
{
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -37,99 +37,109 @@
#include <moveit/collision_detection/allvalid/collision_env_allvalid.h>
#include <moveit/collision_detection/allvalid/collision_detector_allocator_allvalid.h>

const std::string collision_detection::CollisionDetectorAllocatorAllValid::NAME("ALL_VALID");
namespace collision_detection
{
namespace
{
static const std::string NAME = "ALL_VALID";
} // namespace

collision_detection::CollisionEnvAllValid::CollisionEnvAllValid(const moveit::core::RobotModelConstPtr& robot_model,
double padding, double scale)
CollisionEnvAllValid::CollisionEnvAllValid(const moveit::core::RobotModelConstPtr& robot_model, double padding,
double scale)
: CollisionEnv(robot_model, padding, scale)
{
}

collision_detection::CollisionEnvAllValid::CollisionEnvAllValid(const moveit::core::RobotModelConstPtr& robot_model,
const WorldPtr& world, double padding, double scale)
CollisionEnvAllValid::CollisionEnvAllValid(const moveit::core::RobotModelConstPtr& robot_model, const WorldPtr& world,
double padding, double scale)
: CollisionEnv(robot_model, world, padding, scale)
{
}

collision_detection::CollisionEnvAllValid::CollisionEnvAllValid(const CollisionEnv& other, const WorldPtr& world)
CollisionEnvAllValid::CollisionEnvAllValid(const CollisionEnv& other, const WorldPtr& world)
: CollisionEnv(other, world)
{
}

void collision_detection::CollisionEnvAllValid::checkRobotCollision(const CollisionRequest& req, CollisionResult& res,
const moveit::core::RobotState& state) const
void CollisionEnvAllValid::checkRobotCollision(const CollisionRequest& req, CollisionResult& res,
const moveit::core::RobotState& state) const
{
res.collision = false;
if (req.verbose)
ROS_INFO_NAMED("collision_detection", "Using AllValid collision detection. No collision checking is performed.");
}

void collision_detection::CollisionEnvAllValid::checkRobotCollision(const CollisionRequest& req, CollisionResult& res,
const moveit::core::RobotState& state,
const AllowedCollisionMatrix& acm) const
void CollisionEnvAllValid::checkRobotCollision(const CollisionRequest& req, CollisionResult& res,
const moveit::core::RobotState& state,
const AllowedCollisionMatrix& acm) const
{
res.collision = false;
if (req.verbose)
ROS_INFO_NAMED("collision_detection", "Using AllValid collision detection. No collision checking is performed.");
}

void collision_detection::CollisionEnvAllValid::checkRobotCollision(const CollisionRequest& req, CollisionResult& res,
const moveit::core::RobotState& state1,
const moveit::core::RobotState& state2) const
void CollisionEnvAllValid::checkRobotCollision(const CollisionRequest& req, CollisionResult& res,
const moveit::core::RobotState& state1,
const moveit::core::RobotState& state2) const
{
res.collision = false;
if (req.verbose)
ROS_INFO_NAMED("collision_detection", "Using AllValid collision detection. No collision checking is performed.");
}

void collision_detection::CollisionEnvAllValid::checkRobotCollision(const CollisionRequest& req, CollisionResult& res,
const moveit::core::RobotState& state1,
const moveit::core::RobotState& state2,
const AllowedCollisionMatrix& acm) const
void CollisionEnvAllValid::checkRobotCollision(const CollisionRequest& req, CollisionResult& res,
const moveit::core::RobotState& state1,
const moveit::core::RobotState& state2,
const AllowedCollisionMatrix& acm) const
{
res.collision = false;
if (req.verbose)
ROS_INFO_NAMED("collision_detection", "Using AllValid collision detection. No collision checking is performed.");
}

void collision_detection::CollisionEnvAllValid::distanceRobot(const collision_detection::DistanceRequest& req,
collision_detection::DistanceResult& res,
const moveit::core::RobotState& state) const
void CollisionEnvAllValid::distanceRobot(const DistanceRequest& req, DistanceResult& res,
const moveit::core::RobotState& state) const
{
res.collision = false;
}

double collision_detection::CollisionEnvAllValid::distanceRobot(const moveit::core::RobotState& state) const
double CollisionEnvAllValid::distanceRobot(const moveit::core::RobotState& state) const
{
return 0.0;
}

double collision_detection::CollisionEnvAllValid::distanceRobot(const moveit::core::RobotState& state,
const AllowedCollisionMatrix& acm) const
double CollisionEnvAllValid::distanceRobot(const moveit::core::RobotState& state,
const AllowedCollisionMatrix& acm) const
{
return 0.0;
}

void collision_detection::CollisionEnvAllValid::checkSelfCollision(const CollisionRequest& req, CollisionResult& res,
const moveit::core::RobotState& state) const
void CollisionEnvAllValid::checkSelfCollision(const CollisionRequest& req, CollisionResult& res,
const moveit::core::RobotState& state) const
{
res.collision = false;
if (req.verbose)
ROS_INFO_NAMED("collision_detection", "Using AllValid collision detection. No collision checking is performed.");
}

void collision_detection::CollisionEnvAllValid::checkSelfCollision(const CollisionRequest& req, CollisionResult& res,
const moveit::core::RobotState& state,
const AllowedCollisionMatrix& acm) const
void CollisionEnvAllValid::checkSelfCollision(const CollisionRequest& req, CollisionResult& res,
const moveit::core::RobotState& state,
const AllowedCollisionMatrix& acm) const
{
res.collision = false;
if (req.verbose)
ROS_INFO_NAMED("collision_detection", "Using AllValid collision detection. No collision checking is performed.");
}

void collision_detection::CollisionEnvAllValid::distanceSelf(const collision_detection::DistanceRequest& req,
collision_detection::DistanceResult& res,
const moveit::core::RobotState& state) const
void CollisionEnvAllValid::distanceSelf(const DistanceRequest& req, DistanceResult& res,
const moveit::core::RobotState& state) const
{
res.collision = false;
}

const std::string& CollisionDetectorAllocatorAllValid::getName() const
{
return NAME;
}

} // namespace collision_detection
Original file line number Diff line number Diff line change
Expand Up @@ -46,6 +46,6 @@ class CollisionDetectorAllocatorBullet
: public CollisionDetectorAllocatorTemplate<CollisionEnvBullet, CollisionDetectorAllocatorBullet>
{
public:
static const std::string NAME; // defined in collision_env_bullet.cpp
const std::string& getName() const override;
};
} // namespace collision_detection
Original file line number Diff line number Diff line change
Expand Up @@ -43,9 +43,12 @@

namespace collision_detection
{
const std::string CollisionDetectorAllocatorBullet::NAME("Bullet");
namespace
{
static const std::string NAME = "Bullet";
const double MAX_DISTANCE_MARGIN = 99;
constexpr char LOGNAME[] = "collision_detection.bullet";
} // namespace

CollisionEnvBullet::CollisionEnvBullet(const moveit::core::RobotModelConstPtr& model, double padding, double scale)
: CollisionEnv(model, padding, scale)
Expand Down Expand Up @@ -436,4 +439,9 @@ void CollisionEnvBullet::addLinkAsCollisionObject(const urdf::LinkSharedPtr& lin
}
}

} // end of namespace collision_detection
const std::string& CollisionDetectorAllocatorBullet::getName() const
{
return NAME;
}

} // namespace collision_detection
Original file line number Diff line number Diff line change
Expand Up @@ -46,6 +46,6 @@ class CollisionDetectorAllocatorFCL
: public CollisionDetectorAllocatorTemplate<CollisionEnvFCL, CollisionDetectorAllocatorFCL>
{
public:
static const std::string NAME; // defined in collision_env_fcl.cpp
const std::string& getName() const override;
};
} // namespace collision_detection
13 changes: 11 additions & 2 deletions moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -46,9 +46,13 @@

namespace collision_detection
{
const std::string CollisionDetectorAllocatorFCL::NAME("FCL");
namespace
{
static const std::string NAME = "FCL";
constexpr char LOGNAME[] = "collision_detection.fcl";

} // namespace

CollisionEnvFCL::CollisionEnvFCL(const moveit::core::RobotModelConstPtr& model, double padding, double scale)
: CollisionEnv(model, padding, scale)
{
Expand Down Expand Up @@ -432,4 +436,9 @@ void CollisionEnvFCL::updatedPaddingOrScaling(const std::vector<std::string>& li
}
}

} // end of namespace collision_detection
const std::string& CollisionDetectorAllocatorFCL::getName() const
{
return NAME;
}

} // namespace collision_detection
Original file line number Diff line number Diff line change
Expand Up @@ -46,6 +46,6 @@ class CollisionDetectorAllocatorDistanceField
: public CollisionDetectorAllocatorTemplate<CollisionEnvDistanceField, CollisionDetectorAllocatorDistanceField>
{
public:
static const std::string NAME; // defined in collision_env_distance_field.cpp
const std::string& getName() const override;
};
} // namespace collision_detection
Original file line number Diff line number Diff line change
Expand Up @@ -46,6 +46,6 @@ class CollisionDetectorAllocatorHybrid
: public CollisionDetectorAllocatorTemplate<CollisionEnvHybrid, CollisionDetectorAllocatorHybrid>
{
public:
static const std::string NAME; // defined in collision_env_hybrid.cpp
const std::string& getName() const override;
};
} // namespace collision_detection
Original file line number Diff line number Diff line change
Expand Up @@ -48,9 +48,11 @@

namespace collision_detection
{
namespace
{
static const std::string NAME = "DISTANCE_FIELD";
const double EPSILON = 0.001f;

const std::string collision_detection::CollisionDetectorAllocatorDistanceField::NAME("DISTANCE_FIELD");
} // namespace

CollisionEnvDistanceField::CollisionEnvDistanceField(
const moveit::core::RobotModelConstPtr& robot_model,
Expand Down Expand Up @@ -1803,4 +1805,10 @@ CollisionEnvDistanceField::generateDistanceFieldCacheEntryWorld()
dfce->distance_field_->addPointsToField(add_points);
return dfce;
}

const std::string& CollisionDetectorAllocatorDistanceField::getName() const
{
return NAME;
}

} // namespace collision_detection
Original file line number Diff line number Diff line change
Expand Up @@ -39,7 +39,10 @@

namespace collision_detection
{
const std::string collision_detection::CollisionDetectorAllocatorHybrid::NAME("HYBRID");
namespace
{
static const std::string NAME = "HYBRID";
} // namespace

CollisionEnvHybrid::CollisionEnvHybrid(
const moveit::core::RobotModelConstPtr& robot_model,
Expand Down Expand Up @@ -181,4 +184,10 @@ void CollisionEnvHybrid::getAllCollisions(const CollisionRequest& req, Collision
{
cenv_distance_->getAllCollisions(req, res, state, acm, gsr);
}

const std::string& CollisionDetectorAllocatorHybrid::getName() const
{
return NAME;
}

} // namespace collision_detection
Original file line number Diff line number Diff line change
Expand Up @@ -44,6 +44,18 @@
#include <boost/function.hpp>
#include <string>

// Import/export for windows dll's and visibility for gcc shared libraries.

#ifdef ROS_BUILD_SHARED_LIBS // ros is being built around shared libraries
#ifdef moveit_kinematics_base_EXPORTS // we are building a shared lib/dll
#define MOVEIT_KINEMATICS_BASE_DECL ROS_HELPER_EXPORT
#else // we are using shared lib/dll
#define MOVEIT_KINEMATICS_BASE_DECL ROS_HELPER_IMPORT
#endif
#else // ros is being built around static libraries
#define MOVEIT_KINEMATICS_BASE_DECL
#endif

namespace moveit
{
namespace core
Expand Down Expand Up @@ -144,8 +156,8 @@ MOVEIT_CLASS_FORWARD(KinematicsBase); // Defines KinematicsBasePtr, ConstPtr, W
class KinematicsBase
{
public:
static const double DEFAULT_SEARCH_DISCRETIZATION; /* = 0.1 */
static const double DEFAULT_TIMEOUT; /* = 1.0 */
static MOVEIT_KINEMATICS_BASE_DECL const double DEFAULT_SEARCH_DISCRETIZATION; /* = 0.1 */
static MOVEIT_KINEMATICS_BASE_DECL const double DEFAULT_TIMEOUT; /* = 1.0 */

/** @brief Signature for a callback to validate an IK solution. Typically used for collision checking. */
using IKCallbackFn =
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -56,6 +56,18 @@
#include <boost/concept_check.hpp>
#include <memory>

// Import/export for windows dll's and visibility for gcc shared libraries.

#ifdef ROS_BUILD_SHARED_LIBS // ros is being built around shared libraries
#ifdef moveit_planning_scene_EXPORTS // we are building a shared lib/dll
#define MOVEIT_PLANNING_SCENE_DECL ROS_HELPER_EXPORT
#else // we are using shared lib/dll
#define MOVEIT_PLANNING_SCENE_DECL ROS_HELPER_IMPORT
#endif
#else // ros is being built around static libraries
#define MOVEIT_PLANNING_SCENE_DECL
#endif

/** \brief This namespace includes the central class for representing planning contexts */
namespace planning_scene
{
Expand Down Expand Up @@ -98,8 +110,8 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from
const urdf::ModelInterfaceSharedPtr& urdf_model, const srdf::ModelConstSharedPtr& srdf_model,
const collision_detection::WorldPtr& world = collision_detection::WorldPtr(new collision_detection::World()));

static const std::string OCTOMAP_NS;
static const std::string DEFAULT_SCENE_NAME;
static MOVEIT_PLANNING_SCENE_DECL const std::string OCTOMAP_NS;
static MOVEIT_PLANNING_SCENE_DECL const std::string DEFAULT_SCENE_NAME;

~PlanningScene();

Expand Down

0 comments on commit d2eecfa

Please sign in to comment.