38#include <geometric_shapes/body_operations.h>
41#include <rclcpp/clock.hpp>
42#include <rclcpp/logger.hpp>
43#include <rclcpp/time.hpp>
47static const double EPSILON{ 0.0001 };
61 std::vector<CollisionSphere> css;
63 if (body->getType() == shapes::ShapeType::SPHERE)
70 bodies::BoundingCylinder cyl;
71 body->computeBoundingCylinder(cyl);
72 unsigned int num_points = ceil(cyl.length / (cyl.radius / 2.0));
73 double spacing{ cyl.length / ((num_points * 1.0) - 1.0) };
74 relative_transform = cyl.pose;
76 for (
unsigned int i{ 1 }; i < num_points - 1; i++)
79 relative_transform * Eigen::Vector3d(0, 0, (-cyl.length / 2.0) + i * spacing), cyl.radius);
88 const EigenSTL::vector_Vector3d& sphere_centers,
90 double tolerance,
bool subtract_radii,
double maximum_value,
91 bool stop_at_first_collision)
95 bool in_collision{
false };
96 for (
unsigned int i{ 0 }; i < sphere_list.size(); ++i)
98 Eigen::Vector3d p = sphere_centers[i];
99 Eigen::Vector3d grad(0, 0, 0);
101 double dist =
getDistanceGradient(p.x(), p.y(), p.z(), grad.x(), grad.y(), grad.z(), in_bounds);
102 if (!in_bounds && grad.norm() > 0)
108 if (dist < maximum_value)
112 dist -= sphere_list[i].radius_;
114 if ((dist < 0) && (-dist >= tolerance))
119 dist = std::abs(dist);
123 if (sphere_list[i].radius_ - dist > tolerance)
136 gradient.
types[i] = type;
142 if (stop_at_first_collision && in_collision)
151 const std::vector<CollisionSphere>& sphere_list,
152 const EigenSTL::vector_Vector3d& sphere_centers,
GradientInfo& gradient,
153 const CollisionType& type,
double tolerance,
bool subtract_radii,
double maximum_value,
154 bool stop_at_first_collision)
158 bool in_collision{
false };
159 for (
unsigned int i{ 0 }; i < sphere_list.size(); ++i)
161 Eigen::Vector3d p = sphere_centers[i];
162 Eigen::Vector3d grad;
164 double dist =
distance_field->getDistanceGradient(p.x(), p.y(), p.z(), grad.x(), grad.y(), grad.z(), in_bounds);
165 if (!in_bounds && grad.norm() >
EPSILON)
167 RCLCPP_DEBUG(getLogger(),
"Collision sphere point is out of bounds %lf, %lf, %lf", p.x(), p.y(), p.z());
171 if (dist < maximum_value)
175 dist -= sphere_list[i].radius_;
177 if ((dist < 0) && (-dist >= tolerance))
184 if (sphere_list[i].radius_ - dist > tolerance)
197 gradient.
types[i] = type;
203 if (stop_at_first_collision && in_collision)
212 const std::vector<CollisionSphere>& sphere_list,
213 const EigenSTL::vector_Vector3d& sphere_centers,
double maximum_value,
216 for (
unsigned int i{ 0 }; i < sphere_list.size(); ++i)
218 Eigen::Vector3d p = sphere_centers[i];
219 Eigen::Vector3d grad;
220 bool in_bounds =
true;
221 double dist =
distance_field->getDistanceGradient(p.x(), p.y(), p.z(), grad.x(), grad.y(), grad.z(), in_bounds);
223 if (!in_bounds && grad.norm() > 0)
225 RCLCPP_DEBUG(getLogger(),
"Collision sphere point is out of bounds");
229 if ((maximum_value > dist) && (sphere_list[i].radius_ - dist > tolerance))
239 const std::vector<CollisionSphere>& sphere_list,
240 const EigenSTL::vector_Vector3d& sphere_centers,
double maximum_value,
241 double tolerance,
unsigned int num_coll, std::vector<unsigned int>& colls)
244 for (
unsigned int i = 0; i < sphere_list.size(); ++i)
246 Eigen::Vector3d p = sphere_centers[i];
247 Eigen::Vector3d grad;
248 bool in_bounds{
true };
249 double dist =
distance_field->getDistanceGradient(p.x(), p.y(), p.z(), grad.x(), grad.y(), grad.z(), in_bounds);
250 if (!in_bounds && (grad.norm() > 0))
252 RCLCPP_DEBUG(getLogger(),
"Collision sphere point is out of bounds");
255 if (maximum_value > dist && (sphere_list[i].radius_ - dist > tolerance))
263 if (colls.size() >= num_coll)
270 return !colls.empty();
279 std::vector<shapes::ShapeConstPtr>
shapes;
280 EigenSTL::vector_Isometry3d poses(1, Eigen::Isometry3d::Identity());
287 const EigenSTL::vector_Isometry3d& poses,
double resolution,
double padding)
293 double resolution,
double padding)
296 for (
unsigned int i{ 0 }; i <
shapes.size(); ++i)
304 std::vector<CollisionSphere> body_spheres;
305 EigenSTL::vector_Vector3d body_collision_points;
306 for (
unsigned int i{ 0 }; i <
bodies_.getCount(); ++i)
308 body_spheres.clear();
309 body_collision_points.clear();
316 body_collision_points.end());
326 std::vector<bodies::BoundingSphere> bounding_spheres(
bodies_.getCount());
327 for (
unsigned int i{ 0 }; i <
bodies_.getCount(); ++i)
329 bodies_.getBody(i)->computeBoundingSphere(bounding_spheres[i]);
333 RCLCPP_DEBUG(getLogger(),
"BodyDecomposition generated %zu collision spheres out of %zu shapes",
343 : body_decomposition_(body_decomposition)
349 const Eigen::Isometry3d& trans)
350 : body_decomposition_(body_decomposition)
356 : body_decomposition_()
358 int num_nodes = octree->getNumLeafNodes();
360 for (octomap::OcTree::tree_iterator tree_iter = octree->begin_tree(); tree_iter != octree->end_tree(); ++tree_iter)
362 Eigen::Vector3d p = Eigen::Vector3d(tree_iter.getX(), tree_iter.getY(), tree_iter.getZ());
381 : body_decomposition_(body_decomposition)
409 const PosedBodySphereDecompositionConstPtr& p2)
411 Eigen::Vector3d p1_sphere_center = p1->getBoundingSphereCenter();
412 Eigen::Vector3d p2_sphere_center = p2->getBoundingSphereCenter();
413 double p1_radius{ p1->getBoundingSphereRadius() };
414 double p2_radius{ p2->getBoundingSphereRadius() };
416 double dist{ (p1_sphere_center - p2_sphere_center).squaredNorm() };
417 return dist < (p1_radius + p2_radius);
421 const std::string& ns,
const rclcpp::Duration& dur,
422 const std::vector<PosedBodySphereDecompositionPtr>& posed_decompositions,
423 visualization_msgs::msg::MarkerArray& arr)
425 unsigned int count{ 0 };
426 rclcpp::Clock ros_clock;
427 for (
const auto& posed_decomposition : posed_decompositions)
429 if (posed_decomposition)
431 for (
unsigned int j{ 0 }; j < posed_decomposition->getCollisionSpheres().size(); ++j)
433 visualization_msgs::msg::Marker sphere;
434 sphere.type = visualization_msgs::msg::Marker::SPHERE;
435 sphere.header.stamp = ros_clock.now();
436 sphere.header.frame_id = frame_id;
439 sphere.lifetime = dur;
440 sphere.color = color;
441 sphere.scale.x = sphere.scale.y = sphere.scale.z = posed_decomposition->getCollisionSpheres()[j].radius_ * 2.0;
442 sphere.pose.position.x = posed_decomposition->getSphereCenters()[j].x();
443 sphere.pose.position.y = posed_decomposition->getSphereCenters()[j].y();
444 sphere.pose.position.z = posed_decomposition->getSphereCenters()[j].z();
445 arr.markers.push_back(sphere);
452 const std::vector<PosedBodySphereDecompositionPtr>& posed_decompositions,
453 const std::vector<PosedBodySphereDecompositionVectorPtr>& posed_vector_decompositions,
454 const std::vector<GradientInfo>& gradients, visualization_msgs::msg::MarkerArray& arr)
456 rclcpp::Clock ros_clock;
457 if (gradients.size() != posed_decompositions.size() + posed_vector_decompositions.size())
459 RCLCPP_WARN(getLogger(),
"Size mismatch between gradients %u and decompositions %u",
460 static_cast<unsigned int>(gradients.size()),
461 static_cast<unsigned int>((posed_decompositions.size() + posed_vector_decompositions.size())));
464 for (
unsigned int i{ 0 }; i < gradients.size(); ++i)
466 for (
unsigned int j{ 0 }; j < gradients[i].distances.size(); ++j)
468 visualization_msgs::msg::Marker arrow_mark;
469 arrow_mark.header.frame_id = frame_id;
470 arrow_mark.header.stamp = rclcpp::Clock(RCL_ROS_TIME).now();
473 arrow_mark.ns =
"self_coll_gradients";
479 arrow_mark.id = i * 1000 + j;
480 double xscale{ 0.0 };
481 double yscale{ 0.0 };
482 double zscale{ 0.0 };
483 if (gradients[i].distances[j] > 0.0 && gradients[i].distances[j] != DBL_MAX)
485 if (gradients[i].gradients[j].norm() > 0.0)
487 xscale = gradients[i].gradients[j].x() / gradients[i].gradients[j].norm();
488 yscale = gradients[i].gradients[j].y() / gradients[i].gradients[j].norm();
489 zscale = gradients[i].gradients[j].z() / gradients[i].gradients[j].norm();
493 RCLCPP_DEBUG(getLogger(),
"Negative length for %u %d %lf", i, arrow_mark.id, gradients[i].gradients[j].norm());
498 RCLCPP_DEBUG(getLogger(),
"Negative dist %lf for %u %d", gradients[i].distances[j], i, arrow_mark.id);
500 arrow_mark.points.resize(2);
501 if (i < posed_decompositions.size())
503 arrow_mark.points[1].x = posed_decompositions[i]->getSphereCenters()[j].x();
504 arrow_mark.points[1].y = posed_decompositions[i]->getSphereCenters()[j].y();
505 arrow_mark.points[1].z = posed_decompositions[i]->getSphereCenters()[j].z();
509 arrow_mark.points[1].x =
510 posed_vector_decompositions[i - posed_decompositions.size()]->getSphereCenters()[j].x();
511 arrow_mark.points[1].y =
512 posed_vector_decompositions[i - posed_decompositions.size()]->getSphereCenters()[j].y();
513 arrow_mark.points[1].z =
514 posed_vector_decompositions[i - posed_decompositions.size()]->getSphereCenters()[j].z();
516 arrow_mark.points[0] = arrow_mark.points[1];
517 arrow_mark.points[0].x -= xscale * gradients[i].distances[j];
518 arrow_mark.points[0].y -= yscale * gradients[i].distances[j];
519 arrow_mark.points[0].z -= zscale * gradients[i].distances[j];
520 arrow_mark.scale.x = 0.01;
521 arrow_mark.scale.y = 0.03;
522 arrow_mark.color.a = 1.0;
523 if (gradients[i].types[j] ==
SELF)
525 arrow_mark.color.r = 1.0;
526 arrow_mark.color.g = 0.2;
527 arrow_mark.color.b = .5;
529 else if (gradients[i].types[j] ==
INTRA)
531 arrow_mark.color.r = .2;
532 arrow_mark.color.g = 1.0;
533 arrow_mark.color.b = .5;
537 arrow_mark.color.r = .2;
538 arrow_mark.color.g = .5;
539 arrow_mark.color.b = 1.0;
541 else if (gradients[i].types[j] ==
NONE)
543 arrow_mark.color.r = 1.0;
544 arrow_mark.color.g = .2;
545 arrow_mark.color.b = 1.0;
547 arr.markers.push_back(arrow_mark);
553 const std::vector<PosedBodySphereDecompositionPtr>& posed_decompositions,
554 const std::vector<PosedBodySphereDecompositionVectorPtr>& posed_vector_decompositions,
555 const std::vector<GradientInfo>& gradients, visualization_msgs::msg::MarkerArray& arr)
557 rclcpp::Clock ros_clock;
558 if (gradients.size() != posed_decompositions.size() + posed_vector_decompositions.size())
560 RCLCPP_WARN(getLogger(),
"Size mismatch between gradients %zu and decompositions %zu", gradients.size(),
561 posed_decompositions.size() + posed_vector_decompositions.size());
564 for (
unsigned int i{ 0 }; i < gradients.size(); ++i)
566 for (
unsigned int j{ 0 }; j < gradients[i].types.size(); ++j)
568 visualization_msgs::msg::Marker sphere_mark;
569 sphere_mark.type = visualization_msgs::msg::Marker::SPHERE;
570 sphere_mark.header.frame_id = frame_id;
571 sphere_mark.header.stamp = rclcpp::Clock(RCL_ROS_TIME).now();
574 sphere_mark.ns =
"distance_collisions";
580 sphere_mark.id = i * 1000 + j;
581 if (i < posed_decompositions.size())
583 sphere_mark.scale.x = sphere_mark.scale.y = sphere_mark.scale.z =
584 posed_decompositions[i]->getCollisionSpheres()[j].radius_ * 2.0;
585 sphere_mark.pose.position.x = posed_decompositions[i]->getSphereCenters()[j].x();
586 sphere_mark.pose.position.y = posed_decompositions[i]->getSphereCenters()[j].y();
587 sphere_mark.pose.position.z = posed_decompositions[i]->getSphereCenters()[j].z();
591 sphere_mark.scale.x = sphere_mark.scale.y = sphere_mark.scale.z =
592 posed_vector_decompositions[i - posed_decompositions.size()]->getCollisionSpheres()[j].radius_ * 2.0;
593 sphere_mark.pose.position.x =
594 posed_vector_decompositions[i - posed_decompositions.size()]->getSphereCenters()[j].x();
595 sphere_mark.pose.position.y =
596 posed_vector_decompositions[i - posed_decompositions.size()]->getSphereCenters()[j].y();
597 sphere_mark.pose.position.z =
598 posed_vector_decompositions[i - posed_decompositions.size()]->getSphereCenters()[j].z();
600 sphere_mark.pose.orientation.w = 1.0;
601 sphere_mark.color.a = 1.0;
602 if (gradients[i].types[j] ==
SELF)
604 sphere_mark.color.r = 1.0;
605 sphere_mark.color.g = 0.2;
606 sphere_mark.color.b = .5;
608 else if (gradients[i].types[j] ==
INTRA)
610 sphere_mark.color.r = .2;
611 sphere_mark.color.g = 1.0;
612 sphere_mark.color.b = .5;
616 sphere_mark.color.r = .2;
617 sphere_mark.color.g = .5;
618 sphere_mark.color.b = 1.0;
622 sphere_mark.color.r = 1.0;
623 sphere_mark.color.g = .2;
624 sphere_mark.color.b = 1.0;
626 arr.markers.push_back(sphere_mark);
std::vector< CollisionSphere > collision_spheres_
EIGEN_MAKE_ALIGNED_OPERATOR_NEW BodyDecomposition(const shapes::ShapeConstPtr &shape, double resolution, double padding=0.01)
void init(const std::vector< shapes::ShapeConstPtr > &shapes, const EigenSTL::vector_Isometry3d &poses, double resolution, double padding)
bodies::BoundingSphere relative_bounding_sphere_
Eigen::Isometry3d relative_cylinder_pose_
EigenSTL::vector_Vector3d relative_collision_points_
bodies::BodyVector bodies_
std::vector< double > sphere_radii_
EIGEN_MAKE_ALIGNED_OPERATOR_NEW PosedBodyPointDecomposition(const BodyDecompositionConstPtr &body_decomposition)
void updatePose(const Eigen::Isometry3d &linkTransform)
EigenSTL::vector_Vector3d posed_collision_points_
BodyDecompositionConstPtr body_decomposition_
EIGEN_MAKE_ALIGNED_OPERATOR_NEW PosedBodySphereDecomposition(const BodyDecompositionConstPtr &body_decomposition)
Eigen::Vector3d posed_bounding_sphere_center_
EigenSTL::vector_Vector3d posed_collision_points_
BodyDecompositionConstPtr body_decomposition_
void updatePose(const Eigen::Isometry3d &linkTransform)
EigenSTL::vector_Vector3d sphere_centers_
double getDistanceGradient(double x, double y, double z, double &gradient_x, double &gradient_y, double &gradient_z, bool &in_bounds) const
Gets not only the distance to the nearest cell but the gradient direction. The point defined by the x...
bool getCollisionSphereGradients(const std::vector< CollisionSphere > &sphere_list, const EigenSTL::vector_Vector3d &sphere_centers, GradientInfo &gradient, const CollisionType &type, double tolerance, bool subtract_radii, double maximum_value, bool stop_at_first_collision)
DistanceField is an abstract base class for computing distances from sets of 3D obstacle points....
rclcpp::Logger getLogger()
void getProximityGradientMarkers(const std::string &frame_id, const std::string &ns, const rclcpp::Duration &dur, const std::vector< PosedBodySphereDecompositionPtr > &posed_decompositions, const std::vector< PosedBodySphereDecompositionVectorPtr > &posed_vector_decompositions, const std::vector< GradientInfo > &gradients, visualization_msgs::msg::MarkerArray &arr)
std::vector< CollisionSphere > determineCollisionSpheres(const bodies::Body *body, Eigen::Isometry3d &relativeTransform)
void getCollisionMarkers(const std::string &frame_id, const std::string &ns, const rclcpp::Duration &dur, const std::vector< PosedBodySphereDecompositionPtr > &posed_decompositions, const std::vector< PosedBodySphereDecompositionVectorPtr > &posed_vector_decompositions, const std::vector< GradientInfo > &gradients, visualization_msgs::msg::MarkerArray &arr)
void getCollisionSphereMarkers(const std_msgs::msg::ColorRGBA &color, const std::string &frame_id, const std::string &ns, const rclcpp::Duration &dur, const std::vector< PosedBodySphereDecompositionPtr > &posed_decompositions, visualization_msgs::msg::MarkerArray &arr)
bool getCollisionSphereCollision(const distance_field::DistanceField *distance_field, const std::vector< CollisionSphere > &sphere_list, const EigenSTL::vector_Vector3d &sphere_centers, double maximum_value, double tolerance)
bool doBoundingSpheresIntersect(const PosedBodySphereDecompositionConstPtr &p1, const PosedBodySphereDecompositionConstPtr &p2)
bool getCollisionSphereGradients(const distance_field::DistanceField *distance_field, const std::vector< CollisionSphere > &sphere_list, const EigenSTL::vector_Vector3d &sphere_centers, GradientInfo &gradient, const CollisionType &type, double tolerance, bool subtract_radii, double maximum_value, bool stop_at_first_collision)
Namespace for holding classes that generate distance fields.
void findInternalPointsConvex(const bodies::Body &body, double resolution, EigenSTL::vector_Vector3d &points)
Find all points on a regular grid that are internal to the body, assuming the body is a convex shape....
rclcpp::Logger getLogger(const std::string &name)
Creates a namespaced logger.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW double closest_distance
EigenSTL::vector_Vector3d gradients
std::vector< double > distances
std::vector< CollisionType > types