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

up

Browse Source
This commit is contained in:
acki
2025-05-23 14:04:59 -04:00
parent 0789157951
commit daa72726c6
9 changed files with 140 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
TODO.md
View File

@@ -1,2 +1,4 @@
- rework position system with that https://docs.ros2.org/latest/api/geometry_msgs/index-msg.html
- API (check for the middleware already implemented or fill the API package)
- API (check for the middleware already implemented or fill the API package)
- Check every value and after that update the max TIME mission

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

@@ -2,6 +2,7 @@
#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/string.hpp>
#include <std_msgs/msg/empty.hpp>
#include <modelec_interfaces/srv/add_serial_listener.hpp>
#include <modelec_interfaces/msg/action_asc_pos.hpp>
#include <modelec_interfaces/msg/action_relay_state.hpp>
@@ -60,6 +61,14 @@ namespace Modelec
rclcpp::Publisher<modelec_interfaces::msg::ActionServoPos>::SharedPtr servo_move_res_pub_;
rclcpp::Publisher<modelec_interfaces::msg::ActionRelayState>::SharedPtr relay_move_res_pub_;
rclcpp::Publisher<std_msgs::msg::Empty>::SharedPtr tir_start_pub_;
rclcpp::Publisher<std_msgs::msg::Empty>::SharedPtr tir_arm_pub_;
rclcpp::Publisher<std_msgs::msg::Empty>::SharedPtr tir_disarm_pub_;
rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr tir_start_sub_;
rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr tir_arm_sub_;
rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr tir_disarm_sub_;
bool isOk = false;
public:
@@ -70,5 +79,6 @@ namespace Modelec
void GetData(const std::string& elem, const std::vector<std::string>& data = {}) const;
void SendOrder(const std::string& elem, const std::vector<std::string>& data = {}) const;
void SendMove(const std::string& elem, const std::vector<std::string>& data = {}) const;
void RespondEvent(const std::string& elem, const std::vector<std::string>& data = {}) const;
};
}

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

@@ -156,6 +156,36 @@ namespace Modelec
relay_move_res_pub_ = this->create_publisher<modelec_interfaces::msg::ActionRelayState>(
"action/move/relay/res", 10);
tir_start_pub_ = this->create_publisher<std_msgs::msg::Empty>(
"action/tir/start", 10);
tir_start_sub_ = this->create_subscription<std_msgs::msg::Empty>(
"action/tir/start/res", 10,
[this](const std_msgs::msg::Empty::SharedPtr)
{
RespondEvent("TIR", {"START"});
});
tir_arm_pub_ = this->create_publisher<std_msgs::msg::Empty>(
"action/tir/arm", 10);
tir_arm_sub_ = this->create_subscription<std_msgs::msg::Empty>(
"action/tir/arm/res", 10,
[this](const std_msgs::msg::Empty::SharedPtr)
{
RespondEvent("TIR", {"ARM"});
});
tir_disarm_pub_ = this->create_publisher<std_msgs::msg::Empty>(
"action/tir/disarm", 10);
tir_disarm_sub_ = this->create_subscription<std_msgs::msg::Empty>(
"action/tir/disarm/res", 10,
[this](const std_msgs::msg::Empty::SharedPtr)
{
RespondEvent("TIR", {"DIS"});
});
// TODO : check for real value there
asc_value_mapper_ = {
@@ -203,7 +233,6 @@ namespace Modelec
}
relay_value_ = {
{0, false},
{1, false},
{2, false},
};
@@ -383,6 +412,27 @@ namespace Modelec
relay_move_res_pub_->publish(relay_msg);
}
}
else if (tokens[0] == "EVENT")
{
if (tokens[1] == "TIR")
{
if (tokens[2] == "START")
{
tir_start_pub_->publish(std_msgs::msg::Empty());
RespondEvent(tokens[1], {tokens[2]});
}
else if (tokens[2] == "ARM")
{
tir_arm_pub_->publish(std_msgs::msg::Empty());
RespondEvent(tokens[1], {tokens[2]});
}
else if (tokens[2] == "DIS")
{
tir_disarm_pub_->publish(std_msgs::msg::Empty());
RespondEvent(tokens[1], {tokens[2]});
}
}
}
else
{
RCLCPP_WARN(this->get_logger(), "Unknown message format");
@@ -427,6 +477,11 @@ namespace Modelec
{
SendToPCB("MOV", elem, data);
}
void PCBActionInterface::RespondEvent(const std::string& elem, const std::vector<std::string>& data) const
{
SendToPCB("ACK", elem, data);
}
}
int main(int argc, char** argv)

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

