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

first try ACTION

Browse Source
This commit is contained in:
acki
2025-05-15 19:31:46 -04:00
parent b74b5bb8af
commit d00958756e
7 changed files with 242 additions and 63 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
src/modelec_com/include/modelec_com/pcb_action_interface.hpp
View File

@@ -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<rclcpp::executors::SingleThreadedExecutor> pcb_executor_;
@@ -28,10 +21,10 @@ namespace Modelec
~PCBActionInterface() override;
protected:
std::map<ASCState, int> asc_value_mapper_;
std::map<int, int> asc_value_mapper_;
std::map<int, std::map<int, int>> servo_pos_mapper_;
ASCState asc_state_ = LOW;
int asc_state_ = 0;
std::map<int, int> servo_value_;
std::map<int, bool> relay_value_;

25
src/modelec_com/src/pcb_action_interface.cpp
View File

@@ -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<modelec_interfaces::msg::ActionServoPos>(
@@ -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<modelec_interfaces::msg::ActionServoPos>(
@@ -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_;

6
src/modelec_interfaces/msg/Action/ActionAscPos.msg
View File

@@ -1,7 +1,3 @@
int32 LOW=0
int32 HIGH=1
int32 MOVING=2
int32 pos
int32 value
bool success
bool success

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

@@ -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> step_;
bool action_done_ = true;
bool step_done_ = true;
int step_running_ = 0;
private:
rclcpp::Node::SharedPtr node_;

2
src/modelec_strat/include/modelec_strat/missions/banner_mission.hpp
View File

@@ -25,8 +25,8 @@ namespace Modelec
enum Step
{
GO_TO_FRONT,
DEPLOY_BANNER,
GO_TO_FRONT,
GO_FORWARD,
DONE
} step_;

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

@@ -47,21 +47,21 @@ namespace Modelec
asc_move_res_sub_ = node_->create_subscription<modelec_interfaces::msg::ActionAscPos>(
"/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<modelec_interfaces::msg::ActionServoPos>(
"/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<modelec_interfaces::msg::ActionRelayState>(
"/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();
}

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

@@ -5,7 +5,7 @@
namespace Modelec
{
BannerMission::BannerMission(const std::shared_ptr<NavigationHelper>& nav,
const std::shared_ptr<ActionExecutor>& action_executor) : step_(GO_TO_FRONT),
const std::shared_ptr<ActionExecutor>& 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<std_msgs::msg::Int64>("/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;