diff --git a/src/modelec_com/src/pcb_alim_interface.cpp b/src/modelec_com/src/pcb_alim_interface.cpp index 8d4e0ca..281a1ae 100644 --- a/src/modelec_com/src/pcb_alim_interface.cpp +++ b/src/modelec_com/src/pcb_alim_interface.cpp @@ -10,7 +10,7 @@ namespace Modelec auto request = std::make_shared(); request->name = "pcb_alim"; request->bauds = 115200; - request->serial_port = "/dev/pts/10"; // TODO : check the real serial port + request->serial_port = "/dev/pts/11"; // TODO : check the real serial port auto client = this->create_client("add_serial_listener"); while (!client->wait_for_service(std::chrono::seconds(1))) { diff --git a/src/modelec_gui/CMakeLists.txt b/src/modelec_gui/CMakeLists.txt index 9944a4a..17f7fbe 100644 --- a/src/modelec_gui/CMakeLists.txt +++ b/src/modelec_gui/CMakeLists.txt @@ -14,6 +14,7 @@ set(CMAKE_AUTOUIC ON) find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) +find_package(std_srvs REQUIRED) find_package(modelec_interfaces REQUIRED) find_package(modelec_utils COMPONENTS modelec_utils @@ -45,6 +46,7 @@ add_executable(modelec_gui ament_target_dependencies(modelec_gui rclcpp std_msgs + std_srvs modelec_interfaces ) target_link_libraries(modelec_gui diff --git a/src/modelec_gui/include/modelec_gui/modelec_gui.hpp b/src/modelec_gui/include/modelec_gui/modelec_gui.hpp index 9bc80f3..f18c96d 100644 --- a/src/modelec_gui/include/modelec_gui/modelec_gui.hpp +++ b/src/modelec_gui/include/modelec_gui/modelec_gui.hpp @@ -33,5 +33,7 @@ namespace ModelecGUI QAction* playmat_map_; QAction* playmat_grid_; + QAction* toggle_show_obstacle_action_; + }; } diff --git a/src/modelec_gui/include/modelec_gui/pages/map_page.hpp b/src/modelec_gui/include/modelec_gui/pages/map_page.hpp index 648c710..74289b0 100644 --- a/src/modelec_gui/include/modelec_gui/pages/map_page.hpp +++ b/src/modelec_gui/include/modelec_gui/pages/map_page.hpp @@ -10,8 +10,13 @@ #include +#include + #include #include +#include +#include +#include namespace ModelecGUI { class MapPage : public QWidget @@ -26,6 +31,8 @@ namespace ModelecGUI { void setPlaymatMap(); + void toggleShowObstacle(); + protected: void paintEvent(QPaintEvent*) override; @@ -33,6 +40,8 @@ namespace ModelecGUI { void onOdometryReceived(const modelec_interfaces::msg::OdometryPos::SharedPtr msg); + void OnObstacleReceived(const modelec_interfaces::msg::Obstacle::SharedPtr msg); + QSvgRenderer* renderer; QVBoxLayout* v_layout; @@ -45,11 +54,20 @@ namespace ModelecGUI { bool lastWapointWasEnd = false; + std::map obstacle_; + bool show_obstacle_ = true; + + int map_width_ = 0; + int map_height_ = 0; + rclcpp::Node::SharedPtr node_; rclcpp::Subscription::SharedPtr add_waypoint_sub_; rclcpp::Subscription::SharedPtr odometry_sub_; rclcpp::Publisher::SharedPtr go_to_pub_; + rclcpp::Client::SharedPtr map_client_; + rclcpp::Subscription::SharedPtr obstacle_sub_; + rclcpp::Client::SharedPtr ask_map_obstacle_client_; }; } diff --git a/src/modelec_gui/package.xml b/src/modelec_gui/package.xml index 1e190b4..63160b8 100644 --- a/src/modelec_gui/package.xml +++ b/src/modelec_gui/package.xml @@ -12,6 +12,7 @@ modelec_interfaces rclcpp std_msgs + std_srvs ament_lint_auto ament_lint_common diff --git a/src/modelec_gui/src/modelec_gui.cpp b/src/modelec_gui/src/modelec_gui.cpp index 638a631..2cf2916 100644 --- a/src/modelec_gui/src/modelec_gui.cpp +++ b/src/modelec_gui/src/modelec_gui.cpp @@ -35,6 +35,8 @@ namespace ModelecGUI { playmat_map_ = new QAction("Map", this); playmat_grid_ = new QAction("Grid", this); + toggle_show_obstacle_action_ = new QAction("Toggle Show Obstacle", this); + exit_action_ = new QAction("Exit", this); optionsMenu->addAction(home_action_); @@ -44,6 +46,7 @@ namespace ModelecGUI { optionsMenu->addMenu(playmat_map_menu_); playmat_map_menu_->addAction(playmat_map_); playmat_map_menu_->addAction(playmat_grid_); + optionsMenu->addAction(toggle_show_obstacle_action_); optionsMenu->addSeparator(); optionsMenu->addAction(exit_action_); @@ -73,6 +76,14 @@ namespace ModelecGUI { } }); + connect(toggle_show_obstacle_action_, &QAction::triggered, this, [this]() { + auto map_page = dynamic_cast(stackedWidget->currentWidget()); + if (map_page) + { + map_page->toggleShowObstacle(); + } + }); + connect(exit_action_, &QAction::triggered, this, [this]() { close(); }); diff --git a/src/modelec_gui/src/pages/map_page.cpp b/src/modelec_gui/src/pages/map_page.cpp index 3821651..ad4759c 100644 --- a/src/modelec_gui/src/pages/map_page.cpp +++ b/src/modelec_gui/src/pages/map_page.cpp @@ -6,7 +6,7 @@ namespace ModelecGUI { - MapPage::MapPage(rclcpp::Node::SharedPtr node, QWidget* parent) : QWidget(parent), renderer(new QSvgRenderer(QString(":/img/playmat/grid_v1.svg"), this)), node_(std::move(node)) + MapPage::MapPage(rclcpp::Node::SharedPtr node, QWidget* parent) : QWidget(parent), renderer(new QSvgRenderer(QString(":/img/playmat/2025_FINAL.svg"), this)), node_(node) { v_layout = new QVBoxLayout(this); v_layout->addStretch(); @@ -22,7 +22,7 @@ namespace ModelecGUI qpoints = {}; points = {}; - add_waypoint_sub_ = node_->create_subscription("odometry/add_waypoint", 10, + add_waypoint_sub_ = node_->create_subscription("odometry/add_waypoint", 100, [this](const modelec_interfaces::msg::OdometryAddWaypoint::SharedPtr msg) { if (lastWapointWasEnd) { @@ -50,7 +50,53 @@ namespace ModelecGUI update(); }); + obstacle_sub_ = node_->create_subscription("nav/obstacle", 40, + [this](const modelec_interfaces::msg::Obstacle::SharedPtr msg) { + OnObstacleReceived(msg); + }); + go_to_pub_ = node_->create_publisher("nav/go_to", 10); + + // client to nav/map + map_client_ = node_->create_client("nav/map_size"); + while (!map_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..."); + } + + auto result = map_client_->async_send_request(std::make_shared()); + if (rclcpp::spin_until_future_complete(node_->get_node_base_interface(), result) == + rclcpp::FutureReturnCode::SUCCESS) + { + if (auto res = result.get()) + { + 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 + */ + } + } + + ask_map_obstacle_client_ = node_->create_client("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..."); + } + + auto result2 = ask_map_obstacle_client_->async_send_request(std::make_shared()); + rclcpp::spin_until_future_complete(node_->get_node_base_interface(), result2); } void MapPage::setPlaymatGrid() @@ -65,6 +111,11 @@ namespace ModelecGUI update(); } + void MapPage::toggleShowObstacle() + { + show_obstacle_ = !show_obstacle_; + } + void MapPage::paintEvent(QPaintEvent* paint_event) { QWidget::paintEvent(paint_event); @@ -100,6 +151,16 @@ namespace ModelecGUI painter.setPen(Qt::NoPen); painter.drawEllipse(qpoints[i], radius, radius); } + + if (show_obstacle_) + { + float ratioBetweenMapAndWidget = height() / 2000.0f; + for (auto & [index, obs] : obstacle_) + { + painter.setBrush(Qt::black); + painter.drawRect(obs.x * ratioBetweenMapAndWidget, height() - (obs.y + obs.height) * ratioBetweenMapAndWidget, obs.width * ratioBetweenMapAndWidget, obs.height * ratioBetweenMapAndWidget); + } + } } void MapPage::mousePressEvent(QMouseEvent* event) @@ -109,6 +170,7 @@ namespace ModelecGUI 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); @@ -137,4 +199,9 @@ namespace ModelecGUI update();*/ } + + void MapPage::OnObstacleReceived(const modelec_interfaces::msg::Obstacle::SharedPtr msg) + { + obstacle_.emplace(msg->id, *msg); + } } diff --git a/src/modelec_gui/src/pages/test_page.cpp b/src/modelec_gui/src/pages/test_page.cpp index 332524e..b54fe0f 100644 --- a/src/modelec_gui/src/pages/test_page.cpp +++ b/src/modelec_gui/src/pages/test_page.cpp @@ -5,7 +5,7 @@ namespace ModelecGUI { - TestPage::TestPage(rclcpp::Node::SharedPtr node, QWidget* parent) : QWidget(parent), node_(std::move(node)) + TestPage::TestPage(rclcpp::Node::SharedPtr node, QWidget* parent) : QWidget(parent), node_(node) { startButton_ = new QPushButton("Start"); connect(startButton_, &QPushButton::clicked, this, [this]() { @@ -41,12 +41,15 @@ namespace ModelecGUI askPID_ = new QPushButton("Ask PID"); connect(askPID_, &QPushButton::clicked, this, [this]() { + RCLCPP_INFO(node_->get_logger(), "Ask PID button clicked."); // Create a request for the PID service auto request = std::make_shared(); // Make the asynchronous service call client_get_pid_->async_send_request(request, [this](rclcpp::Client::SharedFuture response) { + RCLCPP_INFO(node_->get_logger(), "Received PID response."); if (auto res = response.get()) { + RCLCPP_INFO(node_->get_logger(), "PID values received: p=%f, i=%f, d=%f", res->p, res->i, res->d); QMetaObject::invokeMethod(this, [this, res]() { pPIDBox_->setText(QString("%1").arg(res->p)); iPIDBox_->setText(QString("%1").arg(res->i)); @@ -158,10 +161,7 @@ namespace ModelecGUI pub_add_waypoint_ = node_->create_publisher( "/odometry/add_waypoint", 10); - // Set up service client client_ = node_->create_client("odometry/speed"); - - // Wait for the service to be available while (!client_->wait_for_service(std::chrono::seconds(1))) { if (!rclcpp::ok()) { RCLCPP_ERROR(node_->get_logger(), "Interrupted while waiting for the service. Exiting."); @@ -171,7 +171,6 @@ namespace ModelecGUI } client_start_ = node_->create_client("odometry/start"); - while (!client_start_->wait_for_service(std::chrono::seconds(1))) { if (!rclcpp::ok()) { RCLCPP_ERROR(node_->get_logger(), "Interrupted while waiting for the service. Exiting."); diff --git a/src/modelec_interfaces/CMakeLists.txt b/src/modelec_interfaces/CMakeLists.txt index d66853b..139425c 100644 --- a/src/modelec_interfaces/CMakeLists.txt +++ b/src/modelec_interfaces/CMakeLists.txt @@ -19,6 +19,8 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/Odometry/OdometryAddWaypoint.msg" "msg/Odometry/OdometryStart.msg" "msg/Odometry/PID/OdometryPid.msg" + "msg/Map/Map.msg" + "msg/Map/Obstacle.msg" "msg/PCA9685Servo.msg" "msg/ServoMode.msg" "msg/Solenoid.msg" @@ -35,6 +37,8 @@ rosidl_generate_interfaces(${PROJECT_NAME} "srv/Odometry/OdometryAddWaypoint.srv" "srv/Odometry/PID/OdometryGetPid.srv" "srv/Odometry/PID/OdometrySetPid.srv" + "srv/Map/Map.srv" + "srv/Map/MapSize.srv" "srv/AddServoMotor.srv" "srv/AddSolenoid.srv" "srv/Tirette.srv" diff --git a/src/modelec_interfaces/msg/Map/Map.msg b/src/modelec_interfaces/msg/Map/Map.msg new file mode 100644 index 0000000..3e46851 --- /dev/null +++ b/src/modelec_interfaces/msg/Map/Map.msg @@ -0,0 +1,3 @@ +int32 width +int32 height +int32[] map diff --git a/src/modelec_interfaces/msg/Map/Obstacle.msg b/src/modelec_interfaces/msg/Map/Obstacle.msg new file mode 100644 index 0000000..4743a2c --- /dev/null +++ b/src/modelec_interfaces/msg/Map/Obstacle.msg @@ -0,0 +1,5 @@ +int32 id +int32 x +int32 y +int32 width +int32 height \ No newline at end of file diff --git a/src/modelec_interfaces/srv/Map/Map.srv b/src/modelec_interfaces/srv/Map/Map.srv new file mode 100644 index 0000000..05506b7 --- /dev/null +++ b/src/modelec_interfaces/srv/Map/Map.srv @@ -0,0 +1,4 @@ +--- +int32 width +int32 height +int32[] map \ No newline at end of file diff --git a/src/modelec_interfaces/srv/Map/MapSize.srv b/src/modelec_interfaces/srv/Map/MapSize.srv new file mode 100644 index 0000000..baa7662 --- /dev/null +++ b/src/modelec_interfaces/srv/Map/MapSize.srv @@ -0,0 +1,3 @@ +--- +int32 width +int32 height diff --git a/src/modelec_strat/CMakeLists.txt b/src/modelec_strat/CMakeLists.txt index 49e5026..04a00c2 100644 --- a/src/modelec_strat/CMakeLists.txt +++ b/src/modelec_strat/CMakeLists.txt @@ -7,14 +7,16 @@ endif() find_package(ament_cmake REQUIRED) find_package(std_msgs REQUIRED) +find_package(std_srvs REQUIRED) +find_package(tinyxml2 REQUIRED) 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) -ament_target_dependencies(strat_fsm rclcpp std_msgs modelec_interfaces) -target_link_libraries(strat_fsm modelec_utils::modelec_utils) +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 $ $ diff --git a/src/modelec_strat/include/modelec_strat/action_executor.hpp b/src/modelec_strat/include/modelec_strat/action_executor.hpp index cec4911..4b6715c 100644 --- a/src/modelec_strat/include/modelec_strat/action_executor.hpp +++ b/src/modelec_strat/include/modelec_strat/action_executor.hpp @@ -10,7 +10,7 @@ namespace Modelec public: ActionExecutor(); - ActionExecutor(rclcpp::Node::SharedPtr node); + ActionExecutor(const rclcpp::Node::SharedPtr& node); rclcpp::Node::SharedPtr getNode() const; diff --git a/src/modelec_strat/include/modelec_strat/mission_manager.hpp b/src/modelec_strat/include/modelec_strat/mission_manager.hpp index 89ec3e2..fbb3c38 100644 --- a/src/modelec_strat/include/modelec_strat/mission_manager.hpp +++ b/src/modelec_strat/include/modelec_strat/mission_manager.hpp @@ -10,7 +10,7 @@ namespace Modelec public: MissionManager(); - MissionManager(rclcpp::Node::SharedPtr node); + MissionManager(const rclcpp::Node::SharedPtr& node); rclcpp::Node::SharedPtr getNode() const; diff --git a/src/modelec_strat/include/modelec_strat/navigation_helper.hpp b/src/modelec_strat/include/modelec_strat/navigation_helper.hpp index d516229..99341a5 100644 --- a/src/modelec_strat/include/modelec_strat/navigation_helper.hpp +++ b/src/modelec_strat/include/modelec_strat/navigation_helper.hpp @@ -13,11 +13,12 @@ namespace Modelec { public: NavigationHelper(); - NavigationHelper(rclcpp::Node::SharedPtr node); + NavigationHelper(const rclcpp::Node::SharedPtr& node); rclcpp::Node::SharedPtr getNode() const; void SendWaypoint() const; + void SendWaypoint(const std::vector &waypoints) const; void AddWaypoint(const PosMsg &pos, int index); void AddWaypoint(const WaypointMsg &waypoint); diff --git a/src/modelec_strat/include/modelec_strat/pathfinding.hpp b/src/modelec_strat/include/modelec_strat/pathfinding.hpp index 32a8424..7ad608c 100644 --- a/src/modelec_strat/include/modelec_strat/pathfinding.hpp +++ b/src/modelec_strat/include/modelec_strat/pathfinding.hpp @@ -2,9 +2,18 @@ #include +#include + +#include + #include #include #include +#include +#include +#include +#include + namespace Modelec { @@ -36,15 +45,47 @@ namespace Modelec { public: Pathfinding(); - Pathfinding(rclcpp::Node::SharedPtr node); + Pathfinding(const rclcpp::Node::SharedPtr& node); rclcpp::Node::SharedPtr getNode() const; WaypointListMsg FindPath(const PosMsg::SharedPtr& start, const PosMsg::SharedPtr& goal); + bool LoadObstaclesFromXML(const std::string& filename); + + protected: + void HandleMapRequest( + const std::shared_ptr request, + const std::shared_ptr response); + + void HandleMapSizeRequest( + const std::shared_ptr request, + const std::shared_ptr response); + + void HandleAskObstacleRequest( + const std::shared_ptr request, + const std::shared_ptr response); + private: + int robot_width_mm_; + int robot_length_mm_; + + int grid_width_; + int grid_height_; + + std::vector> grid_; + + std::map obstacle_map_; + rclcpp::Node::SharedPtr node_; + + rclcpp::Service::SharedPtr map_srv_; + rclcpp::Service::SharedPtr map_size_srv_; + rclcpp::Service::SharedPtr ask_obstacle_srv_; + + rclcpp::Publisher::SharedPtr map_pub_; + }; -} \ No newline at end of file +} diff --git a/src/modelec_strat/map/obstacles.xml b/src/modelec_strat/map/obstacles.xml new file mode 100644 index 0000000..de909b6 --- /dev/null +++ b/src/modelec_strat/map/obstacles.xml @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/modelec_strat/package.xml b/src/modelec_strat/package.xml index 716917a..9cb29b9 100644 --- a/src/modelec_strat/package.xml +++ b/src/modelec_strat/package.xml @@ -13,6 +13,8 @@ modelec_utils rclcpp std_msgs + std_srvs + tinyxml2 sensor_msgs geometry_msgs diff --git a/src/modelec_strat/src/action_executor.cpp b/src/modelec_strat/src/action_executor.cpp index 4088875..4f2ebd4 100644 --- a/src/modelec_strat/src/action_executor.cpp +++ b/src/modelec_strat/src/action_executor.cpp @@ -6,7 +6,7 @@ namespace Modelec { } - ActionExecutor::ActionExecutor(rclcpp::Node::SharedPtr node) : node_(std::move(node)) + ActionExecutor::ActionExecutor(const rclcpp::Node::SharedPtr& node) : node_(node) { } diff --git a/src/modelec_strat/src/mission_manager.cpp b/src/modelec_strat/src/mission_manager.cpp index 731a1d7..1094808 100644 --- a/src/modelec_strat/src/mission_manager.cpp +++ b/src/modelec_strat/src/mission_manager.cpp @@ -6,7 +6,7 @@ namespace Modelec { } - MissionManager::MissionManager(rclcpp::Node::SharedPtr node) : node_(std::move(node)) + MissionManager::MissionManager(const rclcpp::Node::SharedPtr& node) : node_(node) { } diff --git a/src/modelec_strat/src/navigation_helper.cpp b/src/modelec_strat/src/navigation_helper.cpp index 700982b..6ba6c5a 100644 --- a/src/modelec_strat/src/navigation_helper.cpp +++ b/src/modelec_strat/src/navigation_helper.cpp @@ -6,9 +6,9 @@ namespace Modelec { { } - NavigationHelper::NavigationHelper(rclcpp::Node::SharedPtr node) : node_(std::move(node)) + NavigationHelper::NavigationHelper(const rclcpp::Node::SharedPtr& node) : node_(node) { - pathfinding_ = std::make_unique(std::move(node)); + pathfinding_ = std::make_unique(node); waypoint_reach_sub_ = node_->create_subscription( "odometry/waypoint_reach", 10, @@ -16,7 +16,7 @@ namespace Modelec { OnWaypointReach(msg); }); - waypoint_pub_ = node_->create_publisher("odometry/add_waypoint", 30); + waypoint_pub_ = node_->create_publisher("odometry/add_waypoint", 100); pos_sub_ = node_->create_subscription( "odometry/position", 20, @@ -43,6 +43,14 @@ namespace Modelec { } } + void NavigationHelper::SendWaypoint(const std::vector& waypoints) const + { + for (auto & w : waypoints) + { + waypoint_pub_->publish(w); + } + } + void NavigationHelper::AddWaypoint(const PosMsg& pos, int index) { for (auto w = waypoints_.begin(); w != waypoints_.end(); ++w) @@ -148,14 +156,7 @@ namespace Modelec { void NavigationHelper::GoTo(const PosMsg::SharedPtr& goal) { - auto waypointList = FindPath(goal); - waypoints_.clear(); - for (const auto& waypoint : waypointList) - { - waypoints_.emplace_back(waypoint); - } - - SendWaypoint(); + SendWaypoint(FindPath(goal)); } WaypointListMsg NavigationHelper::FindPath(const PosMsg::SharedPtr& goal) diff --git a/src/modelec_strat/src/pathfinding.cpp b/src/modelec_strat/src/pathfinding.cpp index 40e67e9..bc14cc3 100644 --- a/src/modelec_strat/src/pathfinding.cpp +++ b/src/modelec_strat/src/pathfinding.cpp @@ -2,12 +2,69 @@ namespace Modelec { + struct AStarNode + { + int x, y; + double g = std::numeric_limits::infinity(); // Cost from start + double f = std::numeric_limits::infinity(); // g + heuristic + int parent_x = -1; + int parent_y = -1; + + bool operator>(const AStarNode& other) const + { + return f > other.f; + } + }; + + double heuristic(int x1, int y1, int x2, int y2) + { + return std::hypot(x1 - x2, y1 - y2); // Euclidean distance + } + Pathfinding::Pathfinding() { } - Pathfinding::Pathfinding(rclcpp::Node::SharedPtr node) : node_(std::move(node)) + Pathfinding::Pathfinding(const rclcpp::Node::SharedPtr& node) : node_(node) { + robot_length_mm_ = 300.0f; + robot_width_mm_ = 300.0f; + + grid_width_ = 300; + grid_height_ = 200; + + if (!LoadObstaclesFromXML("/home/acki/ros2_ws/src/modelec_strat/map/obstacles.xml")) + { + RCLCPP_WARN(node_->get_logger(), "Failed to load obstacles from XML"); + } + + map_pub_ = node_->create_publisher( + "nav/obstacle", 40); + + ask_obstacle_srv_ = node_->create_service( + "nav/ask_map_obstacle", + [this](const std::shared_ptr request, + const std::shared_ptr response) { + RCLCPP_INFO(node_->get_logger(), "Ask obstacle request received"); + HandleAskObstacleRequest(request, response); + }); + + map_srv_ = node_->create_service( + "nav/map", + [this](const std::shared_ptr request, + const std::shared_ptr response) { + RCLCPP_INFO(node_->get_logger(), "Map request received"); + HandleMapRequest(request, response); + }); + + map_size_srv_ = node_->create_service( + "nav/map_size", + [this](const std::shared_ptr request, + const std::shared_ptr response) { + RCLCPP_INFO(node_->get_logger(), "Map size request received"); + HandleMapSizeRequest(request, response); + }); + } rclcpp::Node::SharedPtr Pathfinding::getNode() const @@ -17,23 +74,316 @@ namespace Modelec { WaypointListMsg Pathfinding::FindPath(const PosMsg::SharedPtr& start, const PosMsg::SharedPtr& goal) { - /* TODO - pathfinding - * This is a stub for the pathfinding algorithm. The pathfinding algorithm should - */ - WaypointListMsg waypoints; - waypoints.push_back(Waypoint(*start, 0)); - WaypointMsg middleW; - middleW.x = 500; - middleW.y = 1500; - middleW.theta = 0; - middleW.id = 1; - middleW.is_end = false; - waypoints.push_back(middleW); - waypoints.push_back(Waypoint(*goal, 2, true)); + + // 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_; + + // Robot dimensions + const int inflate_x = std::ceil((robot_width_mm_ / 2.0f) / cell_size_mm_x); + const int inflate_y = std::ceil((robot_length_mm_ / 2.0f) / cell_size_mm_y); + + // 1. Build fresh empty grid + grid_.clear(); + grid_.resize(grid_height_); + for (auto& row : grid_) + { + row.assign(grid_width_, 0); // 0 = free + } + + // 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); + + // Inverser l'axe Y + y_start = (grid_height_ - 1) - y_start; + y_end = (grid_height_ - 1) - y_end; + if (y_start > y_end) std::swap(y_start, y_end); + + for (int y = y_start; y <= y_end; ++y) + { + for (int x = x_start; x <= x_end; ++x) + { + grid_[y][x] = 1; // mark as obstacle + } + } + } + + // 3. Convert start and goal (avec inversion Y) + const int start_x = start->x / cell_size_mm_x; + const int start_y = (grid_height_ - 1) - (start->y / cell_size_mm_y); + const int goal_x = goal->x / cell_size_mm_x; + const int goal_y = (grid_height_ - 1) - (goal->y / cell_size_mm_y); + + if (start_x < 0 || start_y < 0 || goal_x < 0 || goal_y < 0 || + start_x >= grid_width_ || start_y >= grid_height_ || + goal_x >= grid_width_ || goal_y >= grid_height_) + { + RCLCPP_WARN(node_->get_logger(), "Start or Goal out of bounds"); + return waypoints; + } + + if (grid_[start_y][start_x] == 1 || grid_[goal_y][goal_x] == 1) + { + RCLCPP_WARN(node_->get_logger(), "Start or Goal inside an obstacle"); + return waypoints; + } + + // 4. A* algorithm + std::priority_queue, std::greater> open_list; + std::unordered_map all_nodes; + + auto hash = [](int x, int y) { + return (int64_t)x << 32 | (uint32_t)y; + }; + + auto neighbors = [](int x, int y) { + return std::vector>{ + {x+1, y}, {x-1, y}, {x, y+1}, {x, y-1}, + {x+1, y+1}, {x-1, y+1}, {x+1, y-1}, {x-1, y-1} // diagonals + }; + }; + + AStarNode start_node{start_x, start_y}; + start_node.g = 0; + start_node.f = heuristic(start_x, start_y, goal_x, goal_y); + + open_list.push(start_node); + all_nodes[hash(start_x, start_y)] = start_node; + + bool path_found = false; + + while (!open_list.empty()) + { + AStarNode current = open_list.top(); + open_list.pop(); + + if (current.x == goal_x && current.y == goal_y) + { + path_found = true; + break; + } + + for (const auto& [nx, ny] : neighbors(current.x, current.y)) + { + if (nx < 0 || ny < 0 || ny >= (int)grid_.size() || nx >= (int)grid_[0].size()) + continue; + + if (grid_[ny][nx] == 1) + continue; + + double tentative_g = current.g + heuristic(current.x, current.y, nx, ny); + int64_t neighbor_hash = hash(nx, ny); + + if (all_nodes.find(neighbor_hash) == all_nodes.end() || tentative_g < all_nodes[neighbor_hash].g) + { + AStarNode neighbor; + neighbor.x = nx; + neighbor.y = ny; + neighbor.g = tentative_g; + neighbor.f = tentative_g + heuristic(nx, ny, goal_x, goal_y); + neighbor.parent_x = current.x; + neighbor.parent_y = current.y; + all_nodes[neighbor_hash] = neighbor; + open_list.push(neighbor); + } + } + } + + if (!path_found) + { + RCLCPP_WARN(node_->get_logger(), "No path found"); + return waypoints; + } + + // 5. Reconstruct path + std::vector> path; + int cx = goal_x; + int cy = goal_y; + + while (!(cx == start_x && cy == start_y)) + { + path.emplace_back(cx, cy); + auto it = all_nodes.find(hash(cx, cy)); + if (it == all_nodes.end()) + break; + cx = it->second.parent_x; + cy = it->second.parent_y; + } + path.emplace_back(start_x, start_y); + std::reverse(path.begin(), path.end()); + + // 6. Path smoothing + std::vector> smooth_path; + size_t current = 0; + while (current < path.size()) + { + size_t next = current + 1; + while (next < path.size()) + { + bool clear = true; + int x0 = path[current].first; + int y0 = path[current].second; + int x1 = path[next].first; + int y1 = path[next].second; + + int dx = std::abs(x1 - x0); + int dy = -std::abs(y1 - y0); + int sx = x0 < x1 ? 1 : -1; + int sy = y0 < y1 ? 1 : -1; + int err = dx + dy; + int x = x0; + int y = y0; + + while (true) + { + if (grid_[y][x] == 1) + { + clear = false; + break; + } + if (x == x1 && y == y1) + break; + int e2 = 2 * err; + if (e2 >= dy) + { + err += dy; + x += sx; + } + if (e2 <= dx) + { + err += dx; + y += sy; + } + } + + if (!clear) + break; + next++; + } + + smooth_path.push_back(path[current]); + if (next == path.size()) + { + smooth_path.push_back(path.back()); + break; + } + current = next - 1; + } + + // 7. Fill Waypoints (reconvertir grille -> millimètres) + int id = 0; + for (size_t i = 0; i < smooth_path.size(); ++i) + { + const auto& [x, y] = smooth_path[i]; + + // Skip first point if it's too close to robot position + if (i == 0) + { + float world_x = x * cell_size_mm_x; + float world_y = (grid_height_ - 1 - y) * cell_size_mm_y; + + float dx = std::abs(world_x - start->x); + float dy = std::abs(world_y - start->y); + + if (dx < 50 && dy < 50) // <== seuil de 50mm + continue; + } + + WaypointMsg wp; + wp.x = x * cell_size_mm_x; + wp.y = (grid_height_ - 1 - y) * cell_size_mm_y; + wp.theta = 0; + wp.id = id++; + wp.is_end = false; + waypoints.push_back(wp); + } + if (!waypoints.empty()) + { + waypoints.back().is_end = true; + } + 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 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::HandleMapRequest(const std::shared_ptr, + const std::shared_ptr response) + { + response->width = grid_[0].size(); + response->height = grid_.size(); + response->map = std::vector(grid_.size() * grid_[0].size()); + for (size_t i = 0; i < grid_.size(); i++) + { + for (size_t j = 0; j < grid_[i].size(); j++) + { + response->map[i * grid_[i].size() + j] = grid_[i][j]; + } + } + } + + void Pathfinding::HandleMapSizeRequest(const std::shared_ptr, + const std::shared_ptr response) + { + response->width = grid_width_; + response->height = grid_height_; + } + + void Pathfinding::HandleAskObstacleRequest(const std::shared_ptr, + const std::shared_ptr) + { + for (auto & [index, obs] : obstacle_map_) + { + map_pub_->publish(obs); + } + } + Waypoint::Waypoint(const modelec_interfaces::msg::OdometryPos& pos, int index, bool isLast) { id = index; diff --git a/src/modelec_strat/src/strat_fms.cpp b/src/modelec_strat/src/strat_fms.cpp index c570f78..bc5baa4 100644 --- a/src/modelec_strat/src/strat_fms.cpp +++ b/src/modelec_strat/src/strat_fms.cpp @@ -11,7 +11,7 @@ namespace Modelec nav_ = std::make_unique(shared_from_this()); mission_manager_ = std::make_unique(shared_from_this()); action_executor_ = std::make_unique(shared_from_this()); - RCLCPP_INFO(this->get_logger(), "Strat fully initialized"); + RCLCPP_INFO(this->get_logger(), "StratFMS fully initialized"); } }