From 03a64908f32ca45ddcd12c19d76516c02e365f83 Mon Sep 17 00:00:00 2001 From: acki Date: Fri, 25 Apr 2025 12:17:11 -0400 Subject: [PATCH] start the strat : - nav --- src/modelec_com/src/pcb_alim_interface.cpp | 2 +- src/modelec_com/src/pcb_odo_interface.cpp | 4 +- .../launch/test.modelec.launch.yml | 5 + .../include/modelec_gui/pages/map_page.hpp | 13 +- src/modelec_gui/src/pages/map_page.cpp | 52 +++-- src/modelec_gui/src/pages/test_page.cpp | 2 +- src/modelec_strat/CMakeLists.txt | 18 ++ .../include/modelec_strat/action_executor.hpp | 21 ++ .../include/modelec_strat/mission_manager.hpp | 21 ++ .../modelec_strat/navigation_helper.hpp | 56 ++++++ .../include/modelec_strat/pathfinding.hpp | 50 +++++ .../include/modelec_strat/strat_fms.hpp | 44 +++++ src/modelec_strat/src/action_executor.cpp | 17 ++ src/modelec_strat/src/mission_manager.cpp | 17 ++ src/modelec_strat/src/navigation_helper.cpp | 182 ++++++++++++++++++ src/modelec_strat/src/pathfinding.cpp | 61 ++++++ src/modelec_strat/src/strat_fms.cpp | 26 +++ 17 files changed, 560 insertions(+), 31 deletions(-) create mode 100644 src/modelec_strat/include/modelec_strat/action_executor.hpp create mode 100644 src/modelec_strat/include/modelec_strat/mission_manager.hpp create mode 100644 src/modelec_strat/include/modelec_strat/navigation_helper.hpp create mode 100644 src/modelec_strat/include/modelec_strat/pathfinding.hpp create mode 100644 src/modelec_strat/include/modelec_strat/strat_fms.hpp create mode 100644 src/modelec_strat/src/action_executor.cpp create mode 100644 src/modelec_strat/src/mission_manager.cpp create mode 100644 src/modelec_strat/src/navigation_helper.cpp create mode 100644 src/modelec_strat/src/pathfinding.cpp create mode 100644 src/modelec_strat/src/strat_fms.cpp diff --git a/src/modelec_com/src/pcb_alim_interface.cpp b/src/modelec_com/src/pcb_alim_interface.cpp index 281a1ae..8d4e0ca 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/11"; // TODO : check the real serial port + request->serial_port = "/dev/pts/10"; // 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_com/src/pcb_odo_interface.cpp b/src/modelec_com/src/pcb_odo_interface.cpp index eaf4235..3785f6e 100644 --- a/src/modelec_com/src/pcb_odo_interface.cpp +++ b/src/modelec_com/src/pcb_odo_interface.cpp @@ -10,7 +10,7 @@ namespace Modelec auto request = std::make_shared(); request->name = "pcb_odo"; request->bauds = 115200; - request->serial_port = "/dev/pts/6"; // TODO : check the real serial port + request->serial_port = "/dev/pts/8"; // TODO : check the real serial port auto client = this->create_client("add_serial_listener"); while (!client->wait_for_service(std::chrono::seconds(1))) { @@ -69,7 +69,7 @@ namespace Modelec } odo_pos_publisher_ = this->create_publisher( - "odometry/get_position", 10); + "odometry/position", 10); odo_speed_publisher_ = this->create_publisher( "odometry/speed", 10); diff --git a/src/modelec_core/launch/test.modelec.launch.yml b/src/modelec_core/launch/test.modelec.launch.yml index b122144..7ceb5d1 100644 --- a/src/modelec_core/launch/test.modelec.launch.yml +++ b/src/modelec_core/launch/test.modelec.launch.yml @@ -24,3 +24,8 @@ launch: pkg: 'modelec_core' exec: 'speed_result' name: 'speed_result' + +- node: + pkg: 'modelec_strat' + exec: 'strat_fsm' + name: 'strat_fsm' 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 aceecc4..648c710 100644 --- a/src/modelec_gui/include/modelec_gui/pages/map_page.hpp +++ b/src/modelec_gui/include/modelec_gui/pages/map_page.hpp @@ -31,24 +31,25 @@ namespace ModelecGUI { void mousePressEvent(QMouseEvent* event) override; - void onStartButtonClicked(); - void onOdometryReceived(const modelec_interfaces::msg::OdometryPos::SharedPtr msg); QSvgRenderer* renderer; - QPushButton* startButton; - QVBoxLayout* v_layout; QHBoxLayout* h_layout; QPoint robotPosPoint = QPoint(0, 0); std::vector qpoints; std::vector points; + modelec_interfaces::msg::OdometryPos go_to_point; + + bool lastWapointWasEnd = false; rclcpp::Node::SharedPtr node_; - rclcpp::Publisher::SharedPtr add_waypoint_publisher_; - rclcpp::Subscription::SharedPtr odometry_subscriber_; + rclcpp::Subscription::SharedPtr add_waypoint_sub_; + + rclcpp::Subscription::SharedPtr odometry_sub_; + rclcpp::Publisher::SharedPtr go_to_pub_; }; } diff --git a/src/modelec_gui/src/pages/map_page.cpp b/src/modelec_gui/src/pages/map_page.cpp index cca8596..3821651 100644 --- a/src/modelec_gui/src/pages/map_page.cpp +++ b/src/modelec_gui/src/pages/map_page.cpp @@ -8,34 +8,49 @@ 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)) { - startButton = new QPushButton("Start", this); - v_layout = new QVBoxLayout(this); v_layout->addStretch(); h_layout = new QHBoxLayout(this); h_layout->addStretch(); - h_layout->addWidget(startButton); h_layout->addStretch(); v_layout->addLayout(h_layout); this->setLayout(v_layout); - connect(startButton, &QPushButton::clicked, this, &MapPage::onStartButtonClicked); - qpoints = {}; points = {}; - add_waypoint_publisher_ = node_->create_publisher("odometry/add_waypoint", 30); + add_waypoint_sub_ = node_->create_subscription("odometry/add_waypoint", 10, + [this](const modelec_interfaces::msg::OdometryAddWaypoint::SharedPtr msg) { + if (lastWapointWasEnd) + { + qpoints.clear(); + points.clear(); + lastWapointWasEnd = false; + } + + if (msg->is_end) + { + lastWapointWasEnd = true; + } + + qpoints.push_back(QPoint(Modelec::mapValue(static_cast(msg->x), 0, 3000, 0, width()), + height() - Modelec::mapValue(static_cast(msg->y), 0, 2000, 0, height()))); + points.push_back(*msg); + update(); + }); // lambda - odometry_subscriber_ = node_->create_subscription("odometry/get_position", 10, + odometry_sub_ = node_->create_subscription("odometry/position", 10, [this](const modelec_interfaces::msg::OdometryPos::SharedPtr msg) { robotPosPoint.setX(Modelec::mapValue(static_cast(msg->x), 0, 3000, 0, width())); robotPosPoint.setY(height() - Modelec::mapValue(static_cast(msg->y), 0, 2000, 0, height())); update(); }); + + go_to_pub_ = node_->create_publisher("nav/go_to", 10); } void MapPage::setPlaymatGrid() @@ -57,6 +72,7 @@ namespace ModelecGUI QPainter painter(this); renderer->render(&painter, rect()); // Scales SVG to widget size + painter.setRenderHint(QPainter::Antialiasing); // --- Draw lines --- @@ -90,6 +106,13 @@ 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); + + go_to_pub_->publish(msg); + + /* qpoints.push_back(event->pos()); modelec_interfaces::msg::OdometryAddWaypoint msg; @@ -112,19 +135,6 @@ namespace ModelecGUI points.push_back(msg); - update(); - } - - void MapPage::onStartButtonClicked() - { - points.back().is_end = true; - - for (const auto& point : points) { - add_waypoint_publisher_->publish(point); - } - - qpoints.clear(); - points.clear(); - update(); + update();*/ } } diff --git a/src/modelec_gui/src/pages/test_page.cpp b/src/modelec_gui/src/pages/test_page.cpp index 9de30e3..332524e 100644 --- a/src/modelec_gui/src/pages/test_page.cpp +++ b/src/modelec_gui/src/pages/test_page.cpp @@ -152,7 +152,7 @@ namespace ModelecGUI // Set up subscription sub_ = node_->create_subscription( - "/odometry/get_position", 10, + "/odometry/position", 10, std::bind(&TestPage::PositionCallback, this, std::placeholders::_1)); pub_add_waypoint_ = node_->create_publisher( diff --git a/src/modelec_strat/CMakeLists.txt b/src/modelec_strat/CMakeLists.txt index 7046159..49e5026 100644 --- a/src/modelec_strat/CMakeLists.txt +++ b/src/modelec_strat/CMakeLists.txt @@ -6,6 +6,19 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() find_package(ament_cmake REQUIRED) +find_package(std_msgs 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) +target_include_directories(strat_fsm PUBLIC + $ + $ +) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) @@ -14,4 +27,9 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() endif() +install(TARGETS + strat_fsm + DESTINATION lib/${PROJECT_NAME} +) + ament_package() diff --git a/src/modelec_strat/include/modelec_strat/action_executor.hpp b/src/modelec_strat/include/modelec_strat/action_executor.hpp new file mode 100644 index 0000000..cec4911 --- /dev/null +++ b/src/modelec_strat/include/modelec_strat/action_executor.hpp @@ -0,0 +1,21 @@ +#pragma once + +#include + +namespace Modelec +{ + + class ActionExecutor + { + public: + ActionExecutor(); + + ActionExecutor(rclcpp::Node::SharedPtr node); + + rclcpp::Node::SharedPtr getNode() const; + + private: + rclcpp::Node::SharedPtr node_; + }; + +} \ No newline at end of file diff --git a/src/modelec_strat/include/modelec_strat/mission_manager.hpp b/src/modelec_strat/include/modelec_strat/mission_manager.hpp new file mode 100644 index 0000000..89ec3e2 --- /dev/null +++ b/src/modelec_strat/include/modelec_strat/mission_manager.hpp @@ -0,0 +1,21 @@ +#pragma once + +#include + +namespace Modelec +{ + + class MissionManager + { + public: + MissionManager(); + + MissionManager(rclcpp::Node::SharedPtr node); + + rclcpp::Node::SharedPtr getNode() const; + + private: + rclcpp::Node::SharedPtr node_; + }; + +} \ No newline at end of file diff --git a/src/modelec_strat/include/modelec_strat/navigation_helper.hpp b/src/modelec_strat/include/modelec_strat/navigation_helper.hpp new file mode 100644 index 0000000..d516229 --- /dev/null +++ b/src/modelec_strat/include/modelec_strat/navigation_helper.hpp @@ -0,0 +1,56 @@ +#pragma once + +#include +#include +#include +#include + +#include "pathfinding.hpp" + +namespace Modelec { + + class NavigationHelper { + public: + NavigationHelper(); + + NavigationHelper(rclcpp::Node::SharedPtr node); + + rclcpp::Node::SharedPtr getNode() const; + + void SendWaypoint() const; + + void AddWaypoint(const PosMsg &pos, int index); + void AddWaypoint(const WaypointMsg &waypoint); + + void AddWaypoints(const std::initializer_list &pos_list, int index); + void AddWaypoints(const std::initializer_list &waypoint_list); + + void SetWaypoints(const std::list &waypoints); + + bool HasArrived() const; + + void GoTo(const PosMsg::SharedPtr &goal); + + WaypointListMsg FindPath(const PosMsg::SharedPtr &goal); + + protected: + void OnWaypointReach(const WaypointReachMsg::SharedPtr msg); + + void OnPos(const PosMsg::SharedPtr msg); + + private: + rclcpp::Node::SharedPtr node_; + + std::unique_ptr pathfinding_; + + std::list waypoints_; + + PosMsg::SharedPtr current_pos_; + + rclcpp::Subscription::SharedPtr waypoint_reach_sub_; + rclcpp::Publisher::SharedPtr waypoint_pub_; + + rclcpp::Subscription::SharedPtr go_to_sub_; + rclcpp::Subscription::SharedPtr pos_sub_; + }; +} \ No newline at end of file diff --git a/src/modelec_strat/include/modelec_strat/pathfinding.hpp b/src/modelec_strat/include/modelec_strat/pathfinding.hpp new file mode 100644 index 0000000..32a8424 --- /dev/null +++ b/src/modelec_strat/include/modelec_strat/pathfinding.hpp @@ -0,0 +1,50 @@ +#pragma once + +#include + +#include +#include +#include + +namespace Modelec { + + using WaypointMsg = modelec_interfaces::msg::OdometryAddWaypoint; + using WaypointReachMsg = modelec_interfaces::msg::OdometryWaypointReach; + using PosMsg = modelec_interfaces::msg::OdometryPos; + using WaypointListMsg = std::vector; + + class Waypoint : public WaypointMsg + { + public: + bool reached = false; + + Waypoint(const PosMsg& pos, int index, bool isLast = false); + + explicit Waypoint(const WaypointMsg& waypoint); + + WaypointMsg toMsg() const; + }; + + /* + * - Pathfinding DStar Lite + * + * + * + */ + + class Pathfinding { + public: + Pathfinding(); + + Pathfinding(rclcpp::Node::SharedPtr node); + + rclcpp::Node::SharedPtr getNode() const; + + WaypointListMsg FindPath(const PosMsg::SharedPtr& start, + const PosMsg::SharedPtr& goal); + + private: + rclcpp::Node::SharedPtr node_; + }; + +} \ No newline at end of file diff --git a/src/modelec_strat/include/modelec_strat/strat_fms.hpp b/src/modelec_strat/include/modelec_strat/strat_fms.hpp new file mode 100644 index 0000000..0fcf12c --- /dev/null +++ b/src/modelec_strat/include/modelec_strat/strat_fms.hpp @@ -0,0 +1,44 @@ +#pragma once + +#include + +#include "action_executor.hpp" +#include "mission_manager.hpp" +#include "navigation_helper.hpp" + +namespace Modelec +{ + + /* + * TODO + * - yaml strat + * - setup des missions + * - gestion des interruptions (robot adverses / obstacles) + * - scoring des missions + * + * + * + * - Banderole + * + * + * + * + * - Gradin + */ + + class StratFMS : public rclcpp::Node + { + public: + StratFMS(); + + void Init(); + + private: + // rclcpp::TimerBase::SharedPtr timer_; + + std::unique_ptr nav_; + std::unique_ptr mission_manager_; + std::unique_ptr action_executor_; + + }; +} diff --git a/src/modelec_strat/src/action_executor.cpp b/src/modelec_strat/src/action_executor.cpp new file mode 100644 index 0000000..4088875 --- /dev/null +++ b/src/modelec_strat/src/action_executor.cpp @@ -0,0 +1,17 @@ +#include + +namespace Modelec +{ + ActionExecutor::ActionExecutor() + { + } + + ActionExecutor::ActionExecutor(rclcpp::Node::SharedPtr node) : node_(std::move(node)) + { + } + + rclcpp::Node::SharedPtr ActionExecutor::getNode() const + { + return node_; + } +} diff --git a/src/modelec_strat/src/mission_manager.cpp b/src/modelec_strat/src/mission_manager.cpp new file mode 100644 index 0000000..731a1d7 --- /dev/null +++ b/src/modelec_strat/src/mission_manager.cpp @@ -0,0 +1,17 @@ +#include + +namespace Modelec +{ + MissionManager::MissionManager() + { + } + + MissionManager::MissionManager(rclcpp::Node::SharedPtr node) : node_(std::move(node)) + { + } + + rclcpp::Node::SharedPtr MissionManager::getNode() const + { + return node_; + } +} diff --git a/src/modelec_strat/src/navigation_helper.cpp b/src/modelec_strat/src/navigation_helper.cpp new file mode 100644 index 0000000..700982b --- /dev/null +++ b/src/modelec_strat/src/navigation_helper.cpp @@ -0,0 +1,182 @@ +#include +#include + +namespace Modelec { + NavigationHelper::NavigationHelper() + { + } + + NavigationHelper::NavigationHelper(rclcpp::Node::SharedPtr node) : node_(std::move(node)) + { + pathfinding_ = std::make_unique(std::move(node)); + + waypoint_reach_sub_ = node_->create_subscription( + "odometry/waypoint_reach", 10, + [this](const WaypointReachMsg::SharedPtr msg) { + OnWaypointReach(msg); + }); + + waypoint_pub_ = node_->create_publisher("odometry/add_waypoint", 30); + + pos_sub_ = node_->create_subscription( + "odometry/position", 20, + [this](const modelec_interfaces::msg::OdometryPos::SharedPtr msg) { + OnPos(msg); + }); + + go_to_sub_ = node_->create_subscription( + "nav/go_to", 10, [this](const PosMsg::SharedPtr msg) { + GoTo(msg); + }); + } + + rclcpp::Node::SharedPtr NavigationHelper::getNode() const + { + return node_; + } + + void NavigationHelper::SendWaypoint() const + { + for (auto & w : waypoints_) + { + waypoint_pub_->publish(w.toMsg()); + } + } + + void NavigationHelper::AddWaypoint(const PosMsg& pos, int index) + { + for (auto w = waypoints_.begin(); w != waypoints_.end(); ++w) + { + if (w->id == index) + { + waypoints_.insert(w, Waypoint(pos, index)); + + while (w != waypoints_.end()) + { + w->id++; + ++w; + } + + return; + } + } + waypoints_.back().is_end = false; + auto newWaypoint = Waypoint(pos, index); + newWaypoint.is_end = true; + waypoints_.emplace_back(newWaypoint); + } + + void NavigationHelper::AddWaypoint(const WaypointMsg& waypoint) + { + for (auto w = waypoints_.begin(); w != waypoints_.end(); ++w) + { + if (w->id == waypoint.id) + { + waypoints_.insert(w, Waypoint(waypoint)); + + while (w != waypoints_.end()) + { + w->id++; + ++w; + } + + return; + } + } + + waypoints_.back().is_end = false; + auto newWaypoint = Waypoint(waypoint); + newWaypoint.is_end = true; + waypoints_.emplace_back(newWaypoint); + } + + void NavigationHelper::AddWaypoints(const std::initializer_list& pos_list, int index) + { + for (auto w = waypoints_.begin(); w != waypoints_.end(); ++w) + { + if (w->id == index) + { + waypoints_.erase(w, waypoints_.end()); + + for (const auto& pos : pos_list) + { + waypoints_.emplace_back(pos, index); + index++; + } + } + } + } + + void NavigationHelper::AddWaypoints(const std::initializer_list& waypoint_list) + { + int index = waypoint_list.begin()->id; + for (auto w = waypoints_.begin(); w != waypoints_.end(); ++w) + { + if (w->id == index) + { + waypoints_.erase(w, waypoints_.end()); + + for (const auto& pos : waypoint_list) + { + waypoints_.emplace_back(pos); + index++; + } + } + } + waypoints_.back().is_end = false; + + for (auto & w : waypoint_list) + { + waypoints_.emplace_back(w); + } + waypoints_.back().is_end = true; + } + + void NavigationHelper::SetWaypoints(const std::list& waypoints) + { + waypoints_.clear(); + for (const auto& waypoint : waypoints) + { + waypoints_.emplace_back(waypoint); + } + } + + bool NavigationHelper::HasArrived() const + { + return waypoints_.back().reached; + } + + void NavigationHelper::GoTo(const PosMsg::SharedPtr& goal) + { + auto waypointList = FindPath(goal); + waypoints_.clear(); + for (const auto& waypoint : waypointList) + { + waypoints_.emplace_back(waypoint); + } + + SendWaypoint(); + } + + WaypointListMsg NavigationHelper::FindPath(const PosMsg::SharedPtr& goal) + { + return pathfinding_->FindPath(current_pos_, goal); + } + + void NavigationHelper::OnWaypointReach(const WaypointReachMsg::SharedPtr msg) + { + for (auto & waypoint : waypoints_) + { + if (waypoint.id == msg->id) + { + waypoint.reached = true; + } + } + } + + void NavigationHelper::OnPos(const PosMsg::SharedPtr msg) + { + current_pos_ = msg; + } + +} diff --git a/src/modelec_strat/src/pathfinding.cpp b/src/modelec_strat/src/pathfinding.cpp new file mode 100644 index 0000000..40e67e9 --- /dev/null +++ b/src/modelec_strat/src/pathfinding.cpp @@ -0,0 +1,61 @@ +#include + +namespace Modelec { + + Pathfinding::Pathfinding() + { + } + + Pathfinding::Pathfinding(rclcpp::Node::SharedPtr node) : node_(std::move(node)) + { + } + + rclcpp::Node::SharedPtr Pathfinding::getNode() const + { + return node_; + } + + 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)); + return waypoints; + } + + Waypoint::Waypoint(const modelec_interfaces::msg::OdometryPos& pos, int index, bool isLast) + { + id = index; + x = pos.x; + y = pos.y; + theta = pos.theta; + is_end = isLast; + reached = false; + } + + Waypoint::Waypoint(const WaypointMsg& waypoint) + { + id = waypoint.id; + x = waypoint.x; + y = waypoint.y; + theta = waypoint.theta; + is_end = waypoint.is_end; + reached = false; + } + + WaypointMsg Waypoint::toMsg() const + { + return static_cast>>(*this); + } +} diff --git a/src/modelec_strat/src/strat_fms.cpp b/src/modelec_strat/src/strat_fms.cpp new file mode 100644 index 0000000..c570f78 --- /dev/null +++ b/src/modelec_strat/src/strat_fms.cpp @@ -0,0 +1,26 @@ +#include + +namespace Modelec +{ + StratFMS::StratFMS() : Node("start_fms") + { + } + + void StratFMS::Init() + { + 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"); + } +} + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + node->Init(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +}