diff --git a/src/modelec_strat/src/action_executor.cpp b/src/modelec_strat/src/action_executor.cpp index 3e7aa26..62bad23 100644 --- a/src/modelec_strat/src/action_executor.cpp +++ b/src/modelec_strat/src/action_executor.cpp @@ -78,6 +78,7 @@ namespace Modelec void ActionExecutor::Update() { + RCLCPP_INFO(node_->get_logger(), "ActionExecutor::Update()"); if (step_.empty()) { action_ = NONE; @@ -85,12 +86,17 @@ namespace Modelec return; } - if (step_running_ == 0) + RCLCPP_INFO(node_->get_logger(), "ActionExecutor::Update() - action_ = %d", action_); + RCLCPP_INFO(node_->get_logger(), "is the right one = %d", step_.front() == DEPLOY_BANNER_STEP); + RCLCPP_INFO(node_->get_logger(), "step running = %d", step_running_); + + if (step_running_ <= 0) { switch (step_.front()) { case DEPLOY_BANNER_STEP: { + RCLCPP_INFO(node_->get_logger(), "ActionExecutor::Update() - DEPLOY_BANNER_STEP"); modelec_interfaces::msg::ActionServoPos msg; msg.id = 5; // TODO : to define msg.pos = 1; @@ -100,9 +106,10 @@ namespace Modelec } break; - case ASC_GO_DOWN: { + RCLCPP_INFO(node_->get_logger(), "is the right one = %d", step_.front() == ASC_GO_DOWN); + modelec_interfaces::msg::ActionAscPos asc_msg; asc_msg.pos = 1; asc_move_pub_->publish(asc_msg); @@ -321,6 +328,8 @@ namespace Modelec { action_ = DEPLOY_BANNER; action_done_ = false; + step_running_ = 0; + step_.push(DEPLOY_BANNER_STEP); Update(); @@ -333,6 +342,7 @@ namespace Modelec { action_ = TAKE_POT; action_done_ = false; + step_running_ = 0; if (two_floor) { @@ -361,6 +371,7 @@ namespace Modelec { action_ = PLACE_POT; action_done_ = false; + step_running_ = 0; if (two_floor) { diff --git a/src/modelec_strat/src/missions/banner_mission.cpp b/src/modelec_strat/src/missions/banner_mission.cpp index b0b80ff..928ec6f 100644 --- a/src/modelec_strat/src/missions/banner_mission.cpp +++ b/src/modelec_strat/src/missions/banner_mission.cpp @@ -27,11 +27,23 @@ namespace Modelec void BannerMission::Update() { - if (status_ != MissionStatus::RUNNING) return; + if (status_ != MissionStatus::RUNNING) + { + RCLCPP_INFO(node_->get_logger(), "Mission not running"); + return; + } - if (!action_executor_->IsActionDone()) return; + if (!action_executor_->IsActionDone()) + { + RCLCPP_INFO(node_->get_logger(), "Waiting for action to finish"); + return; + } - if (!nav_->HasArrived()) return; + if (!nav_->HasArrived()) + { + RCLCPP_INFO(node_->get_logger(), "Waiting for navigation to finish"); + return; + } switch (step_) { diff --git a/src/modelec_strat/src/missions/prepare_concert_mission.cpp b/src/modelec_strat/src/missions/prepare_concert_mission.cpp index 629066d..76339ee 100644 --- a/src/modelec_strat/src/missions/prepare_concert_mission.cpp +++ b/src/modelec_strat/src/missions/prepare_concert_mission.cpp @@ -15,7 +15,13 @@ namespace Modelec { node_ = node; - mission_score_ = Config::get("config.mission_score.concert.niv_2", 0); + if (two_floor_) + { + mission_score_ = Config::get("config.mission_score.concert.niv_2", 0); + } else + { + mission_score_ = Config::get("config.mission_score.concert.niv_1", 0); + } score_pub_ = node_->create_publisher("/strat/score", 10); diff --git a/src/modelec_strat/src/strat_fms.cpp b/src/modelec_strat/src/strat_fms.cpp index 5479816..9a3f9d1 100644 --- a/src/modelec_strat/src/strat_fms.cpp +++ b/src/modelec_strat/src/strat_fms.cpp @@ -10,8 +10,6 @@ namespace Modelec tir_sub_ = create_subscription( "/action/tir/start", 10, [this](const std_msgs::msg::Empty::SharedPtr) { - RCLCPP_INFO(get_logger(), "TIR started"); - if (setup_ && !started_) { started_ = true; @@ -40,14 +38,12 @@ namespace Modelec reset_strat_sub_ = create_subscription( "/strat/reset", 10, [this](const std_msgs::msg::Empty::SharedPtr) { - RCLCPP_INFO(get_logger(), "Resetting strat"); Reset(); }); tir_arm_sub_ = create_subscription( "/action/tir/arm", 10, [this](const std_msgs::msg::Empty::SharedPtr) { - RCLCPP_INFO(get_logger(), "TIR armed"); setup_ = true; }); @@ -80,6 +76,7 @@ namespace Modelec { nav_ = std::make_shared(shared_from_this()); action_executor_ = std::make_unique(shared_from_this()); + ResetStrat(); RCLCPP_INFO(this->get_logger(), "StratFMS fully initialized"); }