diff --git a/rmf_traffic/CHANGELOG.rst b/rmf_traffic/CHANGELOG.rst index ccc8fb4d..b16f3892 100644 --- a/rmf_traffic/CHANGELOG.rst +++ b/rmf_traffic/CHANGELOG.rst @@ -12,6 +12,7 @@ Changelog for package rmf_traffic * Add the blockade system for traffic light management: [#226](https://github.com/osrf/rmf_core/pull/226) * Access trajectory waypoints by element index: [#226](https://github.com/osrf/rmf_core/pull/226) * Get trajectory index of each plan waypoint: [#226](https://github.com/osrf/rmf_core/pull/226) +* Support for robot multi geometry: [#258] (https://github.com/osrf/rmf_core/pull/258) 1.1.0 (2020-09-24) ------------------ diff --git a/rmf_traffic/include/rmf_traffic/Profile.hpp b/rmf_traffic/include/rmf_traffic/Profile.hpp index 6b23be19..28bc699c 100644 --- a/rmf_traffic/include/rmf_traffic/Profile.hpp +++ b/rmf_traffic/include/rmf_traffic/Profile.hpp @@ -22,6 +22,7 @@ #include #include +#include namespace rmf_traffic { @@ -44,6 +45,23 @@ class Profile geometry::ConstFinalConvexShapePtr footprint, geometry::ConstFinalConvexShapePtr vicinity = nullptr); + /// Add an extra shape to the footprint of the participant. + /// The onus is on the user to update the vicinity of the robot + /// + /// \param[in] shape + /// An estimate of the space that this extra shape occupies. + /// + /// \param[in] offset + /// Offset to the additional shape, in the robot's coordinate frame + /// + void add_extra_footprint(geometry::ConstFinalConvexShapePtr shape, Eigen::Vector3d offset); + + /// Get the number of extra footprint shapes + uint extra_footprint_count() const; + + /// Removes all extra footprint shapes + void clear_extra_footprints(); + /// Set the footprint of the participant. Profile& footprint(geometry::ConstFinalConvexShapePtr shape); diff --git a/rmf_traffic/src/rmf_traffic/DetectConflict.cpp b/rmf_traffic/src/rmf_traffic/DetectConflict.cpp index d2d1432c..2543dd25 100644 --- a/rmf_traffic/src/rmf_traffic/DetectConflict.cpp +++ b/rmf_traffic/src/rmf_traffic/DetectConflict.cpp @@ -22,6 +22,7 @@ #include "StaticMotion.hpp" #include "DetectConflictInternal.hpp" +#include #ifdef RMF_TRAFFIC__USING_FCL_0_6 #include @@ -294,15 +295,41 @@ BoundingProfile get_bounding_profile( BoundingBox base_box = get_bounding_box(spline); const auto& footprint = profile.footprint; - const auto f_box = footprint ? - adjust_bounding_box(base_box, footprint->get_characteristic_length()) : + + double max_footprint_length = footprint ? + footprint->get_characteristic_length() : 0.0; + for (uint i=0; iget_characteristic_length(); + if (max_footprint_length < dist) + max_footprint_length = dist; + } + auto f_box = footprint ? + adjust_bounding_box(base_box, max_footprint_length) : void_box(); + // std::cout << "footprint_box\n"; + // std::cout << f_box.min << std::endl; + // std::cout << f_box.max << std::endl; + const auto& vicinity = profile.vicinity; - const auto v_box = vicinity ? + auto v_box = vicinity ? adjust_bounding_box(base_box, vicinity->get_characteristic_length()) : void_box(); + if (v_box.min[0] > f_box.min[0] && v_box.max[0] < f_box.max[0] && + v_box.min[1] > f_box.min[1] && v_box.max[1] < f_box.max[1]) + { + v_box.min = f_box.min; + v_box.max = f_box.max; + } + // std::cout << "vicinity_box\n"; + // std::cout << v_box.min << std::endl; + // std::cout << v_box.max << std::endl; + + return BoundingProfile{f_box, v_box}; } @@ -328,12 +355,18 @@ bool overlap( using FclContinuousCollisionRequest = fcl::ContinuousCollisionRequestd; using FclContinuousCollisionResult = fcl::ContinuousCollisionResultd; using FclContinuousCollisionObject = fcl::ContinuousCollisionObjectd; +using FclCollisionRequest = fcl::CollisionRequestd; +using FclCollisionResult = fcl::CollisionResultd; +using FclCollisionObject = fcl::CollisionObjectd; using FclCollisionGeometry = fcl::CollisionGeometryd; using FclVec3 = fcl::Vector3d; #else using FclContinuousCollisionRequest = fcl::ContinuousCollisionRequest; using FclContinuousCollisionResult = fcl::ContinuousCollisionResult; using FclContinuousCollisionObject = fcl::ContinuousCollisionObject; +using FclCollisionRequest = fcl::CollisionRequest; +using FclCollisionResult = fcl::CollisionResult; +using FclCollisionObject = fcl::CollisionObject; using FclCollisionGeometry = fcl::CollisionGeometry; using FclVec3 = fcl::Vec3f; #endif @@ -371,26 +404,68 @@ FclContinuousCollisionRequest make_fcl_request() //============================================================================== rmf_utils::optional check_collision( - const geometry::FinalConvexShape& shape_a, + const Profile::Implementation& profile_footprint_a, const std::shared_ptr& motion_a, const geometry::FinalConvexShape& shape_b, const std::shared_ptr& motion_b, const FclContinuousCollisionRequest& request) { - const auto obj_a = FclContinuousCollisionObject( - geometry::FinalConvexShape::Implementation::get_collision(shape_a), + motion_a->integrate(0.0); + motion_b->integrate(0.0); + + if (profile_footprint_a.extra_footprint_count == 0) + { + const auto obj_a = FclContinuousCollisionObject( + geometry::FinalConvexShape::Implementation::get_collision(*profile_footprint_a.footprint), motion_a); - const auto obj_b = FclContinuousCollisionObject( - geometry::FinalConvexShape::Implementation::get_collision(shape_b), - motion_b); + const auto obj_b = FclContinuousCollisionObject( + geometry::FinalConvexShape::Implementation::get_collision(shape_b), + motion_b); - FclContinuousCollisionResult result; - fcl::collide(&obj_a, &obj_b, request, result); + FclContinuousCollisionResult result; + fcl::collide(&obj_a, &obj_b, request, result); - if (result.is_collide) - return result.time_of_contact; + if (result.is_collide) + return result.time_of_contact; + } + else + { + std::vector a_shapes; + std::vector b_shapes; + FclTransform3 identity; + identity.setIdentity(); + + a_shapes.emplace_back(identity, profile_footprint_a.footprint->get_characteristic_length()); + + for (uint i=0; iget_characteristic_length()); + } + b_shapes.emplace_back(identity, shape_b.get_characteristic_length()); + + double impact_time = 0.0; + double tolerance = 0.1; + uint dist_checks = 0; + const uint max_dist_checks = 120; + bool collide = collide_seperable_circles(*motion_a, *motion_b, a_shapes, b_shapes, + impact_time, dist_checks, max_dist_checks, tolerance); + + // std::cout << "dist_checks " << dist_checks << std::endl; + if (collide) + return impact_time; + } return rmf_utils::nullopt; } @@ -443,65 +518,138 @@ bool check_overlap( const Spline& spline_b, const Time time) { - using ConvexPair = std::array; - // TODO(MXG): If footprint and vicinity are equal, we can probably reduce this - // to just one check. - std::array pairs = { - ConvexPair{profile_a.footprint, profile_b.vicinity}, - ConvexPair{profile_a.vicinity, profile_b.footprint} - }; - #ifdef RMF_TRAFFIC__USING_FCL_0_6 - fcl::CollisionRequestd request; - fcl::CollisionResultd result; - for (const auto pair : pairs) - { - auto pos_a = spline_a.compute_position(time); - auto pos_b = spline_b.compute_position(time); - - auto rot_a = fcl::AngleAxisd(pos_a[2], Eigen::Vector3d::UnitZ()).toRotationMatrix(); - auto rot_b = fcl::AngleAxisd(pos_b[2], Eigen::Vector3d::UnitZ()).toRotationMatrix(); - - fcl::CollisionObjectd obj_a( - geometry::FinalConvexShape::Implementation::get_collision(*pair[0]), - rot_a, - fcl::Vector3d(pos_a[0], pos_a[1], 0.0)); - - fcl::CollisionObjectd obj_b( - geometry::FinalConvexShape::Implementation::get_collision(*pair[1]), - rot_b, - fcl::Vector3d(pos_b[0], pos_b[1], 0.0)); - - if (fcl::collide(&obj_a, &obj_b, request, result) > 0) + auto check_footprint_vicinity_collision = []( + const Profile::Implementation& profile_footprint, + Eigen::Vector3d pos_footprint, + Eigen::Matrix3d rotation_footprint, + const FclCollisionObject& obj_vicinity) -> bool + { + FclCollisionRequest request; + FclCollisionResult result; + + // test with main footprint + FclCollisionObject obj_footprint( + geometry::FinalConvexShape::Implementation::get_collision(*profile_footprint.footprint), + rotation_footprint, + Eigen::Vector3d(pos_footprint[0], pos_footprint[1], 0.0)); + + if (fcl::collide(&obj_footprint, &obj_vicinity, request, result) > 0) return true; - } -#else - fcl::CollisionRequest request; - fcl::CollisionResult result; + auto tx = obj_footprint.getTransform(); + + // go through the extra footprint shapes and do collision checks + for (uint i=0; i 0) + return true; + } + return false; + }; + + auto pos_a = spline_a.compute_position(time); + auto rot_a = fcl::AngleAxisd(pos_a[2], Eigen::Vector3d::UnitZ()).toRotationMatrix(); + FclCollisionObject obj_a_vicinity( + geometry::FinalConvexShape::Implementation::get_collision(*profile_a.vicinity), + rot_a, Eigen::Vector3d(pos_a[0], pos_a[1], 0.0)); + + auto pos_b = spline_b.compute_position(time); + auto rot_b = fcl::AngleAxisd(pos_b[2], Eigen::Vector3d::UnitZ()).toRotationMatrix(); + FclCollisionObject obj_b_vicinity( + geometry::FinalConvexShape::Implementation::get_collision(*profile_b.vicinity), + rot_b, Eigen::Vector3d(pos_b[0], pos_b[1], 0.0)); + + bool collide = check_footprint_vicinity_collision( + profile_a, pos_a, rot_a, obj_b_vicinity); + if (collide) + return true; + + collide = check_footprint_vicinity_collision( + profile_b, pos_b, rot_b, obj_a_vicinity); + if (collide) + return true; + return false; +#else auto convert = [](Eigen::Vector3d p) -> fcl::Transform3f { fcl::Matrix3f R; R.setEulerZYX(0.0, 0.0, p[2]); return fcl::Transform3f(R, fcl::Vec3f(p[0], p[1], 0.0)); }; + auto check_footprint_vicinity_collision = []( + const Profile::Implementation& profile_footprint, + fcl::Transform3f tx_footprint, + const FclCollisionObject& obj_vicinity) -> bool + { + FclCollisionRequest request; + FclCollisionResult result; + + // test with main footprint + FclCollisionObject obj_footprint( + geometry::FinalConvexShape::Implementation::get_collision(*profile_footprint.footprint), + tx_footprint); + + if (fcl::collide(&obj_footprint, &obj_vicinity, request, result) > 0) + return true; - for (const auto& pair : pairs) - { - fcl::CollisionObject obj_a( - geometry::FinalConvexShape::Implementation::get_collision(*pair[0]), - convert(spline_a.compute_position(time))); - - fcl::CollisionObject obj_b( - geometry::FinalConvexShape::Implementation::get_collision(*pair[1]), - convert(spline_b.compute_position(time))); + auto tx = obj_footprint.getTransform(); - if (fcl::collide(&obj_a, &obj_b, request, result) > 0) - return true; - } + // go through the extra footprint shapes and do collision checks + for (uint i=0; i 0) + return true; + } + return false; + }; + + auto tx_a = convert(spline_a.compute_position(time)); + FclCollisionObject obj_a_vicinity( + geometry::FinalConvexShape::Implementation::get_collision(*profile_a.vicinity), + tx_a); + + auto tx_b = convert(spline_b.compute_position(time)); + FclCollisionObject obj_b_vicinity( + geometry::FinalConvexShape::Implementation::get_collision(*profile_b.vicinity), + tx_b); + + bool collide = check_footprint_vicinity_collision( + profile_a, tx_a, obj_b_vicinity); + if (collide) + return true; + + collide = check_footprint_vicinity_collision( + profile_b, tx_b, obj_a_vicinity); + if (collide) + return true; return false; +#endif } //============================================================================== @@ -545,9 +693,11 @@ rmf_utils::optional detect_invasion( // This flag lets us know that we need to test both a's footprint in b's // vicinity and b's footprint in a's vicinity. - const bool test_complement = + bool test_complement = (profile_a.vicinity != profile_a.footprint) || (profile_b.vicinity != profile_b.footprint); + if (profile_a.extra_footprint_count != 0 || profile_b.extra_footprint_count != 0) + test_complement = true; if (output_conflicts) output_conflicts->clear(); @@ -575,7 +725,7 @@ rmf_utils::optional detect_invasion( if (overlap(bound_a.footprint, bound_b.vicinity)) { if (const auto collision = check_collision( - *profile_a.footprint, motion_a, + profile_a, motion_a, *profile_b.vicinity, motion_b, request)) { const auto time = compute_time(*collision, start_time, finish_time); @@ -590,8 +740,8 @@ rmf_utils::optional detect_invasion( if (test_complement && overlap(bound_a.vicinity, bound_b.footprint)) { if (const auto collision = check_collision( - *profile_a.vicinity, motion_a, - *profile_b.footprint, motion_b, request)) + profile_b, motion_b, + *profile_a.vicinity, motion_a, request)) { const auto time = compute_time(*collision, start_time, finish_time); if (!output_conflicts) diff --git a/rmf_traffic/src/rmf_traffic/Profile.cpp b/rmf_traffic/src/rmf_traffic/Profile.cpp index 774e6282..a443a720 100644 --- a/rmf_traffic/src/rmf_traffic/Profile.cpp +++ b/rmf_traffic/src/rmf_traffic/Profile.cpp @@ -26,12 +26,29 @@ Profile::Profile( : _pimpl(rmf_utils::make_impl( Implementation{ std::move(footprint), - std::move(vicinity) + std::move(vicinity), + 0, + Implementation::ExtraFootprintArray() })) { // Do nothing } +void Profile::add_extra_footprint(geometry::ConstFinalConvexShapePtr shape, Eigen::Vector3d offset) +{ + _pimpl->add_extra_footprint(shape, offset); +} + +uint Profile::extra_footprint_count() const +{ + return _pimpl->extra_footprint_count; +} + +void Profile::clear_extra_footprints() +{ + _pimpl->clear_extra_footprints(); +} + //============================================================================== Profile& Profile::footprint(geometry::ConstFinalConvexShapePtr shape) { diff --git a/rmf_traffic/src/rmf_traffic/ProfileInternal.hpp b/rmf_traffic/src/rmf_traffic/ProfileInternal.hpp index 0595f8a6..3354e571 100644 --- a/rmf_traffic/src/rmf_traffic/ProfileInternal.hpp +++ b/rmf_traffic/src/rmf_traffic/ProfileInternal.hpp @@ -30,10 +30,39 @@ class Profile::Implementation geometry::ConstFinalConvexShapePtr footprint; geometry::ConstFinalConvexShapePtr vicinity; + uint extra_footprint_count = 0; + static const size_t MAX_EXTRA_FOOTPRINTS = 1; + struct FootprintWithOffset + { + geometry::ConstFinalConvexShapePtr shape; + Eigen::Vector3d offset; + }; + using ExtraFootprintArray = + std::array; + ExtraFootprintArray extra_footprints; + static const Implementation& get(const Profile& profile) { return *profile._pimpl; } + + void add_extra_footprint(geometry::ConstFinalConvexShapePtr shape, Eigen::Vector3d offset) + { + if (extra_footprint_count >= MAX_EXTRA_FOOTPRINTS) + throw std::runtime_error("Maximum additional footprint shape count reached"); + + auto& footprint_shape = extra_footprints[extra_footprint_count]; + footprint_shape.shape = std::move(shape); + footprint_shape.offset = offset; + ++extra_footprint_count; + } + + void clear_extra_footprints() + { + for (uint i=0; i compute_coefficients( + const Eigen::Vector3d& x0, + const Eigen::Vector3d& x1, + const Eigen::Vector3d& v0, + const Eigen::Vector3d& v1) +{ + std::array coeffs; + for (int i = 0; i < 3; ++i) + { + // *INDENT-OFF* + std::size_t si = static_cast(i); + coeffs[si][0] = x0[i]; // = d + coeffs[si][1] = v0[i]; // = c + coeffs[si][2] = -v1[i] - 2*v0[i] + 3*x1[i] - 3*x0[i]; // = b + coeffs[si][3] = v1[i] + v0[i] - 2*x1[i] + 2*x0[i]; // = a + // *INDENT-ON* + } + + return coeffs; +} //============================================================================== Eigen::Matrix4d make_M_inv() @@ -38,32 +62,49 @@ Eigen::Matrix4d make_M_inv() } //============================================================================== -double compute_delta_t(const Time& finish_time, const Time& start_time) -{ - using Sec64 = std::chrono::duration; - return std::chrono::duration_cast(finish_time - start_time).count(); -} +const Eigen::Matrix4d M_inv = make_M_inv(); -//============================================================================== -std::array compute_coefficients( +FclSplineMotion to_fcl( const Eigen::Vector3d& x0, const Eigen::Vector3d& x1, const Eigen::Vector3d& v0, const Eigen::Vector3d& v1) { - std::array coeffs; - for (int i = 0; i < 3; ++i) + const std::array subspline_coeffs = + compute_coefficients(x0, x1, v0, v1); + + std::array knots; + for (std::size_t i = 0; i < 3; ++i) { - // *INDENT-OFF* - std::size_t si = static_cast(i); - coeffs[si][0] = x0[i]; // = d - coeffs[si][1] = v0[i]; // = c - coeffs[si][2] = -v1[i] - 2*v0[i] + 3*x1[i] - 3*x0[i]; // = b - coeffs[si][3] = v1[i] + v0[i] - 2*x1[i] + 2*x0[i]; // = a - // *INDENT-ON* + const Eigen::Vector4d p = M_inv * subspline_coeffs[i]; + for (int j = 0; j < 4; ++j) + knots[j][i] = p[j]; } - return coeffs; + std::array Td; + std::array Rd; + + for (std::size_t i = 0; i < 4; ++i) + { + const Eigen::Vector3d p = knots[i]; + Td[i] = FclVec3(p[0], p[1], 0.0); + Rd[i] = FclVec3(0.0, 0.0, p[2]); + } + + return FclSplineMotion( + Td[0], Td[1], Td[2], Td[3], + Rd[0], Rd[1], Rd[2], Rd[3]); +} + +namespace { + +const double time_tolerance = 1e-4; + +//============================================================================== +double compute_delta_t(const Time& finish_time, const Time& start_time) +{ + using Sec64 = std::chrono::duration; + return std::chrono::duration_cast(finish_time - start_time).count(); } //============================================================================== @@ -85,7 +126,6 @@ Spline::Parameters compute_parameters( const Eigen::Vector3d x1 = finish.position(); const Eigen::Vector3d v0 = delta_t * start.velocity(); const Eigen::Vector3d v1 = delta_t * finish.velocity(); - return { compute_coefficients(x0, x1, v0, v1), delta_t, @@ -201,9 +241,6 @@ Eigen::Vector3d compute_acceleration( } // anonymous namespace -//============================================================================== -const Eigen::Matrix4d M_inv = make_M_inv(); - //============================================================================== Spline::Spline(const Trajectory::const_iterator& it) : params(compute_parameters(it)) @@ -258,13 +295,14 @@ std::array Spline::compute_knots( FclSplineMotion Spline::to_fcl( const Time start_time, const Time finish_time) const { -#ifdef RMF_TRAFFIC__USING_FCL_0_6 - using FclVec3 = fcl::Vector3d; -#else - using FclVec3 = fcl::Vec3f; -#endif std::array knots = compute_knots(start_time, finish_time); + return to_fcl(knots); +} +//============================================================================== +FclSplineMotion Spline::to_fcl( + const std::array& knots) const +{ std::array Td; std::array Rd; diff --git a/rmf_traffic/src/rmf_traffic/Spline.hpp b/rmf_traffic/src/rmf_traffic/Spline.hpp index c49aab4e..1bff93be 100644 --- a/rmf_traffic/src/rmf_traffic/Spline.hpp +++ b/rmf_traffic/src/rmf_traffic/Spline.hpp @@ -34,8 +34,12 @@ namespace rmf_traffic { #ifdef RMF_TRAFFIC__USING_FCL_0_6 using FclSplineMotion = fcl::SplineMotion; + using FclTransform3 = fcl::Transform3d; + using FclVec3 = fcl::Vector3d; #else using FclSplineMotion = fcl::SplineMotion; + using FclTransform3 = fcl::Transform3f; + using FclVec3 = fcl::Vec3f; #endif //============================================================================== @@ -60,7 +64,10 @@ class Spline std::array compute_knots( const Time start_time, const Time finish_time) const; - FclSplineMotion to_fcl(const Time start_time, const Time finish_time) const; + FclSplineMotion to_fcl( + const Time start_time, const Time finish_time) const; + + FclSplineMotion to_fcl(const std::array& knots) const; Time start_time() const; Time finish_time() const; @@ -83,13 +90,19 @@ class Spline /// Get a const reference to the parameters of this spline const Parameters& get_params() const; - + private: Parameters params; }; +FclSplineMotion to_fcl( + const Eigen::Vector3d& x0, + const Eigen::Vector3d& x1, + const Eigen::Vector3d& v0, + const Eigen::Vector3d& v1); + //============================================================================== /// This class helps compute the differentials of the distance between two /// splines. @@ -116,6 +129,24 @@ class DistanceDifferential }; +//============================================================================== +struct ModelSpaceShape +{ + ModelSpaceShape(const FclTransform3& tx, double r) + :_transform(tx), _radius(r) + { } + FclTransform3 _transform; + double _radius; +}; + +extern bool collide_seperable_circles( + FclSplineMotion& motion_a, + FclSplineMotion& motion_b, + const std::vector& a_shapes, + const std::vector& b_shapes, + double& impact_time, uint& dist_checks, + uint safety_maximum_checks, double tolerance); + } // namespace rmf_traffic #endif // SRC__RMF_TRAFFIC__SPLINE_HPP diff --git a/rmf_traffic/src/rmf_traffic/SplineCCD.cpp b/rmf_traffic/src/rmf_traffic/SplineCCD.cpp new file mode 100644 index 00000000..0ea7a4cc --- /dev/null +++ b/rmf_traffic/src/rmf_traffic/SplineCCD.cpp @@ -0,0 +1,244 @@ + +#ifdef RMF_TRAFFIC__USING_FCL_0_6 +#include +#include +#include +#else +#include +#include +#include +#endif + +#include "Spline.hpp" +#include + +namespace rmf_traffic { + +// Check osrf/rmf_planner_viz for a demo of these 2 functions + +// attempts +static double max_splinemotion_advancement(double current_t, + FclSplineMotion& motion_a, + FclSplineMotion& motion_b, + const std::vector& a_shapes, + const std::vector& b_shapes, + const FclVec3& d_normalized, + double max_dist, + uint& dist_checks, double tolerance) +{ + assert(tolerance >= 0.0); + + double lower_t_limit = current_t; + double upper_t_limit = 1.0; + uint bilateral_adv_iter = 0; + + double s1 = max_dist, s2 = max_dist; + double sample_t = 0.0; + for (;;) + { +#ifdef DO_LOGGING + if (bilateral_adv_iter < 3) + printf("#1: (%f,%f) #2: (%f,%f)\n", lower_t_limit, s1, upper_t_limit, s2); +#endif + + // alternate between bisection and false position methods + if (bilateral_adv_iter & 1) + { + // use false position method + // solve for t where (t, tolerance) for a line with + // endpoints (lower_t_limit, s1) and (upper_t_limit, s2) + // where s2 is negative and s1 is positive + double inv_m = (upper_t_limit - lower_t_limit) / (s2 - s1); + sample_t = lower_t_limit + (tolerance - s1) * inv_m; + } + else // bisection method + sample_t = lower_t_limit + 0.5 * (upper_t_limit - lower_t_limit); +#ifdef DO_LOGGING + printf("iteration: %d picked t: %f\n", bilateral_adv_iter, sample_t); +#endif + + // integrate + motion_a.integrate(sample_t); + motion_b.integrate(sample_t); + + FclTransform3 a_tx, b_tx; + motion_a.getCurrentTransform(a_tx); + motion_b.getCurrentTransform(b_tx); + + double s = DBL_MAX; + // compute closest distance between all 4 shapes in direction d + for (const auto& a_shape : a_shapes) + { + auto a_shape_tx = a_tx * a_shape._transform; + for (const auto& b_shape : b_shapes) + { + auto b_shape_tx = b_tx * b_shape._transform; +#ifdef RMF_TRAFFIC__USING_FCL_0_6 + auto b_to_a = a_shape_tx.translation() - b_shape_tx.translation(); +#else + auto b_to_a = a_shape_tx.getTranslation() - b_shape_tx.getTranslation(); +#endif + + double b_to_a_dist = b_to_a.norm(); + double dist_between_shapes_along_d = 0.0; + if (b_to_a_dist > 1e-04) + { + auto b_to_a_norm = b_to_a / b_to_a_dist; + double dist_along_b_to_a = b_to_a_dist - (a_shape._radius + b_shape._radius); + auto v = dist_along_b_to_a * b_to_a_norm; + dist_between_shapes_along_d = v.dot(d_normalized); + } + + // get the minimum + if (dist_between_shapes_along_d < s) + s = dist_between_shapes_along_d; + } + } + +#ifdef DO_LOGGING + printf("dist_output: %f\n", s); +#endif + if (abs(s) < tolerance) + { +#ifdef DO_LOGGING + printf("minimal dist %f within tolerance range %f\n", s, tolerance); +#endif + break; + } + + // our window is very small and we're hopping around, so we stop. + // Also, this is what box2d does. + if (bilateral_adv_iter >= 25) + { +#ifdef DO_LOGGING + printf("range too small\n"); +#endif + break; + } + + if (s < 0.0) + { + upper_t_limit = sample_t; + s2 = s; + } + else if (s > 0.0) + { + lower_t_limit = sample_t; + s1 = s; + } + ++bilateral_adv_iter; + } + dist_checks += bilateral_adv_iter; + + return sample_t; +} + +bool collide_seperable_circles( + FclSplineMotion& motion_a, + FclSplineMotion& motion_b, + const std::vector& a_shapes, + const std::vector& b_shapes, + double& impact_time, uint& dist_checks, + uint safety_maximum_checks, double tolerance) +{ + if (a_shapes.empty() || b_shapes.empty()) + return false; + + auto calc_min_dist = []( + const FclTransform3& a_tx, + const FclTransform3& b_tx, + const std::vector& a_shapes, + const std::vector& b_shapes, + FclVec3& d, double& min_dist) + { + min_dist = DBL_MAX; + for (const auto& a_shape : a_shapes) + { + auto a_shape_tx = a_tx * a_shape._transform; + + for (const auto& b_shape : b_shapes) + { + auto b_shape_tx = b_tx * b_shape._transform; +#ifdef RMF_TRAFFIC__USING_FCL_0_6 + auto b_to_a = a_shape_tx.translation() - b_shape_tx.translation(); +#else + auto b_to_a = a_shape_tx.getTranslation() - b_shape_tx.getTranslation(); +#endif + double dist = b_to_a.norm() - (a_shape._radius + b_shape._radius); + if (dist < min_dist) + { + min_dist = dist; + d = b_to_a; + } + } + } + }; + + FclTransform3 a_start_tf, b_start_tf; + FclTransform3 a_tf, b_tf; + + motion_a.integrate(0.0); + motion_b.integrate(0.0); + motion_a.getCurrentTransform(a_start_tf); + motion_b.getCurrentTransform(b_start_tf); + + double dist_along_d_to_cover = 0.0; + FclVec3 d(0,0,0); + calc_min_dist(a_start_tf, b_start_tf, a_shapes, b_shapes, + d, dist_along_d_to_cover); + + double t = 0.0; + uint iter = 0; + while (dist_along_d_to_cover > tolerance && t < 1.0) + { +#ifdef RMF_TRAFFIC__USING_FCL_0_6 + FclVec3 d_normalized = d.normalized(); +#else + FclVec3 d_normalized = d.normalize(); +#endif +#ifdef DO_LOGGING + printf("======= iter:%d\n", iter); + std::cout << "d_norm: \n" << d_normalized << std::endl; +#endif + + t = max_splinemotion_advancement(t, motion_a, motion_b, a_shapes, b_shapes, + d_normalized, dist_along_d_to_cover, dist_checks, tolerance); +#ifdef DO_LOGGING + printf("max_splinemotion_advancement returns t: %f\n", t); +#endif + + motion_a.integrate(t); + motion_b.integrate(t); + + motion_a.getCurrentTransform(a_tf); + motion_b.getCurrentTransform(b_tf); + + calc_min_dist(a_tf, b_tf, a_shapes, b_shapes, + d, dist_along_d_to_cover); + + ++dist_checks; + ++iter; + + //infinite loop prevention. you should increase safety_maximum_checks if you still want a solution + if (dist_checks > safety_maximum_checks) + break; + } + + if (dist_checks > safety_maximum_checks) + return false; + + if (t >= 0.0 && t < 1.0) + { + impact_time = t; +#ifdef DO_LOGGING + printf("time of impact: %f\n", t); +#endif + return true; + } +#ifdef DO_LOGGING + printf("no collide\n"); +#endif + return false; +} + +} \ No newline at end of file diff --git a/rmf_traffic/test/unit/test_Conflict.cpp b/rmf_traffic/test/unit/test_Conflict.cpp index 0ae7dde1..b5201bb4 100644 --- a/rmf_traffic/test/unit/test_Conflict.cpp +++ b/rmf_traffic/test/unit/test_Conflict.cpp @@ -530,6 +530,169 @@ SCENARIO("DetectConflict unit tests") CHECK(conflicts.front().a_it == trajectory.find(begin_time + 20s)); } } + + GIVEN( + "A 2-point trajectory t2 with 2 circles for the robot's footprint against a stationary robot") + { + const auto box_shape = rmf_traffic::geometry::make_final_convex< + rmf_traffic::geometry::Box>(1.0, 1.f); + + const auto circle_shape = rmf_traffic::geometry::make_final_convex< + rmf_traffic::geometry::Circle>(0.5); + const auto circle_shape_ex = rmf_traffic::geometry::make_final_convex< + rmf_traffic::geometry::Circle>(0.6); + + rmf_traffic::Profile profile_circle { circle_shape }; + + rmf_traffic::Profile profile_circle_with_circle_offset { circle_shape }; + profile_circle_with_circle_offset.add_extra_footprint( + circle_shape_ex, Eigen::Vector3d(0, -1.0, 0)); + + const rmf_traffic::Time time = std::chrono::steady_clock::now(); + Eigen::Vector3d pos = Eigen::Vector3d(0, 0, 0); + Eigen::Vector3d vel = Eigen::Vector3d(0, 0, 0); + rmf_traffic::Trajectory t1; + t1.insert(time, pos, vel); + t1.insert(time + 1s, pos, vel); + + WHEN("t2 is following a straight line trajectory") + { + for (double x = -50.0; x <= 50.0; x += 0.125) + { + if (x == 0.0) + { + //TODO: This particular case fails because the + //the "const auto approach_times = D.approach_times();" + //in the detect_approach function gives 0 timings for + //a differential spline generated by 2 splines with + //stationary motion. + continue; + } + rmf_traffic::Trajectory t2; + t2.insert(time, Eigen::Vector3d(-x, 2, 0), Eigen::Vector3d(0, 0, 0)); + t2.insert(time + 1s, Eigen::Vector3d(x, 2, 0), Eigen::Vector3d(0, 0, 0)); + + const auto start_time = std::chrono::high_resolution_clock::now(); + + CHECK(rmf_traffic::DetectConflict::between( + profile_circle, t1, profile_circle_with_circle_offset, t2)); + + const auto end_time = std::chrono::high_resolution_clock::now(); + + std::chrono::duration dur = end_time - start_time; + double ms = dur.count(); + CHECK(ms <= 8.0); + //printf("Time taken (ms): %.10g\n", ms); + } + + for (double y = -50.0; y <= 50.0; y += 0.125) + { + if (y == 0.0) + { + //TODO: Same as the above + continue; + } + rmf_traffic::Trajectory t2; + t2.insert(time, Eigen::Vector3d(-1, y, 0), Eigen::Vector3d(0, 0, 0)); + t2.insert(time + 1s, Eigen::Vector3d(-1, -y, 0), Eigen::Vector3d(0, 0, 0)); + + const auto start_time = std::chrono::high_resolution_clock::now(); + + CHECK(rmf_traffic::DetectConflict::between( + profile_circle, t1, profile_circle_with_circle_offset, t2)); + + const auto end_time = std::chrono::high_resolution_clock::now(); + + std::chrono::duration dur = end_time - start_time; + double ms = dur.count(); + CHECK(ms <= 8.0); + //printf("Time taken (ms): %.10g\n", ms); + } + } + + WHEN("t2 is following a curved trajectory") + { + { + rmf_traffic::Trajectory t2; + t2.insert(time, Eigen::Vector3d(-2, 2, 0), Eigen::Vector3d(0, -2, 0)); + t2.insert(time + 1s, Eigen::Vector3d(2, 2, 0), Eigen::Vector3d(0, 2, 0)); + + CHECK(rmf_traffic::DetectConflict::between(profile_circle, t1, profile_circle_with_circle_offset, t2)); + } + + { + rmf_traffic::Trajectory t2; + t2.insert(time, Eigen::Vector3d(-3, 0, 0), Eigen::Vector3d(0, 4, 0)); + t2.insert(time + 1s, Eigen::Vector3d(3, 0, 0), Eigen::Vector3d(0, -4, 0)); + + CHECK(rmf_traffic::DetectConflict::between(profile_circle, t1, profile_circle_with_circle_offset, t2)); + } + } + } + + GIVEN( + "Stationary Robot with Sidecar Rotation hitting a Stationary Robot") + { + const auto box_shape = rmf_traffic::geometry::make_final_convex< + rmf_traffic::geometry::Box>(1.0, 1.f); + const auto circle_shape = rmf_traffic::geometry::make_final_convex< + rmf_traffic::geometry::Circle>(0.5); + const auto circle_shape_ex = rmf_traffic::geometry::make_final_convex< + rmf_traffic::geometry::Circle>(0.6); + + rmf_traffic::Profile profile_a { circle_shape }; + + rmf_traffic::Profile profile_b { circle_shape }; + profile_b.add_extra_footprint(circle_shape_ex, Eigen::Vector3d(0, -1, 0)); + + const rmf_traffic::Time time = std::chrono::steady_clock::now(); + rmf_traffic::Trajectory t1; + t1.insert(time, Eigen::Vector3d(0, 0, 0), Eigen::Vector3d(0, 0, 0)); + t1.insert(time + 1s, Eigen::Vector3d(0, 0, 0), Eigen::Vector3d(0, 0, 0)); + + rmf_traffic::Trajectory t2; + t2.insert(time, Eigen::Vector3d(-2, 0, 0), Eigen::Vector3d(0, 0, 0)); + t2.insert(time + 1s, Eigen::Vector3d(-2, 0, EIGEN_PI), Eigen::Vector3d(0, 0, 0)); + + CHECK(rmf_traffic::DetectConflict::between(profile_a, t1, profile_b, t2)); + } + + GIVEN( + "Robots with sidecar with trajectories that do not conflict") + { + const auto box_shape = rmf_traffic::geometry::make_final_convex< + rmf_traffic::geometry::Box>(1.0, 1.f); + const auto circle_shape = rmf_traffic::geometry::make_final_convex< + rmf_traffic::geometry::Circle>(0.5); + const auto circle_shape_ex = rmf_traffic::geometry::make_final_convex< + rmf_traffic::geometry::Circle>(0.6); + + rmf_traffic::Profile profile_a { circle_shape }; + profile_a.add_extra_footprint(circle_shape_ex, Eigen::Vector3d(0, 1, 0)); + + rmf_traffic::Profile profile_b { circle_shape }; + profile_b.add_extra_footprint(circle_shape_ex, Eigen::Vector3d(0, -1, 0)); + + const rmf_traffic::Time time = std::chrono::steady_clock::now(); + rmf_traffic::Trajectory t1; + t1.insert(time, Eigen::Vector3d(3, 0, 0), Eigen::Vector3d(0, 0, 0)); + t1.insert(time + 1s, Eigen::Vector3d(1, 0, -EIGEN_PI), Eigen::Vector3d(0, 0, 0)); + + rmf_traffic::Trajectory t2; + t2.insert(time, Eigen::Vector3d(-3, 0, 0), Eigen::Vector3d(0, 0, 0)); + t2.insert(time + 1s, Eigen::Vector3d(-1, 0, EIGEN_PI), Eigen::Vector3d(0, 0, 0)); + + const auto start_time = std::chrono::high_resolution_clock::now(); + + CHECK_FALSE(rmf_traffic::DetectConflict::between(profile_a, t1, profile_b, t2)); + + const auto end_time = std::chrono::high_resolution_clock::now(); + + std::chrono::duration dur = end_time - start_time; + double ms = dur.count(); + CHECK(ms <= 8.0); + //printf("Time taken (ms): %.10g\n", ms); + } } // A useful website for playing with 2D cubic splines: https://www.desmos.com/calculator/ diff --git a/rmf_traffic/test/unit/test_Profile.cpp b/rmf_traffic/test/unit/test_Profile.cpp index 4ba1ffc7..935efd16 100644 --- a/rmf_traffic/test/unit/test_Profile.cpp +++ b/rmf_traffic/test/unit/test_Profile.cpp @@ -123,6 +123,21 @@ SCENARIO("Testing Construction") CHECK(profile.vicinity()->get_characteristic_length() == Approx(1.0)); } } + + WHEN("Additional footprint shapes are added") + { + const auto circle_1 = rmf_traffic::geometry::make_final_convex< + rmf_traffic::geometry::Circle>(1.0); + const auto circle_2 = rmf_traffic::geometry::make_final_convex< + rmf_traffic::geometry::Circle>(2.0); + + auto profile = Profile{circle_1, circle_2}; + profile.add_extra_footprint(circle_1, Eigen::Vector3d(0, 2, 0)); + CHECK(profile.extra_footprint_count() == 1); + + profile.clear_extra_footprints(); + CHECK(profile.extra_footprint_count() == 0); + } } SCENARIO("Testing conflicts", "[close_start]")