@@ -56,6 +56,8 @@ namespace Modelec
void PlacePot();
void ReInit();
protected:
/*
rclcpp::Publisher<modelec_interfaces::msg::ActionAscPos>::SharedPtr asc_get_pub_;

2
src/modelec_strat/include/modelec_strat/navigation_helper.hpp
View File

@@ -26,6 +26,8 @@ namespace Modelec
NavigationHelper(const rclcpp::Node::SharedPtr& node);
void ReInit();
rclcpp::Node::SharedPtr GetNode() const;
std::shared_ptr<Pathfinding> GetPathfinding() const;

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

@@ -42,8 +42,12 @@ namespace Modelec
void Init();
void ReInit();
void Reset();
void ResetStrat();
protected:
void Transition(State next, const std::string& reason);
@@ -61,12 +65,13 @@ namespace Modelec
std::shared_ptr<NavigationHelper> nav_;
std::shared_ptr<ActionExecutor> action_executor_;
rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr tirette_sub_;
rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr tir_sub_;
rclcpp::Publisher<modelec_interfaces::msg::StratState>::SharedPtr state_pub_;
rclcpp::Publisher<std_msgs::msg::Int64>::SharedPtr start_time_pub_;
rclcpp::Subscription<std_msgs::msg::Int8>::SharedPtr team_id_sub_;
rclcpp::Subscription<modelec_interfaces::msg::Spawn>::SharedPtr spawn_id_sub_;
rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr reset_strat_sub_;
rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr tir_arm_sub_;
rclcpp::Client<modelec_interfaces::srv::OdometryStart>::SharedPtr client_start_;
};

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

@@ -120,12 +120,12 @@ namespace Modelec
{
modelec_interfaces::msg::ActionRelayState relay_top_msg;
relay_top_msg.state = true; // TODO : check that
relay_top_msg.id = 0; // TODO : to define
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; // TODO : to define
relay_bottom_msg.id = 1;
relay_move_pub_->publish(relay_bottom_msg);
modelec_interfaces::msg::ActionServoPos first_pot_msg;
@@ -224,12 +224,12 @@ namespace Modelec
{
modelec_interfaces::msg::ActionRelayState relay_top_msg;
relay_top_msg.state = false; // TODO : check that
relay_top_msg.id = 0; // TODO : to define
relay_top_msg.id = 2;
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_bottom_msg.id = 1;
relay_move_pub_->publish(relay_bottom_msg);
modelec_interfaces::msg::ActionServoPos first_pot_msg;
@@ -324,4 +324,15 @@ namespace Modelec
Update();
}
}
void ActionExecutor::ReInit()
{
action_done_ = true;
step_running_ = 0;
while (!step_.empty())
{
step_.pop();
}
action_ = NONE;
}
}

8
src/modelec_strat/src/navigation_helper.cpp
View File

@@ -95,6 +95,11 @@ namespace Modelec
});
}
void NavigationHelper::ReInit()
{
SetPos(spawn_);
}
rclcpp::Node::SharedPtr NavigationHelper::GetNode() const
{
return node_;
@@ -584,12 +589,15 @@ namespace Modelec
{
case YELLOW:
SetPos(spawn_yellow_[name]);
spawn_ = spawn_yellow_[name];
break;
case BLUE:
SetPos(spawn_blue_[name]);
spawn_ = spawn_blue_[name];
break;
default:
SetPos(spawn_yellow_[name]);
spawn_ = spawn_yellow_[name];
break;
}
}

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

@@ -7,10 +7,12 @@ namespace Modelec
StratFMS::StratFMS() : Node("start_fms")
{
tirette_sub_ = create_subscription<std_msgs::msg::Bool>(
"/tirette", 10, [this](const std_msgs::msg::Bool::SharedPtr msg)
tir_sub_ = create_subscription<std_msgs::msg::Empty>(
"/action/tir/start", 10, [this](const std_msgs::msg::Empty::SharedPtr)
{
if (setup_ && !started_ && msg->data)
RCLCPP_INFO(get_logger(), "TIR started");
if (setup_ && !started_)
{
started_ = true;
}
@@ -30,7 +32,6 @@ namespace Modelec
spawn_id_sub_ = create_subscription<modelec_interfaces::msg::Spawn>(
"/strat/spawn", 10, [this](const modelec_interfaces::msg::Spawn::SharedPtr msg)
{
setup_ = true;
team_id_ = msg->team_id;
nav_->SetTeamId(team_id_);
nav_->SetSpawn(msg->name);
@@ -43,6 +44,13 @@ namespace Modelec
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;
});
client_start_ = create_client<modelec_interfaces::srv::OdometryStart>("/odometry/start");
while (!client_start_->wait_for_service(std::chrono::seconds(1))) {
if (!rclcpp::ok()) {
@@ -74,10 +82,30 @@ namespace Modelec
action_executor_ = std::make_unique<ActionExecutor>(shared_from_this());
RCLCPP_INFO(this->get_logger(), "StratFMS fully initialized");
}
void StratFMS::ReInit()
{
nav_->ReInit();
action_executor_->ReInit();
setup_ = false;
}
void StratFMS::Reset()
{
ReInit();
ResetStrat();
}
void StratFMS::ResetStrat()
{
if (timer_)
{
timer_->cancel();
}
state_ = State::INIT;
started_ = false;
setup_ = false;
current_mission_.reset();
match_start_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME);
@@ -87,13 +115,6 @@ namespace Modelec
});
}
void StratFMS::Reset()
{
timer_->cancel();
Init();
}
void StratFMS::Transition(State next, const std::string& reason)
{
RCLCPP_INFO(get_logger(), "Transition %d -> %d: %s", static_cast<int>(state_), static_cast<int>(next),
@@ -112,8 +133,10 @@ namespace Modelec
switch (state_)
{
case State::INIT:
RCLCPP_INFO_ONCE(get_logger(), "State: INIT");
Transition(State::WAIT_START, "System ready");
if (setup_)
{
Transition(State::WAIT_START, "System ready");
}
break;
case State::WAIT_START:
if (started_)