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

STRAT v2 :

Browse Source 
- début de strat réel
This commit is contained in:
acki
2025-04-30 22:42:56 -04:00
parent 29b7d23977
commit 913286bd2d
27 changed files with 793 additions and 142 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
src/modelec_core/CMakeLists.txt
View File

@@ -26,14 +26,6 @@ target_include_directories(gamecontroller_listener PUBLIC
$<INSTALL_INTERFACE:include>
)
add_executable(move_controller src/move_controller.cpp)
ament_target_dependencies(move_controller rclcpp std_msgs sensor_msgs modelec_interfaces)
target_link_libraries(move_controller modelec_utils::modelec_utils)
target_include_directories(move_controller PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
add_executable(solenoid_controller src/solenoid_controller.cpp)
ament_target_dependencies(solenoid_controller rclcpp modelec_interfaces)
target_link_libraries(solenoid_controller ${WIRINGPI_LIB} modelec_utils::modelec_utils)
@@ -77,7 +69,6 @@ endif()
# Install targets
install(TARGETS
gamecontroller_listener
move_controller
solenoid_controller
arm_controller
button_gpio_controller

21
src/modelec_core/include/modelec/move_controller.hpp
View File

@@ -1,21 +0,0 @@
#pragma once
#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/string.hpp>
#define REFRESH_RATE 100
namespace Modelec {
class MoveController : public rclcpp::Node {
public:
MoveController();
private:
rclcpp::TimerBase::SharedPtr timer_;
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
float x = 0, y = 0, theta = 0;
void PublishPosition();
};
}

29
src/modelec_core/src/move_controller.cpp
View File

@@ -1,29 +0,0 @@
#include <modelec/move_controller.hpp>
namespace Modelec {
MoveController::MoveController() : Node("move_controller")
{
// Initialize the publisher
publisher_ = this->create_publisher<std_msgs::msg::String>("robot_position", 10);
timer_ = this->create_wall_timer(
std::chrono::milliseconds(REFRESH_RATE),
std::bind(&MoveController::PublishPosition, this)
);
}
void MoveController::PublishPosition()
{
std_msgs::msg::String msg;
msg.data = std::to_string(x) + ";" + std::to_string(y) + ";" + std::to_string(theta);
publisher_->publish(msg);
}
}
int main(int argc, char **argv)
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<Modelec::MoveController>());
rclcpp::shutdown();
return 0;
}

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

@@ -69,5 +69,9 @@ namespace ModelecGUI {
rclcpp::Client<modelec_interfaces::srv::MapSize>::SharedPtr map_client_;
rclcpp::Subscription<modelec_interfaces::msg::Obstacle>::SharedPtr obstacle_sub_;
rclcpp::Client<std_srvs::srv::Empty>::SharedPtr ask_map_obstacle_client_;
modelec_interfaces::msg::OdometryPos enemy_pos_;
rclcpp::Publisher<modelec_interfaces::msg::OdometryPos>::SharedPtr enemy_pos_pub_;
};
}

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

@@ -22,6 +22,10 @@ namespace ModelecGUI
qpoints = {};
points = {};
enemy_pos_.x = 1775;
enemy_pos_.y = 200;
enemy_pos_.theta = 3.1415/2;
add_waypoint_sub_ = node_->create_subscription<modelec_interfaces::msg::OdometryAddWaypoint>("odometry/add_waypoint", 100,
[this](const modelec_interfaces::msg::OdometryAddWaypoint::SharedPtr msg) {
if (lastWapointWasEnd)
@@ -57,6 +61,8 @@ namespace ModelecGUI
go_to_pub_ = node_->create_publisher<modelec_interfaces::msg::OdometryPos>("nav/go_to", 10);
enemy_pos_pub_ = node_->create_publisher<modelec_interfaces::msg::OdometryPos>("enemy/position", 10);
// 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))) {
@@ -123,7 +129,6 @@ namespace ModelecGUI
QPainter painter(this);
renderer->render(&painter, rect()); // Scales SVG to widget size
painter.setRenderHint(QPainter::Antialiasing);
// --- Draw lines ---
@@ -160,6 +165,9 @@ namespace ModelecGUI
painter.setBrush(Qt::black);
painter.drawRect(obs.x * ratioBetweenMapAndWidget, height() - (obs.y + obs.height) * ratioBetweenMapAndWidget, obs.width * ratioBetweenMapAndWidget, obs.height * ratioBetweenMapAndWidget);
}
painter.setBrush(Qt::red);
painter.drawRect((enemy_pos_.x - 150.0f) * ratioBetweenMapAndWidget, height() - (enemy_pos_.y + 150.0f) * ratioBetweenMapAndWidget, 300.0f*ratioBetweenMapAndWidget, 300.0f*ratioBetweenMapAndWidget);
}
}
@@ -167,37 +175,23 @@ namespace ModelecGUI
{
QWidget::mousePressEvent(event);
modelec_interfaces::msg::OdometryPos 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;
go_to_pub_->publish(msg);
/*
qpoints.push_back(event->pos());
modelec_interfaces::msg::OdometryAddWaypoint 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.is_end = false;
msg.id = points.size();
if (points.empty())
if (event->button() == Qt::LeftButton)
{
QPointF vec = QPoint(msg.x, msg.y) - robotPosPoint;
msg.theta = std::atan2(vec.y(), vec.x());
modelec_interfaces::msg::OdometryPos 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;
go_to_pub_->publish(msg);
}
else
else if (event->button() == Qt::RightButton)
{
auto lastPoint = points.back();
QPointF vec = QPoint(msg.x, msg.y) - QPoint(lastPoint.x, lastPoint.y);
msg.theta = std::atan2(vec.y(), vec.x());
enemy_pos_.x = Modelec::mapValue(event->pos().x(), 0, width(), 0, 3000);
enemy_pos_.y = 2000 - Modelec::mapValue(event->pos().y(), 0, height(), 0, 2000);
enemy_pos_.theta = 0;
enemy_pos_pub_->publish(enemy_pos_);
}
points.push_back(msg);
update();*/
}
void MapPage::OnObstacleReceived(const modelec_interfaces::msg::Obstacle::SharedPtr msg)

