diff --git a/include/constraints.hpp b/include/constraints.hpp index 1c5c5fe..05a76d7 100644 --- a/include/constraints.hpp +++ b/include/constraints.hpp @@ -24,12 +24,16 @@ struct Constraints { * @param imin_accel The minimum allowable acceleration for the robot in * meters per second per second. */ - Constraints(double imax_vel = std::numeric_limits::max(), + Constraints(double imax_vel, double imax_accel = std::numeric_limits::max(), double imax_jerk = std::numeric_limits::max(), double imin_accel = std::nan("")) : max_vel(imax_vel), max_accel(imax_accel), max_jerk(imax_jerk) { - min_accel = std::isnan(imin_accel) ? -imax_accel : imin_accel; + if (imax_accel == std::numeric_limits::max()) { + min_accel = std::numeric_limits::lowest(); + } else { + min_accel = std::isnan(imin_accel) ? -imax_accel : imin_accel; + } } /** diff --git a/include/physicalmodel/passthroughmodel.hpp b/include/physicalmodel/passthroughmodel.hpp index 995c5ed..0792605 100644 --- a/include/physicalmodel/passthroughmodel.hpp +++ b/include/physicalmodel/passthroughmodel.hpp @@ -17,14 +17,18 @@ class PassthroughModel : public PhysicalModel { */ PassthroughModel() = default; - Constraints constraints(UNUSED const Pose pose, - UNUSED double curvature, - UNUSED double vel) override { - return Constraints(); + Constraints constraints(const Pose pose, + double curvature, + double vel) override { + UNUSED(pose); + UNUSED(curvature); + return Constraints(vel); }; - std::vector linear_to_wheel_vels(UNUSED double lin_vel, - UNUSED double curvature) override { + std::vector linear_to_wheel_vels(double lin_vel, + double curvature) override { + UNUSED(lin_vel); + UNUSED(curvature); return std::vector{}; } diff --git a/include/physicalmodel/physicalmodel.hpp b/include/physicalmodel/physicalmodel.hpp index 50de232..66bd0fe 100644 --- a/include/physicalmodel/physicalmodel.hpp +++ b/include/physicalmodel/physicalmodel.hpp @@ -11,7 +11,7 @@ #include "geometry/pose.hpp" // This silences a warning in GCC/Clang about not using passed parameters -#define UNUSED __attribute__((unused)) +#define UNUSED(var) ((void)var) namespace squiggles { class PhysicalModel { diff --git a/include/physicalmodel/tankmodel.hpp b/include/physicalmodel/tankmodel.hpp index d4bf284..bda1139 100644 --- a/include/physicalmodel/tankmodel.hpp +++ b/include/physicalmodel/tankmodel.hpp @@ -25,7 +25,7 @@ class TankModel : public PhysicalModel { TankModel(double itrack_width, Constraints ilinear_constraints); Constraints - constraints(UNUSED const Pose pose, double curvature, double vel) override; + constraints(const Pose pose, double curvature, double vel) override; std::vector linear_to_wheel_vels(double lin_vel, double curvature) override; diff --git a/src/tankmodel.cpp b/src/tankmodel.cpp index d8f4c2b..944bdd3 100644 --- a/src/tankmodel.cpp +++ b/src/tankmodel.cpp @@ -14,15 +14,17 @@ TankModel::TankModel(double itrack_width, Constraints ilinear_constraints) : track_width(itrack_width), linear_constraints(ilinear_constraints) {} Constraints -TankModel::constraints(UNUSED const Pose pose, double curvature, double vel) { +TankModel::constraints(const Pose pose, double curvature, double vel) { + UNUSED(pose); auto max_vel = vel_constraint(pose, curvature, vel); auto [min_accel, max_accel] = accel_constraint(pose, curvature, vel); return Constraints(max_vel, max_accel, 0.0, min_accel); } -double TankModel::vel_constraint(UNUSED const Pose pose, +double TankModel::vel_constraint(const Pose pose, double curvature, double vel) { + UNUSED(pose); auto wheels = linear_to_wheel_vels(vel, curvature); auto left = wheels[0]; auto right = wheels[1]; @@ -37,9 +39,10 @@ double TankModel::vel_constraint(UNUSED const Pose pose, return ((left + right) / 2.0); } -std::tuple TankModel::accel_constraint(UNUSED const Pose pose, +std::tuple TankModel::accel_constraint(const Pose pose, double curvature, double vel) const { + UNUSED(pose); // auto [left, right] = linear_to_wheel_vels(vel, curvature); // auto max_wheel_speed = std::max(left, right); // auto min_wheel_speed = std::min(left, right); diff --git a/tst/model-constraints-test.cpp b/tst/model-constraints-test.cpp index e8a3644..9262625 100644 --- a/tst/model-constraints-test.cpp +++ b/tst/model-constraints-test.cpp @@ -10,6 +10,23 @@ using namespace squiggles; +TEST(model_constraints_test, min_constraints) { + auto constraints = Constraints(1.0); + auto model = std::make_shared(0.4, constraints); + auto spline = SplineGenerator(constraints, model, 0.1); + auto path = spline.generate({ + ControlVector(Pose(0, 0, 0), 0.0, 0.0), + ControlVector(Pose(0, 2, 0), 0.0, 0.0), + }); + + ASSERT_NEAR(path.back().vector.pose.x, 0, TEST_EPSILON); + ASSERT_NEAR(path.back().vector.pose.y, 2, TEST_EPSILON); + ASSERT_NEAR(path.back().vector.pose.yaw, 0, TEST_EPSILON); + + ASSERT_NEAR(path.front().vector.vel, 0.0, TEST_EPSILON); + ASSERT_LE(path.back().vector.vel, 0.1); +} + TEST(model_constraints_test, sharp_turn) { auto constraints = Constraints(2.0, 2.0, 1.0); auto model = std::make_shared(0.4, constraints);