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

mission updated

Browse Source
This commit is contained in:
acki
2025-05-23 14:20:45 -04:00
parent daa72726c6
commit 1ed638c861
6 changed files with 85 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
src/modelec_strat/include/modelec_strat/action_executor.hpp
View File

@@ -33,6 +33,9 @@ namespace Modelec
ASC_GO_UP_TO_TAKE_POT,
PLACE_FIRST_PLATE,
// Take One floor
STICK_ALL,
// Place Pot
ASC_FINAL,
@@ -52,9 +55,9 @@ namespace Modelec
void DeployBanner();
void TakePot();
void TakePot(bool two_floor = true);
void PlacePot();
void PlacePot(bool two_floor = true);
void ReInit();

3
src/modelec_strat/include/modelec_strat/missions/prepare_concert_mission.hpp
View File

@@ -12,7 +12,7 @@ namespace Modelec
{
public:
PrepareConcertMission(const std::shared_ptr<NavigationHelper>& nav,
const std::shared_ptr<ActionExecutor>& action_executor);
const std::shared_ptr<ActionExecutor>& action_executor, bool two_floor = true);
void Start(rclcpp::Node::SharedPtr node) override;
void Update() override;
@@ -38,6 +38,7 @@ namespace Modelec
std::shared_ptr<NavigationHelper> nav_;
std::shared_ptr<ActionExecutor> action_executor_;
rclcpp::Node::SharedPtr node_;
bool two_floor_;
std::shared_ptr<ColumnObstacle> column_;
std::shared_ptr<DepositeZone> closestDepoZone_;

1
src/modelec_strat/include/modelec_strat/strat_fms.hpp
View File

@@ -61,6 +61,7 @@ namespace Modelec
bool setup_ = false;
std::unique_ptr<Mission> current_mission_;
int team_id_ = 0;
bool is_banner_done_ = false;
std::shared_ptr<NavigationHelper> nav_;
std::shared_ptr<ActionExecutor> action_executor_;

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

@@ -175,6 +175,26 @@ namespace Modelec
break;
case STICK_POT:
{
modelec_interfaces::msg::ActionRelayState relay_top_msg;
relay_top_msg.state = true; // TODO : check that
relay_top_msg.id = 2;
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;
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);
modelec_interfaces::msg::ActionServoPos second_pot_msg;
second_pot_msg.id = 1; // TODO : to define
second_pot_msg.pos = 1;
@@ -185,7 +205,7 @@ namespace Modelec
third_pot_msg.pos = 1;
servo_move_pub_->publish(third_pot_msg);
step_running_ = 2;
step_running_ = 6;
}
break;
@@ -209,6 +229,22 @@ namespace Modelec
step_running_ = 1;
}
break;
case STICK_ALL:
{
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_FINAL:
{
@@ -291,35 +327,52 @@ namespace Modelec
}
}
void ActionExecutor::TakePot()
void ActionExecutor::TakePot(bool two_floor)
{
if (action_done_)
{
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);
if (two_floor)
{
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);
}
else
{
step_.push(ASC_GO_DOWN);
step_.push(STICK_ALL);
}
Update();
}
}
void ActionExecutor::PlacePot()
void ActionExecutor::PlacePot(bool two_floor)
{
if (action_done_)
{
action_ = PLACE_POT;
action_done_ = false;
step_.push(ASC_FINAL);
step_.push(FREE_ALL);
step_.push(REMOVE_ACTION_STEP);
if (two_floor)
{
step_.push(ASC_FINAL);
step_.push(FREE_ALL);
step_.push(REMOVE_ACTION_STEP);
}
else
{
step_.push(FREE_ALL);
step_.push(REMOVE_ACTION_STEP);
}
Update();
}

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

@@ -5,8 +5,8 @@
namespace Modelec
{
PrepareConcertMission::PrepareConcertMission(const std::shared_ptr<NavigationHelper>& nav,
const std::shared_ptr<ActionExecutor>& action_executor) :
step_(GO_TO_COLUMN),
const std::shared_ptr<ActionExecutor>& action_executor, bool two_floor) :
step_(GO_TO_COLUMN), two_floor_(two_floor),
status_(MissionStatus::READY), nav_(nav), action_executor_(action_executor)
{
}
@@ -79,7 +79,7 @@ namespace Modelec
step_ = GO_CLOSE_TO_COLUMN;
break;
case GO_CLOSE_TO_COLUMN:
action_executor_->TakePot();
action_executor_->TakePot(two_floor_);
nav_->GetPathfinding()->RemoveObstacle(column_->GetId());
step_ = TAKE_COLUMN;
@@ -175,7 +175,7 @@ namespace Modelec
case GO_CLOSE_TO_PLATFORM:
{
action_executor_->PlacePot();
action_executor_->PlacePot(two_floor_);
column_->SetX(closestDepoZonePoint_.x);
column_->SetY(closestDepoZonePoint_.y);

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

@@ -156,8 +156,9 @@ namespace Modelec
case State::SELECT_MISSION:
{
auto elapsed = now - match_start_time_;
// select mission in a good way there.
if (elapsed.seconds() < 2)
// TODO : next thing to upgrade to have a good strat
if (!is_banner_done_)
{
Transition(State::DO_PROMOTION, "Start promotion");
}
@@ -175,7 +176,7 @@ namespace Modelec
case State::DO_PREPARE_CONCERT:
if (!current_mission_)
{
current_mission_ = std::make_unique<PrepareConcertMission>(nav_, action_executor_);
current_mission_ = std::make_unique<PrepareConcertMission>(nav_, action_executor_, (now - match_start_time_).seconds() < 70);
current_mission_->Start(shared_from_this());
}
current_mission_->Update();
@@ -207,11 +208,13 @@ namespace Modelec
if (current_mission_->GetStatus() == MissionStatus::DONE)
{
current_mission_.reset();
is_banner_done_ = true;
Transition(State::SELECT_MISSION, "Promotion finished");
}
else if (current_mission_->GetStatus() == MissionStatus::FAILED)
{
current_mission_.reset();
is_banner_done_ = true;
Transition(State::SELECT_MISSION, "Promotion failed");
}
break;