36 #include <rclcpp/logger.hpp>
37 #include <rclcpp/logging.hpp>
39 static const rclcpp::Logger
BULLET_LOGGER = rclcpp::get_logger(
"collision_detection.bullet");
45 auto manager = std::make_shared<BulletDiscreteBVHManager>();
47 for (
const std::pair<const std::string, CollisionObjectWrapperPtr>& cow :
link2cow_)
49 CollisionObjectWrapperPtr new_cow = cow.second->clone();
51 assert(new_cow->getCollisionShape());
52 assert(new_cow->getCollisionShape()->getShapeType() != CUSTOM_CONVEX_SHAPE_TYPE);
54 new_cow->setWorldTransform(cow.second->getWorldTransform());
56 manager->addCollisionObject(new_cow);
59 manager->setActiveCollisionObjects(
active_);
72 btOverlappingPairCache* pair_cache =
broadphase_->getOverlappingPairCache();
74 RCLCPP_DEBUG_STREAM(
BULLET_LOGGER,
"Num overlapping candidates " << pair_cache->getNumOverlappingPairs());
78 pair_cache->processAllOverlappingPairs(&collision_callback,
dispatcher_.get());
81 <<
" collision with " << collisions.
contact_count <<
" collisions");
Definition of a structure for the allowed collision matrix. All elements in the collision world are r...
std::vector< std::string > active_
A list of the active collision links.
std::unique_ptr< btBroadphaseInterface > broadphase_
The bullet broadphase interface.
double contact_distance_
The contact distance threshold.
std::map< std::string, CollisionObjectWrapperPtr > link2cow_
A map of collision objects being managed.
btDispatcherInfo dispatch_info_
The bullet collision dispatcher configuration information.
std::unique_ptr< btCollisionDispatcher > dispatcher_
The bullet collision dispatcher used for getting object to object collision algorithm.
void contactTest(collision_detection::CollisionResult &collisions, const collision_detection::CollisionRequest &req, const collision_detection::AllowedCollisionMatrix *acm, bool self) override
Perform a contact test for all objects in the manager.
void addCollisionObject(const CollisionObjectWrapperPtr &cow) override
Add a bullet collision object to the manager.
BulletDiscreteBVHManagerPtr clone() const
Clone the manager.
A callback function that is called as part of the broadphase collision checking.
void addCollisionObjectToBroadphase(const CollisionObjectWrapperPtr &cow, const std::unique_ptr< btBroadphaseInterface > &broadphase, const std::unique_ptr< btCollisionDispatcher > &dispatcher)
Add the collision object to broadphase.
const rclcpp::Logger BULLET_LOGGER
Representation of a collision checking request.
Representation of a collision checking result.
bool collision
True if collision was found, false otherwise.
std::size_t contact_count
Number of contacts returned.