From 8d6bacc44409b7a95fc79bd4b9f4709ef9ed4fec Mon Sep 17 00:00:00 2001 From: acki Date: Wed, 18 Mar 2026 17:30:20 +0100 Subject: [PATCH] two sided --- src/modelec_strat/data/config.xml | 19 +- .../include/modelec_strat/strat_fms.hpp | 2 + src/modelec_strat/src/strat_fms.cpp | 171 +++++++----------- 3 files changed, 80 insertions(+), 112 deletions(-) diff --git a/src/modelec_strat/data/config.xml b/src/modelec_strat/data/config.xml index 97da187..f77fe13 100644 --- a/src/modelec_strat/data/config.xml +++ b/src/modelec_strat/data/config.xml @@ -78,14 +78,17 @@ 77 65 - - false - - - - - - + + + false + + + + + + + false + 20 0.6 diff --git a/src/modelec_strat/include/modelec_strat/strat_fms.hpp b/src/modelec_strat/include/modelec_strat/strat_fms.hpp index a7938a6..369331b 100644 --- a/src/modelec_strat/include/modelec_strat/strat_fms.hpp +++ b/src/modelec_strat/include/modelec_strat/strat_fms.hpp @@ -69,6 +69,8 @@ namespace Modelec float factor_thermo_; int timer_period_ms_ = 100; + bool two_sided_; + std::shared_ptr nav_; std::shared_ptr action_executor_; diff --git a/src/modelec_strat/src/strat_fms.cpp b/src/modelec_strat/src/strat_fms.cpp index 585f72d..a741aed 100644 --- a/src/modelec_strat/src/strat_fms.cpp +++ b/src/modelec_strat/src/strat_fms.cpp @@ -33,10 +33,11 @@ namespace Modelec RCLCPP_ERROR(get_logger(), "Failed to load config file: %s", (data_dir + "/action.xml").c_str()); } - static_strat_ = Config::get("config.static_strat.enabled", false); + static_strat_ = Config::get("config.strat.static.enabled", false); factor_obs_ = Config::get("config.factor.obs", 1.0); factor_thermo_ = Config::get("config.factor.thermo", 1.0); timer_period_ms_ = Config::get("config.timer.strat.ms", 100); + two_sided_ = Config::get("config.strat.two_sided", false); tir_sub_ = create_subscription( "/action/tir/start", 10, [this](const std_msgs::msg::Empty::SharedPtr) @@ -213,110 +214,78 @@ namespace Modelec } case State::SELECT_GAME_ACTION: { - - if (static_strat_) { - if (game_action_sequence_.empty()) { - Transition(State::DO_GO_HOME, "No more game actions in sequence"); - return; - } - - auto next_action = game_action_sequence_.front(); - game_action_sequence_.pop(); - - Transition(next_action, "Selecting next game action from sequence"); - return; - } + if (static_strat_) { + if (game_action_sequence_.empty()) { + Transition(State::DO_GO_HOME, "No more game actions in sequence"); + return; + } + auto next_action = game_action_sequence_.front(); + game_action_sequence_.pop(); + Transition(next_action, "Selecting next game action from sequence"); + return; + } auto pos = nav_->GetCurrentPos(); auto closestBox = nav_->GetClosestObstacle(pos); auto closestDeposite = nav_->GetClosestDepositeZone(pos); - auto thermoPos = nav_->GetThermoPositions()[0]; - double distToBox = Point::distance(Point(pos->x, pos->y, pos->theta), - closestBox->GetPosition()) * factor_obs_; - double distToDeposite = Point::distance(Point(pos->x, pos->y, pos->theta), - closestDeposite->GetPosition()); - double distToThermo = Point::distance(Point(pos->x, pos->y, pos->theta), - thermoPos) * factor_thermo_; + // Calculate weighted distances + double distToBox = closestBox ? Point::distance(Point(pos->x, pos->y, pos->theta), closestBox->GetPosition()) * factor_obs_ : std::numeric_limits::max(); + double distToDeposite = closestDeposite ? Point::distance(Point(pos->x, pos->y, pos->theta), closestDeposite->GetPosition()) : std::numeric_limits::max(); + double distToThermo = Point::distance(Point(pos->x, pos->y, pos->theta), thermoPos) * factor_thermo_; - if (distToThermo < distToBox && distToThermo < distToDeposite && !ThermoMission::IsThermoDone) + // 1. High Priority: Thermo Mission (if not done and closest) + if (!ThermoMission::IsThermoDone && distToThermo < distToBox && distToThermo < distToDeposite) { - RCLCPP_INFO(get_logger(), "Choosing THERMO mission (dist to thermo: %.2f < dist to box: %.2f, dist to deposite: %.2f)", - distToThermo, distToBox, distToDeposite); - Transition(State::THERMO_MISSION, "Selecting THERMO mission"); - } else - { - if (action_executor_->IsFull()) - { - RCLCPP_INFO(get_logger(), "All box are present on robot, selecting FREE mission"); - Transition(State::FREE_MISSION, "Selecting FREE mission"); - } - else if (action_executor_->IsEmpty()) - { - RCLCPP_INFO(get_logger(), "No box present on robot, selecting TAKE mission"); - Transition(State::TAKE_MISSION, "Selecting TAKE mission"); - } - else - { + Transition(State::THERMO_MISSION, "Thermo is closest and not done"); + break; + } - if (closestBox && closestDeposite) - { - if (distToBox < distToDeposite) - { - RCLCPP_INFO(get_logger(), "Choosing TAKE mission (dist to box: %.2f < dist to deposite: %.2f)", - distToBox, distToDeposite); - Transition(State::TAKE_MISSION, "Selecting TAKE mission"); - } - else - { - RCLCPP_INFO(get_logger(), "Choosing FREE mission (dist to deposite: %.2f < dist to box: %.2f)", - distToDeposite, distToBox); - Transition(State::FREE_MISSION, "Selecting FREE mission"); - } - } - else if (closestBox) - { - RCLCPP_INFO(get_logger(), "Only box available, selecting TAKE mission"); - Transition(State::TAKE_MISSION, "Selecting TAKE mission"); - } - else if (closestDeposite) - { - RCLCPP_INFO(get_logger(), "Only deposite available, selecting FREE mission"); - Transition(State::FREE_MISSION, "Selecting FREE mission"); - } - else - { - RCLCPP_WARN(get_logger(), "No box or deposite available, cannot select mission!"); - Transition(State::STOP, "No missions available"); - } + // 2. Capacity Check + bool canTake = !action_executor_->IsFull(); + bool canFree = !action_executor_->IsEmpty(); + + // If one-sided, we might be "full" with just one box + if (!two_sided_) { + canTake = !action_executor_->HasFrontBox(); + canFree = action_executor_->HasFrontBox(); + } + + // 3. Decision Tree based on availability and distance + if (canTake && canFree) { + if (distToBox < distToDeposite) { + Transition(State::TAKE_MISSION, "Taking: Box is closer"); + } else { + Transition(State::FREE_MISSION, "Freeing: Deposit is closer"); } } - } + else if (canTake && closestBox) { + Transition(State::TAKE_MISSION, "Taking: Robot has space and boxes exist"); + } + else if (canFree && closestDeposite) { + Transition(State::FREE_MISSION, "Freeing: Robot has items to drop"); + } + else { + Transition(State::DO_GO_HOME, "No viable game actions left"); + } - break; + break; + } case State::TAKE_MISSION: if (!current_mission_) { - if (action_executor_->HasFrontBox()) - { - if (action_executor_->HasBackBox()) - { - RCLCPP_WARN(get_logger(), "Both front and back box obstacles are occupied!"); - current_mission_.reset(); - Transition(State::SELECT_MISSION, "Cannot take box, both sides occupied"); - break; - } + BaseAction::Side side_to_use = BaseAction::FRONT; - RCLCPP_INFO(get_logger(), "Front box obstacle is occupied, taking from back"); - current_mission_ = std::make_unique(nav_, action_executor_, BaseAction::BACK); - current_mission_->Start(shared_from_this()); - } - else - { - current_mission_ = std::make_unique(nav_, action_executor_, BaseAction::FRONT); - current_mission_->Start(shared_from_this()); + if (two_sided_) { + side_to_use = action_executor_->HasFrontBox() ? BaseAction::BACK : BaseAction::FRONT; + } else if (action_executor_->HasFrontBox()) { + Transition(State::SELECT_MISSION, "Front full (One-sided mode)"); + break; } + + current_mission_ = std::make_unique(nav_, action_executor_, side_to_use); + current_mission_->Start(shared_from_this()); } current_mission_->Update(); if (current_mission_->GetStatus() == MissionStatus::DONE) @@ -335,24 +304,18 @@ namespace Modelec case State::FREE_MISSION: if (!current_mission_) { - if (!action_executor_->HasFrontBox()) - { - if (!action_executor_->HasBackBox()) - { - RCLCPP_WARN(get_logger(), "Both front and back box obstacles are free!"); - Transition(State::SELECT_MISSION, "Cannot free box, both sides empty"); - break; - } + BaseAction::Side side_to_use = BaseAction::FRONT; - RCLCPP_INFO(get_logger(), "Front box obstacle is occupied, taking from back"); - current_mission_ = std::make_unique(nav_, action_executor_, BaseAction::BACK); - current_mission_->Start(shared_from_this()); - } - else - { - current_mission_ = std::make_unique(nav_, action_executor_, BaseAction::FRONT); - current_mission_->Start(shared_from_this()); + if (two_sided_) { + // In two-sided mode, drop front first, then back + side_to_use = action_executor_->HasFrontBox() ? BaseAction::FRONT : BaseAction::BACK; + } else if (!action_executor_->HasFrontBox()) { + Transition(State::SELECT_MISSION, "Front empty (One-sided mode)"); + break; } + + current_mission_ = std::make_unique(nav_, action_executor_, side_to_use); + current_mission_->Start(shared_from_this()); } current_mission_->Update(); if (current_mission_->GetStatus() == MissionStatus::DONE)