diff --git a/.gitmodules b/.gitmodules deleted file mode 100644 index bcee83da..00000000 --- a/.gitmodules +++ /dev/null @@ -1,4 +0,0 @@ -[submodule "micras_proxy/libs/lsm6dsv-pid"] - path = micras_proxy/libs/lsm6dsv-pid - url = https://github.com/STMicroelectronics/lsm6dsv-pid.git - branch = master diff --git a/.vscode/settings.json b/.vscode/settings.json index 098f6903..2ffee386 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -15,10 +15,5 @@ "doxdocgen.file.fileOrder": [ "file", - ], - - "clangd.arguments": [ - "--pretty", - "--compile-commands-dir=${workspaceFolder}/build/", ] } diff --git a/config/constants.hpp b/config/constants.hpp index 0c16b8ff..73cecc70 100644 --- a/config/constants.hpp +++ b/config/constants.hpp @@ -24,10 +24,11 @@ constexpr float cell_size{0.18}; constexpr uint32_t loop_time_us{1042}; constexpr float wall_thickness{0.0126F}; constexpr float start_offset{0.04F + wall_thickness / 2.0F}; -constexpr float exploration_speed{0.5F}; -constexpr float max_linear_acceleration{1.0F}; -constexpr float max_angular_acceleration{200.0F}; -constexpr float crash_acceleration{20.0F}; +constexpr float max_linear_acceleration{9.0F}; +constexpr float max_linear_deceleration{9.0F}; +constexpr float max_angular_acceleration{300.0F}; +constexpr float crash_acceleration{35.0F}; +constexpr float fan_speed{100.0F}; constexpr core::WallSensorsIndex wall_sensors_index{ .left_front = 0, @@ -51,24 +52,23 @@ using Maze = TMaze; const nav::ActionQueuer::Config action_queuer_config{ .cell_size = cell_size, .start_offset = start_offset, + .curve_safety_margin = 0.0375F + 0.015F, .exploring = { - .max_linear_speed = exploration_speed, + .max_linear_speed = 0.4F, .max_linear_acceleration = max_linear_acceleration, - .max_linear_deceleration = max_linear_acceleration, - .curve_radius = cell_size / 2.0F, + .max_linear_deceleration = max_linear_deceleration, .max_centrifugal_acceleration = 2.78F, .max_angular_acceleration = max_angular_acceleration, }, .solving = { - .max_linear_speed = exploration_speed, + .max_linear_speed = 3.0F, .max_linear_acceleration = max_linear_acceleration, - .max_linear_deceleration = max_linear_acceleration, - .curve_radius = cell_size / 2.0F, - .max_centrifugal_acceleration = 1.0F, + .max_linear_deceleration = max_linear_deceleration, + .max_centrifugal_acceleration = 5.0F, .max_angular_acceleration = max_angular_acceleration, - }, + } }; const nav::FollowWall::Config follow_wall_config{ @@ -81,10 +81,11 @@ const nav::FollowWall::Config follow_wall_config{ .saturation = 1.0F, .max_integral = -1.0F, }, - .max_linear_speed = 0.1F, - .post_threshold = 16.5F, + .max_angular_acceleration = max_angular_acceleration, .cell_size = cell_size, - .post_clearance = 0.2F * cell_size, + .post_threshold = 16.5F, + .post_reference = 0.44F * cell_size, + .post_clearance = 0.025F, }; const nav::Maze::Config maze_config{ @@ -96,17 +97,16 @@ const nav::Maze::Config maze_config{ {(maze_width - 1) / 2, (maze_height - 1) / 2}, }}, .cost_margin = 1.2F, + .action_queuer_config = action_queuer_config, }; const nav::Odometry::Config odometry_config{ .linear_cutoff_frequency = 5.0F, .wheel_radius = 0.0112F, - .initial_pose = {{0.0F, 0.0F}, 0.0F}, + .initial_pose = {{cell_size / 2.0F, start_offset}, std::numbers::pi / 2.0F}, }; const nav::SpeedController::Config speed_controller_config{ - .max_linear_acceleration = max_linear_acceleration, - .max_angular_acceleration = max_angular_acceleration, .linear_pid = { .kp = 10.0F, diff --git a/include/micras/interface.hpp b/include/micras/interface.hpp index e4aa01ff..f6d5d711 100644 --- a/include/micras/interface.hpp +++ b/include/micras/interface.hpp @@ -27,7 +27,15 @@ class Interface { SOLVE = 1, CALIBRATE = 2, ERROR = 3, - NUMBER_OF_EVENTS = 4, + TURN_ON_FAN = 4, + TURN_OFF_FAN = 5, + TURN_ON_DIAGONAL = 6, + TURN_OFF_DIAGONAL = 7, + TURN_ON_BOOST = 8, + TURN_OFF_BOOST = 9, + TURN_ON_RISKY = 10, + TURN_OFF_RISKY = 11, + NUMBER_OF_EVENTS = 12, }; /** @@ -74,6 +82,16 @@ class Interface { bool peek_event(Event event) const; private: + /** + * @brief Enum for what each dip switch pin does. + */ + enum DipSwitchPins : uint8_t { + FAN = 0, + DIAGONAL = 1, + BOOST = 2, + RISKY = 3, + }; + /** * @brief Addressable RGB LED object. */ @@ -103,6 +121,11 @@ class Interface { * @brief Array of the listed events. */ std::array events{}; + + /** + * @brief Array to store the last dip switch states. + */ + std::array dip_switch_states{}; }; } // namespace micras diff --git a/include/micras/micras.hpp b/include/micras/micras.hpp index 2d6e247a..529e1b13 100644 --- a/include/micras/micras.hpp +++ b/include/micras/micras.hpp @@ -134,11 +134,16 @@ class Micras { */ void load_best_route(); + /** + * @brief Handle events from the interface. + */ + void handle_events(); + private: /** * @brief Enum for the type of calibration being performed. */ - enum CalibrationType : uint8_t { + enum class CalibrationType : uint8_t { SIDE_WALLS = 0, // Calibrate side walls and front free space detection. FRONT_WALL = 1, // Calibrate front wall detection. }; @@ -190,7 +195,7 @@ class Micras { /** * @brief Finite state machine for the robot. */ - core::FSM fsm{State::INIT}; + core::Fsm fsm{State::INIT}; /** * @brief Class for controlling the interface with the external world. diff --git a/include/micras/states/base.hpp b/include/micras/states/base.hpp index 32da2eec..de28b6dd 100644 --- a/include/micras/states/base.hpp +++ b/include/micras/states/base.hpp @@ -9,7 +9,7 @@ #include "micras/micras.hpp" namespace micras { -class BaseState : public core::FSM::State { +class BaseState : public core::Fsm::State { public: /** * @brief Construct a new BaseState object. diff --git a/include/micras/states/idle.hpp b/include/micras/states/idle.hpp index ead7da1c..42218549 100644 --- a/include/micras/states/idle.hpp +++ b/include/micras/states/idle.hpp @@ -26,6 +26,8 @@ class IdleState : public BaseState { * @return The id of the next state. */ uint8_t execute() override { + this->micras.handle_events(); + if (this->micras.acknowledge_event(Interface::Event::EXPLORE)) { this->micras.set_objective(core::Objective::EXPLORE); diff --git a/micras_core/include/micras/core/fsm.hpp b/micras_core/include/micras/core/fsm.hpp index 95023e0a..9755c216 100644 --- a/micras_core/include/micras/core/fsm.hpp +++ b/micras_core/include/micras/core/fsm.hpp @@ -10,7 +10,7 @@ #include namespace micras::core { -class FSM { +class Fsm { public: class State { public: @@ -67,7 +67,7 @@ class FSM { * * @param initial_state_id The id of the initial state. */ - explicit FSM(uint8_t initial_state_id); + explicit Fsm(uint8_t initial_state_id); /** * @brief Add a state to the FSM. diff --git a/micras_core/include/micras/core/pid_controller.hpp b/micras_core/include/micras/core/pid_controller.hpp index 5a62612f..64528477 100644 --- a/micras_core/include/micras/core/pid_controller.hpp +++ b/micras_core/include/micras/core/pid_controller.hpp @@ -94,6 +94,11 @@ class PidController { */ float max_integral; + /** + * @brief Flag indicating whether this is the first run of the controller. + */ + bool first_run = true; + /** * @brief Accumulated error for integrative term. */ diff --git a/micras_core/include/micras/core/types.hpp b/micras_core/include/micras/core/types.hpp index 5a29bba5..981df502 100644 --- a/micras_core/include/micras/core/types.hpp +++ b/micras_core/include/micras/core/types.hpp @@ -20,7 +20,7 @@ struct Observation { /** * @brief Possible objectives of the robot. */ -enum Objective : uint8_t { +enum class Objective : uint8_t { EXPLORE = 0, RETURN = 1, SOLVE = 2 diff --git a/micras_core/include/micras/core/utils.hpp b/micras_core/include/micras/core/utils.hpp index 0553c1f1..ca788ac5 100644 --- a/micras_core/include/micras/core/utils.hpp +++ b/micras_core/include/micras/core/utils.hpp @@ -38,7 +38,7 @@ constexpr T remap(T value, T in_min, T in_max, T out_min, T out_max) { */ template constexpr T move_towards(T value, T target, T step) { - return std::max(std::min(value + step, target), value - step); + return std::clamp(target, value - step, value + step); } /** diff --git a/micras_core/src/fsm.cpp b/micras_core/src/fsm.cpp index f0d1b4c9..9517969e 100644 --- a/micras_core/src/fsm.cpp +++ b/micras_core/src/fsm.cpp @@ -5,19 +5,19 @@ #include "micras/core/fsm.hpp" namespace micras::core { -FSM::State::State(uint8_t id) : id{id} { } +Fsm::State::State(uint8_t id) : id{id} { } -uint8_t FSM::State::get_id() const { +uint8_t Fsm::State::get_id() const { return this->id; } -FSM::FSM(uint8_t initial_state_id) : current_state_id{initial_state_id} { } +Fsm::Fsm(uint8_t initial_state_id) : current_state_id{initial_state_id} { } -void FSM::add_state(std::unique_ptr state) { +void Fsm::add_state(std::unique_ptr state) { this->states.emplace(state->get_id(), std::move(state)); } -void FSM::update() { +void Fsm::update() { if (this->current_state_id != this->previous_state_id) { this->states.at(this->current_state_id)->on_entry(); } diff --git a/micras_core/src/pid_controller.cpp b/micras_core/src/pid_controller.cpp index baa44c65..ac20ac76 100644 --- a/micras_core/src/pid_controller.cpp +++ b/micras_core/src/pid_controller.cpp @@ -33,9 +33,15 @@ void PidController::reset() { this->error_acc = 0; this->prev_state = 0; this->last_response = 0; + this->first_run = true; } float PidController::compute_response(float state, float elapsed_time, bool save) { + if (first_run) { + this->prev_state = state; + this->first_run = false; + } + const float state_change = (state - this->prev_state) / elapsed_time; return this->compute_response(state, elapsed_time, state_change, save); } diff --git a/micras_nav/include/micras/nav/action_queuer.hpp b/micras_nav/include/micras/nav/action_queuer.hpp index 5119f03b..4849858d 100644 --- a/micras_nav/include/micras/nav/action_queuer.hpp +++ b/micras_nav/include/micras/nav/action_queuer.hpp @@ -5,9 +5,9 @@ #ifndef MICRAS_NAV_ACTION_QUEUER_HPP #define MICRAS_NAV_ACTION_QUEUER_HPP +#include #include #include -#include #include "micras/nav/actions/move.hpp" #include "micras/nav/actions/turn.hpp" @@ -25,10 +25,9 @@ class ActionQueuer { STOP = 0, START = 1, MOVE_FORWARD = 2, - MOVE_HALF = 3, - TURN_LEFT = 4, - TURN_RIGHT = 5, - TURN_BACK = 6, + TURN = 3, + SPIN = 4, + DIAGONAL = 5, }; /** @@ -39,13 +38,13 @@ class ActionQueuer { float max_linear_speed; float max_linear_acceleration; float max_linear_deceleration; - float curve_radius; float max_centrifugal_acceleration; float max_angular_acceleration; }; float cell_size; float start_offset; + float curve_safety_margin; Dynamic exploring; Dynamic solving; }; @@ -58,12 +57,12 @@ class ActionQueuer { explicit ActionQueuer(Config config); /** - * @brief Push an action to the queue. + * @brief Push an exploring action to the queue. * - * @param current_pose Current pose of the robot. + * @param origin_pose Origin pose where the action will be performed. * @param target_position Target position to move to. */ - void push(const GridPose& current_pose, const GridPoint& target_position); + void push_exploring(const GridPose& origin_pose, const GridPoint& target_position); /** * @brief Pop an action from the queue. @@ -82,23 +81,124 @@ class ActionQueuer { /** * @brief Fill the action queue with a sequence of actions to the end. */ - void recompute(const std::list& best_route); + void recompute(const std::list& best_route, bool add_start = true); + + /** + * @brief Get the total time to complete the actions in the queue. + * + * @return Total time to complete the actions in the queue. + */ + float get_total_time() const; + + /** + * @brief Get the actions to perform from the origin pose to the target pose. + * + * @param origin_pose Origin pose of the robot. + * @param target_pose Target pose to move to. + * @param cell_size Size of the cells in the grid. + * @return List of actions to perform. + */ + static Action::Id get_action(const GridPose& origin_pose, const GridPoint& target_point, float cell_size); private: + /** + * @brief Struct to hold the parameters for a curve. + */ + struct CurveParameters { + float curve_radius; + float max_angular_speed; + float linear_speed; + float side_displacement; + float forward_displacement; + }; + + /** + * @brief Enum for the types of curves. + */ + enum CurveType : uint8_t { + REGULAR_45 = 0, + REGULAR_90 = 1, + DIAGONAL_90 = 2, + REGULAR_135 = 3, + REGULAR_180 = 4, + NUMBER_OF_CURVES = 5, + }; + + /** + * @brief Add diagonals to the list of actions. + * + * @param actions List of actions to add diagonals to. + */ + void add_diagonals(std::list& actions) const; + + /** + * @brief Join consecutive actions of the same type. + * + * @param actions List of actions to join. + */ + void join_actions(std::list& actions); + + /** + * @brief Get the trim distances for a turn action. + * + * @param action_before Id of the action before the turn action. + * @param turn_action Turn action to get the trim distances for. + * @return Pair of trim distances before and after the turn. + */ + std::pair get_trim_distances(const Action::Id& action_before, const Action::Id& turn_action); + + /** + * @brief Get the curve parameters for a given angle and diagonal status. + * + * @param angle Angle of the curve. + * @param is_diagonal True if the curve is diagonal, false otherwise. + * @return Reference to the CurveParameters struct for the given angle and diagonal status. + */ + CurveParameters& get_curve_parameters(float angle, bool is_diagonal); + + /** + * @brief Compute the curve parameters for a given angle and diagonal status. + * + * @param angle Angle of the curve. + * @param is_diagonal True if the curve is diagonal, false otherwise. + */ + void compute_curve_parameters(float angle, bool is_diagonal); + + /** + * @brief Calculate the radius of the curve based on the angle, cell size, and safety margin. + * + * @param angle Angle of the curve in radians. + * @param cell_size Size of the maze cells. + * @param safety_margin Minimum distance between the robot and the wall during the curve. + * @param is_diagonal True if the curve is diagonal, false otherwise. + * @return Calculated radius of the curve. + */ + static constexpr float calculate_curve_radius(float angle, float cell_size, float safety_margin, bool is_diagonal); + /** * @brief Size of the cells in the grid. */ float cell_size; /** - * @brief Dynamic exploring parameters. + * @brief Distance of the robot to the on on the start cell. */ - Config::Dynamic exploring_params; + float start_offset; + + /** + * @brief Minimum distance between the robot and the wall during the curve. + */ + float curve_safety_margin; /** * @brief Dynamic solving parameters. */ - // Config::Dynamic solving_params; + Config::Dynamic solving_params; + + /** + * @brief Pre-computed curve parameters for different angles. + */ + std::array curves_parameters{}; /** * @brief Pre-built actions to use in the exploration. @@ -108,6 +208,8 @@ class ActionQueuer { std::shared_ptr start; std::shared_ptr move_forward; std::shared_ptr move_half; + std::shared_ptr move_to_turn; + std::shared_ptr move_from_turn; std::shared_ptr turn_left; std::shared_ptr turn_right; std::shared_ptr turn_back; @@ -116,7 +218,7 @@ class ActionQueuer { /** * @brief Queue of actions to be performed. */ - std::queue> action_queue; + std::deque> action_queue; }; } // namespace micras::nav diff --git a/micras_nav/include/micras/nav/actions/base.hpp b/micras_nav/include/micras/nav/actions/base.hpp index 93f24eab..d14334f3 100644 --- a/micras_nav/include/micras/nav/actions/base.hpp +++ b/micras_nav/include/micras/nav/actions/base.hpp @@ -13,12 +13,20 @@ namespace micras::nav { */ class Action { public: + struct Id { + uint8_t type; + float value; + }; + /** * @brief Constructor for the Action class. * * @param action_id The ID of the action. + * @param follow_wall Whether the robot can follow walls while executing this action. + * @param total_time The total time it takes to perform the action. */ - explicit Action(uint8_t action_id) : id(action_id) { } + Action(const Id& action_id, bool follow_wall, float total_time = 0.0F) : + id{action_id}, follow_wall{follow_wall}, total_time{total_time} { } /** * @brief Virtual destructor for the Action class. @@ -28,32 +36,40 @@ class Action { /** * @brief Get the desired speeds for the robot. * - * @param pose The current pose of the robot. + * @param current_pose The current pose of the robot. + * @param time_step The time step for the action in seconds. * @return The desired speeds for the robot. */ - virtual Twist get_speeds(const Pose& pose) const = 0; + virtual Twist get_speeds(const Pose& current_pose, float time_step) = 0; /** * @brief Check if the action is finished. * - * @param pose The current pose of the robot. + * @param current_pose The current pose of the robot. * @return True if the action is finished, false otherwise. */ - virtual bool finished(const Pose& pose) const = 0; + virtual bool finished(const Pose& current_pose) = 0; + + /** + * @brief Get the ID of the action. + * + * @return The ID of the action. + */ + const Id& get_id() const { return id; } /** * @brief Check if the action allow the robot to follow walls. * * @return True if the action allows the robot to follow walls, false otherwise. */ - virtual bool allow_follow_wall() const = 0; + bool allow_follow_wall() const { return follow_wall; } /** - * @brief Get the ID of the action. + * @brief Get the total time it takes to perform the action. * - * @return The ID of the action. + * @return The total time of the action in seconds. */ - uint8_t get_id() const { return id; } + float get_total_time() const { return this->total_time; }; protected: /** @@ -70,7 +86,17 @@ class Action { /** * @brief The ID of the action. */ - uint8_t id; + Id id; + + /** + * @brief Whether the robot can follow walls while executing this action. + */ + bool follow_wall; + + /** + * @brief The total time it takes to perform the action. + */ + float total_time; }; } // namespace micras::nav diff --git a/micras_nav/include/micras/nav/actions/move.hpp b/micras_nav/include/micras/nav/actions/move.hpp index fa9a7e30..00114930 100644 --- a/micras_nav/include/micras/nav/actions/move.hpp +++ b/micras_nav/include/micras/nav/actions/move.hpp @@ -15,30 +15,40 @@ namespace micras::nav { */ class MoveAction : public Action { public: + /** + * @brief Configuration parameters for the MoveAction. + */ + struct Config { + float start_speed; + float end_speed; + float max_speed; + float max_acceleration; + float max_deceleration; + }; + /** * @brief Construct a new Move Action object. * - * @param action_id The ID of the action. + * @param action_type The type of the action to be performed. * @param distance Distance to move in meters. - * @param start_speed Initial speed in m/s. - * @param end_speed Final speed in m/s. - * @param max_speed Maximum speed in m/s. - * @param max_acceleration Maximum acceleration in m/s^2. - * @param max_deceleration Maximum deceleration in m/s^2. + * @param config Dynamic parameters for the action. * @param follow_wall Whether the robot can follow wall while executing this action. */ - MoveAction( - uint8_t action_id, float distance, float start_speed, float end_speed, float max_speed, float max_acceleration, - float max_deceleration, bool follow_wall = true - ) : - Action{action_id}, + MoveAction(uint8_t action_type, float distance, const Config& config, bool follow_wall = true) : + Action{ + {action_type, distance}, + follow_wall, + calculate_total_time( + distance, config.start_speed, config.end_speed, config.max_speed, config.max_acceleration, + config.max_deceleration + ) + }, distance(distance), - start_speed_2(start_speed * start_speed), - end_speed_2(end_speed * end_speed), - max_speed{max_speed}, - max_acceleration_doubled{2.0F * max_acceleration}, - max_deceleration_doubled{2.0F * max_deceleration}, - follow_wall(follow_wall), + start_speed_2(config.start_speed * config.start_speed), + end_speed_2(config.end_speed * config.end_speed), + max_speed{config.max_speed}, + max_acceleration_doubled{2.0F * config.max_acceleration}, + max_deceleration_doubled{2.0F * config.max_deceleration}, decelerate_distance{ (end_speed_2 - start_speed_2 + max_deceleration_doubled * distance) / (max_acceleration_doubled + max_deceleration_doubled) @@ -47,18 +57,20 @@ class MoveAction : public Action { /** * @brief Get the desired speeds for the robot to complete the action. * - * @param pose The current pose of the robot. + * @param current_pose The current pose of the robot. + * @param time_step The time step for the action in seconds. * @return The desired speeds for the robot to complete the action. * * @details The desired velocity is calculated from the linear displacement based on the Torricelli equation. */ - Twist get_speeds(const Pose& pose) const override { - const float current_distance = pose.position.magnitude(); + Twist get_speeds(const Pose& current_pose, float time_step) override { + const float current_distance = current_pose.position.magnitude(); Twist twist{}; if (current_distance < this->decelerate_distance) { twist = { - .linear = std::sqrt(this->start_speed_2 + this->max_acceleration_doubled * current_distance), + .linear = std::sqrt(this->start_speed_2 + this->max_acceleration_doubled * current_distance) + + time_step * this->max_acceleration_doubled / 2.0F, .angular = 0.0F, }; } else { @@ -69,7 +81,7 @@ class MoveAction : public Action { }; } - twist.linear = std::fminf(twist.linear, this->max_speed); + twist.linear = std::clamp(twist.linear, 0.0F, this->max_speed); return twist; } @@ -80,14 +92,44 @@ class MoveAction : public Action { * @param pose The current pose of the robot. * @return True if the action is finished, false otherwise. */ - bool finished(const Pose& pose) const override { return pose.position.magnitude() >= this->distance; } + bool finished(const Pose& pose) override { return pose.position.magnitude() >= this->distance; } /** - * @brief Check if the action allows the robot to follow walls. + * @brief Calculate the total time to complete the action. * - * @return True if the action allows the robot to follow walls, false otherwise. + * @param distance Distance to move in meters. + * @param start_speed Initial speed in m/s. + * @param end_speed Final speed in m/s. + * @param max_speed Maximum speed in m/s. + * @param max_acceleration Maximum acceleration in m/s^2. + * @param max_deceleration Maximum deceleration in m/s^2. + * @return Total time to complete the action in seconds. */ - bool allow_follow_wall() const override { return this->follow_wall; } + static constexpr float calculate_total_time( + float distance, float start_speed, float end_speed, float max_speed, float max_acceleration, + float max_deceleration + ) { + const float peak_velocity = std::sqrt( + (2.0F * distance * max_acceleration * max_deceleration + start_speed * start_speed * max_deceleration + + end_speed * end_speed * max_acceleration) / + (max_acceleration + max_deceleration) + ); + + if (peak_velocity < max_speed) { + return (peak_velocity - start_speed) / max_acceleration + (peak_velocity - end_speed) / max_deceleration; + } + + const float acceleration_time = (max_speed - start_speed) / max_acceleration; + const float deceleration_time = (max_speed - end_speed) / max_deceleration; + + const float acceleration_distance = + (start_speed * start_speed + max_speed * max_speed) / (2.0F * max_acceleration); + const float deceleration_distance = (max_speed * max_speed + end_speed * end_speed) / (2.0F * max_deceleration); + const float constant_speed_distance = distance - acceleration_distance - deceleration_distance; + const float constant_speed_time = constant_speed_distance / max_speed; + + return acceleration_time + deceleration_time + constant_speed_time; + } private: /** @@ -96,12 +138,12 @@ class MoveAction : public Action { float distance; /** - * @brief Initial speed squared. + * @brief Initial linear speed squared. */ float start_speed_2; /** - * @brief Final speed squared. + * @brief Final linear speed squared. */ float end_speed_2; @@ -120,11 +162,6 @@ class MoveAction : public Action { */ float max_deceleration_doubled; - /** - * @brief Whether the robot can follow walls while executing this action. - */ - bool follow_wall; - /** * @brief Distance from the start where the robot should start to decelerate in meters. */ diff --git a/micras_nav/include/micras/nav/actions/turn.hpp b/micras_nav/include/micras/nav/actions/turn.hpp index c7a2a189..a5586b73 100644 --- a/micras_nav/include/micras/nav/actions/turn.hpp +++ b/micras_nav/include/micras/nav/actions/turn.hpp @@ -7,65 +7,67 @@ #include #include +#include #include "micras/nav/actions/base.hpp" namespace micras::nav { -/** - * @brief Correction factor to adjust the maximum angular speed. - * - * @details This factor is determined by analyzing the graph of the maximum angular speed. - */ -static constexpr float correction_factor{0.95F}; - /** * @brief Action to turn the robot following a curve radius. */ class TurnAction : public Action { public: + /** + * @brief Configuration parameters for the TurnAction. + */ + struct Config { + float max_angular_speed; + float linear_speed; + float max_angular_acceleration; + }; + /** * @brief Construct a new Turn Action object. * - * @param action_id The ID of the action. + * @param action_type The type of the action to be performed. * @param angle Angle to turn in radians. - * @param curve_radius Radius of the curve in meters. - * @param linear_speed Linear speed in m/s. - * @param max_angular_acceleration Maximum angular acceleration in rad/s^2. + * @param config Dynamic parameters for the action. */ - TurnAction(uint8_t action_id, float angle, float curve_radius, float linear_speed, float max_angular_acceleration) : - Action{action_id}, - start_orientation{max_angular_acceleration * 0.001F * 0.001F / 2.0F}, + TurnAction(uint8_t action_type, float angle, const Config& config) : + Action{ + {action_type, angle}, + false, + calculate_total_time(angle, config.max_angular_speed, config.max_angular_acceleration) + }, angle{angle}, - linear_speed{linear_speed}, - acceleration{max_angular_acceleration}, - max_angular_speed{calculate_max_angular_speed(angle, curve_radius, linear_speed, max_angular_acceleration)} { } + max_angular_speed{config.max_angular_speed}, + linear_speed{config.linear_speed}, + acceleration{config.max_angular_acceleration} { } /** * @brief Get the desired speeds for the robot to complete the action. * - * @param pose Current pose of the robot. + * @param current_pose The current pose of the robot. + * @param time_step The time step for the action in seconds. * @return The desired speeds for the robot to complete the action. - * - * @details The desired velocity is calculated from the angular displacement based on the Torricelli equation. */ - Twist get_speeds(const Pose& pose) const override { - Twist twist{}; - const float current_orientation = std::max(std::abs(pose.orientation), this->start_orientation); + Twist get_speeds(const Pose& /*current_pose*/, float time_step) override { + this->elapsed_time += time_step; + Twist twist{}; - if (current_orientation < std::abs(this->angle)) { + if (this->elapsed_time < this->get_total_time() / 2.0F) { twist = { .linear = linear_speed, - .angular = std::sqrt(2.0F * this->acceleration * current_orientation), + .angular = this->acceleration * this->elapsed_time, }; } else { twist = { .linear = linear_speed, - .angular = std::sqrt(2.0F * this->acceleration * (std::abs(this->angle) - current_orientation)), + .angular = this->acceleration * (this->get_total_time() - this->elapsed_time), }; } - twist.angular = - std::copysign(std::clamp(twist.angular, -this->max_angular_speed, this->max_angular_speed), this->angle); + twist.angular = std::copysign(std::clamp(twist.angular, 0.0F, this->max_angular_speed), this->angle); return twist; } @@ -73,26 +75,25 @@ class TurnAction : public Action { /** * @brief Check if the action is finished. * - * @param pose Current pose of the robot. + * @param current_pose The current pose of the robot. * @return True if the action is finished, false otherwise. */ - bool finished(const Pose& pose) const override { return std::abs(pose.orientation) >= std::abs(this->angle); } + bool finished(const Pose& /*current_pose*/) override { + if (this->elapsed_time >= this->get_total_time()) { + this->elapsed_time = 0.0F; + return true; + } - /** - * @brief Check if the action allows following the wall. - * - * @return True if the action allows following the wall, false otherwise. - */ - bool allow_follow_wall() const override { return false; } + return false; + } -private: /** * @brief Calculate the maximum angular speed for a given curve radius and linear speed. * * @param angle Angle to turn in radians. * @param curve_radius Radius of the curve in meters. - * @param linear_speed Linear speed in m/s. * @param max_angular_acceleration Maximum angular acceleration in rad/s^2. + * @param max_centripetal_acceleration Maximum centripetal acceleration in m/s^2. * @return Maximum angular speed in rad/s. * * @details Maximum angular velocity is computed to generate a curve equivalent in displacement to one that @@ -100,29 +101,101 @@ class TurnAction : public Action { * If linear speed is zero, a minimal angular speed is assigned. */ static constexpr float calculate_max_angular_speed( - float angle, float curve_radius, float linear_speed, float max_angular_acceleration + float angle, float curve_radius, float max_angular_acceleration, float max_centripetal_acceleration ) { - if (linear_speed == 0.0F) { + if (curve_radius == 0.0F) { return max_angular_acceleration * 0.01F; } - const float radius_speed_ratio = curve_radius / linear_speed; - const float discriminant = std::pow(radius_speed_ratio, 2.0F) - - (2.0F * (1 - std::cos(angle))) / (correction_factor * max_angular_acceleration); + const float correction_factor = + 14.35F - 13.57F * std::cos(std::abs(angle) - 2) - 10.0F / max_angular_acceleration; + + const float transformed_acceleration = + std::pow(1.0F - std::cos(angle), 2.0F) * correction_factor * max_angular_acceleration; + + const float max_angular_speed = std::sqrt( + (max_centripetal_acceleration * transformed_acceleration) / + (curve_radius * transformed_acceleration - max_centripetal_acceleration) + ); + + if (std::isnan(max_angular_speed)) { + std::terminate(); + } - return correction_factor * max_angular_acceleration * (radius_speed_ratio - std::sqrt(discriminant)); + return max_angular_speed; } /** - * @brief Start orientation in radians. Being zero causes the robot to not move. + * @brief Calculate the total side displacement for a curve. + * + * @param angle Angle to turn in radians. + * @param max_angular_speed Maximum angular speed in rad/s. + * @param linear_speed Linear speed in m/s. + * @param max_angular_acceleration Maximum angular acceleration in rad/s^2. + * @return Side displacement in meters. */ - float start_orientation; + static constexpr float calculate_side_displacement( + float angle, float max_angular_speed, float linear_speed, float max_angular_acceleration + ) { + const float transformed_speed = max_angular_speed / (1.0F - std::cos(angle)); + const float correction_factor = + 14.35F - 13.57F * std::cos(std::abs(angle) - 2) - 10.0F / max_angular_acceleration; + + return linear_speed * + (1.0F / transformed_speed + transformed_speed / (correction_factor * max_angular_acceleration)); + } + /** + * @brief Calculate the total forward displacement for a curve. + * + * @param angle Angle to turn in radians. + * @param max_angular_speed Maximum angular speed in rad/s. + * @param linear_speed Linear speed in m/s. + * @param max_angular_acceleration Maximum angular acceleration in rad/s^2. + * @return Forward displacement in meters. + */ + static constexpr float calculate_forward_displacement( + float angle, float max_angular_speed, float linear_speed, float max_angular_acceleration + ) { + const float transformed_speed = max_angular_speed / std::sin(std::abs(angle)); + const float correction_factor = + 14.12F - 13.3F * std::cos(std::abs(angle) - 1.146F) - 10.0F / max_angular_acceleration; + + return linear_speed * + (1.0F / transformed_speed + transformed_speed / (correction_factor * max_angular_acceleration)); + } + + /** + * @brief Calculate the total time to complete the action. + * + * @param angle Angle to turn in radians. + * @param max_angular_speed Maximum angular speed in rad/s. + * @param max_angular_acceleration Maximum angular acceleration in rad/s^2. + * @return Total time to complete the action in seconds. + */ + static constexpr float calculate_total_time(float angle, float max_angular_speed, float max_angular_acceleration) { + const float peak_velocity = std::sqrt(std::abs(angle) * max_angular_acceleration); + + if (peak_velocity < max_angular_speed) { + return 2.0F * peak_velocity / max_angular_acceleration; + } + + const float acceleration_time = max_angular_speed / max_angular_acceleration; + + return std::abs(angle) / max_angular_speed + acceleration_time; + } + +private: /** * @brief Angle to turn in radians. */ float angle; + /** + * @brief Maximum angular speed in rad/s. + */ + float max_angular_speed; + /** * @brief Linear speed in m/s while turning. */ @@ -134,9 +207,9 @@ class TurnAction : public Action { float acceleration; /** - * @brief Maximum angular speed in rad/s while turning. + * @brief Elapsed time in seconds since the action started. */ - float max_angular_speed; + float elapsed_time{}; }; } // namespace micras::nav diff --git a/micras_nav/include/micras/nav/costmap.hpp b/micras_nav/include/micras/nav/costmap.hpp index 6e20b3db..2506db43 100644 --- a/micras_nav/include/micras/nav/costmap.hpp +++ b/micras_nav/include/micras/nav/costmap.hpp @@ -26,7 +26,7 @@ class Costmap { /** * @brief Type to store the state of a wall. */ - enum WallState : uint8_t { + enum class WallState : uint8_t { UNKNOWN = 0, NO_WALL = 1, WALL = 2, diff --git a/micras_nav/include/micras/nav/follow_wall.hpp b/micras_nav/include/micras/nav/follow_wall.hpp index 09ceff98..4715f976 100644 --- a/micras_nav/include/micras/nav/follow_wall.hpp +++ b/micras_nav/include/micras/nav/follow_wall.hpp @@ -24,9 +24,10 @@ class FollowWall { struct Config { core::PidController::Config pid; core::WallSensorsIndex wall_sensor_index{}; - float max_linear_speed{}; - float post_threshold{}; + float max_angular_acceleration{}; float cell_size{}; + float post_threshold{}; + float post_reference{}; float post_clearance{}; }; @@ -34,21 +35,18 @@ class FollowWall { * @brief Construct a new FollowWall object. * * @param wall_sensors The wall sensors of the robot. - * @param absolute_pose Reference to the absolute pose of the robot. * @param config The configuration for the FollowWall class. */ - FollowWall( - const std::shared_ptr>& wall_sensors, const Pose& absolute_pose, const Config& config - ); + FollowWall(const std::shared_ptr>& wall_sensors, const Config& config); /** * @brief Calculate the desired angular speed to follow wall. * * @param elapsed_time The time elapsed since the last update. - * @param linear_speed Current linear speed of the robot. + * @param state The current state of the robot. * @return The desired angular speed to follow wall. */ - float compute_angular_correction(float elapsed_time, float linear_speed); + float compute_angular_correction(float elapsed_time, State& state); /** * @brief Get the observation of the walls around the robot. @@ -57,28 +55,30 @@ class FollowWall { */ core::Observation get_observation() const; - /** - * @brief Reset the PID controller and the relative pose. - */ - void reset(); - private: - /** - * @brief Reset the displacement of the robot. - * - * @param reset_by_post Whether the reset was triggered by a post. - */ - void reset_displacement(bool reset_by_post = false); - /** * @brief Check if the robot saw a post. * + * @param pose The current pose of the robot. * @return True if the robot saw a post, false otherwise. * * @details This function uses the derivative of the distance sensors readings with respect * to the robot's traveled distance, comparing it to a threshold to detect posts. */ - bool check_posts(); + bool check_posts(const Pose& pose); + + /** + * @brief Reset the PID controller and the relative pose. + */ + void reset(); + + /** + * @brief Clear the positional error of the robot. + * + * @param state The state of the robot. + * @param error The error to be cleared. + */ + static void clear_position_error(State& state, float error); /** * @brief Wall sensors of the robot. @@ -96,9 +96,14 @@ class FollowWall { core::WallSensorsIndex sensor_index; /** - * @brief Maximum linear speed of the robot. + * @brief Maximum angular acceleration of the robot. */ - float max_linear_speed; + float max_angular_acceleration; + + /** + * @brief Size of the cells in the map. + */ + float cell_size; /** * @brief Derivative threshold for detecting posts. @@ -106,24 +111,24 @@ class FollowWall { float post_threshold; /** - * @brief Current pose of the robot relative to the last time it was reset. + * @brief Cell advance of the robot when it starts to see a post. */ - RelativePose blind_pose; + float post_reference{}; /** - * @brief Last distance of the robot to the start point, used to compute the derivative of the distance sensors. + * @brief Distance the robot should travel after seeing a post to start making observations. */ - float last_blind_distance{}; + float post_clearance{}; /** - * @brief Size of the cells in the map. + * @brief Last grid pose of the robot. */ - float cell_size; + GridPose last_grid_pose{}; /** - * @brief Margin in front of a post to stop seeing it. + * @brief Last pose of the robot. */ - float post_clearance; + Pose last_pose{}; /** * @brief Flag to indicate if the robot is currently following the left wall. @@ -136,19 +141,19 @@ class FollowWall { bool following_right{true}; /** - * @brief Last error measured by the left wall sensor, used to compute the derivative of the distance sensors. + * @brief Last reading measured by the left wall sensor, used to compute the derivative of the distance sensors. */ - float last_left_error{}; + float last_left_reading{}; /** - * @brief Last error measured by the right wall sensor, used to compute the derivative of the distance sensors. + * @brief Last reading measured by the right wall sensor, used to compute the derivative of the distance sensors. */ - float last_right_error{}; + float last_right_reading{}; /** - * @brief Flag to indicate if relative pose was reset by a post. + * @brief Last response returned by the Follow Wall. */ - bool reset_by_post{}; + float last_response{}; }; } // namespace micras::nav diff --git a/micras_nav/include/micras/nav/grid_pose.hpp b/micras_nav/include/micras/nav/grid_pose.hpp index d1c6b222..54c27799 100644 --- a/micras_nav/include/micras/nav/grid_pose.hpp +++ b/micras_nav/include/micras/nav/grid_pose.hpp @@ -44,6 +44,7 @@ struct GridPoint { /** * @brief Convert the point to a grid point. * + * @param point The point to convert. * @param cell_size The size of the grid cells. * @return The grid point corresponding to the point. */ @@ -52,7 +53,6 @@ struct GridPoint { /** * @brief Convert a grid point to a point. * - * @param grid_point The grid point to convert. * @param cell_size The size of the grid cells. * @return The point corresponding to the grid point. */ @@ -114,6 +114,14 @@ struct GridPose { */ GridPose turned_right() const; + /** + * @brief Gets the relative side of a grid point compared to current pose. + * + * @param other The grid point to check. + * @return LEFT/RIGHT for left/right of pose, UP for in front, DOWN for behind. + */ + Side get_relative_side(const GridPoint& other) const; + /** * @brief Compare two poses for equality. * @@ -148,6 +156,20 @@ struct hash { return h1 ^ (h2 << 1); } }; + +/** + * @brief Hash specialization for the GridPose type. + * + * @tparam T GridPose type. + */ +template <> +struct hash { + size_t operator()(const micras::nav::GridPose& pose) const noexcept { + const size_t h1 = hash{}(pose.position); + const size_t h2 = hash{}(pose.orientation); + return h1 ^ (h2 << 1); + } +}; } // namespace std #endif // MICRAS_NAV_GRID_POSE_HPP diff --git a/micras_nav/include/micras/nav/maze.hpp b/micras_nav/include/micras/nav/maze.hpp index e28fc782..5e0a1389 100644 --- a/micras_nav/include/micras/nav/maze.hpp +++ b/micras_nav/include/micras/nav/maze.hpp @@ -11,6 +11,7 @@ #include "micras/core/serializable.hpp" #include "micras/core/types.hpp" +#include "micras/nav/action_queuer.hpp" #include "micras/nav/costmap.hpp" #include "micras/nav/grid_pose.hpp" @@ -24,14 +25,20 @@ namespace micras::nav { template class TMaze : public core::ISerializable { public: + /** + * @brief Configuration structure for the maze. + */ struct Config { GridPose start{}; std::unordered_set goal; float cost_margin{}; + ActionQueuer::Config action_queuer_config{}; }; /** - * @brief Construct a new Maze object. + * @brief Construct a new TMaze object. + * + * @param config The configuration for the maze. */ explicit TMaze(Config config); @@ -71,7 +78,7 @@ class TMaze : public core::ISerializable { * * @return The best route to the goal. */ - const std::list& get_best_route() const; + const std::list& get_best_route() const; /** * @brief Serialize the best route to the goal. @@ -98,6 +105,11 @@ class TMaze : public core::ISerializable { NUM_OF_LAYERS = 2, }; + /** + * @brief Compute the minumum cost from the start to the end considering only discoverd cells. + */ + void compute_minimum_cost(); + /** * @brief Update the cell costs at the given position. * @@ -110,9 +122,17 @@ class TMaze : public core::ISerializable { * * @param pose The current pose of the robot. * @param discover Whether the robot is discovering new cells. - * @return The next discovery goal for the robot. + * @return A pair containing the next discovery goal for the robot and the total distance. */ - GridPose get_next_bfs_goal(const GridPose& pose, bool discover) const; + std::pair get_next_bfs_goal(const GridPose& pose, bool discover) const; + + void recursive_backtracking( + const GridPoint& position, std::list& route, std::unordered_set& visited + ); + + uint16_t heuristic(const GridPoint& position) const; + + float get_route_time(const std::list& route); /** * @brief Check if the cell is a dead end. @@ -144,6 +164,8 @@ class TMaze : public core::ISerializable { */ Costmap costmap; + ActionQueuer action_queuer; + /** * @brief Start pose of the robot in the maze. */ @@ -172,7 +194,9 @@ class TMaze : public core::ISerializable { /** * @brief Current best found route to the goal. */ - std::list best_route; + std::list best_route; + + float best_route_time{std::numeric_limits::max()}; }; } // namespace micras::nav diff --git a/micras_nav/include/micras/nav/odometry.hpp b/micras_nav/include/micras/nav/odometry.hpp index 6b3352ab..3ed018bd 100644 --- a/micras_nav/include/micras/nav/odometry.hpp +++ b/micras_nav/include/micras/nav/odometry.hpp @@ -60,6 +60,13 @@ class Odometry { */ const State& get_state() const; + /** + * @brief Get the state of the robot. + * + * @return Current state of the robot in space. + */ + State& get_state(); + /** * @brief Set the state of the robot. * @@ -88,6 +95,11 @@ class Odometry { */ float wheel_radius; + /** + * @brief Initial pose of the robot. + */ + Pose initial_pose; + /** * @brief Last left rotary sensor position. */ diff --git a/micras_nav/include/micras/nav/speed_controller.hpp b/micras_nav/include/micras/nav/speed_controller.hpp index 9bbacda4..46f904ab 100644 --- a/micras_nav/include/micras/nav/speed_controller.hpp +++ b/micras_nav/include/micras/nav/speed_controller.hpp @@ -27,8 +27,6 @@ class SpeedController { float angular_acceleration; }; - float max_linear_acceleration{}; - float max_angular_acceleration{}; core::PidController::Config linear_pid; core::PidController::Config angular_pid; FeedForward left_feed_forward{}; @@ -78,16 +76,6 @@ class SpeedController { */ static float feed_forward(const Twist& speed, const Twist& acceleration, const Config::FeedForward& config); - /** - * @brief The maximum linear acceleration of the robot. - */ - float max_linear_acceleration; - - /** - * @brief The maximum angular acceleration of the robot. - */ - float max_angular_acceleration; - /** * @brief The last linear speed of the robot. */ diff --git a/micras_nav/src/action_queuer.cpp b/micras_nav/src/action_queuer.cpp index 20768ebc..30e14273 100644 --- a/micras_nav/src/action_queuer.cpp +++ b/micras_nav/src/action_queuer.cpp @@ -2,6 +2,8 @@ * @file */ +#include +#include #include #include "micras/nav/action_queuer.hpp" @@ -9,66 +11,100 @@ namespace micras::nav { ActionQueuer::ActionQueuer(Config config) : cell_size{config.cell_size}, - exploring_params{config.exploring}, - // solving_params{config.solving}, - stop{std::make_shared( - ActionType::STOP, cell_size / 2.0F, exploring_params.max_linear_speed, 0.0F, exploring_params.max_linear_speed, - exploring_params.max_linear_acceleration, exploring_params.max_linear_deceleration, false - )}, - start{std::make_shared( - ActionType::START, cell_size - config.start_offset, 0.001F * exploring_params.max_linear_acceleration, - exploring_params.max_linear_speed, exploring_params.max_linear_speed, exploring_params.max_linear_acceleration, - exploring_params.max_linear_deceleration - )}, - move_forward{std::make_shared( - ActionType::MOVE_FORWARD, cell_size, exploring_params.max_linear_speed, exploring_params.max_linear_speed, - exploring_params.max_linear_speed, exploring_params.max_linear_acceleration, - exploring_params.max_linear_deceleration - )}, - move_half{std::make_shared( - ActionType::MOVE_HALF, cell_size / 2.0F, 0.001F * exploring_params.max_linear_acceleration, - exploring_params.max_linear_speed, exploring_params.max_linear_speed, exploring_params.max_linear_acceleration, - exploring_params.max_linear_deceleration, false - )}, - turn_left{std::make_shared( - ActionType::TURN_LEFT, std::numbers::pi_v / 2.0F, cell_size / 2.0F, exploring_params.max_linear_speed, - exploring_params.max_angular_acceleration - )}, - turn_right{std::make_shared( - ActionType::TURN_RIGHT, -std::numbers::pi_v / 2.0F, cell_size / 2.0F, exploring_params.max_linear_speed, - exploring_params.max_angular_acceleration - )}, - turn_back{std::make_shared( - ActionType::TURN_BACK, std::numbers::pi_v, 0.0F, 0.0F, exploring_params.max_angular_acceleration - )} { } - -void ActionQueuer::push(const GridPose& current_pose, const GridPoint& target_position) { - if (current_pose.front().position == target_position) { - this->action_queue.emplace(move_forward); + start_offset{config.start_offset}, + curve_safety_margin{config.curve_safety_margin}, + solving_params{config.solving} { + const float exploration_curve_radius = config.exploring.max_linear_speed * config.exploring.max_linear_speed / + config.exploring.max_centrifugal_acceleration; + + const float exploration_max_angular_speed = TurnAction::calculate_max_angular_speed( + std::numbers::pi_v / 2.0F, exploration_curve_radius, config.exploring.max_angular_acceleration, + config.exploring.max_centrifugal_acceleration + ); + + const float exploration_linear_speed = + config.exploring.max_centrifugal_acceleration / exploration_max_angular_speed; + + MoveAction::Config move_config = { + .start_speed = 0.0F, + .end_speed = exploration_linear_speed, + .max_speed = exploration_linear_speed, + .max_acceleration = config.exploring.max_linear_acceleration, + .max_deceleration = config.exploring.max_linear_deceleration + }; + + this->start = std::make_shared(ActionType::START, this->cell_size - config.start_offset, move_config); + this->move_half = std::make_shared(ActionType::MOVE_FORWARD, cell_size / 2.0F, move_config); + + move_config.start_speed = exploration_linear_speed; + + this->move_forward = std::make_shared(ActionType::MOVE_FORWARD, cell_size, move_config); + this->move_to_turn = std::make_shared( + ActionType::MOVE_FORWARD, this->cell_size / 2.0F - exploration_curve_radius, move_config, false + ); + this->move_from_turn = std::make_shared( + ActionType::MOVE_FORWARD, this->cell_size / 2.0F - exploration_curve_radius, move_config + ); + + move_config.end_speed = 0.0F; + + this->stop = std::make_shared(ActionType::STOP, cell_size / 2.0F, move_config, false); + + const TurnAction::Config turn_config = { + .max_angular_speed = exploration_max_angular_speed, + .linear_speed = exploration_linear_speed, + .max_angular_acceleration = config.exploring.max_angular_acceleration + }; + + this->turn_left = std::make_shared(ActionType::TURN, std::numbers::pi_v / 2.0F, turn_config); + this->turn_right = std::make_shared(ActionType::TURN, -std::numbers::pi_v / 2.0F, turn_config); + + const TurnAction::Config turn_back_config = { + .max_angular_speed = config.exploring.max_angular_acceleration * 0.01F, + .linear_speed = 0.0F, + .max_angular_acceleration = config.exploring.max_angular_acceleration + }; + + this->turn_back = std::make_shared(ActionType::SPIN, std::numbers::pi_v, turn_back_config); + + this->compute_curve_parameters(std::numbers::pi_v / 4.0F, false); + this->compute_curve_parameters(std::numbers::pi_v / 2.0F, false); + this->compute_curve_parameters(std::numbers::pi_v / 2.0F, true); + this->compute_curve_parameters(3.0F * std::numbers::pi_v / 4.0F, false); + this->compute_curve_parameters(std::numbers::pi_v, false); +} + +void ActionQueuer::push_exploring(const GridPose& origin_pose, const GridPoint& target_position) { + if (origin_pose.front().position == target_position) { + this->action_queue.emplace_back(this->move_forward); return; } - if (current_pose.turned_left().front().position == target_position) { - this->action_queue.emplace(turn_left); + if (origin_pose.turned_left().front().position == target_position) { + this->action_queue.emplace_back(this->move_to_turn); + this->action_queue.emplace_back(this->turn_left); + this->action_queue.emplace_back(this->move_to_turn); return; } - if (current_pose.turned_right().front().position == target_position) { - this->action_queue.emplace(turn_right); + if (origin_pose.turned_right().front().position == target_position) { + this->action_queue.emplace_back(this->move_to_turn); + this->action_queue.emplace_back(this->turn_right); + this->action_queue.emplace_back(this->move_from_turn); return; } - if (current_pose.turned_back().front().position == target_position) { - this->action_queue.emplace(stop); - this->action_queue.emplace(turn_back); - this->action_queue.emplace(move_half); + if (origin_pose.turned_back().front().position == target_position) { + this->action_queue.emplace_back(this->stop); + this->action_queue.emplace_back(this->turn_back); + this->action_queue.emplace_back(this->move_half); return; } } std::shared_ptr ActionQueuer::pop() { - const auto& action = this->action_queue.front(); - this->action_queue.pop(); + auto action = this->action_queue.front(); + this->action_queue.pop_front(); return action; } @@ -76,16 +112,271 @@ bool ActionQueuer::empty() const { return this->action_queue.empty(); } -void ActionQueuer::recompute(const std::list& best_route) { +void ActionQueuer::recompute(const std::list& best_route, bool add_start) { this->action_queue = {}; - this->action_queue.emplace(start); + + if (add_start) { + this->action_queue.emplace_back(start); + } if (best_route.empty()) { return; } - for (auto it = std::next(best_route.begin()); std::next(it) != best_route.end(); it++) { - this->push(*it, std::next(it)->position); + std::list actions; + + Side direction = best_route.front().direction(*std::next(best_route.begin())); + + for (auto route_it = best_route.begin(); std::next(route_it) != best_route.end(); route_it++) { + actions.push_back(get_action({*route_it, direction}, *std::next(route_it), this->cell_size)); + direction = (*route_it).direction(*std::next(route_it)); } + + this->add_diagonals(actions); + this->join_actions(actions); + + actions.front().value -= this->start_offset; + float start_speed = 0.0F; + + for (auto action_it = std::next(actions.begin()); action_it != actions.end(); action_it++) { + if (action_it->type != ActionType::TURN) { + continue; + } + + if (std::prev(action_it)->value < 0.0F) { + std::terminate(); + } + + const CurveParameters& curve_parameters = + this->get_curve_parameters(action_it->value, std::prev(action_it)->type == ActionType::DIAGONAL); + + const MoveAction::Config move_config = { + .start_speed = start_speed, + .end_speed = curve_parameters.linear_speed, + .max_speed = this->solving_params.max_linear_speed, + .max_acceleration = this->solving_params.max_linear_acceleration, + .max_deceleration = this->solving_params.max_linear_deceleration + }; + this->action_queue.emplace_back(std::make_shared( + std::prev(action_it)->type, std::prev(action_it)->value, move_config, + std::prev(action_it)->type != ActionType::DIAGONAL + )); + + const TurnAction::Config turn_config = { + .max_angular_speed = curve_parameters.max_angular_speed, + .linear_speed = curve_parameters.linear_speed, + .max_angular_acceleration = this->solving_params.max_angular_acceleration + }; + this->action_queue.emplace_back(std::make_shared(action_it->type, action_it->value, turn_config)); + + start_speed = curve_parameters.linear_speed; + } + + const MoveAction::Config stop_config = { + .start_speed = start_speed, + .end_speed = 0.0F, + .max_speed = this->solving_params.max_linear_speed, + .max_acceleration = this->solving_params.max_linear_acceleration, + .max_deceleration = this->solving_params.max_linear_deceleration + }; + + if (actions.back().type == ActionType::MOVE_FORWARD) { + this->action_queue.emplace_back( + std::make_shared(ActionType::STOP, actions.back().value + this->cell_size / 2.0F, stop_config) + ); + } else { + this->action_queue.emplace_back( + std::make_shared(ActionType::STOP, this->cell_size / 2.0F, stop_config) + ); + } +} + +float ActionQueuer::get_total_time() const { + float total_time = 0.0F; + + for (const auto& action : this->action_queue) { + total_time += action->get_total_time(); + } + + return total_time; +} + +Action::Id ActionQueuer::get_action(const GridPose& origin_pose, const GridPoint& target_point, float cell_size) { + if (origin_pose.front().position == target_point) { + const float distance = origin_pose.position.to_vector(cell_size).distance(target_point.to_vector(cell_size)); + return {ActionType::MOVE_FORWARD, distance}; + } + + if (origin_pose.turned_left().front().position == target_point) { + return {ActionType::TURN, std::numbers::pi_v / 2.0F}; + } + + if (origin_pose.turned_right().front().position == target_point) { + return {ActionType::TURN, -std::numbers::pi_v / 2.0F}; + } + + return {ActionType::STOP, cell_size / 2.0F}; +} + +void ActionQueuer::add_diagonals(std::list& actions) const { + for (auto action_it = actions.begin(); std::next(action_it) != actions.end(); action_it++) { + uint8_t diagonal_count = 1; + + while (action_it->type == ActionType::TURN and std::next(action_it)->type == ActionType::TURN and + std::abs(action_it->value) == std::numbers::pi_v / 2.0F and + action_it->value == -std::next(action_it)->value) { + diagonal_count++; + action_it = actions.erase(action_it); + } + + if (diagonal_count == 1) { + continue; + } + + action_it->value /= 2.0F; + const float first_angle = diagonal_count % 2 == 0 ? -action_it->value : action_it->value; + const float diagonal_distance = diagonal_count * this->cell_size * std::numbers::sqrt2_v / 2.0F; + actions.insert(action_it, {{ActionType::TURN, first_angle}, {ActionType::DIAGONAL, diagonal_distance}}); + } +} + +void ActionQueuer::join_actions(std::list& actions) { + for (auto action_it = actions.begin(); std::next(action_it) != actions.end();) { + if (action_it->type == std::next(action_it)->type) { + const float value = action_it->value; + action_it = actions.erase(action_it); + action_it->value += value; + continue; + } + + if (action_it->type != ActionType::TURN) { + action_it++; + continue; + } + + auto [trim_before_distance, trim_after_distance] = this->get_trim_distances(*std::prev(action_it), *action_it); + + std::prev(action_it)->value -= trim_before_distance; + std::next(action_it)->value -= trim_after_distance; + action_it++; + } +} + +std::pair + ActionQueuer::get_trim_distances(const Action::Id& action_before, const Action::Id& turn_action) { + const float turn_angle = std::abs(turn_action.value); + const CurveParameters& curve_parameters = + this->get_curve_parameters(turn_angle, action_before.type == ActionType::DIAGONAL); + + if (turn_angle == std::numbers::pi_v / 4.0F) { + const float trim_before_distance = curve_parameters.forward_displacement - curve_parameters.side_displacement; + const float trim_after_distance = curve_parameters.side_displacement * std::numbers::sqrt2_v; + + return {trim_before_distance, trim_after_distance}; + } + + if (turn_angle == std::numbers::pi_v / 2.0F) { + float trim_before_distance = curve_parameters.forward_displacement; + float trim_after_distance = curve_parameters.side_displacement; + + if (action_before.type != ActionType::DIAGONAL) { + trim_before_distance -= this->cell_size / 2.0F; + trim_after_distance -= this->cell_size / 2.0F; + } + + return {trim_before_distance, trim_after_distance}; + } + + if (turn_angle == 3.0F * std::numbers::pi_v / 4.0F) { + float trim_before_distance = curve_parameters.forward_displacement + curve_parameters.side_displacement; + float trim_after_distance = std::numbers::sqrt2_v * curve_parameters.side_displacement; + + if (action_before.type == ActionType::DIAGONAL) { + trim_before_distance -= this->cell_size * std::numbers::sqrt2_v / 2.0F; + trim_after_distance -= this->cell_size; + } else { + trim_before_distance -= this->cell_size; + trim_after_distance -= this->cell_size * std::numbers::sqrt2_v / 2.0F; + } + + return {trim_before_distance, trim_after_distance}; + } + + if (turn_angle == std::numbers::pi_v) { + const float trim_distance = this->cell_size / 2.0F - this->curve_safety_margin; + + return {trim_distance, trim_distance}; + } + + return {0.0F, 0.0F}; +} + +ActionQueuer::CurveParameters& ActionQueuer::get_curve_parameters(float angle, bool is_diagonal) { + const float turn_angle = std::abs(angle); + + if (turn_angle == std::numbers::pi_v / 4.0F) { + return this->curves_parameters[CurveType::REGULAR_45]; + } + + if (turn_angle == std::numbers::pi_v / 2.0F) { + return is_diagonal ? this->curves_parameters[CurveType::DIAGONAL_90] : + this->curves_parameters[CurveType::REGULAR_90]; + } + + if (turn_angle == 3.0F * std::numbers::pi_v / 4.0F) { + return this->curves_parameters[CurveType::REGULAR_135]; + } + + return this->curves_parameters[CurveType::REGULAR_180]; +} + +void ActionQueuer::compute_curve_parameters(float angle, bool is_diagonal) { + const float curve_radius = calculate_curve_radius(angle, this->cell_size, this->curve_safety_margin, is_diagonal); + const float max_angular_speed = TurnAction::calculate_max_angular_speed( + angle, curve_radius, this->solving_params.max_angular_acceleration, + this->solving_params.max_centrifugal_acceleration + ); + const float curve_linear_speed = this->solving_params.max_centrifugal_acceleration / max_angular_speed; + + this->get_curve_parameters(angle, is_diagonal) = { + .curve_radius = curve_radius, + .max_angular_speed = max_angular_speed, + .linear_speed = curve_linear_speed, + .side_displacement = TurnAction::calculate_side_displacement( + angle, max_angular_speed, curve_linear_speed, this->solving_params.max_angular_acceleration + ), + .forward_displacement = TurnAction::calculate_forward_displacement( + angle, max_angular_speed, curve_linear_speed, this->solving_params.max_angular_acceleration + ), + }; +} + +constexpr float + ActionQueuer::calculate_curve_radius(float angle, float cell_size, float safety_margin, bool is_diagonal) { + const float turn_angle = std::abs(angle); + + if (turn_angle == std::numbers::pi_v / 4.0F) { + return cell_size * (1.0F + std::numbers::sqrt2_v) / 2.0F; + } + + if (turn_angle == std::numbers::pi_v / 2.0F) { + const float curve_radius = (cell_size - 2.0F * safety_margin) / (2.0F * std::numbers::sqrt2_v - 2.0F); + + if (is_diagonal) { + return curve_radius; + } + + return curve_radius + cell_size / 2.0F; + } + + if (turn_angle == 3.0F * std::numbers::pi_v / 4.0F) { + const float constant = 10.0F * std::numbers::sqrt2_v - 14.0F; + const float root_term = + (std::sqrt(constant * (cell_size * cell_size - 2.0F * cell_size * safety_margin)) + cell_size) / 2.0F; + + return root_term - safety_margin * (std::numbers::sqrt2_v - 1.0F); + } + + return cell_size / 2.0F; } } // namespace micras::nav diff --git a/micras_nav/src/follow_wall.cpp b/micras_nav/src/follow_wall.cpp index 1b5f1775..9b02a01e 100644 --- a/micras_nav/src/follow_wall.cpp +++ b/micras_nav/src/follow_wall.cpp @@ -5,30 +5,52 @@ #include "micras/nav/follow_wall.hpp" namespace micras::nav { -FollowWall::FollowWall( - const std::shared_ptr>& wall_sensors, const Pose& absolute_pose, const Config& config -) : +FollowWall::FollowWall(const std::shared_ptr>& wall_sensors, const Config& config) : wall_sensors{wall_sensors}, pid{config.pid}, sensor_index{config.wall_sensor_index}, - max_linear_speed{config.max_linear_speed}, - post_threshold{config.post_threshold}, - blind_pose{absolute_pose}, + max_angular_acceleration{config.max_angular_acceleration}, cell_size{config.cell_size}, + post_threshold{config.post_threshold}, + post_reference{config.post_reference}, post_clearance{config.post_clearance} { } -float FollowWall::compute_angular_correction(float elapsed_time, float linear_speed) { +float FollowWall::compute_angular_correction(float elapsed_time, State& state) { + float cell_advance = state.pose.to_cell(this->cell_size).y; + const GridPose grid_pose = state.pose.to_grid(this->cell_size); + if (this->wall_sensors.use_count() == 1) { this->wall_sensors->update(); } - if (this->check_posts()) { + if (grid_pose == this->last_grid_pose) { + if (this->check_posts(state.pose)) { + clear_position_error(state, this->post_reference - cell_advance); + cell_advance = this->post_reference; + this->pid.reset(); + } + } else { + if (grid_pose != this->last_grid_pose.front()) { + this->reset(); + } + } + + this->last_grid_pose = grid_pose; + this->last_pose = state.pose; + + if (cell_advance <= this->post_reference + this->post_clearance and + cell_advance >= this->post_reference - this->post_clearance / 2.0F) { return 0.0F; } - if ((not this->following_left or not this->following_right) and - (this->last_blind_distance >= (this->cell_size + (this->reset_by_post ? this->post_clearance : 0.0F)))) { - this->reset(); + if (not this->following_left and this->wall_sensors->get_wall(this->sensor_index.left)) { + this->following_left = true; + this->pid.reset(); + } + + if (not this->following_right and this->wall_sensors->get_wall(this->sensor_index.right)) { + this->following_right = true; + this->pid.reset(); } float error{}; @@ -44,48 +66,39 @@ float FollowWall::compute_angular_correction(float elapsed_time, float linear_sp return 0.0F; } - const float response = this->pid.compute_response(error, elapsed_time); + const float response = state.velocity.linear * this->pid.compute_response(error, elapsed_time); + const float clamped_response = + core::move_towards(this->last_response, response, this->max_angular_acceleration * elapsed_time); - return response * linear_speed / this->max_linear_speed; + this->last_response = clamped_response; + return clamped_response; } -bool FollowWall::check_posts() { - const float current_distance = this->blind_pose.get().position.magnitude(); - const float delta_distance = current_distance - this->last_blind_distance; +bool FollowWall::check_posts(const Pose& pose) { + const float delta_distance = (pose.position - this->last_pose.position).magnitude(); if (delta_distance <= 0.0F) { return false; } - this->last_blind_distance = current_distance; bool found_posts = false; if (this->following_left and - -(this->wall_sensors->get_sensor_error(this->sensor_index.left) - this->last_left_error) / delta_distance >= + -(this->wall_sensors->get_adc_reading(this->sensor_index.left) - this->last_left_reading) / delta_distance >= this->post_threshold) { this->following_left = false; - - if (this->following_right) { - this->reset_displacement(true); - } - found_posts = true; } if (this->following_right and - -(this->wall_sensors->get_sensor_error(this->sensor_index.right) - this->last_right_error) / delta_distance >= + -(this->wall_sensors->get_adc_reading(this->sensor_index.right) - this->last_right_reading) / delta_distance >= this->post_threshold) { this->following_right = false; - - if (this->following_left) { - this->reset_displacement(true); - } - found_posts = true; } - this->last_left_error = this->wall_sensors->get_sensor_error(this->sensor_index.left); - this->last_right_error = this->wall_sensors->get_sensor_error(this->sensor_index.right); + this->last_left_reading = this->wall_sensors->get_adc_reading(this->sensor_index.left); + this->last_right_reading = this->wall_sensors->get_adc_reading(this->sensor_index.right); return found_posts; } @@ -102,16 +115,29 @@ core::Observation FollowWall::get_observation() const { }; } -void FollowWall::reset() { - this->pid.reset(); - this->reset_displacement(); - this->following_left = this->wall_sensors->get_wall(this->sensor_index.left); - this->following_right = this->wall_sensors->get_wall(this->sensor_index.right); +void FollowWall::clear_position_error(State& state, float error) { + switch (angle_to_grid(state.pose.orientation)) { + case Side::RIGHT: + state.pose.position.x += error; + break; + case Side::UP: + state.pose.position.y += error; + break; + case Side::LEFT: + state.pose.position.x -= error; + break; + case Side::DOWN: + state.pose.position.y -= error; + break; + } } -void FollowWall::reset_displacement(bool reset_by_post) { - this->blind_pose.reset_reference(); - this->last_blind_distance = 0.0F; - this->reset_by_post = reset_by_post; +void FollowWall::reset() { + this->pid.reset(); + this->last_response = 0.0F; + this->following_left = false; + this->following_right = false; + this->last_left_reading = 0.0F; + this->last_right_reading = 0.0F; } } // namespace micras::nav diff --git a/micras_nav/src/grid_pose.cpp b/micras_nav/src/grid_pose.cpp index ecb21b1a..21786f2d 100644 --- a/micras_nav/src/grid_pose.cpp +++ b/micras_nav/src/grid_pose.cpp @@ -9,7 +9,7 @@ namespace micras::nav { Side angle_to_grid(float angle) { const int32_t grid_angle = std::lround(2.0F * angle / std::numbers::pi_v); - return static_cast(grid_angle < 0 ? 4 + (grid_angle % 4) : grid_angle % 4); + return static_cast(grid_angle < 0 ? (4 + (grid_angle % 4)) % 4 : grid_angle % 4); } Side GridPoint::direction(const GridPoint& next) const { @@ -75,6 +75,25 @@ GridPose GridPose::turned_right() const { return {this->position, static_cast((this->orientation + 3) % 4)}; } +Side GridPose::get_relative_side(const GridPoint& other) const { + if (((this->orientation == Side::RIGHT or this->orientation == Side::LEFT) and this->position.y == other.y) or + ((this->orientation == Side::UP or this->orientation == Side::DOWN) and this->position.x == other.x)) { + return this->position.direction(other) == this->orientation ? Side::UP : Side::DOWN; + } + + switch (this->orientation) { + case Side::RIGHT: + return this->position.y < other.y ? Side::LEFT : Side::RIGHT; + case Side::UP: + return this->position.x > other.x ? Side::LEFT : Side::RIGHT; + case Side::LEFT: + return this->position.y > other.y ? Side::LEFT : Side::RIGHT; + case Side::DOWN: + default: + return this->position.x < other.x ? Side::LEFT : Side::RIGHT; + } +} + bool GridPose::operator==(const GridPose& other) const { return this->position == other.position and this->orientation == other.orientation; } diff --git a/micras_nav/src/maze.cpp b/micras_nav/src/maze.cpp index a88068b6..da184698 100644 --- a/micras_nav/src/maze.cpp +++ b/micras_nav/src/maze.cpp @@ -6,13 +6,18 @@ #define MICRAS_NAV_MAZE_CPP #include +#include +#include -#include "micras/nav/grid_pose.hpp" #include "micras/nav/maze.hpp" namespace micras::nav { template -TMaze::TMaze(Config config) : start{config.start}, goal{config.goal}, cost_margin(config.cost_margin) { +TMaze::TMaze(Config config) : + action_queuer(config.action_queuer_config), + start{config.start}, + goal{config.goal}, + cost_margin{config.cost_margin} { this->costmap.update_wall(this->start, false); this->costmap.update_wall(start.turned_right(), true); @@ -71,8 +76,8 @@ void TMaze::update_walls(const GridPose& pose, const core::Observ template GridPose TMaze::get_next_goal(const GridPose& pose, bool returning) { if (returning and not this->finished_discovery) { - this->compute_best_route(); - const auto& next_goal = this->get_next_bfs_goal(pose, true); + this->compute_minimum_cost(); + const auto& [next_goal, _] = this->get_next_bfs_goal(pose, true); if (next_goal != this->start) { return next_goal; @@ -84,9 +89,14 @@ GridPose TMaze::get_next_goal(const GridPose& pose, bool returnin int16_t current_cost = max_cost; GridPose next_pose = {}; - for (Side side : - {pose.turned_right().orientation, pose.turned_left().orientation, pose.orientation, - pose.turned_back().orientation}) { + auto sides_order = { + pose.turned_right().orientation, + pose.turned_left().orientation, + pose.orientation, + pose.turned_back().orientation, + }; + + for (Side side : sides_order) { if (this->costmap.has_wall({pose.position, side})) { continue; } @@ -111,34 +121,91 @@ bool TMaze::finished(const GridPoint& position, bool returning) c return returning ? this->start.position == position : this->goal.contains(position); } +template +void TMaze::compute_minimum_cost() { + this->minimum_cost = this->get_next_bfs_goal(this->start, false).second; +} + template void TMaze::compute_best_route() { - GridPose current_pose = this->start; - this->best_route.clear(); - this->best_route.emplace_back(this->start); + std::unordered_set visited; + std::list current_route; + + visited.insert(this->start.position); + current_route.push_back(this->start.position); + + this->recursive_backtracking(this->start.position, current_route, visited); +} + +template +void TMaze::recursive_backtracking( + const GridPoint& position, std::list& route, std::unordered_set& visited +) { + if (this->goal.contains(position)) { + float current_route_time = get_route_time(route); + + if (current_route_time < this->best_route_time) { + this->best_route_time = current_route_time; + this->best_route = route; + } + + return; + } + + if (not this->best_route.empty() and + route.size() + this->heuristic(position) >= this->cost_margin * this->best_route.size()) { + return; + } + + for (uint8_t i = Side::RIGHT; i <= Side::DOWN; i++) { + Side side = static_cast(i); - while (not this->goal.contains(current_pose.position)) { - current_pose = this->get_next_bfs_goal(current_pose, false); - this->best_route.emplace_back(current_pose); + if (this->costmap.has_wall({position, side}, true)) { + continue; + } + + GridPoint next_position = position + side; + + if (not this->was_visited(this->costmap.get_cell(next_position)) or visited.contains(next_position)) { + continue; + } + + visited.insert(next_position); + route.push_back(next_position); + this->recursive_backtracking(next_position, route, visited); + visited.erase(next_position); + route.pop_back(); + } +} + +template +uint16_t TMaze::heuristic(const GridPoint& position) const { + uint16_t minimum_distance = std::numeric_limits::max(); + + for (const auto& goal_position : this->goal) { + uint16_t distance = std::abs(position.x - goal_position.x) + std::abs(position.y - goal_position.y); + + if (distance < minimum_distance) { + minimum_distance = distance; + } } - this->minimum_cost = this->best_route.size(); + return minimum_distance; } template -const std::list& TMaze::get_best_route() const { +const std::list& TMaze::get_best_route() const { return this->best_route; } template std::vector TMaze::serialize() const { std::vector buffer; - buffer.reserve(3 * this->best_route.size()); + buffer.reserve(2 * this->best_route.size()); - for (const auto& grid_pose : this->best_route) { - buffer.emplace_back(grid_pose.position.x); - buffer.emplace_back(grid_pose.position.y); - buffer.emplace_back(grid_pose.orientation); + for (const auto& grid_point : this->best_route) { + buffer.emplace_back(grid_point.x); + buffer.emplace_back(grid_point.y); } return buffer; @@ -148,10 +215,10 @@ template void TMaze::deserialize(const uint8_t* buffer, uint16_t size) { this->best_route.clear(); - for (uint32_t i = 0; i < size; i += 3) { + for (uint32_t i = 0; i < size; i += 2) { this->best_route.emplace_back( // NOLINTNEXTLINE(cppcoreguidelines-pro-bounds-pointer-arithmetic) - GridPoint{buffer[i], buffer[i + 1]}, static_cast(buffer[i + 2]) + GridPoint{buffer[i], buffer[i + 1]} ); } } @@ -180,32 +247,41 @@ void TMaze::update_cell(const GridPoint& position) { } template -GridPose TMaze::get_next_bfs_goal(const GridPose& pose, bool discover) const { - std::queue queue; - std::array, height> checked{}; - GridPose next_goal = this->start; +std::pair TMaze::get_next_bfs_goal(const GridPose& pose, bool discover) const { + std::queue queue; + std::array, height> distance{}; + std::pair result_pair = {this->start, 0}; - checked.at(pose.position.y).at(pose.position.x) = true; + distance.at(pose.position.y).at(pose.position.x) = 1; - for (Side side : - {pose.turned_right().orientation, pose.turned_left().orientation, pose.orientation, - pose.turned_back().orientation}) { + auto sides_order = { + pose.turned_right().orientation, + pose.turned_left().orientation, + pose.orientation, + pose.turned_back().orientation, + }; + + for (Side side : sides_order) { if (not this->costmap.has_wall({pose.position, side})) { - queue.emplace(pose.position + side, side); + GridPoint next_position = pose.position + side; + queue.emplace(next_position, side); + distance.at(next_position.y).at(next_position.x) = 1; } } while (not queue.empty()) { GridPose current_pose = queue.front(); queue.pop(); - checked.at(current_pose.position.y).at(current_pose.position.x) = true; if ((discover and this->must_visit( this->costmap.get_cell(current_pose.position), std::round(this->minimum_cost * this->cost_margin) )) or (not discover and this->goal.contains(current_pose.position))) { - next_goal = {pose.position + current_pose.orientation, current_pose.orientation}; + result_pair = { + {pose.position + current_pose.orientation, current_pose.orientation}, + distance.at(current_pose.position.y).at(current_pose.position.x) + }; break; } @@ -219,13 +295,21 @@ GridPose TMaze::get_next_bfs_goal(const GridPose& pose, bool disc const GridPoint front_position = current_pose.position + side; if ((discover or this->was_visited(this->costmap.get_cell(front_position))) and - not checked.at(front_position.y).at(front_position.x)) { + distance.at(front_position.y).at(front_position.x) == 0) { queue.emplace(front_position, current_pose.orientation); + distance.at(front_position.y).at(front_position.x) = + distance.at(current_pose.position.y).at(current_pose.position.x) + 1; } } } - return next_goal; + return result_pair; +} + +template +float TMaze::get_route_time(const std::list& route) { + this->action_queuer.recompute(route); + return this->action_queuer.get_total_time(); } template diff --git a/micras_nav/src/odometry.cpp b/micras_nav/src/odometry.cpp index 55f60eaf..5a53712e 100644 --- a/micras_nav/src/odometry.cpp +++ b/micras_nav/src/odometry.cpp @@ -16,6 +16,7 @@ Odometry::Odometry( right_rotary_sensor{right_rotary_sensor}, imu{imu}, wheel_radius{config.wheel_radius}, + initial_pose(config.initial_pose), left_last_position{left_rotary_sensor->get_position()}, right_last_position{right_rotary_sensor->get_position()}, linear_filter{config.linear_cutoff_frequency}, @@ -55,13 +56,17 @@ void Odometry::update(float elapsed_time) { void Odometry::reset() { this->left_last_position = this->left_rotary_sensor->get_position(); this->right_last_position = this->right_rotary_sensor->get_position(); - this->state = {{{0.0F, 0.0F}, 0.0F}, {0.0F, 0.0F}}; + this->state = {this->initial_pose, {0.0F, 0.0F}}; } const nav::State& Odometry::get_state() const { return this->state; } +nav::State& Odometry::get_state() { + return this->state; +} + void Odometry::set_state(const nav::State& new_state) { this->state = new_state; } diff --git a/micras_nav/src/speed_controller.cpp b/micras_nav/src/speed_controller.cpp index c27cd02f..2e70cff0 100644 --- a/micras_nav/src/speed_controller.cpp +++ b/micras_nav/src/speed_controller.cpp @@ -9,8 +9,6 @@ namespace micras::nav { SpeedController::SpeedController(const Config& config) : - max_linear_acceleration{config.max_linear_acceleration}, - max_angular_acceleration{config.max_angular_acceleration}, linear_pid(config.linear_pid), angular_pid(config.angular_pid), left_feed_forward{config.left_feed_forward}, @@ -29,11 +27,9 @@ std::pair SpeedController::compute_control_commands( } std::pair SpeedController::compute_feed_forward_commands(const Twist& desired_twist, float elapsed_time) { - const float linear_acceleration = (desired_twist.linear - this->last_linear_speed) / elapsed_time; - const float angular_acceleration = (desired_twist.angular - this->last_angular_speed) / elapsed_time; const Twist acceleration_twist{ - .linear = std::clamp(linear_acceleration, -this->max_linear_acceleration, this->max_linear_acceleration), - .angular = std::clamp(angular_acceleration, -this->max_angular_acceleration, this->max_angular_acceleration), + .linear = (desired_twist.linear - this->last_linear_speed) / elapsed_time, + .angular = (desired_twist.angular - this->last_angular_speed) / elapsed_time, }; this->last_linear_speed = desired_twist.linear; diff --git a/micras_proxy/CMakeLists.txt b/micras_proxy/CMakeLists.txt index dc523d56..e1bc81b2 100644 --- a/micras_proxy/CMakeLists.txt +++ b/micras_proxy/CMakeLists.txt @@ -1,25 +1,34 @@ project(micras_proxy) file(GLOB_RECURSE PROJECT_SOURCES CONFIGURE_DEPENDS "src/*.c*") -file(GLOB_RECURSE PROJECT_LIBS_SOURCES CONFIGURE_DEPENDS "libs/*.c*") + +include(FetchContent) +FetchContent_Declare( + lsm6dsv-pid + GIT_REPOSITORY https://github.com/STMicroelectronics/lsm6dsv-pid.git + GIT_TAG v3.3.0 +) + +FetchContent_Populate(lsm6dsv-pid) +file(GLOB_RECURSE LIB_SOURCES CONFIGURE_DEPENDS "${lsm6dsv-pid_SOURCE_DIR}/*.c") # Remove warnings from libraries sources set_source_files_properties( - ${PROJECT_LIBS_SOURCES} + ${LIB_SOURCES} PROPERTIES COMPILE_FLAGS "-w" ) add_library(${PROJECT_NAME} STATIC ${PROJECT_SOURCES} - ${PROJECT_LIBS_SOURCES} + ${LIB_SOURCES} ) add_library(micras::proxy ALIAS ${PROJECT_NAME}) target_include_directories(${PROJECT_NAME} PUBLIC include - libs/lsm6dsv-pid + ${lsm6dsv-pid_SOURCE_DIR} ) target_link_libraries(${PROJECT_NAME} PUBLIC diff --git a/micras_proxy/include/micras/proxy/button.hpp b/micras_proxy/include/micras/proxy/button.hpp index b2120d83..1e349fa7 100644 --- a/micras_proxy/include/micras/proxy/button.hpp +++ b/micras_proxy/include/micras/proxy/button.hpp @@ -19,7 +19,7 @@ class Button { /** * @brief Enum for button status. */ - enum Status : uint8_t { + enum class Status : uint8_t { NO_PRESS = 0, SHORT_PRESS = 1, LONG_PRESS = 2, @@ -29,7 +29,7 @@ class Button { /** * @brief Enum for button pull resistor. */ - enum PullResistor : uint8_t { + enum class PullResistor : uint8_t { PULL_UP = 0, PULL_DOWN = 1, }; @@ -145,7 +145,7 @@ class Button { /** * @brief Current status of the button. */ - Status current_status{NO_PRESS}; + Status current_status{Status::NO_PRESS}; }; } // namespace micras::proxy diff --git a/micras_proxy/include/micras/proxy/fan.hpp b/micras_proxy/include/micras/proxy/fan.hpp index 01dccc8d..b7edf180 100644 --- a/micras_proxy/include/micras/proxy/fan.hpp +++ b/micras_proxy/include/micras/proxy/fan.hpp @@ -65,7 +65,7 @@ class Fan { /** * @brief Enum for rotation direction. */ - enum RotationDirection : uint8_t { + enum class RotationDirection : uint8_t { FORWARD = 0, BACKWARDS = 1 }; diff --git a/micras_proxy/include/micras/proxy/imu.hpp b/micras_proxy/include/micras/proxy/imu.hpp index 7c296b0e..4c6038ca 100644 --- a/micras_proxy/include/micras/proxy/imu.hpp +++ b/micras_proxy/include/micras/proxy/imu.hpp @@ -36,7 +36,7 @@ class Imu { /** * @brief Enum to select the axis of the IMU. */ - enum Axis : uint8_t { + enum class Axis : uint8_t { X = 0, Y = 1, Z = 2 diff --git a/micras_proxy/libs/lsm6dsv-pid b/micras_proxy/libs/lsm6dsv-pid deleted file mode 160000 index 7b986287..00000000 --- a/micras_proxy/libs/lsm6dsv-pid +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 7b98628713090244cf14f54d9059938cb00342fd diff --git a/micras_proxy/src/button.cpp b/micras_proxy/src/button.cpp index 86b9e457..5e9473be 100644 --- a/micras_proxy/src/button.cpp +++ b/micras_proxy/src/button.cpp @@ -28,20 +28,20 @@ void Button::update() { this->status_stopwatch.reset_ms(); } else if (this->is_falling_edge()) { if (this->status_stopwatch.elapsed_time_ms() > extra_long_press_delay) { - this->current_status = EXTRA_LONG_PRESS; + this->current_status = Status::EXTRA_LONG_PRESS; return; } if (this->status_stopwatch.elapsed_time_ms() > long_press_delay) { - this->current_status = LONG_PRESS; + this->current_status = Status::LONG_PRESS; return; } - this->current_status = SHORT_PRESS; + this->current_status = Status::SHORT_PRESS; return; } - this->current_status = NO_PRESS; + this->current_status = Status::NO_PRESS; } bool Button::get_raw_reading() const { diff --git a/src/interface.cpp b/src/interface.cpp index 00d761e7..eb9e1c54 100644 --- a/src/interface.cpp +++ b/src/interface.cpp @@ -21,6 +21,30 @@ void Interface::update() { this->send_event(Event::CALIBRATE); } + for (uint8_t i = 0; i < 4; i++) { + if (this->dip_switch->get_switch_state(i) == this->dip_switch_states.at(i)) { + continue; + } + + this->dip_switch_states.at(i) = this->dip_switch->get_switch_state(i); + const auto dip_switch_pin = static_cast(i); + + switch (dip_switch_pin) { + case DipSwitchPins::FAN: + this->send_event(this->dip_switch_states.at(i) ? Event::TURN_ON_FAN : Event::TURN_OFF_FAN); + break; + case DipSwitchPins::DIAGONAL: + this->send_event(this->dip_switch_states.at(i) ? Event::TURN_ON_DIAGONAL : Event::TURN_OFF_DIAGONAL); + break; + case DipSwitchPins::BOOST: + this->send_event(this->dip_switch_states.at(i) ? Event::TURN_ON_BOOST : Event::TURN_OFF_BOOST); + break; + case DipSwitchPins::RISKY: + this->send_event(this->dip_switch_states.at(i) ? Event::TURN_ON_RISKY : Event::TURN_OFF_RISKY); + break; + } + } + if (this->acknowledge_event(Event::ERROR)) { this->led->turn_on(); } diff --git a/src/main.cpp b/src/main.cpp index 4933051a..f836bb58 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -2,14 +2,49 @@ * @file */ -#include "micras/micras.hpp" +#include + #include "micras/hal/mcu.hpp" +#include "micras/micras.hpp" + +// NOLINTNEXTLINE(readability-function-cognitive-complexity) +void signal_handler(int signal) { + if (signal == SIGABRT) { + HAL_GPIO_WritePin(micras::led_config.gpio.port, micras::led_config.gpio.pin, GPIO_PinState::GPIO_PIN_SET); + + __HAL_TIM_SET_COMPARE( + micras::locomotion_config.left_motor.backwards_pwm.handle, + micras::locomotion_config.left_motor.backwards_pwm.timer_channel, 0 + ); + __HAL_TIM_SET_COMPARE( + micras::locomotion_config.left_motor.forward_pwm.handle, + micras::locomotion_config.left_motor.forward_pwm.timer_channel, 0 + ); + __HAL_TIM_SET_COMPARE( + micras::locomotion_config.right_motor.backwards_pwm.handle, + micras::locomotion_config.right_motor.backwards_pwm.timer_channel, 0 + ); + __HAL_TIM_SET_COMPARE( + micras::locomotion_config.right_motor.forward_pwm.handle, + micras::locomotion_config.right_motor.forward_pwm.timer_channel, 0 + ); -/***************************************** - * Main Function - *****************************************/ + __HAL_TIM_SET_COMPARE(micras::fan_config.pwm.handle, micras::fan_config.pwm.timer_channel, 0); + + __HAL_TIM_SET_COMPARE( + micras::wall_sensors_config.led_0_pwm.handle, micras::wall_sensors_config.led_0_pwm.timer_channel, 0 + ); + __HAL_TIM_SET_COMPARE( + micras::wall_sensors_config.led_1_pwm.handle, micras::wall_sensors_config.led_1_pwm.timer_channel, 0 + ); + + __HAL_TIM_SET_COMPARE(micras::buzzer_config.pwm.handle, micras::buzzer_config.pwm.timer_channel, 0); + } +} int main() { + std::signal(SIGABRT, signal_handler); + micras::hal::Mcu::init(); micras::Micras micras; diff --git a/src/micras.cpp b/src/micras.cpp index db4e0a2b..a040e8ff 100644 --- a/src/micras.cpp +++ b/src/micras.cpp @@ -28,7 +28,7 @@ Micras::Micras() : maze{maze_config}, odometry{rotary_sensor_left, rotary_sensor_right, imu, odometry_config}, speed_controller{speed_controller_config}, - follow_wall{wall_sensors, odometry.get_state().pose, follow_wall_config}, + follow_wall{wall_sensors, follow_wall_config}, interface{argb, button, buzzer, dip_switch, led}, action_pose{odometry.get_state().pose} { this->fsm.add_state(std::make_unique(State::CALIBRATE, *this)); @@ -89,15 +89,15 @@ void Micras::prepare() { bool Micras::run() { this->odometry.update(this->elapsed_time); - const micras::nav::State& state = this->odometry.get_state(); - core::Observation observation{}; + micras::nav::State& state = this->odometry.get_state(); + core::Observation observation{}; if (this->current_action->finished(this->action_pose.get())) { if (this->finished) { this->finished = false; this->locomotion.stop(); - if (this->objective == core::Objective::EXPLORE) { + if (this->objective != core::Objective::SOLVE) { this->maze.compute_best_route(); } @@ -127,21 +127,16 @@ bool Micras::run() { next_goal = this->maze.get_next_goal(this->grid_pose, returning); } - this->action_queuer.push(this->grid_pose, next_goal.position); + this->action_queuer.push_exploring(this->grid_pose, next_goal.position); this->current_action = this->action_queuer.pop(); this->grid_pose = next_goal; } - - if (this->current_action->allow_follow_wall()) { - this->follow_wall.reset(); - } } - this->desired_speeds = this->current_action->get_speeds(this->action_pose.get()); + this->desired_speeds = this->current_action->get_speeds(this->action_pose.get(), this->elapsed_time); if (this->current_action->allow_follow_wall()) { - this->desired_speeds.angular = - this->follow_wall.compute_angular_correction(this->elapsed_time, state.velocity.linear); + this->desired_speeds.angular = this->follow_wall.compute_angular_correction(this->elapsed_time, state); } std::tie(this->left_response, this->right_response) = @@ -165,13 +160,13 @@ void Micras::stop() { void Micras::init() { this->wall_sensors->turn_on(); this->locomotion.enable(); - this->odometry.reset(); this->imu->calibrate(); this->action_pose.reset_reference(); } void Micras::reset() { this->grid_pose = maze_config.start; + this->odometry.reset(); this->finished = false; } @@ -189,7 +184,8 @@ void Micras::save_best_route() { void Micras::load_best_route() { this->maze_storage.sync("maze", this->maze); - this->action_queuer.recompute(this->maze.get_best_route()); + this->action_queuer.recompute(this->maze.get_best_route(), false); + this->fan.set_speed(fan_speed); } core::Objective Micras::get_objective() const { @@ -215,4 +211,12 @@ bool Micras::acknowledge_event(Interface::Event event) { bool Micras::peek_event(Interface::Event event) const { return this->interface.peek_event(event); } + +void Micras::handle_events() { + if (this->interface.acknowledge_event(Interface::Event::TURN_ON_FAN)) { + this->fan.enable(); + } else if (this->interface.acknowledge_event(Interface::Event::TURN_OFF_FAN)) { + this->fan.disable(); + } +} } // namespace micras