diff --git a/navground_core/include/navground/core/common.h b/navground_core/include/navground/core/common.h index 800577a1..8671e948 100644 --- a/navground_core/include/navground/core/common.h +++ b/navground_core/include/navground/core/common.h @@ -233,7 +233,7 @@ struct NAVGROUND_CORE_EXPORT Twist2 { bool is_almost_zero(ng_float_t epsilon_speed = 1e-6, ng_float_t epsilon_angular_speed = 1e-6) const { return velocity.norm() < epsilon_speed && - abs(angular_speed) < epsilon_angular_speed; + std::abs(angular_speed) < epsilon_angular_speed; } /** @@ -243,13 +243,13 @@ struct NAVGROUND_CORE_EXPORT Twist2 { * @param[in] epsilon The tolerance */ void snap_to_zero(ng_float_t epsilon = 1e-6) { - if (abs(velocity[0]) < epsilon) { + if (std::abs(velocity[0]) < epsilon) { velocity[0] = 0; } - if (abs(velocity[1]) < epsilon) { + if (std::abs(velocity[1]) < epsilon) { velocity[1] = 0; } - if (abs(angular_speed) < epsilon) { + if (std::abs(angular_speed) < epsilon) { angular_speed = 0; } } @@ -278,7 +278,7 @@ struct NAVGROUND_CORE_EXPORT Twist2 { if (acc.norm() > max_acceleration) { acc = acc.normalized() * max_acceleration; } - if (abs(ang_acc) > max_angular_acceleration) { + if (std::abs(ang_acc) > max_angular_acceleration) { ang_acc = std::clamp(ang_acc, -max_angular_acceleration, max_angular_acceleration); } diff --git a/navground_core/include/navground/core/controller_3d.h b/navground_core/include/navground/core/controller_3d.h index 4ce5facd..3933497b 100644 --- a/navground_core/include/navground/core/controller_3d.h +++ b/navground_core/include/navground/core/controller_3d.h @@ -480,7 +480,7 @@ class NAVGROUND_CORE_EXPORT Controller3 : public Controller { return t; } return std::max( - t, abs(altitude.target - altitude.value) / altitude.optimal_speed); + t, std::abs(altitude.target - altitude.value) / altitude.optimal_speed); } /** @@ -488,7 +488,7 @@ class NAVGROUND_CORE_EXPORT Controller3 : public Controller { */ bool is_still() const override { return Controller::is_still() && - (limit_to_2d || abs(altitude.speed) < speed_tolerance); + (limit_to_2d || std::abs(altitude.speed) < speed_tolerance); } /** diff --git a/navground_core/include/navground/core/states/geometric.h b/navground_core/include/navground/core/states/geometric.h index b7bbb78b..48d333e2 100644 --- a/navground_core/include/navground/core/states/geometric.h +++ b/navground_core/include/navground/core/states/geometric.h @@ -258,7 +258,7 @@ struct LineSegment { const ng_float_t x = delta.dot(e1); if (x < 0) return delta.norm(); if (x > length) return (point - p2).norm(); - return abs(delta.dot(e2)); + return std::abs(delta.dot(e2)); } // negative <=> penetration diff --git a/navground_core/include/navground/core/target.h b/navground_core/include/navground/core/target.h index f3e7e762..782749bf 100644 --- a/navground_core/include/navground/core/target.h +++ b/navground_core/include/navground/core/target.h @@ -80,7 +80,7 @@ struct Target { bool satisfied(Radians value) const { return !orientation || - abs(normalize(*orientation - value)) < orientation_tolerance; + std::abs(normalize(*orientation - value)) < orientation_tolerance; } bool satisfied(const Vector2& value) const { diff --git a/navground_core/src/behaviors/HL.cpp b/navground_core/src/behaviors/HL.cpp index 60c33b91..26550b5d 100644 --- a/navground_core/src/behaviors/HL.cpp +++ b/navground_core/src/behaviors/HL.cpp @@ -7,6 +7,7 @@ #include #include +#include #include #include "navground/core/collision_computation.h" @@ -87,11 +88,11 @@ std::valarray HLBehavior::get_collision_angles() const { static inline ng_float_t distance_from_target(Radians angle, ng_float_t free_distance, ng_float_t horizon) { - if (cos(angle) * horizon < free_distance) { - return fabs(sin(angle) * horizon); + if (std::cos(angle) * horizon < free_distance) { + return std::abs(std::sin(angle) * horizon); } - return sqrt(horizon * horizon + free_distance * free_distance - - 2 * free_distance * horizon * cos(angle)); + return std::sqrt(horizon * horizon + free_distance * free_distance - + 2 * free_distance * horizon * std::cos(angle)); } // TODO(J:revision2023): check why we need effective_horizon @@ -108,7 +109,7 @@ Vector2 HLBehavior::desired_velocity_towards_point( // effective_horizon = horizon; // effective_horizon=fmin(horizon,D); // Vector2 effectiveTarget = agentToTarget / D * effective_horizon; - const ng_float_t max_distance = effective_horizon; // - radius; + const ng_float_t max_distance = effective_horizon; // - radius; const Radians max_angle = static_cast(1.6); // Radians::PI_OVER_TW0; Radians angle = 0; // relative to target direction @@ -118,7 +119,7 @@ Vector2 HLBehavior::desired_velocity_towards_point( bool found = false; while (angle < max_angle && !(out[0] == 2 && out[1] == 2)) { ng_float_t distance_from_target_lower_bound = - fabs(sin(angle) * max_distance); + std::abs(std::sin(angle) * max_distance); // distance_from_target_lower_bound=2*D*Sin(0.5*optimal_angle); if (optimal_distance_from_target <= distance_from_target_lower_bound) { // break; @@ -126,7 +127,7 @@ Vector2 HLBehavior::desired_velocity_towards_point( for (int i = 0; i < 2; ++i) { const ng_float_t s_angle = i == 0 ? angle : -angle; const bool inside = - abs(normalize(relative_start_angle + s_angle)) < aperture; + std::abs(normalize(relative_start_angle + s_angle)) < aperture; if (out[i] == 1 && !inside) out[i] = 2; if (out[i] == 0 && inside) out[i] = 1; if (inside) { @@ -149,7 +150,8 @@ Vector2 HLBehavior::desired_velocity_towards_point( if (!found) return Vector2::Zero(); const ng_float_t static_distance = collision_computation.static_free_distance( start_angle + optimal_angle, max_distance); - const ng_float_t desired_speed = fmin(target_speed, static_distance / eta); + const ng_float_t desired_speed = + std::min(target_speed, static_distance / eta); return desired_speed * unit(start_angle + optimal_angle); } @@ -217,8 +219,7 @@ Twist2 HLBehavior::relax(const Twist2 ¤t_value, const Twist2 &value, Twist2 HLBehavior::compute_cmd_internal(ng_float_t dt, Frame frame) { Twist2 cmd_twist = Behavior::compute_cmd_internal(dt, frame); if (tau > 0) { - cmd_twist = - to_frame(relax(actuated_twist, cmd_twist, dt), cmd_twist.frame); + cmd_twist = to_frame(relax(actuated_twist, cmd_twist, dt), cmd_twist.frame); } return cmd_twist; } diff --git a/navground_core/src/kinematics.cpp b/navground_core/src/kinematics.cpp index 9c661822..e9baf63b 100644 --- a/navground_core/src/kinematics.cpp +++ b/navground_core/src/kinematics.cpp @@ -42,7 +42,7 @@ Twist2 TwoWheelsDifferentialDriveKinematics::feasible( assert(value.frame == Frame::relative); const ng_float_t w_max = get_max_angular_speed(); const ng_float_t w = std::clamp(value.angular_speed, -w_max, w_max); - const ng_float_t v_max = get_max_speed() - abs(w) * get_axis() / 2; + const ng_float_t v_max = get_max_speed() - std::abs(w) * get_axis() / 2; const ng_float_t v = std::clamp(value.velocity[0], 0, v_max); // const ng_float_t v = std::clamp(value.velocity[0], -v_max, v_max); return Twist2{{v, 0}, w, Frame::relative}; @@ -78,10 +78,10 @@ WheelSpeeds TwoWheelsDifferentialDriveKinematics::feasible_wheel_speeds( std::clamp(twist.velocity[0], 0, max_speed); ng_float_t left = linear - rotation; ng_float_t right = linear + rotation; - if (abs(left) > max_speed) { + if (std::abs(left) > max_speed) { left = std::clamp(left, -max_speed, max_speed); right = left + 2 * rotation; - } else if (abs(right) > max_speed) { + } else if (std::abs(right) > max_speed) { right = std::clamp(right, -max_speed, max_speed); left = right - 2 * rotation; } @@ -128,22 +128,22 @@ WheelSpeeds FourWheelsOmniDriveKinematics::feasible_wheel_speeds( ng_float_t front_right = longitudinal + lateral + rotation; ng_float_t rear_left = longitudinal + lateral - rotation; ng_float_t rear_right = longitudinal - lateral + rotation; - if (abs(front_left) > max_speed) { + if (std::abs(front_left) > max_speed) { front_left = std::clamp(front_left, -max_speed, max_speed); front_right = front_left + 2 * lateral + 2 * rotation; rear_left = front_left + 2 * lateral; rear_right = front_left + 2 * rotation; - } else if (abs(front_right) > max_speed) { + } else if (std::abs(front_right) > max_speed) { front_right = std::clamp(front_right, -max_speed, max_speed); front_left = front_right - 2 * lateral - 2 * rotation; rear_left = front_right - 2 * rotation; rear_right = front_right - 2 * lateral; - } else if (abs(rear_left) > max_speed) { + } else if (std::abs(rear_left) > max_speed) { rear_left = std::clamp(rear_left, -max_speed, max_speed); front_left = rear_left - 2 * lateral; front_right = rear_left + 2 * rotation; rear_right = rear_left - 2 * rotation + 2 * rotation; - } else if (abs(rear_right) > max_speed) { + } else if (std::abs(rear_right) > max_speed) { rear_right = std::clamp(rear_right, -max_speed, max_speed); front_left = rear_right - 2 * rotation; front_right = rear_right + 2 * lateral; @@ -185,7 +185,7 @@ Twist2 DynamicTwoWheelsDifferentialDriveKinematics::feasible( const ng_float_t w = std::clamp(feasible_target.angular_speed, w0 - dw_max, w0 + dw_max); const ng_float_t dv_max = get_max_acceleration() * time_step - - abs(w - w0) * get_axis() * get_moi() / 4; + std::abs(w - w0) * get_axis() * get_moi() / 4; const ng_float_t v0 = current.velocity[0]; const ng_float_t v = std::clamp(feasible_target.velocity[0], v0 - dv_max, v0 + dv_max); diff --git a/navground_ros/src/ROSControllerNode.cpp b/navground_ros/src/ROSControllerNode.cpp index 3d2361b7..c8714e2f 100644 --- a/navground_ros/src/ROSControllerNode.cpp +++ b/navground_ros/src/ROSControllerNode.cpp @@ -145,7 +145,7 @@ static float yaw_from(const geometry_msgs::msg::Quaternion &q) { static float yaw_from(const Transform &t) { const auto rpy = t.linear().eulerAngles(2, 1, 0); - if (abs(rpy[1]) > M_PI_2 && abs(rpy[2]) > M_PI_2) { + if (std::abs(rpy[1]) > M_PI_2 && std::abs(rpy[2]) > M_PI_2) { return rpy[0] + M_PI; } return rpy[0]; diff --git a/navground_sim/src/world.cpp b/navground_sim/src/world.cpp index 99a2e992..6cf5495e 100644 --- a/navground_sim/src/world.cpp +++ b/navground_sim/src/world.cpp @@ -40,11 +40,11 @@ std::optional penetration_vector_inside_line(const LineSegment &line, const Vector2 ¢er, ng_float_t radius) { ng_float_t y = (center - line.p1).dot(line.e2); - if (abs(y) < radius) { + if (std::abs(y) < radius) { ng_float_t x = (center - line.p1).dot(line.e1); if (x < radius + 1e-3 || x > line.length - radius - 1e-3) return std::nullopt; - ng_float_t p = radius - abs(y); + ng_float_t p = radius - std::abs(y); if (y < 0) p *= -1; return p * line.e2; } @@ -54,10 +54,10 @@ std::optional penetration_vector_inside_line(const LineSegment &line, ng_float_t penetration_inside_line(const LineSegment &line, const Vector2 ¢er, ng_float_t radius) { ng_float_t y = (center - line.p1).dot(line.e2); - if (abs(y) < radius) { + if (std::abs(y) < radius) { ng_float_t x = (center - line.p1).dot(line.e1); if (x < radius + 1e-3 || x > line.length - radius - 1e-3) return 0.0; - return radius - abs(y); + return radius - std::abs(y); } return 0; }