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

can reset gui (this is not 100% safe so only for test)

Browse Source
This commit is contained in:
acki
2025-05-07 06:41:21 -04:00
parent 0ca87e65a0
commit b852b95177
12 changed files with 103 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
src/modelec_com/src/pcb_odo_interface.cpp
View File

@@ -274,6 +274,10 @@ namespace Modelec
bool success = true;
ResolveSetPIDRequest(success);
}
else if (tokens[1] == "POS")
{
// position set
}
else
{
RCLCPP_INFO(this->get_logger(), "PCB response: %s", msg->data.c_str());

6
src/modelec_gui/include/modelec_gui/pages/home_page.hpp
View File

@@ -8,6 +8,7 @@
#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/int8.hpp>
#include <std_msgs/msg/empty.hpp>
namespace ModelecGUI
{
@@ -18,6 +19,8 @@ namespace ModelecGUI
public:
HomePage(rclcpp::Node::SharedPtr node, QWidget* parent = nullptr);
void Init();
public slots:
void onYellowButtonClicked();
@@ -31,7 +34,8 @@ namespace ModelecGUI
rclcpp::Node::SharedPtr node_;
rclcpp::Publisher<std_msgs::msg::Int8>::SharedPtr team_publisher_;
rclcpp::Publisher<std_msgs::msg::Int8>::SharedPtr team_pub_;
rclcpp::Publisher<std_msgs::msg::Empty>::SharedPtr reset_strat_pub_;
QVBoxLayout* v_layout_;
QHBoxLayout* h_layout_;

6
src/modelec_gui/include/modelec_gui/pages/map_page.hpp
View File

@@ -33,6 +33,10 @@ namespace ModelecGUI {
rclcpp::Node::SharedPtr get_node() const { return node_; }
void AskMap();
void Reset();
protected:
void paintEvent(QPaintEvent*) override;
@@ -42,6 +46,8 @@ namespace ModelecGUI {
void resizeEvent(QResizeEvent* event) override;
rclcpp::TimerBase::SharedPtr reset_timer_;
QSvgRenderer* renderer;
QVBoxLayout* v_layout;

2
src/modelec_gui/include/modelec_gui/pages/test_map_page.hpp
View File

@@ -39,6 +39,8 @@ namespace ModelecGUI {
void toggleShowObstacle();
void AskMap();
protected:
void paintEvent(QPaintEvent*) override;

2
src/modelec_gui/src/modelec_gui.cpp
View File

@@ -61,6 +61,8 @@ namespace ModelecGUI {
connect(home_action_, &QAction::triggered, this, [this]() {
stackedWidget->setCurrentIndex(0);
home_page_->Init();
map_page_->Reset();
});
connect(odo_action_, &QAction::triggered, this, [this]() {

13
src/modelec_gui/src/pages/home_page.cpp
View File

@@ -33,17 +33,24 @@ namespace ModelecGUI
setLayout(v_layout_);
team_publisher_ = node_->create_publisher<std_msgs::msg::Int8>("/strat/team", 10);
team_pub_ = node_->create_publisher<std_msgs::msg::Int8>("/strat/team", 10);
reset_strat_pub_ = node_->create_publisher<std_msgs::msg::Empty>("/strat/reset", 10);
connect(yellow_button_, &QPushButton::clicked, this, &HomePage::onYellowButtonClicked);
connect(blue_button_, &QPushButton::clicked, this, &HomePage::onBlueButtonClicked);
}
void HomePage::Init()
{
reset_strat_pub_->publish(std_msgs::msg::Empty());
}
void HomePage::onYellowButtonClicked()
{
std_msgs::msg::Int8 msg;
msg.data = 0;
team_publisher_->publish(msg);
team_pub_->publish(msg);
emit TeamChoose();
}
@@ -52,7 +59,7 @@ namespace ModelecGUI
{
std_msgs::msg::Int8 msg;
msg.data = 1;
team_publisher_->publish(msg);
team_pub_->publish(msg);
emit TeamChoose();
}

33
src/modelec_gui/src/pages/map_page.cpp
View File

@@ -146,6 +146,37 @@ namespace ModelecGUI
rclcpp::spin_until_future_complete(node_->get_node_base_interface(), result2);
}
void MapPage::AskMap()
{
reset_timer_ = node_->create_wall_timer(
std::chrono::seconds(1),
[this]() {
ask_map_obstacle_client_ = node_->create_client<std_srvs::srv::Empty>("nav/ask_map_obstacle");
while (!ask_map_obstacle_client_->wait_for_service(std::chrono::seconds(1))) {
if (!rclcpp::ok()) {
RCLCPP_ERROR(node_->get_logger(), "Interrupted while waiting for the service. Exiting.");
return;
}
RCLCPP_INFO(node_->get_logger(), "Waiting for the service...");
}
ask_map_obstacle_client_->async_send_request(std::make_shared<std_srvs::srv::Empty::Request>());
reset_timer_->cancel();
});
}
void MapPage::Reset()
{
isGameStarted_ = false;
lastWapointWasEnd = true;
qpoints.clear();
AskMap();
}
void MapPage::paintEvent(QPaintEvent* paint_event)
{
QWidget::paintEvent(paint_event);
@@ -226,7 +257,7 @@ namespace ModelecGUI
void MapPage::OnObstacleReceived(const modelec_interfaces::msg::Obstacle::SharedPtr msg)
{
obstacle_.emplace(msg->id, *msg);
obstacle_[msg->id] = *msg;
}
void MapPage::resizeEvent(QResizeEvent* event)

5
src/modelec_gui/src/pages/test_map_page.cpp
View File

@@ -170,6 +170,11 @@ namespace ModelecGUI
show_obstacle_ = !show_obstacle_;
}
void TestMapPage::AskMap()
{
ask_map_obstacle_client_->async_send_request(std::make_shared<std_srvs::srv::Empty::Request>());
}
void TestMapPage::paintEvent(QPaintEvent* paint_event)
{
QWidget::paintEvent(paint_event);

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

@@ -10,6 +10,7 @@
#include <tinyxml2.h>
#include <std_msgs/msg/int64.hpp>
#include <std_msgs/msg/empty.hpp>
namespace Modelec {
@@ -40,6 +41,7 @@ namespace Modelec {
rclcpp::Publisher<modelec_interfaces::msg::Obstacle>::SharedPtr add_obs_pub_;
rclcpp::Publisher<modelec_interfaces::msg::Obstacle>::SharedPtr remove_obs_pub_;
rclcpp::Publisher<std_msgs::msg::Int64>::SharedPtr score_pub_;
rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr reset_strat_sub_;
};
}

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

@@ -17,6 +17,7 @@
#include <modelec_interfaces/msg/strat_state.hpp>
#include <modelec_interfaces/srv/odometry_start.hpp>
#include <std_msgs/msg/empty.hpp>
namespace Modelec
@@ -41,6 +42,8 @@ namespace Modelec
void Init();
void Reset();
protected:
void transition(State next, const std::string& reason);
@@ -51,6 +54,7 @@ namespace Modelec
rclcpp::TimerBase::SharedPtr timer_;
rclcpp::Time match_start_time_;
bool started_ = false;
bool setup_ = false;
std::unique_ptr<Mission> current_mission_;
int team_id_ = 0;
@@ -62,6 +66,7 @@ namespace Modelec
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<std_msgs::msg::Empty>::SharedPtr reset_strat_sub_;
rclcpp::Client<modelec_interfaces::srv::OdometryStart>::SharedPtr client_start_;
};

7
src/modelec_strat/src/pami_manager.cpp
View File

@@ -73,6 +73,13 @@ namespace Modelec
"obstacle/remove", 10);
score_pub_ = create_publisher<std_msgs::msg::Int64>("/strat/score", 10);
reset_strat_sub_ = create_subscription<std_msgs::msg::Empty>(
"/strat/reset", 10, [this](const std_msgs::msg::Empty::SharedPtr)
{
timer_add_->cancel();
timer_remove_->cancel();
});
}
bool PamiManger::ReadFromXML(const std::string& filename)

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

@@ -5,12 +5,12 @@
namespace Modelec
{
StratFMS::StratFMS() : Node("start_fms"), state_(State::INIT)
StratFMS::StratFMS() : Node("start_fms")
{
tirette_sub_ = create_subscription<std_msgs::msg::Bool>(
"/tirette", 10, [this](const std_msgs::msg::Bool::SharedPtr msg)
{
if (!started_ && msg->data)
if (setup_ && !started_ && msg->data)
{
started_ = true;
}
@@ -23,11 +23,19 @@ namespace Modelec
team_id_sub_ = create_subscription<std_msgs::msg::Int8>(
"/strat/team", 10, [this](const std_msgs::msg::Int8::SharedPtr msg)
{
setup_ = true;
team_id_ = static_cast<int>(msg->data);
nav_->SetTeamId(team_id_);
nav_->SetSpawn();
});
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();
});
client_start_ = create_client<modelec_interfaces::srv::OdometryStart>("/odometry/start");
while (!client_start_->wait_for_service(std::chrono::seconds(1))) {
if (!rclcpp::ok()) {
@@ -61,12 +69,25 @@ namespace Modelec
RCLCPP_INFO(this->get_logger(), "StratFMS fully initialized");
state_ = State::INIT;
started_ = false;
setup_ = false;
current_mission_.reset();
match_start_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME);
timer_ = create_wall_timer(std::chrono::milliseconds(100), [this]
{
update();
});
}
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),
@@ -88,7 +109,6 @@ namespace Modelec
RCLCPP_INFO_ONCE(get_logger(), "State: INIT");
transition(State::WAIT_START, "System ready");
break;
case State::WAIT_START:
if (started_)
{