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

fix + GUI test

Browse Source
This commit is contained in:
acki
2025-05-08 11:29:21 -04:00
parent 8060e518c1
commit e893a7a1ce
13 changed files with 56 additions and 123 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
src/modelec_gui/include/modelec_gui/pages/odo_page.hpp
View File

@@ -42,14 +42,10 @@ namespace ModelecGUI {
QLineEdit *xSpeedBox_, *ySpeedBox_, *thetaSpeedBox_;
QHBoxLayout *speedLayout_;
QPushButton *startTest_;
rclcpp::Node::SharedPtr node_;
rclcpp::Subscription<modelec_interfaces::msg::OdometryPos>::SharedPtr sub_;
rclcpp::Publisher<modelec_interfaces::msg::OdometryAddWaypoint>::SharedPtr pub_add_waypoint_;
// client
rclcpp::Client<modelec_interfaces::srv::OdometrySpeed>::SharedPtr client_;
rclcpp::Client<modelec_interfaces::srv::OdometryStart>::SharedPtr client_start_;

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

@@ -14,6 +14,7 @@
#include <modelec_interfaces/msg/odometry_add_waypoint.hpp>
#include <modelec_interfaces/msg/odometry_pos.hpp>
#include <modelec_interfaces/msg/odometry_go_to.hpp>
#include <modelec_interfaces/srv/map.hpp>
#include <modelec_interfaces/srv/map_size.hpp>
#include <modelec_interfaces/msg/obstacle.hpp>
@@ -54,14 +55,6 @@ namespace ModelecGUI {
QSvgRenderer* renderer;
QVBoxLayout* v_layout;
QHBoxLayout* h_layout;
QPushButton* tirette_button_;
QLabel* timer_label_;
QLabel* score_label_;
modelec_interfaces::msg::OdometryPos robotPos;
std::vector<QPoint> qpoints;
//std::vector<modelec_interfaces::msg::OdometryAddWaypoint> points;
@@ -85,12 +78,10 @@ namespace ModelecGUI {
rclcpp::Node::SharedPtr node_;
rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr tirette_pub_;
rclcpp::Subscription<modelec_interfaces::msg::OdometryAddWaypoint>::SharedPtr add_waypoint_sub_;
rclcpp::Subscription<modelec_interfaces::msg::OdometryPos>::SharedPtr odometry_sub_;
rclcpp::Publisher<modelec_interfaces::msg::OdometryPos>::SharedPtr go_to_pub_;
rclcpp::Publisher<modelec_interfaces::msg::OdometryGoTo>::SharedPtr go_to_pub_;
rclcpp::Client<modelec_interfaces::srv::MapSize>::SharedPtr map_client_;
rclcpp::Subscription<modelec_interfaces::msg::Obstacle>::SharedPtr obstacle_added_sub_;
rclcpp::Subscription<modelec_interfaces::msg::Obstacle>::SharedPtr obstacle_removed_sub_;
@@ -100,11 +91,5 @@ namespace ModelecGUI {
bool hasEnemy = false;
rclcpp::Publisher<modelec_interfaces::msg::OdometryPos>::SharedPtr enemy_pos_pub_;
rclcpp::Subscription<modelec_interfaces::msg::OdometryPos>::SharedPtr enemy_pos_sub_;
rclcpp::Subscription<std_msgs::msg::Int64>::SharedPtr strat_start_sub_;
bool isGameStarted_ = false;
long int start_time_ = 0;
rclcpp::Subscription<modelec_interfaces::msg::StratState>::SharedPtr strat_state_sub_;
};
}

25
src/modelec_gui/src/pages/odo_page.cpp
View File

@@ -138,27 +138,6 @@ namespace ModelecGUI
speedLayout_->addWidget(ySpeedBox_);
speedLayout_->addWidget(thetaSpeedBox_);
startTest_ = new QPushButton("Start Test");
connect(startTest_, &QPushButton::clicked, this, [this]() {
auto firstRequest = std::make_shared<modelec_interfaces::msg::OdometryAddWaypoint>();
firstRequest->id = 0;
firstRequest->is_end = false;
firstRequest->x = 100.0;
firstRequest->y = 0.0;
firstRequest->theta = 0.0;
pub_add_waypoint_->publish(*firstRequest);
auto secondRequest = std::make_shared<modelec_interfaces::msg::OdometryAddWaypoint>();
secondRequest->id = 1;
secondRequest->is_end = true;
secondRequest->x = 0.0;
secondRequest->y = 0.0;
secondRequest->theta = 0.0;
pub_add_waypoint_->publish(*secondRequest);
});
mainLayout_ = new QVBoxLayout(this);
mainLayout_->addWidget(startButton_);
mainLayout_->addLayout(posLayout_);
@@ -167,7 +146,6 @@ namespace ModelecGUI
mainLayout_->addWidget(setPID_);
mainLayout_->addWidget(askSpeed_);
mainLayout_->addLayout(speedLayout_);
mainLayout_->addWidget(startTest_);
setLayout(mainLayout_);
// Set up subscription
@@ -175,9 +153,6 @@ namespace ModelecGUI
"/odometry/position", 10,
std::bind(&OdoPage::PositionCallback, this, std::placeholders::_1));
pub_add_waypoint_ = node_->create_publisher<modelec_interfaces::msg::OdometryAddWaypoint>(
"/odometry/add_waypoint", 10);
client_ = node_->create_client<modelec_interfaces::srv::OdometrySpeed>("odometry/speed");
while (!client_->wait_for_service(std::chrono::seconds(1))) {
if (!rclcpp::ok()) {

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

@@ -13,31 +13,6 @@ namespace ModelecGUI
ratioBetweenMapAndWidgetX_ = width() / 3000.0f;
ratioBetweenMapAndWidgetY_ = height() / 2000.0f;
v_layout = new QVBoxLayout(this);
tirette_button_ = new QPushButton("Tirette", this);
timer_label_ = new QLabel("00", this);
timer_label_->setAlignment(Qt::AlignCenter);
timer_label_->setFont(QFont("Arial", 24));
timer_label_->setStyleSheet("QLabel { color: white; }");
score_label_ = new QLabel("Score: 0", this);
score_label_->setAlignment(Qt::AlignCenter);
score_label_->setFont(QFont("Arial", 24));
score_label_->setStyleSheet("QLabel { color: white; }");
h_layout = new QHBoxLayout(this);
h_layout->addStretch();
h_layout->addWidget(score_label_);
h_layout->addWidget(tirette_button_);
h_layout->addWidget(timer_label_);
h_layout->addStretch();
v_layout->addLayout(h_layout);
v_layout->addStretch();
this->setLayout(v_layout);
qpoints = {};
robot_length_ = Modelec::Config::get<int>("config.robot.size.length_mm", 200);
@@ -81,7 +56,7 @@ namespace ModelecGUI
obstacle_.erase(msg->id);
});
go_to_pub_ = node_->create_publisher<modelec_interfaces::msg::OdometryPos>("nav/go_to", 10);
go_to_pub_ = node_->create_publisher<modelec_interfaces::msg::OdometryGoTo>("nav/go_to", 10);
enemy_pos_pub_ = node_->create_publisher<modelec_interfaces::msg::OdometryPos>("enemy/position", 10);
@@ -94,30 +69,6 @@ namespace ModelecGUI
update();
});
tirette_pub_ = node_->create_publisher<std_msgs::msg::Bool>("tirette", 10);
strat_start_sub_ = node_->create_subscription<std_msgs::msg::Int64>("/strat/start_time", 10,
[this](const std_msgs::msg::Int64::SharedPtr msg){
isGameStarted_ = true;
start_time_ = msg->data;
});
strat_state_sub_ = node_->create_subscription<modelec_interfaces::msg::StratState>("/strat/state", 10,
[this](const modelec_interfaces::msg::StratState::SharedPtr msg){
if (msg->state == modelec_interfaces::msg::StratState::STOP)
{
RCLCPP_INFO(node_->get_logger(), "Game stop");
isGameStarted_ = false;
}
});
connect(tirette_button_, &QPushButton::clicked, this, [this]() {
std_msgs::msg::Bool msg;
msg.data = true;
tirette_pub_->publish(msg);
});
// client to nav/map
map_client_ = node_->create_client<modelec_interfaces::srv::MapSize>("nav/map_size");
while (!map_client_->wait_for_service(std::chrono::seconds(1))) {
@@ -179,15 +130,6 @@ namespace ModelecGUI
{
QWidget::paintEvent(paint_event);
if (isGameStarted_)
{
auto now = std::chrono::system_clock::now().time_since_epoch();
auto start = std::chrono::nanoseconds(start_time_);
auto elapsed = now - start;
auto elapsed_s = std::chrono::duration_cast<std::chrono::seconds>(elapsed).count();
timer_label_->setText(QString::number(elapsed_s));
}
QPainter painter(this);
renderer->render(&painter, rect()); // Scales SVG to widget size
painter.save();
@@ -259,10 +201,19 @@ namespace ModelecGUI
if (event->button() == Qt::LeftButton)
{
modelec_interfaces::msg::OdometryPos msg;
modelec_interfaces::msg::OdometryGoTo msg;
msg.x = Modelec::mapValue(event->pos().x(), 0, width(), 0, 3000);
msg.y = 2000 - Modelec::mapValue(event->pos().y(), 0, height(), 0, 2000);
msg.theta = 0;
msg.close = true;
if (show_obstacle_)
{
msg.mask = modelec_interfaces::msg::OdometryGoTo::FREE | modelec_interfaces::msg::OdometryGoTo::WALL;
}
else
{
msg.mask = modelec_interfaces::msg::OdometryGoTo::FREE | modelec_interfaces::msg::OdometryGoTo::WALL | modelec_interfaces::msg::OdometryGoTo::OBSTACLE | modelec_interfaces::msg::OdometryGoTo::ENEMY;
}
go_to_pub_->publish(msg);
}

1
src/modelec_interfaces/CMakeLists.txt
View File

@@ -13,6 +13,7 @@ find_package(rosidl_default_generators REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME}
"msg/Alim/AlimEmg.msg"
"msg/Odometry/OdometryPos.msg"
"msg/Odometry/OdometryGoTo.msg"
"msg/Odometry/OdometrySpeed.msg"
"msg/Odometry/OdometryToF.msg"
"msg/Odometry/OdometryWaypointReach.msg"

4
src/modelec_interfaces/msg/Odometry/OdometryAddWaypoint.msg
View File

@@ -1,5 +1,5 @@
uint8 id
bool is_end
int64 x
int64 y
int32 x
int32 y
float64 theta

10
src/modelec_interfaces/msg/Odometry/OdometryGoTo.msg Normal file
View File

@@ -0,0 +1,10 @@
int8 FREE=1
int8 WALL=2
int8 OBSTACLE=4
int8 ENEMY=8
int32 x
int32 y
float64 theta
bool close
int8 mask 1

4
src/modelec_interfaces/msg/Odometry/OdometryPos.msg
View File

@@ -1,3 +1,3 @@
int64 x
int64 y
int32 x
int32 y
float64 theta

4
src/modelec_interfaces/msg/Odometry/OdometrySpeed.msg
View File

@@ -1,3 +1,3 @@
int64 x
int64 y
int32 x
int32 y
int64 theta

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

@@ -4,6 +4,7 @@
#include <modelec_interfaces/msg/odometry_add_waypoint.hpp>
#include <modelec_interfaces/msg/odometry_waypoint_reach.hpp>
#include <modelec_interfaces/msg/odometry_pos.hpp>
#include <modelec_interfaces/msg/odometry_go_to.hpp>
#include "deposite_zone.hpp"
#include "pathfinding.hpp"
@@ -123,7 +124,7 @@ namespace Modelec {
rclcpp::Subscription<WaypointReachMsg>::SharedPtr waypoint_reach_sub_;
rclcpp::Publisher<WaypointMsg>::SharedPtr waypoint_pub_;
rclcpp::Subscription<PosMsg>::SharedPtr go_to_sub_;
rclcpp::Subscription<modelec_interfaces::msg::OdometryGoTo>::SharedPtr go_to_sub_;
rclcpp::Subscription<PosMsg>::SharedPtr pos_sub_;
rclcpp::Publisher<PosMsg>::SharedPtr pos_pub_;

4
src/modelec_strat/src/enemy_manager.cpp
View File

@@ -144,7 +144,7 @@ namespace Modelec
last_publish_time_ = this->now();
last_movement_time_ = this->now(); // Mise à jour du dernier vrai mouvement
enemy_pos_pub_->publish(enemy_pos);
RCLCPP_INFO(this->get_logger(), "Enemy moved: x=%ld, y=%ld", enemy_pos.x, enemy_pos.y);
RCLCPP_INFO(this->get_logger(), "Enemy moved: x=%d, y=%d", enemy_pos.x, enemy_pos.y);
}
else
{
@@ -152,7 +152,7 @@ namespace Modelec
if ((now - last_movement_time_).seconds() > max_stationary_time_s_)
{
enemy_long_time_pub_->publish(last_enemy_pos_);
RCLCPP_WARN(this->get_logger(), "Enemy has been stationary for too long at x=%ld y=%ld", last_enemy_pos_.x, last_enemy_pos_.y);
RCLCPP_WARN(this->get_logger(), "Enemy has been stationary for too long at x=%d y=%d", last_enemy_pos_.x, last_enemy_pos_.y);
last_movement_time_ = now;
}

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

@@ -39,9 +39,9 @@ namespace Modelec {
pos_pub_ = node_->create_publisher<PosMsg>("odometry/set_position", 10);
go_to_sub_ = node_->create_subscription<PosMsg>(
"nav/go_to", 10, [this](const PosMsg::SharedPtr msg) {
GoTo(msg, false, Pathfinding::FREE | Pathfinding::WALL);
go_to_sub_ = node_->create_subscription<modelec_interfaces::msg::OdometryGoTo>(
"nav/go_to", 10, [this](const modelec_interfaces::msg::OdometryGoTo::SharedPtr msg) {
GoTo(msg->x, msg->y, msg->theta, msg->close, msg->mask);
});
enemy_pos_sub_ = node_->create_subscription<modelec_interfaces::msg::OdometryPos>(
@@ -478,14 +478,28 @@ namespace Modelec {
bool NavigationHelper::EnemyOnPath(const modelec_interfaces::msg::OdometryPos msg)
{
for (size_t i = -1; i + 1 < waypoints_.size(); ++i)
auto curr = Waypoint(*current_pos_, -1, false);
std::vector<Waypoint> waypointsList;
waypointsList.push_back(curr);
for (auto& waypoint : waypoints_)
{
auto wp = i == -1 ? Waypoint(*current_pos_, -1, false) : waypoints_[i];
auto next_wp = waypoints_[i + 1];
if (waypoint.reached)
{
continue;
}
waypointsList.push_back(waypoint);
}
for (size_t i = 0; i + 1 < waypointsList.size(); ++i)
{
auto wp = waypointsList[i];
auto next_wp = waypointsList[i + 1];
if (DoesLineIntersectCircle(
Point(wp.x, wp.y, wp.theta),
Point(next_wp.x, next_wp.y, next_wp.theta),
Point(msg.x, msg.y, msg.theta), (pathfinding_->enemy_length_mm_ + pathfinding_->robot_length_mm_ + pathfinding_->enemy_margin_mm_) / 2.0f))
Point(msg.x, msg.y, msg.theta),
(pathfinding_->enemy_length_mm_ + pathfinding_->robot_length_mm_ + pathfinding_->enemy_margin_mm_) / 2.0f))
{
return true;
}

2
src/modelec_utils/src/point.cpp
View File

@@ -24,7 +24,7 @@ namespace Modelec
Point Point::GetTakeBasePosition() const
{
return GetTakePosition(350, theta);
return GetTakePosition(320, theta);
}
Point Point::GetTakeClosePosition() const