1
src/modelec_interfaces/CMakeLists.txt
View File

@@ -19,6 +19,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
"msg/Odometry/OdometryAddWaypoint.msg"
"msg/Odometry/OdometryStart.msg"
"msg/Odometry/PID/OdometryPid.msg"
"msg/Strat/StratState.msg"
"msg/Map/Map.msg"
"msg/Map/Obstacle.msg"
"msg/PCA9685Servo.msg"

12
src/modelec_interfaces/msg/Strat/StratState.msg Normal file
View File

@@ -0,0 +1,12 @@
int32 INIT=0
int32 WAIT_START=1
int32 SELECT_MISSION=2
int32 DO_PREPARE_CONCERT=3
int32 DO_PROMOTION=4
int32 DO_GO_HOME=5
int32 STOP=6
int32 state
string reason

4
src/modelec_sensors/include/modelec_sensors/lidar_controller.hpp
View File

@@ -5,9 +5,9 @@
#include <sensor_msgs/msg/laser_scan.hpp>
namespace Modelec {
class LidarController : public rclcpp::Node {
private:
// TODO - DEPRECATED
class LidarController : public rclcpp::Node {
public:
LidarController();
private:

9
src/modelec_sensors/include/modelec_sensors/tirette_controller.hpp
View File

@@ -16,10 +16,11 @@ namespace Modelec {
TiretteController();
private:
rclcpp::Service<TiretteInterface>::SharedPtr service;
rclcpp::TimerBase::SharedPtr timer;
rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr publisher;
bool tirette_state;
rclcpp::Service<TiretteInterface>::SharedPtr service_;
rclcpp::TimerBase::SharedPtr timer_;
rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr publisher_;
rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr publisher_change_;
int tirette_state_;
void check_tirette(const std::shared_ptr<TiretteInterface::Request> request, std::shared_ptr<TiretteInterface::Response> response);
};

1
src/modelec_sensors/src/lidar_controller.cpp
View File

@@ -1,6 +1,7 @@
#include <modelec_sensors/lidar_controller.hpp>
namespace Modelec {
// TODO - DEPRECATED
LidarController::LidarController() : Node("lidar_controller") {
subscription_ = this->create_subscription<sensor_msgs::msg::LaserScan>(
"scan", 10, [this](const sensor_msgs::msg::LaserScan::SharedPtr msg) {

28
src/modelec_sensors/src/tirette_controller.cpp
View File

@@ -14,33 +14,37 @@ namespace Modelec {
pullUpDnControl(GPIO_TIRETTE, PUD_UP);
tirette_state = false;
tirette_state_ = 0;
// Initialize the service
service = this->create_service<modelec_interfaces::srv::Tirette>(
service_ = this->create_service<modelec_interfaces::srv::Tirette>(
"tirette",
std::bind(&TiretteController::check_tirette, this, std::placeholders::_1, std::placeholders::_2));
// Initialize the publisher
publisher = this->create_publisher<std_msgs::msg::Bool>("tirette_state", 10);
publisher_ = this->create_publisher<std_msgs::msg::Bool>("tirette", 10);
publisher_change_ = this->create_publisher<std_msgs::msg::Bool>("tirette/continue", 10);
// Initialize the timer
timer = this->create_wall_timer(std::chrono::milliseconds(REFRESH_RATE), [this]() {
// TODO : change that to publish in continue (so change the main program)
bool lastState = tirette_state;
tirette_state = digitalRead(GPIO_TIRETTE) == LOW;
timer_ = this->create_wall_timer(std::chrono::milliseconds(REFRESH_RATE), [this]() {
// TODO : CHECK THAT
bool lastState = tirette_state_;
tirette_state_ = digitalRead(GPIO_TIRETTE);
if (lastState == LOW && tirette_state == HIGH) {
auto msg = std_msgs::msg::Bool();
msg.data = tirette_state;
publisher->publish(msg);
auto msg = std_msgs::msg::Bool();
msg.data = tirette_state_;
if (lastState == LOW && tirette_state_ == HIGH) {
publisher_->publish(msg);
}
publisher_change_->publish(msg);
});
}
void TiretteController::check_tirette(const std::shared_ptr<TiretteInterface::Request>, std::shared_ptr<TiretteInterface::Response> response)
{
response->state = tirette_state;
response->state = tirette_state_;
}
}

12
src/modelec_strat/CMakeLists.txt
View File

@@ -7,6 +7,7 @@ endif()
find_package(ament_cmake REQUIRED)
find_package(std_msgs REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(std_srvs REQUIRED)
find_package(tinyxml2 REQUIRED)
find_package(rclcpp REQUIRED)
@@ -14,7 +15,7 @@ find_package(rclcpp REQUIRED)
find_package(modelec_interfaces REQUIRED)
find_package(modelec_utils REQUIRED)
add_executable(strat_fsm src/strat_fms.cpp src/navigation_helper.cpp src/pathfinding.cpp src/action_executor.cpp src/mission_manager.cpp)
add_executable(strat_fsm src/strat_fms.cpp src/navigation_helper.cpp src/pathfinding.cpp src/action_executor.cpp src/mission_manager.cpp src/missions/prepare_concert_mission.cpp src/missions/promotion_mission.cpp src/missions/go_home_mission.cpp)
ament_target_dependencies(strat_fsm rclcpp std_msgs std_srvs modelec_interfaces)
target_link_libraries(strat_fsm modelec_utils::modelec_utils tinyxml2::tinyxml2)
target_include_directories(strat_fsm PUBLIC
@@ -22,6 +23,14 @@ target_include_directories(strat_fsm PUBLIC
$<INSTALL_INTERFACE:include>
)
add_executable(enemy_manager src/enemy_manager.cpp)
ament_target_dependencies(enemy_manager rclcpp std_msgs sensor_msgs std_srvs modelec_interfaces)
target_link_libraries(enemy_manager modelec_utils::modelec_utils)
target_include_directories(enemy_manager PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
set(ament_cmake_copyright_FOUND TRUE)
@@ -31,6 +40,7 @@ endif()
install(TARGETS
strat_fsm
enemy_manager
DESTINATION lib/${PROJECT_NAME}
)

35
src/modelec_strat/include/modelec_strat/enemy_manager.hpp Normal file
View File

@@ -0,0 +1,35 @@
#pragma once
#include <rclcpp/rclcpp.hpp>
#include <modelec_interfaces/msg/odometry_pos.hpp>
#include <sensor_msgs/msg/laser_scan.hpp>
namespace Modelec
{
class EnemyManager : public rclcpp::Node
{
public:
EnemyManager();
protected:
void OnCurrentPos(const modelec_interfaces::msg::OdometryPos::SharedPtr msg);
void OnLidarData(const sensor_msgs::msg::LaserScan::SharedPtr msg);
void TimerCallback();
private:
rclcpp::Subscription<modelec_interfaces::msg::OdometryPos>::SharedPtr current_pos_sub_;
rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr laser_scan_sub_;
rclcpp::Publisher<modelec_interfaces::msg::OdometryPos>::SharedPtr enemy_pos_pub_;
rclcpp::TimerBase::SharedPtr timer_;
modelec_interfaces::msg::OdometryPos current_pos_;
modelec_interfaces::msg::OdometryPos last_enemy_pos_;
bool enemy_initialized_ = false;
rclcpp::Time last_publish_time_;
};
}

24
src/modelec_strat/include/modelec_strat/missions/go_home_mission.hpp Normal file
View File

@@ -0,0 +1,24 @@
#pragma once
#include <modelec_strat/navigation_helper.hpp>
#include "mission_base.hpp"
namespace Modelec
{
class GoHomeMission : public Mission
{
public:
GoHomeMission(const std::shared_ptr<NavigationHelper>& nav);
void start(rclcpp::Node::SharedPtr node) override;
void update() override;
MissionStatus getStatus() const override;
std::string name() const override { return "GoHome"; }
private:
MissionStatus status_;
std::shared_ptr<NavigationHelper> nav_;
rclcpp::Node::SharedPtr node_;
};
}

25
src/modelec_strat/include/modelec_strat/missions/mission_base.hpp Normal file
View File

@@ -0,0 +1,25 @@
#pragma once
#include <rclcpp/rclcpp.hpp>
#include <string>
namespace Modelec
{
enum class MissionStatus
{
READY,
RUNNING,
DONE,
FAILED
};
class Mission
{
public:
virtual ~Mission() = default;
virtual void start(rclcpp::Node::SharedPtr node) = 0;
virtual void update() = 0;
virtual MissionStatus getStatus() const = 0;
virtual std::string name() const = 0;
};
}

31
src/modelec_strat/include/modelec_strat/missions/prepare_concert_mission.hpp Normal file
View File

@@ -0,0 +1,31 @@
#pragma once
#include <modelec_strat/missions/mission_base.hpp>
#include <modelec_strat/navigation_helper.hpp>
namespace Modelec {
class PrepareConcertMission : public Mission {
public:
PrepareConcertMission(const std::shared_ptr<NavigationHelper>& nav);
void start(rclcpp::Node::SharedPtr node) override;
void update() override;
MissionStatus getStatus() const override;
std::string name() const override { return "PrepareConcert"; }
private:
enum Step {
GO_TO_COLUMN,
TAKE_COLUMN,
GO_TO_PLATFORM,
PLACE_PLATFORM,
DONE
} step_;
MissionStatus status_;
std::shared_ptr<NavigationHelper> nav_;
rclcpp::Node::SharedPtr node_;
};
}

33
src/modelec_strat/include/modelec_strat/missions/promotion_mission.hpp Normal file
View File

@@ -0,0 +1,33 @@
#pragma once
#include <modelec_strat/navigation_helper.hpp>
#include "mission_base.hpp"
namespace Modelec
{
/*
* Banderole mission
*
*/
class PromotionMission : public Mission
{
public:
PromotionMission(const std::shared_ptr<NavigationHelper>& nav);
void start(rclcpp::Node::SharedPtr node) override;
void update() override;
MissionStatus getStatus() const override;
std::string name() const override { return "Promotion"; }
enum Step {
GO_TO_FRONT,
DEPLOY_BANNER,
DONE
} step_;
MissionStatus status_;
std::shared_ptr<NavigationHelper> nav_;
rclcpp::Node::SharedPtr node_;
};
}

42
src/modelec_strat/include/modelec_strat/pathfinding.hpp
View File

@@ -34,13 +34,6 @@ namespace Modelec {
WaypointMsg toMsg() const;
};
/*
* - Pathfinding DStar Lite
*
*
*
*/
class Pathfinding {
public:
Pathfinding();
@@ -54,6 +47,10 @@ namespace Modelec {
bool LoadObstaclesFromXML(const std::string& filename);
//void SetStartAndGoal(const PosMsg::SharedPtr& start, const PosMsg::SharedPtr& goal);
void SetCurrentPos(const PosMsg::SharedPtr& pos);
protected:
void HandleMapRequest(
const std::shared_ptr<modelec_interfaces::srv::Map::Request> request,
@@ -67,25 +64,38 @@ namespace Modelec {
const std::shared_ptr<std_srvs::srv::Empty::Request> request,
const std::shared_ptr<std_srvs::srv::Empty::Response> response);
private:
int robot_width_mm_;
int robot_length_mm_;
void OnEnemyPosition(const modelec_interfaces::msg::OdometryPos::SharedPtr msg);
int grid_width_;
int grid_height_;
//bool EnemyOnPath(const modelec_interfaces::msg::OdometryPos& enemy_pos);
//void Replan();
private:
rclcpp::Node::SharedPtr node_;
std::vector<std::vector<int>> grid_;
int grid_width_ = 0;
int grid_height_ = 0;
int map_width_mm_ = 0;
int map_height_mm_ = 0;
float robot_width_mm_ = 0;
float robot_length_mm_ = 0;
std::map<int, modelec_interfaces::msg::Obstacle> obstacle_map_;
rclcpp::Node::SharedPtr node_;
PosMsg::SharedPtr current_start_;
PosMsg::SharedPtr current_goal_;
WaypointListMsg current_waypoints_;
modelec_interfaces::msg::OdometryPos last_enemy_pos_;
bool has_enemy_pos_ = false;
rclcpp::Subscription<modelec_interfaces::msg::OdometryPos>::SharedPtr enemy_pos_sub_;
rclcpp::Publisher<modelec_interfaces::msg::Obstacle>::SharedPtr map_pub_;
rclcpp::Publisher<WaypointMsg>::SharedPtr waypoint_pub_;
rclcpp::Service<modelec_interfaces::srv::Map>::SharedPtr map_srv_;
rclcpp::Service<modelec_interfaces::srv::MapSize>::SharedPtr map_size_srv_;
rclcpp::Service<std_srvs::srv::Empty>::SharedPtr ask_obstacle_srv_;
rclcpp::Publisher<modelec_interfaces::msg::Obstacle>::SharedPtr map_pub_;
};
}

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

@@ -5,9 +5,29 @@
#include "action_executor.hpp"
#include "mission_manager.hpp"
#include "navigation_helper.hpp"
#include "missions/mission_base.hpp"
#include "missions/promotion_mission.hpp"
#include "missions/prepare_concert_mission.hpp"
#include "missions/go_home_mission.hpp"
#include "std_msgs/msg/bool.hpp"
#include "modelec_interfaces/msg/strat_state.hpp"
namespace Modelec
{
enum class State
{
INIT,
WAIT_START,
SELECT_MISSION,
DO_PREPARE_CONCERT,
DO_PROMOTION,
DO_GO_HOME,
STOP
};
/*
* TODO
@@ -15,15 +35,6 @@ namespace Modelec
* - setup des missions
* - gestion des interruptions (robot adverses / obstacles)
* - scoring des missions
*
*
*
* - Banderole
*
*
*
*
* - Gradin
*/
class StratFMS : public rclcpp::Node
@@ -33,12 +44,23 @@ namespace Modelec
void Init();
private:
// rclcpp::TimerBase::SharedPtr timer_;
protected:
void transition(State next, const std::string& reason);
std::unique_ptr<NavigationHelper> nav_;
void update();
private:
State state_;
rclcpp::TimerBase::SharedPtr timer_;
rclcpp::Time match_start_time_;
bool started_ = false;
std::unique_ptr<Mission> current_mission_;
std::shared_ptr<NavigationHelper> nav_;
std::unique_ptr<MissionManager> mission_manager_;
std::unique_ptr<ActionExecutor> action_executor_;
rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr tirette_sub_;
rclcpp::Publisher<modelec_interfaces::msg::StratState>::SharedPtr state_pub_;
};
}

150
src/modelec_strat/src/enemy_manager.cpp Normal file
View File

@@ -0,0 +1,150 @@
#include <modelec_strat/enemy_manager.hpp>
#include <cmath>
namespace Modelec
{
EnemyManager::EnemyManager() : Node("enemy_manager")
{
current_pos_sub_ = this->create_subscription<modelec_interfaces::msg::OdometryPos>(
"odometry/position", 10,
[this](const modelec_interfaces::msg::OdometryPos::SharedPtr msg) {
OnCurrentPos(msg);
});
laser_scan_sub_ = this->create_subscription<sensor_msgs::msg::LaserScan>(
"scan", 10,
[this](const sensor_msgs::msg::LaserScan::SharedPtr msg) {
OnLidarData(msg);
});
enemy_pos_pub_ = this->create_publisher<modelec_interfaces::msg::OdometryPos>(
"enemy/position", 10);
timer_ = this->create_wall_timer(
std::chrono::seconds(1),
std::bind(&EnemyManager::TimerCallback, this)
);
last_publish_time_ = this->now();
}
void EnemyManager::OnCurrentPos(const modelec_interfaces::msg::OdometryPos::SharedPtr msg)
{
current_pos_ = *msg;
}
void EnemyManager::OnLidarData(const sensor_msgs::msg::LaserScan::SharedPtr msg)
{
if (std::isnan(current_pos_.x) || std::isnan(current_pos_.y))
{
RCLCPP_WARN(this->get_logger(), "Current robot position unknown, cannot compute enemy position");
return;
}
const double robot_x = current_pos_.x;
const double robot_y = current_pos_.y;
const double robot_theta = current_pos_.theta;
double angle = msg->angle_min;
double min_distance = std::numeric_limits<double>::max();
double best_x = 0.0;
double best_y = 0.0;
for (size_t i = 0; i < msg->ranges.size(); ++i)
{
float range = msg->ranges[i];
if (std::isnan(range) || range < msg->range_min || range > msg->range_max)
{
angle += msg->angle_increment;
continue;
}
// Convert to local robot frame
double x_local = range * std::cos(angle) * 1000.0; // meters -> mm
double y_local = range * std::sin(angle) * 1000.0; // meters -> mm
// Rotate + translate into global frame
double x_global = robot_x + (x_local * std::cos(robot_theta) - y_local * std::sin(robot_theta));
double y_global = robot_y + (x_local * std::sin(robot_theta) + y_local * std::cos(robot_theta));
// Ignore points outside of the table
if (x_global < 0 || x_global > 3000 || y_global < 0 || y_global > 2000)
{
angle += msg->angle_increment;
continue;
}
if (range < min_distance)
{
min_distance = range;
best_x = x_global;
best_y = y_global;
}
angle += msg->angle_increment;
}
if (min_distance < std::numeric_limits<double>::max())
{
modelec_interfaces::msg::OdometryPos enemy_pos;
enemy_pos.x = best_x;
enemy_pos.y = best_y;
enemy_pos.theta = 0.0;
bool need_publish = false;
if (!enemy_initialized_)
{
need_publish = true;
enemy_initialized_ = true;
}
else
{
float dx = enemy_pos.x - last_enemy_pos_.x;
float dy = enemy_pos.y - last_enemy_pos_.y;
float distance_squared = dx * dx + dy * dy;
constexpr float min_move_threshold_mm = 50.0f;
if (distance_squared > min_move_threshold_mm * min_move_threshold_mm)
{
need_publish = true;
}
}
if (need_publish)
{
last_enemy_pos_ = enemy_pos;
last_publish_time_ = this->now();
enemy_pos_pub_->publish(enemy_pos);
RCLCPP_INFO(this->get_logger(), "Enemy moved: x=%ld, y=%ld", enemy_pos.x, enemy_pos.y);
}
}
else
{
RCLCPP_INFO(this->get_logger(), "No enemy detected in Lidar scan");
}
}
void EnemyManager::TimerCallback()
{
if (!enemy_initialized_)
return;
rclcpp::Duration duration_since_last = this->now() - last_publish_time_;
if (duration_since_last.seconds() >= 2.0)
{
enemy_pos_pub_->publish(last_enemy_pos_);
last_publish_time_ = this->now();
RCLCPP_INFO(this->get_logger(), "Periodic refresh of enemy position");
}
}
}
int main(int argc, char **argv) {
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<Modelec::EnemyManager>());
rclcpp::shutdown();
return 0;
}

13
src/modelec_strat/src/mission_manager.cpp
View File

@@ -2,6 +2,19 @@
namespace Modelec
{
enum class State {
INIT, // Initialisation des capteurs, timers, etc.
WAIT_START, // Attente du feu vert
SELECT_MISSION, // Choix intelligent de la prochaine mission
RETURN_HOME, // Retour zone de fin
FAILSAFE, // En cas d'erreur bloquante
STOP, // Fin du match
GO_TO_OBJECTIVE, // Navigation vers la cible
EXECUTE_MISSION, // Exécution de l'action
};
MissionManager::MissionManager()
{
}

30
src/modelec_strat/src/missions/go_home_mission.cpp Normal file
View File

@@ -0,0 +1,30 @@
#include <modelec_strat/missions/go_home_mission.hpp>
namespace Modelec
{
GoHomeMission::GoHomeMission(const std::shared_ptr<NavigationHelper>& nav) : status_(MissionStatus::READY), nav_(nav)
{
}
void GoHomeMission::start(rclcpp::Node::SharedPtr node)
{
node_ = node;
// go home
status_ = MissionStatus::RUNNING;
}
void GoHomeMission::update()
{
if (nav_->HasArrived())
{
status_ = MissionStatus::DONE;
}
}
MissionStatus GoHomeMission::getStatus() const
{
return status_;
}
}

52
src/modelec_strat/src/missions/prepare_concert_mission.cpp Normal file
View File

@@ -0,0 +1,52 @@
#include <modelec_strat/missions/prepare_concert_mission.hpp>
namespace Modelec
{
PrepareConcertMission::PrepareConcertMission(const std::shared_ptr<NavigationHelper>& nav) : step_(GO_TO_COLUMN), status_(MissionStatus::READY), nav_(nav)
{
}
void PrepareConcertMission::start(rclcpp::Node::SharedPtr node)
{
node_ = node;
status_ = MissionStatus::RUNNING;
}
void PrepareConcertMission::update()
{
if (status_ != MissionStatus::RUNNING) return;
if (!nav_->HasArrived()) return;
switch (step_)
{
case GO_TO_COLUMN:
step_ = TAKE_COLUMN;
break;
case TAKE_COLUMN:
step_ = GO_TO_PLATFORM;
break;
case GO_TO_PLATFORM:
step_ = PLACE_PLATFORM;
break;
case PLACE_PLATFORM:
step_ = DONE;
status_ = MissionStatus::DONE;
break;
default:
break;
}
}
MissionStatus PrepareConcertMission::getStatus() const
{
return status_;
}
}

24
src/modelec_strat/src/missions/promotion_mission.cpp Normal file
View File

@@ -0,0 +1,24 @@
#include <modelec_strat/missions/promotion_mission.hpp>
namespace Modelec
{
PromotionMission::PromotionMission(const std::shared_ptr<NavigationHelper>& nav) : step_(GO_TO_FRONT), status_(MissionStatus::READY), nav_(nav)
{
}
void PromotionMission::start(rclcpp::Node::SharedPtr node)
{
node_ = node;
status_ = MissionStatus::RUNNING;
}
void PromotionMission::update()
{
}
MissionStatus PromotionMission::getStatus() const
{
return status_;
}
}

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

@@ -157,6 +157,7 @@ namespace Modelec {
void NavigationHelper::GoTo(const PosMsg::SharedPtr& goal)
{
SendWaypoint(FindPath(goal));
//pathfinding_->SetStartAndGoal(current_pos_, goal);
}
WaypointListMsg NavigationHelper::FindPath(const PosMsg::SharedPtr& goal)
@@ -178,6 +179,7 @@ namespace Modelec {
void NavigationHelper::OnPos(const PosMsg::SharedPtr msg)
{
current_pos_ = msg;
pathfinding_->SetCurrentPos(msg);
}
}

124
src/modelec_strat/src/pathfinding.cpp
View File

@@ -27,6 +27,9 @@ namespace Modelec {
Pathfinding::Pathfinding(const rclcpp::Node::SharedPtr& node) : node_(node)
{
map_width_mm_ = 3000.0f;
map_height_mm_ = 2000.0f;
robot_length_mm_ = 300.0f;
robot_width_mm_ = 300.0f;
@@ -65,6 +68,14 @@ namespace Modelec {
HandleMapSizeRequest(request, response);
});
enemy_pos_sub_ = node_->create_subscription<modelec_interfaces::msg::OdometryPos>(
"enemy/position", 10,
[this](const modelec_interfaces::msg::OdometryPos::SharedPtr msg) {
OnEnemyPosition(msg);
});
waypoint_pub_ = node_->create_publisher<WaypointMsg>(
"odometry/add_waypoint", 100);
}
rclcpp::Node::SharedPtr Pathfinding::getNode() const
@@ -72,15 +83,47 @@ namespace Modelec {
return node_;
}
/*bool Pathfinding::EnemyOnPath(const modelec_interfaces::msg::OdometryPos& enemy_pos)
{
int ex = enemy_pos.x;
int ey = enemy_pos.y;
for (const auto& wp : current_waypoints_)
{
int wx = wp.x;
int wy = wp.y;
if (std::abs(wx - ex) + std::abs(wy - ey) <= 300)
return true;
}
return false;
}*/
/*void Pathfinding::Replan()
{
if (!current_start_ || !current_goal_)
return;
current_waypoints_ = FindPath(current_start_, current_goal_);
for (const auto& wp : current_waypoints_)
{
waypoint_pub_->publish(wp);
}
}*/
WaypointListMsg Pathfinding::FindPath(const PosMsg::SharedPtr& start, const PosMsg::SharedPtr& goal)
{
if (!start || !goal)
{
RCLCPP_WARN(node_->get_logger(), "Start or Goal position is null");
return WaypointListMsg();
}
WaypointListMsg waypoints;
// Constantes de ta map
constexpr float map_width_mm = 3000.0f; // mm
constexpr float map_height_mm = 2000.0f; // mm
const float cell_size_mm_x = map_width_mm / grid_width_;
const float cell_size_mm_y = map_height_mm / grid_height_;
const float cell_size_mm_x = map_width_mm_ / grid_width_;
const float cell_size_mm_y = map_height_mm_ / grid_height_;
// Robot dimensions
const int inflate_x = std::ceil((robot_width_mm_ / 2.0f) / cell_size_mm_x);
@@ -94,6 +137,46 @@ namespace Modelec {
row.assign(grid_width_, 0); // 0 = free
}
if (has_enemy_pos_)
{
int ex = (last_enemy_pos_.x / cell_size_mm_x) - inflate_x;
int ey = (grid_height_ - 1) - ((last_enemy_pos_.y / cell_size_mm_y) - inflate_y);
const int inflate_enemy_x = std::ceil(150.0 / cell_size_mm_x + inflate_x);
const int inflate_enemy_y = std::ceil(150.0 / cell_size_mm_y + inflate_y);
for (int y = ey - inflate_enemy_y; y <= ey + inflate_enemy_y; ++y)
{
for (int x = ex - inflate_enemy_x; x <= ex + inflate_enemy_x; ++x)
{
if (x >= 0 && y >= 0 && x < grid_width_ && y < grid_height_)
{
grid_[y][x] = 1;
}
}
}
}
// Bord gauche et droit
for (int y = 0; y < grid_height_; ++y)
{
for (int x = 0; x < inflate_x; ++x)
{
grid_[y][x] = 1; // bord gauche
grid_[y][grid_width_ - 1 - x] = 1; // bord droit
}
}
// Bord haut et bas
for (int x = 0; x < grid_width_; ++x)
{
for (int y = 0; y < inflate_y; ++y)
{
grid_[y][x] = 1; // bord bas
grid_[grid_height_ - 1 - y][x] = 1; // bord haut
}
}
// 2. Fill obstacles with inflation
for (const auto& [id, obstacle] : obstacle_map_)
{
@@ -353,6 +436,23 @@ namespace Modelec {
return true;
}
/*void Pathfinding::SetStartAndGoal(const PosMsg::SharedPtr& start, const PosMsg::SharedPtr& goal)
{
current_start_ = start;
current_goal_ = goal;
current_waypoints_ = FindPath(start, goal);
for (const auto& wp : current_waypoints_)
{
waypoint_pub_->publish(wp);
}
}*/
void Pathfinding::SetCurrentPos(const PosMsg::SharedPtr& pos)
{
current_start_ = pos;
}
void Pathfinding::HandleMapRequest(const std::shared_ptr<modelec_interfaces::srv::Map::Request>,
const std::shared_ptr<modelec_interfaces::srv::Map::Response> response)
{
@@ -384,6 +484,20 @@ namespace Modelec {
}
}
void Pathfinding::OnEnemyPosition(const modelec_interfaces::msg::OdometryPos::SharedPtr msg)
{
last_enemy_pos_ = *msg;
has_enemy_pos_ = true;
RCLCPP_INFO(node_->get_logger(), "Enemy position updated: x=%ld, y=%ld", msg->x, msg->y);
/*if (EnemyOnPath(last_enemy_pos_))
{
RCLCPP_INFO(node_->get_logger(), "Enemy is blocking the path, replanning...");
Replan();
}*/
}
Waypoint::Waypoint(const modelec_interfaces::msg::OdometryPos& pos, int index, bool isLast)
{
id = index;

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

@@ -2,20 +2,138 @@
namespace Modelec
{
StratFMS::StratFMS() : Node("start_fms")
StratFMS::StratFMS() : Node("start_fms"), state_(State::INIT)
{
tirette_sub_ = create_subscription<std_msgs::msg::Bool>(
"/tirette", 10, [this](const std_msgs::msg::Bool::SharedPtr msg)
{
if (!started_ && msg->data)
{
started_ = true;
}
});
state_pub_ = create_publisher<modelec_interfaces::msg::StratState>("/strat/state", 10);
}
void StratFMS::Init()
{
nav_ = std::make_unique<NavigationHelper>(shared_from_this());
nav_ = std::make_shared<NavigationHelper>(shared_from_this());
mission_manager_ = std::make_unique<MissionManager>(shared_from_this());
action_executor_ = std::make_unique<ActionExecutor>(shared_from_this());
RCLCPP_INFO(this->get_logger(), "StratFMS fully initialized");
timer_ = create_wall_timer(std::chrono::milliseconds(100), [this]
{
update();
});
}
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),
reason.c_str());
state_ = next;
modelec_interfaces::msg::StratState msg;
msg.state = static_cast<int>(state_);
msg.reason = reason;
state_pub_->publish(msg);
}
void StratFMS::update()
{
auto now = this->now();
switch (state_)
{
case State::INIT:
RCLCPP_INFO_ONCE(get_logger(), "State: INIT");
transition(State::WAIT_START, "System ready");
break;
case State::WAIT_START:
RCLCPP_INFO_ONCE(get_logger(), "State: WAIT_START");
if (started_)
{
match_start_time_ = now;
transition(State::SELECT_MISSION, "Match started");
}
break;
case State::SELECT_MISSION:
{
auto elapsed = now - match_start_time_;
// select mission in a good way there.
if (elapsed.seconds() < 10)
{
transition(State::DO_PROMOTION, "Start by preparing the concert");
}
else if (elapsed.seconds() < 30)
{
transition(State::DO_PROMOTION, "Proceed to promotion");
}
else if (elapsed.seconds() >= 100.0)
{
transition(State::DO_GO_HOME, "Cleanup and finish match");
}
else
{
transition(State::STOP, "Nothing more to do");
}
break;
}
case State::DO_PREPARE_CONCERT:
if (!current_mission_)
{
current_mission_ = std::make_unique<PrepareConcertMission>(nav_);
current_mission_->start(shared_from_this());
}
current_mission_->update();
if (current_mission_->getStatus() == MissionStatus::DONE)
{
current_mission_.reset();
transition(State::SELECT_MISSION, "PrepareConcert finished");
}
break;
case State::DO_PROMOTION:
if (!current_mission_)
{
current_mission_ = std::make_unique<PromotionMission>(nav_);
current_mission_->start(shared_from_this());
}
current_mission_->update();
if (current_mission_->getStatus() == MissionStatus::DONE)
{
current_mission_.reset();
transition(State::SELECT_MISSION, "Promotion finished");
}
break;
case State::DO_GO_HOME:
if (!current_mission_)
{
current_mission_ = std::make_unique<GoHomeMission>(nav_);
current_mission_->start(shared_from_this());
}
current_mission_->update();
if (current_mission_->getStatus() == MissionStatus::DONE)
{
current_mission_.reset();
transition(State::STOP, "Cleanup done");
}
break;
case State::STOP:
RCLCPP_INFO_ONCE(get_logger(), "State: STOP - Match finished");
break;
}
}
}
int main(int argc, char **argv)
int main(int argc, char** argv)
{
rclcpp::init(argc, argv);
auto node = std::make_shared<Modelec::StratFMS>();