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

config + strat

Browse Source
This commit is contained in:
acki
2025-05-01 23:54:08 -04:00
parent f0e11989f2
commit 4420dbb15c
17 changed files with 417 additions and 126 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
src/modelec_com/src/pcb_alim_interface.cpp
View File

@@ -10,7 +10,7 @@ namespace Modelec
auto request = std::make_shared<modelec_interfaces::srv::AddSerialListener::Request>();
request->name = "pcb_alim";
request->bauds = 115200;
request->serial_port = "/dev/pts/11"; // TODO : check the real serial port
request->serial_port = "/dev/pts/10"; // TODO : check the real serial port
auto client = this->create_client<modelec_interfaces::srv::AddSerialListener>("add_serial_listener");
while (!client->wait_for_service(std::chrono::seconds(1)))
{

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

@@ -22,6 +22,7 @@ namespace ModelecGUI {
class MapPage : public QWidget
{
Q_OBJECT
public:
MapPage(rclcpp::Node::SharedPtr node, QWidget *parent = nullptr);
@@ -38,6 +39,12 @@ namespace ModelecGUI {
void mousePressEvent(QMouseEvent* event) override;
void mouseReleaseEvent(QMouseEvent* event) override;
void mouseDoubleClickEvent(QMouseEvent* event) override;
void mouseMoveEvent(QMouseEvent* event) override;
void onOdometryReceived(const modelec_interfaces::msg::OdometryPos::SharedPtr msg);
void OnObstacleReceived(const modelec_interfaces::msg::Obstacle::SharedPtr msg);
@@ -49,10 +56,10 @@ namespace ModelecGUI {
QPoint robotPosPoint = QPoint(0, 0);
std::vector<QPoint> qpoints;
std::vector<modelec_interfaces::msg::OdometryAddWaypoint> points;
//std::vector<modelec_interfaces::msg::OdometryAddWaypoint> points;
modelec_interfaces::msg::OdometryPos go_to_point;
bool lastWapointWasEnd = false;
bool lastWapointWasEnd = true;
std::map<int, modelec_interfaces::msg::Obstacle> obstacle_;
bool show_obstacle_ = true;

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

@@ -20,7 +20,6 @@ namespace ModelecGUI
this->setLayout(v_layout);
qpoints = {};
points = {};
enemy_pos_.x = 1775;
enemy_pos_.y = 200;
@@ -31,8 +30,9 @@ namespace ModelecGUI
if (lastWapointWasEnd)
{
qpoints.clear();
points.clear();
lastWapointWasEnd = false;
qpoints.push_back(QPoint(robotPosPoint.x(), robotPosPoint.y()));
}
if (msg->is_end)
@@ -42,7 +42,6 @@ namespace ModelecGUI
qpoints.push_back(QPoint(Modelec::mapValue(static_cast<int>(msg->x), 0, 3000, 0, width()),
height() - Modelec::mapValue(static_cast<int>(msg->y), 0, 2000, 0, height())));
points.push_back(*msg);
update();
});
@@ -82,13 +81,6 @@ namespace ModelecGUI
RCLCPP_INFO(node_->get_logger(), "Map received: %d x %d", res->width, res->height);
map_width_ = res->width;
map_height_ = res->height;
/* TODO - obstacle
* idea
* send only the size of the map
* and then send the obstacle with id throw the topic obstacle
* problem : if not initialized, idk what to do
* maybe solution ask to send back the obstacle throw an other service
*/
}
}
@@ -133,22 +125,15 @@ namespace ModelecGUI
// --- Draw lines ---
painter.setPen(QPen(Qt::red, 2)); // Red lines, 2px wide
if (!points.empty())
painter.drawLine(robotPosPoint, qpoints[0]);
for (size_t i = 0; i + 1 < points.size(); ++i) {
for (size_t i = 0; i + 1 < qpoints.size(); ++i) {
painter.drawLine(qpoints[i], qpoints[i + 1]);
}
// --- Draw colored points ---
const int radius = 5;
painter.setBrush(Qt::green);
painter.setPen(Qt::NoPen);
painter.drawEllipse(robotPosPoint, radius, radius); // Robot position
for (size_t i = 0; i < points.size(); ++i) {
if (i == points.size() - 1)
for (size_t i = 0; i < qpoints.size(); ++i) {
if (i == qpoints.size() - 1)
painter.setBrush(Qt::blue); // Last = blue
else
painter.setBrush(Qt::red); // Middle = red
@@ -157,6 +142,10 @@ namespace ModelecGUI
painter.drawEllipse(qpoints[i], radius, radius);
}
painter.setBrush(Qt::green);
painter.setPen(Qt::NoPen);
painter.drawEllipse(robotPosPoint, radius, radius); // Robot position
if (show_obstacle_)
{
float ratioBetweenMapAndWidget = height() / 2000.0f;
@@ -184,6 +173,7 @@ namespace ModelecGUI
go_to_pub_->publish(msg);
}
else if (event->button() == Qt::RightButton)
{
enemy_pos_.x = Modelec::mapValue(event->pos().x(), 0, width(), 0, 3000);
@@ -194,6 +184,21 @@ namespace ModelecGUI
}
}
void MapPage::mouseReleaseEvent(QMouseEvent* event)
{
QWidget::mouseReleaseEvent(event);
}
void MapPage::mouseDoubleClickEvent(QMouseEvent* event)
{
QWidget::mouseDoubleClickEvent(event);
}
void MapPage::mouseMoveEvent(QMouseEvent* event)
{
QWidget::mouseMoveEvent(event);
}
void MapPage::OnObstacleReceived(const modelec_interfaces::msg::Obstacle::SharedPtr msg)
{
obstacle_.emplace(msg->id, *msg);

49
src/modelec_strat/CMakeLists.txt
View File

@@ -1,9 +1,9 @@
cmake_minimum_required(VERSION 3.8)
project(modelec_strat)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif ()
find_package(ament_cmake REQUIRED)
find_package(std_msgs REQUIRED)
@@ -11,37 +11,44 @@ find_package(sensor_msgs REQUIRED)
find_package(std_srvs REQUIRED)
find_package(tinyxml2 REQUIRED)
find_package(rclcpp REQUIRED)
find_package(ament_index_cpp 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 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)
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 src/obstacle.cpp src/config.cpp)
ament_target_dependencies(strat_fsm rclcpp std_msgs std_srvs modelec_interfaces ament_index_cpp)
target_link_libraries(strat_fsm modelec_utils::modelec_utils tinyxml2::tinyxml2)
target_include_directories(strat_fsm PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<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)
add_executable(enemy_manager src/enemy_manager.cpp src/config.cpp)
ament_target_dependencies(enemy_manager rclcpp std_msgs sensor_msgs std_srvs modelec_interfaces ament_index_cpp)
target_link_libraries(enemy_manager modelec_utils::modelec_utils tinyxml2::tinyxml2)
target_include_directories(enemy_manager PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
$<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)
set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
if (BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
set(ament_cmake_copyright_FOUND TRUE)
set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif ()
install(TARGETS
strat_fsm
enemy_manager
DESTINATION lib/${PROJECT_NAME}
strat_fsm
enemy_manager
DESTINATION lib/${PROJECT_NAME}
)
install(
DIRECTORY data/
DESTINATION share/${PROJECT_NAME}/data
FILES_MATCHING PATTERN "*.xml"
)
ament_package()

31
src/modelec_strat/data/config.xml Normal file
View File

@@ -0,0 +1,31 @@
<config>
<enemy>
<size>
<width_mm>300</width_mm>
<length_mm>300</length_mm>
<margin_mm>50</margin_mm>
</size>
<detection>
<min_move_threshold_mm>50</min_move_threshold_mm>
<refresh_rate>0.5</refresh_rate>
</detection>
</enemy>
<robot>
<size>
<width_mm>300</width_mm>
<length_mm>300</length_mm>
</size>
</robot>
<map>
<size>
<grid_width>300</grid_width>
<grid_height>200</grid_height>
<map_width_mm>3000</map_width_mm>
<map_height_mm>2000</map_height_mm>
</size>
</map>
</config>

38
src/modelec_strat/map/obstacles.xml → src/modelec_strat/data/obstacles.xml
View File

@@ -1,25 +1,33 @@
<Obstacles>
<!-- TOP -->
<Obstacle id="1" x="600" y="1800" width="450" height="200" />
<Obstacle id="2" x="1050" y="1550" width="900" height="450" />
<Obstacle id="3" x="1950" y="1800" width="450" height="200" />
<!--
<Obstacle id="1" x="600" y="1800" width="450" height="200" type="estrade"/>
<Obstacle id="2" x="1050" y="1550" width="900" height="450" type="estrade"/>
<Obstacle id="3" x="1950" y="1800" width="450" height="200" type="estrade"/>
-->
<!-- Gradin TOP TO BOTTOM LEFT -->
<Obstacle id="10" x="600" y="1650" width="450" height="150" />
<Obstacle id="11" x="0" y="1100" width="150" height="450" />
<Obstacle id="12" x="900" y="900" width="400" height="100" />
<Obstacle id="13" x="0" y="200" width="130" height="400" />
<Obstacle id="14" x="570" y="200" width="400" height="100" />
<!--
<Obstacle id="10" x="600" y="1650" width="450" height="150" type="gradin"/>
<Obstacle id="11" x="0" y="1100" width="150" height="450" type="gradin"/>
<Obstacle id="12" x="900" y="900" width="400" height="100" type="gradin"/>
<Obstacle id="13" x="0" y="200" width="130" height="400" type="gradin"/>
<Obstacle id="14" x="570" y="200" width="400" height="100" type="gradin"/>
-->
<!-- Gradin TOP TO BOTTOM RIGHT -->
<Obstacle id="20" x="1950" y="1650" width="450" height="150" />
<Obstacle id="21" x="2850" y="1100" width="150" height="450" />
<Obstacle id="22" x="1700" y="900" width="400" height="100" />
<Obstacle id="23" x="2850" y="200" width="130" height="400" />
<Obstacle id="24" x="2000" y="200" width="400" height="100" />
<!--
<Obstacle id="20" x="1950" y="1650" width="450" height="150" type="gradin"/>
<Obstacle id="21" x="2850" y="1100" width="150" height="450" type="gradin"/>
<Obstacle id="22" x="1700" y="900" width="400" height="100" type="gradin"/>
<Obstacle id="23" x="2850" y="200" width="130" height="400" type="gradin"/>
<Obstacle id="24" x="2000" y="200" width="400" height="100" type="gradin"/>
-->
<!-- PAMI Start -->
<Obstacle id="30" x="0" y="1550" width="150" height="450" />
<Obstacle id="31" x="2850" y="1550" width="150" height="450" />
<!--
<Obstacle id="30" x="0" y="1550" width="150" height="450" type="pami-start"/>
<Obstacle id="31" x="2850" y="1550" width="150" height="450" type="pami-start"/>
-->
</Obstacles>

50
src/modelec_strat/include/modelec_strat/config.hpp Normal file
View File

@@ -0,0 +1,50 @@
#pragma once
#include <map>
#include <string>
#include <tinyxml2.h>
#include "obstacle.hpp"
namespace Modelec
{
class Config
{
public:
static bool load(const std::string& filepath);
template<typename T>
static T get(const std::string& key, const T& default_value = T());
static void printAll();
private:
static void parseNode(tinyxml2::XMLElement* element, const std::string& prefix);
static inline std::unordered_map<std::string, std::string> values_;
};
template<typename T>
T Config::get(const std::string& key, const T& default_value) {
auto it = values_.find(key);
if (it == values_.end()) return default_value;
std::istringstream iss(it->second);
T result;
if (!(iss >> result)) return default_value;
return result;
}
template<>
inline std::string Config::get<std::string>(const std::string& key, const std::string& default_value) {
auto it = values_.find(key);
return it != values_.end() ? it->second : default_value;
}
template<>
inline bool Config::get<bool>(const std::string& key, const bool& default_value) {
auto str = get<std::string>(key, default_value ? "true" : "false");
return str == "true" || str == "1";
}
}

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

@@ -31,5 +31,8 @@ namespace Modelec
modelec_interfaces::msg::OdometryPos last_enemy_pos_;
bool enemy_initialized_ = false;
rclcpp::Time last_publish_time_;
float min_move_threshold_mm_ = 0.0f;
float refresh_rate_s_ = 0.0f;
};
}

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

@@ -17,6 +17,8 @@ namespace Modelec {
rclcpp::Node::SharedPtr getNode() const;
std::shared_ptr<Pathfinding> getPathfinding() const;
void SendWaypoint() const;
void SendWaypoint(const std::vector<WaypointMsg> &waypoints) const;
@@ -42,7 +44,7 @@ namespace Modelec {
private:
rclcpp::Node::SharedPtr node_;
std::unique_ptr<Pathfinding> pathfinding_;
std::shared_ptr<Pathfinding> pathfinding_;
std::list<Waypoint> waypoints_;

37
src/modelec_strat/include/modelec_strat/obstacle.hpp Normal file
View File

@@ -0,0 +1,37 @@
#pragma once
#include <string>
#include <rclcpp/rclcpp.hpp>
#include <modelec_interfaces/msg/obstacle.hpp>
#include <tinyxml2.h>
namespace Modelec {
class Obstacle {
public:
Obstacle() : id_(-1), x_(0), y_(0), w_(0), h_(0), type_("unknown") {}
Obstacle(int id, int x, int y, int w, int h, const std::string& type);
Obstacle(tinyxml2::XMLElement * obstacleElem);
Obstacle(const modelec_interfaces::msg::Obstacle& msg);
Obstacle(const Obstacle& other) = default;
modelec_interfaces::msg::Obstacle toMsg() const;
int id() const { return id_; }
int x() const { return x_; }
int y() const { return y_; }
int width() const { return w_; }
int height() const { return h_; }
const std::string& type() const { return type_; }
void setId(int id) { id_ = id; }
void setX(int x) { x_ = x; }
void setY(int y) { y_ = y; }
void setWidth(int w) { w_ = w; }
void setHeight(int h) { h_ = h; }
void setType(const std::string& type) { type_ = type; }
private:
int id_, x_, y_, w_, h_;
std::string type_;
};
}

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

@@ -14,6 +14,8 @@
#include <modelec_interfaces/srv/map_size.hpp>
#include <modelec_interfaces/msg/obstacle.hpp>
#include "obstacle.hpp"
namespace Modelec {
@@ -45,10 +47,10 @@ namespace Modelec {
WaypointListMsg FindPath(const PosMsg::SharedPtr& start,
const PosMsg::SharedPtr& goal);
bool LoadObstaclesFromXML(const std::string& filename);
//void SetStartAndGoal(const PosMsg::SharedPtr& start, const PosMsg::SharedPtr& goal);
bool LoadObstaclesFromXML(const std::string& filename);
void SetCurrentPos(const PosMsg::SharedPtr& pos);
protected:
@@ -77,10 +79,14 @@ namespace Modelec {
int grid_height_ = 0;
int map_width_mm_ = 0;
int map_height_mm_ = 0;
float robot_width_mm_ = 0;
float robot_length_mm_ = 0;
int robot_width_mm_ = 0;
int robot_length_mm_ = 0;
int enemy_width_mm_ = 0;
int enemy_length_mm_ = 0;
int enemy_margin_mm_ = 0;
std::map<int, modelec_interfaces::msg::Obstacle> obstacle_map_;
std::map<int, Obstacle> obstacle_map_;
PosMsg::SharedPtr current_start_;
PosMsg::SharedPtr current_goal_;
@@ -93,6 +99,9 @@ namespace Modelec {
rclcpp::Publisher<modelec_interfaces::msg::Obstacle>::SharedPtr map_pub_;
rclcpp::Publisher<WaypointMsg>::SharedPtr waypoint_pub_;
rclcpp::Subscription<modelec_interfaces::msg::Obstacle>::SharedPtr obstacle_add_sub_;
rclcpp::Subscription<modelec_interfaces::msg::Obstacle>::SharedPtr obstacle_remove_sub_;
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_;

52
src/modelec_strat/src/config.cpp Normal file
View File

@@ -0,0 +1,52 @@
#include <modelec_strat/config.hpp>
namespace Modelec
{
bool Config::load(const std::string& filepath)
{
tinyxml2::XMLDocument doc;
if (doc.LoadFile(filepath.c_str()) != tinyxml2::XML_SUCCESS) {
RCLCPP_ERROR(rclcpp::get_logger("Config"), "Failed to load XML file: %s", doc.ErrorName());
return false;
}
auto* root = doc.RootElement();
if (!root)
{
RCLCPP_ERROR(rclcpp::get_logger("Config"), "No root element found in XML file");
return false;
}
parseNode(root, "");
return true;
}
void Config::parseNode(tinyxml2::XMLElement* element, const std::string& prefix) {
std::string key_prefix = prefix.empty() ? element->Name() : prefix + "." + element->Name();
const char* text = element->GetText();
if (text && std::string(text).find_first_not_of(" \n\t") != std::string::npos) {
values_[key_prefix] = text;
}
const tinyxml2::XMLAttribute* attr = element->FirstAttribute();
while (attr) {
std::string attr_key = key_prefix + "@" + attr->Name();
values_[attr_key] = attr->Value();
attr = attr->Next();
}
for (tinyxml2::XMLElement* child = element->FirstChildElement(); child; child = child->NextSiblingElement()) {
parseNode(child, key_prefix);
}
}
void Config::printAll()
{
RCLCPP_INFO(rclcpp::get_logger("Config"), "Loaded %zu configuration values", values_.size());
for (const auto& pair : values_) {
RCLCPP_INFO(rclcpp::get_logger("Config"), "%s: %s", pair.first.c_str(), pair.second.c_str());
}
}
}

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

@@ -1,10 +1,22 @@
#include <modelec_strat/enemy_manager.hpp>
#include <cmath>
#include <modelec_strat/config.hpp>
#include <ament_index_cpp/get_package_share_directory.hpp>
namespace Modelec
{
EnemyManager::EnemyManager() : Node("enemy_manager")
{
std::string config_path = ament_index_cpp::get_package_share_directory("modelec_strat") + "/data/config.xml";
if (!Config::load(config_path))
{
RCLCPP_ERROR(get_logger(), "Failed to load config file: %s", config_path.c_str());
}
min_move_threshold_mm_ = Config::get<float>("config.enemy.detection.min_move_threshold_mm", 50);
refresh_rate_s_ = Config::get<float>("config.enemy.detection.refresh_rate", 1.0);
current_pos_sub_ = this->create_subscription<modelec_interfaces::msg::OdometryPos>(
"odometry/position", 10,
[this](const modelec_interfaces::msg::OdometryPos::SharedPtr msg) {
@@ -106,8 +118,7 @@ namespace Modelec
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)
if (distance_squared > min_move_threshold_mm_ * min_move_threshold_mm_)
{
need_publish = true;
}
@@ -133,7 +144,7 @@ namespace Modelec
return;
rclcpp::Duration duration_since_last = this->now() - last_publish_time_;
if (duration_since_last.seconds() >= 2.0)
if (duration_since_last.seconds() >= refresh_rate_s_)
{
enemy_pos_pub_->publish(last_enemy_pos_);
last_publish_time_ = this->now();

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

@@ -8,7 +8,7 @@ namespace Modelec {
NavigationHelper::NavigationHelper(const rclcpp::Node::SharedPtr& node) : node_(node)
{
pathfinding_ = std::make_unique<Pathfinding>(node);
pathfinding_ = std::make_shared<Pathfinding>(node);
waypoint_reach_sub_ = node_->create_subscription<WaypointReachMsg>(
"odometry/waypoint_reach", 10,
@@ -35,6 +35,11 @@ namespace Modelec {
return node_;
}
std::shared_ptr<Pathfinding> NavigationHelper::getPathfinding() const
{
return pathfinding_;
}
void NavigationHelper::SendWaypoint() const
{
for (auto & w : waypoints_)

43
src/modelec_strat/src/obstacle.cpp Normal file
View File

@@ -0,0 +1,43 @@
#include <modelec_strat/obstacle.hpp>
namespace Modelec
{
Obstacle::Obstacle(int id, int x, int y, int w, int h, const std::string& type)
: id_(id), x_(x), y_(y), w_(w), h_(h), type_(type)
{
}
Obstacle::Obstacle(tinyxml2::XMLElement* obstacleElem)
{
const char* type = nullptr;
if (obstacleElem->QueryIntAttribute("id", &id_) != tinyxml2::XML_SUCCESS ||
obstacleElem->QueryIntAttribute("x", &x_) != tinyxml2::XML_SUCCESS ||
obstacleElem->QueryIntAttribute("y", &y_) != tinyxml2::XML_SUCCESS ||
obstacleElem->QueryIntAttribute("width", &w_) != tinyxml2::XML_SUCCESS ||
obstacleElem->QueryIntAttribute("height", &h_) != tinyxml2::XML_SUCCESS ||
obstacleElem->QueryStringAttribute("type", &type) != tinyxml2::XML_SUCCESS)
{
RCLCPP_WARN(rclcpp::get_logger("Obstacle"), "Failed to parse obstacle element");
return;
}
type_ = type;
}
Obstacle::Obstacle(const modelec_interfaces::msg::Obstacle& msg)
: id_(msg.id), x_(msg.x), y_(msg.y), w_(msg.width), h_(msg.height), type_("unknown")
{
}
modelec_interfaces::msg::Obstacle Obstacle::toMsg() const
{
modelec_interfaces::msg::Obstacle msg;
msg.id = id_;
msg.x = x_;
msg.y = y_;
msg.width = w_;
msg.height = h_;
return msg;
}
}

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

@@ -1,5 +1,8 @@
#include <ament_index_cpp/get_package_share_directory.hpp>
#include <modelec_strat/pathfinding.hpp>
#include <modelec_strat/config.hpp>
namespace Modelec {
struct AStarNode
@@ -27,23 +30,44 @@ namespace Modelec {
Pathfinding::Pathfinding(const rclcpp::Node::SharedPtr& node) : node_(node)
{
map_width_mm_ = 3000.0f;
map_height_mm_ = 2000.0f;
map_width_mm_ = Config::get<int>("config.map.size.map_width_mm", 3000);
map_height_mm_ = Config::get<int>("config.map.size.map_height_mm", 2000);
robot_length_mm_ = 300.0f;
robot_width_mm_ = 300.0f;
robot_length_mm_ = Config::get<int>("config.robot.size.length_mm", 300);
robot_width_mm_ = Config::get<int>("config.robot.size.width_mm", 300);
grid_width_ = 300;
grid_height_ = 200;
enemy_length_mm_ = Config::get<int>("config.enemy.size.length_mm", 300);
enemy_width_mm_ = Config::get<int>("config.enemy.size.width_mm", 300);
if (!LoadObstaclesFromXML("/home/acki/ros2_ws/src/modelec_strat/map/obstacles.xml"))
enemy_margin_mm_ = Config::get<int>("config.enemy.size.margin_mm", 50);
grid_width_ = Config::get<int>("config.map.size.grid_width", 300);
grid_height_ = Config::get<int>("config.map.size.grid_height", 200);
std::string obstacles_path = ament_index_cpp::get_package_share_directory("modelec_strat") + "/data/obstacles.xml";
if (!LoadObstaclesFromXML(obstacles_path))
{
RCLCPP_WARN(node_->get_logger(), "Failed to load obstacles from XML");
RCLCPP_ERROR(node_->get_logger(), "Failed to load obstacles from XML");
}
map_pub_ = node_->create_publisher<modelec_interfaces::msg::Obstacle>(
"nav/obstacle", 40);
obstacle_add_sub_ = node_->create_subscription<modelec_interfaces::msg::Obstacle>(
"nav/obstacle/add", 10,
[this](const modelec_interfaces::msg::Obstacle::SharedPtr msg) {
RCLCPP_INFO(node_->get_logger(), "Obstacle add request received");
obstacle_map_[msg->id] = Obstacle(*msg);
map_pub_->publish(*msg);
});
obstacle_remove_sub_ = node_->create_subscription<modelec_interfaces::msg::Obstacle>(
"nav/obstacle/remove", 10,
[this](const modelec_interfaces::msg::Obstacle::SharedPtr msg) {
RCLCPP_INFO(node_->get_logger(), "Obstacle remove request received");
obstacle_map_.erase(msg->id);
});
ask_obstacle_srv_ = node_->create_service<std_srvs::srv::Empty>(
"nav/ask_map_obstacle",
[this](const std::shared_ptr<std_srvs::srv::Empty::Request> request,
@@ -139,11 +163,11 @@ namespace Modelec {
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);
int ex = (last_enemy_pos_.x / cell_size_mm_x);
int ey = ((map_height_mm_ - last_enemy_pos_.y) / cell_size_mm_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);
const int inflate_enemy_x = std::ceil((enemy_margin_mm_ + (enemy_width_mm_/2)) / cell_size_mm_x) + inflate_x;
const int inflate_enemy_y = std::ceil((enemy_margin_mm_ + (enemy_length_mm_/2)) / cell_size_mm_y) + inflate_y;
for (int y = ey - inflate_enemy_y; y <= ey + inflate_enemy_y; ++y)
{
@@ -180,10 +204,10 @@ namespace Modelec {
// 2. Fill obstacles with inflation
for (const auto& [id, obstacle] : obstacle_map_)
{
int x_start = std::max(0, (int)(obstacle.x / cell_size_mm_x) - inflate_x);
int y_start = std::max(0, (int)(obstacle.y / cell_size_mm_y) - inflate_y);
int x_end = std::min(grid_width_ - 1, (int)((obstacle.x + obstacle.width) / cell_size_mm_x) + inflate_x);
int y_end = std::min(grid_height_ - 1, (int)((obstacle.y + obstacle.height) / cell_size_mm_y) + inflate_y);
int x_start = std::max(0, (int)(obstacle.x() / cell_size_mm_x) - inflate_x);
int y_start = std::max(0, (int)(obstacle.y() / cell_size_mm_y) - inflate_y);
int x_end = std::min(grid_width_ - 1, (int)((obstacle.x() + obstacle.width()) / cell_size_mm_x) + inflate_x);
int y_end = std::min(grid_height_ - 1, (int)((obstacle.y() + obstacle.height()) / cell_size_mm_y) + inflate_y);
// Inverser l'axe Y
y_start = (grid_height_ - 1) - y_start;
@@ -396,46 +420,6 @@ namespace Modelec {
return waypoints;
}
bool Pathfinding::LoadObstaclesFromXML(const std::string& filename)
{
tinyxml2::XMLDocument doc;
if (doc.LoadFile(filename.c_str()) != tinyxml2::XML_SUCCESS)
{
RCLCPP_ERROR(node_->get_logger(), "Failed to load obstacle XML file: %s", filename.c_str());
return false;
}
tinyxml2::XMLElement* root = doc.FirstChildElement("Obstacles");
if (!root)
{
RCLCPP_ERROR(node_->get_logger(), "No <Obstacles> root element in file");
return false;
}
for (tinyxml2::XMLElement* obstacleElem = root->FirstChildElement("Obstacle");
obstacleElem != nullptr;
obstacleElem = obstacleElem->NextSiblingElement("Obstacle"))
{
modelec_interfaces::msg::Obstacle obs;
if (obstacleElem->QueryIntAttribute("id", &obs.id) != tinyxml2::XML_SUCCESS ||
obstacleElem->QueryIntAttribute("x", &obs.x) != tinyxml2::XML_SUCCESS ||
obstacleElem->QueryIntAttribute("y", &obs.y) != tinyxml2::XML_SUCCESS ||
obstacleElem->QueryIntAttribute("width", &obs.width) != tinyxml2::XML_SUCCESS ||
obstacleElem->QueryIntAttribute("height", &obs.height) != tinyxml2::XML_SUCCESS)
{
RCLCPP_WARN(node_->get_logger(), "Incomplete obstacle definition in XML, skipping one obstacle");
continue;
}
obstacle_map_[obs.id] = obs;
}
RCLCPP_INFO(node_->get_logger(), "Loaded %zu obstacles from XML", obstacle_map_.size());
return true;
}
/*void Pathfinding::SetStartAndGoal(const PosMsg::SharedPtr& start, const PosMsg::SharedPtr& goal)
{
current_start_ = start;
@@ -480,7 +464,7 @@ namespace Modelec {
{
for (auto & [index, obs] : obstacle_map_)
{
map_pub_->publish(obs);
map_pub_->publish(obs.toMsg());
}
}
@@ -498,6 +482,34 @@ namespace Modelec {
}*/
}
bool Pathfinding::LoadObstaclesFromXML(const std::string& filename)
{
tinyxml2::XMLDocument doc;
if (doc.LoadFile(filename.c_str()) != tinyxml2::XML_SUCCESS)
{
RCLCPP_ERROR(node_->get_logger(), "Failed to load obstacle XML file: %s", filename.c_str());
return false;
}
tinyxml2::XMLElement* root = doc.FirstChildElement("Obstacles");
if (!root)
{
RCLCPP_ERROR(node_->get_logger(), "No <Obstacles> root element in file");
return false;
}
for (tinyxml2::XMLElement* obstacleElem = root->FirstChildElement("Obstacle");
obstacleElem != nullptr;
obstacleElem = obstacleElem->NextSiblingElement("Obstacle"))
{
Obstacle obs(obstacleElem);
obstacle_map_[obs.id()] = obs;
}
RCLCPP_INFO(node_->get_logger(), "Loaded %zu obstacles from XML", obstacle_map_.size());
return true;
}
Waypoint::Waypoint(const modelec_interfaces::msg::OdometryPos& pos, int index, bool isLast)
{
id = index;

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

@@ -1,3 +1,5 @@
#include <ament_index_cpp/get_package_share_directory.hpp>
#include <modelec_strat/config.hpp>
#include <modelec_strat/strat_fms.hpp>
namespace Modelec
@@ -15,6 +17,12 @@ namespace Modelec
});
state_pub_ = create_publisher<modelec_interfaces::msg::StratState>("/strat/state", 10);
std::string config_path = ament_index_cpp::get_package_share_directory("modelec_strat") + "/data/config.xml";
if (!Config::load(config_path))
{
RCLCPP_ERROR(get_logger(), "Failed to load config file: %s", config_path.c_str());
}
}
void StratFMS::Init()
@@ -22,6 +30,7 @@ namespace Modelec
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]