Skip to content

Commit

Permalink
Merge pull request #3 from mazrk7/humble
Browse files Browse the repository at this point in the history 
Merge ROS 2 Humble migration
  • Loading branch information
@mazrk7
mazrk7 authored Jun 7, 2024
2 parents a20b756 + 35dafc2 commit 4474c6f
Show file tree
Hide file tree
Showing 8 changed files with 284 additions and 75 deletions.
2 changes: 1 addition & 1 deletion CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,7 +36,7 @@ find_package(FCL 0.7.0 REQUIRED)
if(FCL_VERSION VERSION_LESS "0.7.0")
message(WARNING "Your FCL version ${FCL_VERSION} is less than 0.7.0")
else()
message("FCL version ${FCL_VERSION} is good!")
message(STATUS "FCL version ${FCL_VERSION} is good!")
endif()

find_package(Eigen3 REQUIRED)
Expand Down
7 changes: 2 additions & 5 deletions examples/fcl_interface_example.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -57,17 +57,15 @@ bool initCollisionWorld(robot_collision_checking::FCLInterfaceCollisionWorld& wo
sphere.dimensions.resize(1);
sphere.dimensions[shape_msgs::msg::SolidPrimitive::SPHERE_RADIUS] = 0.3;
sphere.type = shape_msgs::msg::SolidPrimitive::SPHERE;
fcl_objects.push_back(std::make_shared<robot_collision_checking::FCLObject>(
sphere, robot_collision_checking::SPHERE, eig_wTs1));
fcl_objects.push_back(std::make_shared<robot_collision_checking::FCLObject>(sphere, eig_wTs1));

// Cylinder
shape_msgs::msg::SolidPrimitive cylinder;
cylinder.dimensions.resize(2);
cylinder.dimensions[shape_msgs::msg::SolidPrimitive::CYLINDER_HEIGHT] = 1.0;
cylinder.dimensions[shape_msgs::msg::SolidPrimitive::CYLINDER_RADIUS] = 0.1;
cylinder.type = shape_msgs::msg::SolidPrimitive::CYLINDER;
fcl_objects.push_back(std::make_shared<robot_collision_checking::FCLObject>(
cylinder, robot_collision_checking::CYLINDER, eig_wTs2));
fcl_objects.push_back(std::make_shared<robot_collision_checking::FCLObject>(cylinder, eig_wTs2));

// Plane Ax+By+Cz+D=0
shape_msgs::msg::Plane plane;
Expand Down Expand Up @@ -176,7 +174,6 @@ int main(int argc, char **argv)
std::vector<robot_collision_checking::FCLInterfaceCollisionObjectPtr> world_objects = collision_world.getCollisionObjects();
std::string world_frame = collision_world.getWorldFrame();

// Ignore even shapes
int num_objects = collision_world.getNumObjects();
for (int i = 0; i < num_objects; /*i++*/)
{
Expand Down
27 changes: 20 additions & 7 deletions include/robot_collision_checking/fcl_interface.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,33 +33,46 @@ FCLCollisionGeometryPtr createCollisionGeometry(const octomap_msgs::msg::Octomap
// Check the collision between an FCL object and a given world
// Returns the number of collisions if any
int checkCollisionObjectWorld(const FCLObjectPtr& obj, const FCLInterfaceCollisionWorld& world);
int checkCollisionObjectWorld(const FCLCollisionObjectPtr& co, const FCLInterfaceCollisionWorld& world);

// Check if two FCL objects are in collision
bool checkCollisionObjects(const FCLObjectPtr& obj1, const FCLObjectPtr& obj2);
bool checkCollisionObjects(const FCLCollisionObjectPtr& co1, const FCLCollisionObjectPtr& co2);

/** Gets the distances from an FCL object to a given world
* Returns:
* 1. obj_distances a vector of distances (len = nbr of objects)
* 2. closest_pt_object a vector of points (len = nbr of objects) on object closest to the given world
* 3. closest_pt_world a vector of points (len = nbr of objects) on given world closest to the object */
void getObjectDistancesWorld(const FCLObjectPtr& obj,
const FCLInterfaceCollisionWorld& world,
std::vector<double>& obj_distances,
std::vector<Eigen::Vector3d>& closest_pt_obj,
std::vector<Eigen::Vector3d>& closest_pt_world);
const FCLInterfaceCollisionWorld& world,
std::vector<double>& obj_distances,
std::vector<Eigen::Vector3d>& closest_pt_obj,
std::vector<Eigen::Vector3d>& closest_pt_world);
void getObjectDistancesWorld(const FCLCollisionObjectPtr& co,
const FCLInterfaceCollisionWorld& world,
std::vector<double>& obj_distances,
std::vector<Eigen::Vector3d>& closest_pt_obj,
std::vector<Eigen::Vector3d>& closest_pt_world);

// Returns the minimum distance between two FCL objects
double getDistanceObjects(const FCLObjectPtr& obj1, const FCLObjectPtr& obj2);
double getDistanceObjects(const FCLCollisionObjectPtr& co1, const FCLCollisionObjectPtr& co2);

/** Returns the minimum distance between two FCL objects
* Also returns the position w.r.t to world frame of the closest points */
double getDistanceObjects(const FCLObjectPtr& obj1,
const FCLObjectPtr& obj2,
Eigen::Vector3d& closest_pt_obj1,
Eigen::Vector3d& closest_pt_obj2);
const FCLObjectPtr& obj2,
Eigen::Vector3d& closest_pt_obj1,
Eigen::Vector3d& closest_pt_obj2);
double getDistanceObjects(const FCLCollisionObjectPtr& co1,
const FCLCollisionObjectPtr& co2,
Eigen::Vector3d& closest_pt_obj1,
Eigen::Vector3d& closest_pt_obj2);

// Get the minimum distance between an FCL object and a given world
double getMinimumObjectDistanceWorld(const FCLObjectPtr& obj, const FCLInterfaceCollisionWorld& world);
double getMinimumObjectDistanceWorld(const FCLCollisionObjectPtr& co, const FCLInterfaceCollisionWorld& world);

// Type conversion methods
void convertGeometryPoseEigenTransform(const geometry_msgs::msg::Pose& geo_pose, Eigen::Affine3d& wTt);
Expand Down
7 changes: 7 additions & 0 deletions include/robot_collision_checking/fcl_interface_collision_world.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,6 +33,7 @@ class FCLInterfaceCollisionWorld
// Returns true if in collision and a list of colliding objects
bool checkCollisionObject(int obj_id, std::vector<int>& collision_object_ids) const;
bool checkCollisionObject(const FCLObjectPtr& obj, std::vector<int>& collision_object_ids) const;
bool checkCollisionObject(const FCLCollisionObjectPtr& co, std::vector<int>& collision_object_ids) const;

/** Gets the distances from an FCL object (with an ID or not) in the known world
* Returns:
Expand All @@ -47,10 +48,15 @@ class FCLInterfaceCollisionWorld
std::vector<double>& obj_distances,
std::vector<Eigen::Vector3d>& closest_pt_obj,
std::vector<Eigen::Vector3d>& closest_pt_world) const;
void getObjectDistances(const FCLCollisionObjectPtr& co,
std::vector<double>& obj_distances,
std::vector<Eigen::Vector3d>& closest_pt_obj,
std::vector<Eigen::Vector3d>& closest_pt_world) const;

// Get the minimum distance between an FCL object and the known world
double getMinimumObjectDistance(int obj_id) const;
double getMinimumObjectDistance(const FCLObjectPtr& obj) const;
double getMinimumObjectDistance(const FCLCollisionObjectPtr& co) const;

inline std::string getWorldFrame() const
{
Expand All @@ -66,6 +72,7 @@ class FCLInterfaceCollisionWorld
{
return obj_counter_;
}

private:
// Collision world frame. All shapes and subframes are defined relative to this frame.
std::string frame_;
Expand Down
23 changes: 21 additions & 2 deletions include/robot_collision_checking/fcl_interface_types.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,9 +32,27 @@ enum ShapeType
// FCL Object is simply a container for object information
struct FCLObject
{
FCLObject(const shape_msgs::msg::SolidPrimitive& solid, ShapeType type, const Eigen::Affine3d& transform)
: object_type(type), object_transform(transform)
FCLObject(const shape_msgs::msg::SolidPrimitive& solid, const Eigen::Affine3d& transform)
: object_transform(transform)
{
if (solid.type == shape_msgs::msg::SolidPrimitive::SPHERE)
{
object_type = SPHERE;
}
else if (solid.type == shape_msgs::msg::SolidPrimitive::BOX)
{
object_type = BOX;
}
else if (solid.type == shape_msgs::msg::SolidPrimitive::CONE)
{
object_type = CONE;
}
else
{
// Should be a cylinder
object_type = CYLINDER;
}

ptr.solid = new shape_msgs::msg::SolidPrimitive(solid);
}

Expand Down Expand Up @@ -124,6 +142,7 @@ struct FCLObject
Eigen::Affine3d object_transform;

// Points to the type of FCL object
// TODO: Replace for std::variant in more modern C++
union
{
const shape_msgs::msg::SolidPrimitive* solid;
Expand Down
122 changes: 111 additions & 11 deletions src/fcl_interface.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -75,7 +75,7 @@ FCLCollisionGeometryPtr createCollisionGeometry(const shape_msgs::msg::Plane& pl

FCLCollisionGeometryPtr createCollisionGeometry(const shape_msgs::msg::Mesh& mesh)
{
std::shared_ptr<fcl::BVHModel<fcl::OBBRSSd>> g(new fcl::BVHModel<fcl::OBBRSSd>());
auto g = std::make_shared<fcl::BVHModel<fcl::OBBRSSd>>();

unsigned int vert_size = mesh.vertices.size();
unsigned int tri_size = mesh.triangles.size();
Expand All @@ -88,16 +88,16 @@ FCLCollisionGeometryPtr createCollisionGeometry(const shape_msgs::msg::Mesh& mes
{
tri_indices[i] =
fcl::Triangle(mesh.triangles[i].vertex_indices[0],
mesh.triangles[i].vertex_indices[1],
mesh.triangles[i].vertex_indices[2]);
mesh.triangles[i].vertex_indices[1],
mesh.triangles[i].vertex_indices[2]);
}

for (unsigned int i = 0; i < vert_size; ++i)
{
points[i] = fcl::Vector3d(mesh.vertices[i].x, mesh.vertices[i].y, mesh.vertices[i].z);
}

g->beginModel();
g->beginModel(tri_size, vert_size);
g->addSubModel(points, tri_indices);
g->endModel();
}
Expand Down Expand Up @@ -133,6 +133,23 @@ int checkCollisionObjectWorld(const FCLObjectPtr& obj, const FCLInterfaceCollisi
return num_contacts;
}

int checkCollisionObjectWorld(const FCLCollisionObjectPtr& co, const FCLInterfaceCollisionWorld& world)
{
fcl::CollisionRequestd col_req;
fcl::CollisionResultd col_result;
std::vector<FCLInterfaceCollisionObjectPtr> world_objects = world.getCollisionObjects();
int num_contacts(0);
for (const auto& other : world_objects)
{
if (checkCollisionObjects(co, other->collision_object))
{
num_contacts++;
}
}

return num_contacts;
}

bool checkCollisionObjects(const FCLObjectPtr& obj1, const FCLObjectPtr& obj2)
{
// Create the collision geometries
Expand All @@ -152,11 +169,20 @@ bool checkCollisionObjects(const FCLObjectPtr& obj1, const FCLObjectPtr& obj2)
return col_result.isCollision();
}

bool checkCollisionObjects(const FCLCollisionObjectPtr& co1, const FCLCollisionObjectPtr& co2)
{
// Use FCL to check for collision
fcl::CollisionRequestd col_req;
fcl::CollisionResultd col_result;
fcl::collide(co1.get(), co2.get(), col_req, col_result);
return col_result.isCollision();
}

void getObjectDistancesWorld(const FCLObjectPtr& obj,
const FCLInterfaceCollisionWorld& world,
std::vector<double>& obj_distances,
std::vector<Eigen::Vector3d>& closest_pt_obj,
std::vector<Eigen::Vector3d>& closest_pt_world)
const FCLInterfaceCollisionWorld& world,
std::vector<double>& obj_distances,
std::vector<Eigen::Vector3d>& closest_pt_obj,
std::vector<Eigen::Vector3d>& closest_pt_world)
{
// Reset output vector variables
int world_size = world.getNumObjects();
Expand All @@ -174,6 +200,28 @@ void getObjectDistancesWorld(const FCLObjectPtr& obj,
}
}

void getObjectDistancesWorld(const FCLCollisionObjectPtr& co,
const FCLInterfaceCollisionWorld& world,
std::vector<double>& obj_distances,
std::vector<Eigen::Vector3d>& closest_pt_obj,
std::vector<Eigen::Vector3d>& closest_pt_world)
{
// Reset output vector variables
int world_size = world.getNumObjects();
obj_distances.clear();
obj_distances.resize(world_size);
closest_pt_obj.clear();
closest_pt_obj.resize(world_size);
closest_pt_world.clear();
closest_pt_world.resize(world_size);

std::vector<FCLInterfaceCollisionObjectPtr> world_objects = world.getCollisionObjects();
for (int i = 0; i < world_size; i++)
{
obj_distances[i] = getDistanceObjects(co, world_objects[i]->collision_object, closest_pt_obj[i], closest_pt_world[i]);
}
}

double getDistanceObjects(const FCLObjectPtr& obj1, const FCLObjectPtr& obj2)
{
// Create the collision geometries
Expand All @@ -197,10 +245,23 @@ double getDistanceObjects(const FCLObjectPtr& obj1, const FCLObjectPtr& obj2)
return dist_result.min_distance;
}

double getDistanceObjects(const FCLCollisionObjectPtr& co1, const FCLCollisionObjectPtr& co2)
{
fcl::DistanceRequestd dist_req;
dist_req.enable_nearest_points = false;
dist_req.gjk_solver_type = fcl::GJKSolverType::GST_LIBCCD;
fcl::DistanceResultd dist_result;

fcl::distance(co1.get(), co2.get(), dist_req, dist_result);

return dist_result.min_distance;
}


double getDistanceObjects(const FCLObjectPtr& obj1,
const FCLObjectPtr& obj2,
Eigen::Vector3d& closest_pt_obj1,
Eigen::Vector3d& closest_pt_obj2)
const FCLObjectPtr& obj2,
Eigen::Vector3d& closest_pt_obj1,
Eigen::Vector3d& closest_pt_obj2)
{
// Create the collision geometries
FCLCollisionGeometryPtr cg1 = createCollisionGeometry(obj1);
Expand Down Expand Up @@ -231,6 +292,29 @@ double getDistanceObjects(const FCLObjectPtr& obj1,
return dist_result.min_distance;
}

double getDistanceObjects(const FCLCollisionObjectPtr& co1,
const FCLCollisionObjectPtr& co2,
Eigen::Vector3d& closest_pt_obj1,
Eigen::Vector3d& closest_pt_obj2)
{
fcl::DistanceRequestd dist_req;
dist_req.enable_nearest_points = true;
dist_req.gjk_solver_type = fcl::GJKSolverType::GST_LIBCCD;
fcl::DistanceResultd dist_result;

dist_result.nearest_points[0].setZero();
dist_result.nearest_points[1].setZero();
fcl::distance(co1.get(), co2.get(), dist_req, dist_result);

for (int i = 0; i < 3; i++)
{
closest_pt_obj1(i) = dist_result.nearest_points[0][i];
closest_pt_obj2(i) = dist_result.nearest_points[1][i];
}

return dist_result.min_distance;
}

double getMinimumObjectDistanceWorld(const FCLObjectPtr& obj, const FCLInterfaceCollisionWorld& world)
{
double min_distance = std::numeric_limits<double>::max();
Expand All @@ -247,6 +331,22 @@ double getMinimumObjectDistanceWorld(const FCLObjectPtr& obj, const FCLInterface
return min_distance;
}

double getMinimumObjectDistanceWorld(const FCLCollisionObjectPtr& co, const FCLInterfaceCollisionWorld& world)
{
double min_distance = std::numeric_limits<double>::max();
std::vector<FCLInterfaceCollisionObjectPtr> world_objects = world.getCollisionObjects();
for (const auto& other : world_objects)
{
double dist_objs = getDistanceObjects(co, other->collision_object);
if (min_distance > dist_objs)
{
min_distance = dist_objs;
}
}

return min_distance;
}

void convertGeometryPoseEigenTransform(const geometry_msgs::msg::Pose& geo_pose, Eigen::Affine3d& wTt)
{
Eigen::Vector3d t(geo_pose.position.x, geo_pose.position.y, geo_pose.position.z);
Expand Down
Loading

0 comments on commit 4474c6f

Please sign in to comment.