diff --git a/src/modelec_com/include/modelec_com/pcb_action_interface.hpp b/src/modelec_com/include/modelec_com/pcb_action_interface.hpp index e50eb97..253bb9a 100644 --- a/src/modelec_com/include/modelec_com/pcb_action_interface.hpp +++ b/src/modelec_com/include/modelec_com/pcb_action_interface.hpp @@ -13,13 +13,6 @@ namespace Modelec { public: - enum ASCState - { - LOW = 0, - HIGH = 1, - MOVING = 2, - }; - PCBActionInterface(); rclcpp::CallbackGroup::SharedPtr pcb_callback_group_; std::shared_ptr pcb_executor_; @@ -28,10 +21,10 @@ namespace Modelec ~PCBActionInterface() override; protected: - std::map asc_value_mapper_; + std::map asc_value_mapper_; std::map> servo_pos_mapper_; - ASCState asc_state_ = LOW; + int asc_state_ = 0; std::map servo_value_; std::map relay_value_; diff --git a/src/modelec_com/src/pcb_action_interface.cpp b/src/modelec_com/src/pcb_action_interface.cpp index 035cbb2..e4cb4b9 100644 --- a/src/modelec_com/src/pcb_action_interface.cpp +++ b/src/modelec_com/src/pcb_action_interface.cpp @@ -106,7 +106,7 @@ namespace Modelec "action/asc/set", 10, [this](const modelec_interfaces::msg::ActionAscPos::SharedPtr msg) { - SendOrder("ASC", {msg->pos == LOW ? "LOW" : "HIGH", std::to_string(msg->value)}); + SendOrder("ASC", {std::to_string(msg->pos), std::to_string(msg->value)}); }); servo_set_sub_ = this->create_subscription( @@ -128,7 +128,7 @@ namespace Modelec "action/asc/move", 10, [this](const modelec_interfaces::msg::ActionAscPos::SharedPtr msg) { - SendMove("ASC", {msg->pos == LOW ? "LOW" : "HIGH"}); + SendMove("ASC", {std::to_string(msg->pos)}); }); servo_move_sub_ = this->create_subscription( @@ -179,18 +179,7 @@ namespace Modelec { if (tokens[1] == "ASC") { - if (tokens[3] == "DESC" || tokens[3] == "CLIMB") - { - asc_state_ = MOVING; - } - else if (tokens[3] == "LOW") - { - asc_state_ = LOW; - } - else if (tokens[3] == "HIGH") - { - asc_state_ = HIGH; - } + asc_state_ = std::stoi(tokens[3]); modelec_interfaces::msg::ActionAscPos asc_msg; asc_msg.pos = asc_state_; @@ -228,12 +217,12 @@ namespace Modelec { if (tokens[1] == "ASC") { - ASCState state = tokens[2] == "LOW" ? LOW : HIGH; + int pos = std::stoi(tokens[2]); int v = std::stoi(tokens[3]); - asc_value_mapper_[state] = v; + asc_value_mapper_[pos] = v; modelec_interfaces::msg::ActionAscPos asc_msg; - asc_msg.pos = state; + asc_msg.pos = pos; asc_msg.value = v; asc_msg.success = true; asc_set_res_pub_->publish(asc_msg); @@ -257,7 +246,7 @@ namespace Modelec { if (tokens[1] == "ASC") { - asc_state_ = tokens[2] == "LOW" ? LOW : HIGH; + asc_state_ = std::stoi(tokens[2]); modelec_interfaces::msg::ActionAscPos asc_msg; asc_msg.pos = asc_state_; diff --git a/src/modelec_interfaces/msg/Action/ActionAscPos.msg b/src/modelec_interfaces/msg/Action/ActionAscPos.msg index 4c2ba2a..4973630 100644 --- a/src/modelec_interfaces/msg/Action/ActionAscPos.msg +++ b/src/modelec_interfaces/msg/Action/ActionAscPos.msg @@ -1,7 +1,3 @@ -int32 LOW=0 -int32 HIGH=1 -int32 MOVING=2 - int32 pos int32 value -bool success \ No newline at end of file +bool success diff --git a/src/modelec_strat/include/modelec_strat/action_executor.hpp b/src/modelec_strat/include/modelec_strat/action_executor.hpp index cc45c6f..e40938c 100644 --- a/src/modelec_strat/include/modelec_strat/action_executor.hpp +++ b/src/modelec_strat/include/modelec_strat/action_executor.hpp @@ -14,13 +14,30 @@ namespace Modelec { NONE, DEPLOY_BANNER, - RETRACT_BANNER, TAKE_POT, PLACE_POT, }; enum Step { + // Banner + DEPLOY_BANNER_STEP, + + // Take Pot + ASC_GO_DOWN, + STICK_TO_STRUCT, + ASC_GO_UP, + RETRACT_BOTTOM_PLATE, + ASC_GO_DOWN_TO_TAKE_POT, + STICK_POT, + ASC_GO_UP_TO_TAKE_POT, + PLACE_FIRST_PLATE, + ASC_FINAL, + + + // Place Pot + FREE_ALL, + REMOVE_ACTION_STEP, }; ActionExecutor(); @@ -35,8 +52,6 @@ namespace Modelec void DeployBanner(); - void RetractBanner(); - void TakePot(); void PlacePot(); @@ -71,7 +86,7 @@ namespace Modelec std::queue step_; bool action_done_ = true; - bool step_done_ = true; + int step_running_ = 0; private: rclcpp::Node::SharedPtr node_; diff --git a/src/modelec_strat/include/modelec_strat/missions/banner_mission.hpp b/src/modelec_strat/include/modelec_strat/missions/banner_mission.hpp index 7e3d836..41d6db8 100644 --- a/src/modelec_strat/include/modelec_strat/missions/banner_mission.hpp +++ b/src/modelec_strat/include/modelec_strat/missions/banner_mission.hpp @@ -25,8 +25,8 @@ namespace Modelec enum Step { - GO_TO_FRONT, DEPLOY_BANNER, + GO_TO_FRONT, GO_FORWARD, DONE } step_; diff --git a/src/modelec_strat/src/action_executor.cpp b/src/modelec_strat/src/action_executor.cpp index f5a1c68..a79c479 100644 --- a/src/modelec_strat/src/action_executor.cpp +++ b/src/modelec_strat/src/action_executor.cpp @@ -47,21 +47,21 @@ namespace Modelec asc_move_res_sub_ = node_->create_subscription( "/action/move/asc/res", 10, [this](const modelec_interfaces::msg::ActionAscPos::SharedPtr) { - step_done_ = true; + step_running_--; Update(); }); servo_move_res_sub_ = node_->create_subscription( "/action/move/servo/res", 10, [this](const modelec_interfaces::msg::ActionServoPos::SharedPtr) { - step_done_ = true; + step_running_--; Update(); }); relay_move_res_sub_ = node_->create_subscription( "/action/move/relay/res", 10, [this](const modelec_interfaces::msg::ActionRelayState::SharedPtr) { - step_done_ = true; + step_running_--; Update(); }); } @@ -85,13 +85,196 @@ namespace Modelec return; } - if (step_done_) + if (step_running_ == 0) { step_.pop(); - step_done_ = false; switch (step_.front()) { + case DEPLOY_BANNER_STEP: + { + modelec_interfaces::msg::ActionServoPos msg; + msg.id = 5; // TODO : to define + msg.pos = 1; + servo_move_pub_->publish(msg); + + step_running_ = 1; + } + + break; + + case ASC_GO_DOWN: + { + modelec_interfaces::msg::ActionAscPos asc_msg; + asc_msg.pos = 1; + asc_move_pub_->publish(asc_msg); + + modelec_interfaces::msg::ActionServoPos servo_action_bottom_msg; + servo_action_bottom_msg.id = 4; // TODO : to define + servo_action_bottom_msg.pos = 1; + servo_move_pub_->publish(servo_action_bottom_msg); + + step_running_ = 2; + } + + break; + case STICK_TO_STRUCT: + { + modelec_interfaces::msg::ActionRelayState relay_top_msg; + relay_top_msg.state = true; // TODO : check that + relay_top_msg.id = 0; // TODO : to define + relay_move_pub_->publish(relay_top_msg); + + modelec_interfaces::msg::ActionRelayState relay_bottom_msg; + relay_bottom_msg.state = true; // TODO : check that + relay_bottom_msg.id = 1; // TODO : to define + relay_move_pub_->publish(relay_bottom_msg); + + modelec_interfaces::msg::ActionServoPos first_pot_msg; + first_pot_msg.id = 0; // TODO : to define + first_pot_msg.pos = 1; + servo_move_pub_->publish(first_pot_msg); + + modelec_interfaces::msg::ActionServoPos fourth_pot_msg; + fourth_pot_msg.id = 3; // TODO : to define + fourth_pot_msg.pos = 1; + servo_move_pub_->publish(fourth_pot_msg); + + step_running_ = 4; + } + + break; + case ASC_GO_UP: + { + modelec_interfaces::msg::ActionAscPos asc_msg; + asc_msg.pos = 3; + asc_move_pub_->publish(asc_msg); + + step_running_ = 1; + } + + break; + case RETRACT_BOTTOM_PLATE: + { + modelec_interfaces::msg::ActionServoPos servo_action_bottom_msg; + servo_action_bottom_msg.id = 4; // TODO : to define + servo_action_bottom_msg.pos = 2; + servo_move_pub_->publish(servo_action_bottom_msg); + + step_running_ = 1; + } + + break; + case ASC_GO_DOWN_TO_TAKE_POT: + { + modelec_interfaces::msg::ActionAscPos asc_msg; + asc_msg.pos = 0; + asc_move_pub_->publish(asc_msg); + + step_running_ = 1; + } + + break; + case STICK_POT: + { + modelec_interfaces::msg::ActionServoPos second_pot_msg; + second_pot_msg.id = 1; // TODO : to define + second_pot_msg.pos = 1; + servo_move_pub_->publish(second_pot_msg); + + modelec_interfaces::msg::ActionServoPos third_pot_msg; + third_pot_msg.id = 2; // TODO : to define + third_pot_msg.pos = 1; + servo_move_pub_->publish(third_pot_msg); + + step_running_ = 2; + } + + break; + case ASC_GO_UP_TO_TAKE_POT: + { + modelec_interfaces::msg::ActionAscPos asc_msg; + asc_msg.pos = 3; + asc_move_pub_->publish(asc_msg); + + step_running_ = 1; + } + + break; + case PLACE_FIRST_PLATE: + { + modelec_interfaces::msg::ActionServoPos action_bottom_msg; + action_bottom_msg.id = 4; // TODO : to define + action_bottom_msg.pos = 0; + servo_move_pub_->publish(action_bottom_msg); + + step_running_ = 1; + } + + break; + case ASC_FINAL: + { + modelec_interfaces::msg::ActionAscPos asc_msg; + asc_msg.pos = 2; + asc_move_pub_->publish(asc_msg); + + step_running_ = 1; + } + + break; + case FREE_ALL: + { + modelec_interfaces::msg::ActionRelayState relay_top_msg; + relay_top_msg.state = false; // TODO : check that + relay_top_msg.id = 0; // TODO : to define + relay_move_pub_->publish(relay_top_msg); + + modelec_interfaces::msg::ActionRelayState relay_bottom_msg; + relay_bottom_msg.state = false; // TODO : check that + relay_bottom_msg.id = 1; // TODO : to define + relay_move_pub_->publish(relay_bottom_msg); + + modelec_interfaces::msg::ActionServoPos first_pot_msg; + first_pot_msg.id = 0; // TODO : to define + first_pot_msg.pos = 0; + servo_move_pub_->publish(first_pot_msg); + + modelec_interfaces::msg::ActionServoPos second_pot_msg; + second_pot_msg.id = 1; // TODO : to define + second_pot_msg.pos = 0; + servo_move_pub_->publish(second_pot_msg); + + modelec_interfaces::msg::ActionServoPos third_pot_msg; + third_pot_msg.id = 2; // TODO : to define + third_pot_msg.pos = 0; + servo_move_pub_->publish(third_pot_msg); + + modelec_interfaces::msg::ActionServoPos fourth_pot_msg; + fourth_pot_msg.id = 3; // TODO : to define + fourth_pot_msg.pos = 0; + servo_move_pub_->publish(fourth_pot_msg); + + step_running_ = 6; + } + + break; + case REMOVE_ACTION_STEP: + { + modelec_interfaces::msg::ActionAscPos asc_msg; + asc_msg.pos = 3; + asc_move_pub_->publish(asc_msg); + + modelec_interfaces::msg::ActionServoPos servo_action_bottom_msg; + servo_action_bottom_msg.id = 4; // TODO : to define + servo_action_bottom_msg.pos = 0; + servo_move_pub_->publish(servo_action_bottom_msg); + + step_running_ = 2; + } + + break; + default: + break; } } } @@ -102,17 +285,7 @@ namespace Modelec { action_ = DEPLOY_BANNER; action_done_ = false; - - Update(); - } - } - - void ActionExecutor::RetractBanner() - { - if (action_done_) - { - action_ = RETRACT_BANNER; - action_done_ = false; + step_.push(DEPLOY_BANNER_STEP); Update(); } @@ -125,6 +298,16 @@ namespace Modelec action_ = TAKE_POT; action_done_ = false; + step_.push(ASC_GO_DOWN); + step_.push(STICK_TO_STRUCT); + step_.push(ASC_GO_UP); + step_.push(RETRACT_BOTTOM_PLATE); + step_.push(ASC_GO_DOWN_TO_TAKE_POT); + step_.push(STICK_POT); + step_.push(ASC_GO_UP_TO_TAKE_POT); + step_.push(PLACE_FIRST_PLATE); + step_.push(ASC_FINAL); + Update(); } } @@ -135,6 +318,8 @@ namespace Modelec { action_ = PLACE_POT; action_done_ = false; + step_.push(FREE_ALL); + step_.push(REMOVE_ACTION_STEP); Update(); } diff --git a/src/modelec_strat/src/missions/banner_mission.cpp b/src/modelec_strat/src/missions/banner_mission.cpp index 2c8cc16..bcfc83c 100644 --- a/src/modelec_strat/src/missions/banner_mission.cpp +++ b/src/modelec_strat/src/missions/banner_mission.cpp @@ -5,7 +5,7 @@ namespace Modelec { BannerMission::BannerMission(const std::shared_ptr& nav, - const std::shared_ptr& action_executor) : step_(GO_TO_FRONT), + const std::shared_ptr& action_executor) : step_(DEPLOY_BANNER), status_(MissionStatus::READY), nav_(nav), action_executor_(action_executor) { } @@ -18,9 +18,7 @@ namespace Modelec score_pub_ = node_->create_publisher("/strat/score", 10); - auto spawn = nav_->GetSpawn(); - - nav_->GoTo(spawn.x, (nav_->GetPathfinding()->robot_length_mm_ / 2) + 5, M_PI_2, true); + action_executor_->DeployBanner(); status_ = MissionStatus::RUNNING; } @@ -29,23 +27,26 @@ namespace Modelec { if (status_ != MissionStatus::RUNNING) return; - if (!nav_->HasArrived()) return; - if (!action_executor_->IsActionDone()) return; + if (!nav_->HasArrived()) return; + switch (step_) { - case GO_TO_FRONT: - action_executor_->DeployBanner(); + case DEPLOY_BANNER: + + auto spawn = nav_->GetSpawn(); + + nav_->GoTo(spawn.x, (nav_->GetPathfinding()->robot_length_mm_ / 2) + 5, M_PI_2, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE); step_ = DEPLOY_BANNER; break; - case DEPLOY_BANNER: + case GO_TO_FRONT: { auto spawn = nav_->GetSpawn(); - nav_->GoTo(spawn.x, (nav_->GetPathfinding()->robot_length_mm_ / 2) + 150, M_PI_2, true); + nav_->GoTo(spawn.x, (nav_->GetPathfinding()->robot_length_mm_ / 2) + 150, M_PI_2, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE); step_ = GO_FORWARD; break;