modelec/modelec-marcel-ROS
1
0
Fork 0
mirror of https://github.com/modelec/modelec-marcel-ROS.git synced 2026-01-18 16:47:43 +01:00
Code Issues Packages Projects Releases Wiki Activity

patch some bugs

Browse Source
This commit is contained in:
acki
2025-05-23 14:57:43 -04:00
parent 213525d1a7
commit f1f64f3b40
4 changed files with 36 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
src/modelec_strat/src/action_executor.cpp
View File

@@ -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)
{

18
src/modelec_strat/src/missions/banner_mission.cpp
View File

@@ -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_)
{

8
src/modelec_strat/src/missions/prepare_concert_mission.cpp
View File

@@ -15,7 +15,13 @@ namespace Modelec
{
node_ = node;
mission_score_ = Config::get<int>("config.mission_score.concert.niv_2", 0);
if (two_floor_)
{
mission_score_ = Config::get<int>("config.mission_score.concert.niv_2", 0);
} else
{
mission_score_ = Config::get<int>("config.mission_score.concert.niv_1", 0);
}
score_pub_ = node_->create_publisher<std_msgs::msg::Int64>("/strat/score", 10);

5
src/modelec_strat/src/strat_fms.cpp
View File

@@ -10,8 +10,6 @@ namespace Modelec
tir_sub_ = create_subscription<std_msgs::msg::Empty>(
"/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<std_msgs::msg::Empty>(
"/strat/reset", 10, [this](const std_msgs::msg::Empty::SharedPtr)
{
RCLCPP_INFO(get_logger(), "Resetting strat");
Reset();
});
tir_arm_sub_ = create_subscription<std_msgs::msg::Empty>(
"/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<NavigationHelper>(shared_from_this());
action_executor_ = std::make_unique<ActionExecutor>(shared_from_this());
ResetStrat();
RCLCPP_INFO(this->get_logger(), "StratFMS fully initialized");
}