diff --git a/simulated_pcb/odo.py b/simulated_pcb/odo.py index 0a454d9..b352f7a 100644 --- a/simulated_pcb/odo.py +++ b/simulated_pcb/odo.py @@ -4,6 +4,7 @@ import math import threading import argparse + class SimulatedPCB: def __init__(self, port='/dev/pts/6', baud=115200): self.ser = serial.Serial(port, baud, timeout=1) diff --git a/src/modelec_com/CMakeLists.txt b/src/modelec_com/CMakeLists.txt index d6867bc..f6f6d7d 100644 --- a/src/modelec_com/CMakeLists.txt +++ b/src/modelec_com/CMakeLists.txt @@ -18,7 +18,7 @@ find_package(modelec_interfaces REQUIRED) find_package(modelec_utils REQUIRED) find_library(WIRINGPI_LIB wiringPi) -# USB Arduino Listener Node +# Serial listener Node add_executable(serial_listener src/multiple_serial_listener.cpp) ament_target_dependencies(serial_listener rclcpp std_msgs modelec_interfaces ament_index_cpp) target_link_libraries(serial_listener Boost::system modelec_utils::utils modelec_utils::config) @@ -27,7 +27,7 @@ target_include_directories(serial_listener PUBLIC $ ) -# USB Arduino Logic Processor Node +# PCB Odometry Node add_executable(pcb_odo_interface src/pcb_odo_interface.cpp) ament_target_dependencies(pcb_odo_interface rclcpp std_msgs modelec_interfaces ament_index_cpp) target_link_libraries(pcb_odo_interface modelec_utils::utils modelec_utils::config) @@ -36,7 +36,7 @@ target_include_directories(pcb_odo_interface PUBLIC $ ) -# USB Arduino Logic Processor Node +# PCB Alim Node add_executable(pcb_alim_interface src/pcb_alim_interface.cpp) ament_target_dependencies(pcb_alim_interface rclcpp std_msgs modelec_interfaces ament_index_cpp) target_link_libraries(pcb_alim_interface modelec_utils::utils modelec_utils::config) @@ -45,6 +45,15 @@ target_include_directories(pcb_alim_interface PUBLIC $ ) +# PCB Action Node +add_executable(pcb_action_interface src/pcb_action_interface.cpp) +ament_target_dependencies(pcb_action_interface rclcpp std_msgs modelec_interfaces ament_index_cpp) +target_link_libraries(pcb_action_interface modelec_utils::utils modelec_utils::config) +target_include_directories(pcb_action_interface PUBLIC + $ + $ +) + # PCA9685 Listener Node add_executable(pca9685_controller src/pca9685_controller.cpp) ament_target_dependencies(pca9685_controller rclcpp std_msgs modelec_interfaces) diff --git a/src/modelec_com/include/modelec_com/multiple_serial_listener.hpp b/src/modelec_com/include/modelec_com/multiple_serial_listener.hpp index 538f7d4..37cba05 100644 --- a/src/modelec_com/include/modelec_com/multiple_serial_listener.hpp +++ b/src/modelec_com/include/modelec_com/multiple_serial_listener.hpp @@ -12,64 +12,65 @@ namespace Modelec { -class SerialListener { -private: - bool status_; - int bauds_; - std::string serial_port_; - int max_message_len_; + class SerialListener + { + private: + bool status_; + int bauds_; + std::string serial_port_; + int max_message_len_; - boost::asio::io_service& io_; - std::vector read_buffer_; - std::deque write_queue_; - std::mutex write_mutex_; - std::thread io_thread_; + boost::asio::io_service& io_; + std::vector read_buffer_; + std::deque write_queue_; + std::mutex write_mutex_; + std::thread io_thread_; - void start_async_read(); - void start_async_write(); + void start_async_read(); + void start_async_write(); -public: - std::string name_; - boost::asio::serial_port port_; + public: + std::string name_; + boost::asio::serial_port port_; - rclcpp::Publisher::SharedPtr publisher_; - rclcpp::Subscription::SharedPtr subscriber_; + rclcpp::Publisher::SharedPtr publisher_; + rclcpp::Subscription::SharedPtr subscriber_; - SerialListener(const std::string& name, int bauds, const std::string& serial_port, - int max_message_len, boost::asio::io_service& io); + SerialListener(const std::string& name, int bauds, const std::string& serial_port, + int max_message_len, boost::asio::io_service& io); - ~SerialListener(); + ~SerialListener(); - void close(); + void close(); - void SetMaxMessageLen(int max_message_len) { max_message_len_ = max_message_len; } - bool IsOk() const { return status_; } + void SetMaxMessageLen(int max_message_len) { max_message_len_ = max_message_len; } + bool IsOk() const { return status_; } - void SetOk() { status_ = true; } + void SetOk() { status_ = true; } - void write(std_msgs::msg::String::SharedPtr msg); -}; + void write(std_msgs::msg::String::SharedPtr msg); + }; -class MultipleSerialListener : public rclcpp::Node -{ -public: - MultipleSerialListener(); - ~MultipleSerialListener() override; + class MultipleSerialListener : public rclcpp::Node + { + public: + MultipleSerialListener(); + ~MultipleSerialListener() override; -private: - int default_max_message_len_ = MAX_MESSAGE_LEN; + private: + int default_max_message_len_ = MAX_MESSAGE_LEN; - std::map> serial_listeners; + std::map> serial_listeners; - boost::asio::io_service io; + boost::asio::io_service io; - rclcpp::Service::SharedPtr add_serial_listener_service_; - void add_serial_listener( - const std::shared_ptr request, - std::shared_ptr response); + rclcpp::Service::SharedPtr add_serial_listener_service_; + void add_serial_listener( + const std::shared_ptr request, + std::shared_ptr response); - OnSetParametersCallbackHandle::SharedPtr parameter_callback_handle_; - rcl_interfaces::msg::SetParametersResult onParameterChange(const std::vector ¶meters); - void updateConnection(); -}; -} // namespace Modelec + OnSetParametersCallbackHandle::SharedPtr parameter_callback_handle_; + rcl_interfaces::msg::SetParametersResult onParameterChange(const std::vector& parameters); + void updateConnection(); + }; +} // namespace Modelec diff --git a/src/modelec_com/include/modelec_com/pca9685_controller.hpp b/src/modelec_com/include/modelec_com/pca9685_controller.hpp index 1806519..720a75d 100644 --- a/src/modelec_com/include/modelec_com/pca9685_controller.hpp +++ b/src/modelec_com/include/modelec_com/pca9685_controller.hpp @@ -17,16 +17,18 @@ #define SERVO_MAX 492 #define PCA9685_ADDR 0x40 // I2C address for the PCA9685 -namespace Modelec { - class PCA9685Controller : public rclcpp::Node { +namespace Modelec +{ + class PCA9685Controller : public rclcpp::Node + { public: PCA9685Controller(); private: - int fd; // File descriptor for I2C communication + int fd; // File descriptor for I2C communication rclcpp::Subscription::SharedPtr servo_subscriber_; rclcpp::Subscription::SharedPtr clear_subscriber_; - int frequency = 50; // Default PWM frequency (Hz) + int frequency = 50; // Default PWM frequency (Hz) std::unordered_set active_servos; @@ -34,7 +36,7 @@ namespace Modelec { rclcpp::Service::SharedPtr add_servo_service_; OnSetParametersCallbackHandle::SharedPtr parameter_callback_handle_; - rcl_interfaces::msg::SetParametersResult onParameterChange(const std::vector ¶meters); + rcl_interfaces::msg::SetParametersResult onParameterChange(const std::vector& parameters); void updatePCA9685(); // PCA9685 configuration and control diff --git a/src/modelec_com/include/modelec_com/pcb_action_interface.hpp b/src/modelec_com/include/modelec_com/pcb_action_interface.hpp new file mode 100644 index 0000000..11b7f5b --- /dev/null +++ b/src/modelec_com/include/modelec_com/pcb_action_interface.hpp @@ -0,0 +1,33 @@ +#pragma once + +#include +#include +#include + +namespace Modelec +{ + class PCBActionInterface : public rclcpp::Node + { + public: + PCBActionInterface(); + rclcpp::CallbackGroup::SharedPtr pcb_callback_group_; + std::shared_ptr pcb_executor_; + std::thread pcb_executor_thread_; + + ~PCBActionInterface() override; + + private: + rclcpp::Publisher::SharedPtr pcb_publisher_; + rclcpp::Subscription::SharedPtr pcb_subscriber_; + + void PCBCallback(const std_msgs::msg::String::SharedPtr msg); + + public: + void SendToPCB(const std::string& data) const; + void SendToPCB(const std::string& order, const std::string& elem, + const std::vector& data = {}) const; + + void GetData(const std::string& elem, const std::vector& data = {}) const; + void SendOrder(const std::string& elem, const std::vector& data = {}) const; + }; +} \ No newline at end of file diff --git a/src/modelec_com/include/modelec_com/pcb_alim_interface.hpp b/src/modelec_com/include/modelec_com/pcb_alim_interface.hpp index 4617488..b3ff29b 100644 --- a/src/modelec_com/include/modelec_com/pcb_alim_interface.hpp +++ b/src/modelec_com/include/modelec_com/pcb_alim_interface.hpp @@ -2,6 +2,7 @@ #include #include +#include #include #include #include @@ -11,96 +12,96 @@ namespace Modelec { -class PCBAlimInterface : public rclcpp::Node -{ -public: - PCBAlimInterface(); - rclcpp::CallbackGroup::SharedPtr pcb_callback_group_; - std::shared_ptr pcb_executor_; - std::thread pcb_executor_thread_; - - ~PCBAlimInterface() override; - - struct PCBData + class PCBAlimInterface : public rclcpp::Node { - bool success; - int value; + public: + PCBAlimInterface(); + rclcpp::CallbackGroup::SharedPtr pcb_callback_group_; + std::shared_ptr pcb_executor_; + std::thread pcb_executor_thread_; + + ~PCBAlimInterface() override; + + struct PCBData + { + bool success; + int value; + }; + + struct PCBBau + { + bool success; + bool activate; + }; + + private: + rclcpp::Publisher::SharedPtr pcb_publisher_; + rclcpp::Subscription::SharedPtr pcb_subscriber_; + + void PCBCallback(const std_msgs::msg::String::SharedPtr msg); + + rclcpp::Subscription::SharedPtr pcb_emg_subscriber_; + + void PCBEmgCallback(const modelec_interfaces::msg::AlimEmg::SharedPtr msg) const; + + rclcpp::Service::SharedPtr pcb_out_service_; + rclcpp::Service::SharedPtr pcb_in_service_; + rclcpp::Service::SharedPtr pcb_bau_service_; + rclcpp::Service::SharedPtr pcb_emg_service_; + rclcpp::Service::SharedPtr pcb_temp_service_; + + std::queue> pcb_out_promises_; + std::mutex pcb_out_mutex_; + + std::queue> pcb_in_promises_; + std::mutex pcb_in_mutex_; + + std::queue> pcb_bau_promises_; + std::mutex pcb_bau_mutex_; + + std::queue> pcb_emg_promises_; + std::mutex pcb_emg_mutex_; + + std::queue> pcb_temp_promises_; + std::mutex pcb_temp_mutex_; + + void HandleGetPCBOutData( + const std::shared_ptr request, + std::shared_ptr response); + + void HandleSetPCBOutData( + const std::shared_ptr request, + std::shared_ptr response); + + void HandleGetPCBInData( + const std::shared_ptr request, + std::shared_ptr response); + + void HandleGetPCBBauData( + const std::shared_ptr request, + std::shared_ptr response); + + void HandleSetPCBEmgData( + const std::shared_ptr request, + std::shared_ptr response); + + void HandleGetPCBTempData( + const std::shared_ptr request, + std::shared_ptr response); + + void ResolveGetPCBOutRequest(const PCBData& value); + void ResolveSetPCBOutRequest(const PCBData& value); + void ResolveGetPCBInRequest(const PCBData& value); + void ResolveGetPCBBauRequest(const PCBBau& value); + void ResolveSetPCBEmgRequest(bool value); + void ResolveGetPCBTempRequest(const PCBData& value); + + public: + void SendToPCB(const std::string& data) const; + void SendToPCB(const std::string& order, const std::string& elem, + const std::vector& data = {}) const; + + void GetData(const std::string& elem, const std::vector& data = {}) const; + void SendOrder(const std::string& elem, const std::vector& data = {}) const; }; - - struct PCBBau - { - bool success; - bool activate; - }; -private: - - rclcpp::Publisher::SharedPtr pcb_publisher_; - rclcpp::Subscription::SharedPtr pcb_subscriber_; - - void PCBCallback(const std_msgs::msg::String::SharedPtr msg); - - rclcpp::Subscription::SharedPtr pcb_emg_subscriber_; - - void PCBEmgCallback(const modelec_interfaces::msg::AlimEmg::SharedPtr msg) const; - - rclcpp::Service::SharedPtr pcb_out_service_; - rclcpp::Service::SharedPtr pcb_in_service_; - rclcpp::Service::SharedPtr pcb_bau_service_; - rclcpp::Service::SharedPtr pcb_emg_service_; - rclcpp::Service::SharedPtr pcb_temp_service_; - - std::queue> pcb_out_promises_; - std::mutex pcb_out_mutex_; - - std::queue> pcb_in_promises_; - std::mutex pcb_in_mutex_; - - std::queue> pcb_bau_promises_; - std::mutex pcb_bau_mutex_; - - std::queue> pcb_emg_promises_; - std::mutex pcb_emg_mutex_; - - std::queue> pcb_temp_promises_; - std::mutex pcb_temp_mutex_; - - void HandleGetPCBOutData( - const std::shared_ptr request, - std::shared_ptr response); - - void HandleSetPCBOutData( - const std::shared_ptr request, - std::shared_ptr response); - - void HandleGetPCBInData( - const std::shared_ptr request, - std::shared_ptr response); - - void HandleGetPCBBauData( - const std::shared_ptr request, - std::shared_ptr response); - - void HandleSetPCBEmgData( - const std::shared_ptr request, - std::shared_ptr response); - - void HandleGetPCBTempData( - const std::shared_ptr request, - std::shared_ptr response); - - void ResolveGetPCBOutRequest(const PCBData& value); - void ResolveSetPCBOutRequest(const PCBData& value); - void ResolveGetPCBInRequest(const PCBData& value); - void ResolveGetPCBBauRequest(const PCBBau& value); - void ResolveSetPCBEmgRequest(bool value); - void ResolveGetPCBTempRequest(const PCBData& value); - -public: - void SendToPCB(const std::string &data) const; - void SendToPCB(const std::string& order, const std::string& elem, const std::vector& data = {}) const; - - void GetData(const std::string& elem, const std::vector& data = {}) const; - void SendOrder(const std::string &elem, const std::vector &data = {}) const; - -}; -} // namespace Modelec +} // namespace Modelec diff --git a/src/modelec_com/include/modelec_com/pcb_odo_interface.hpp b/src/modelec_com/include/modelec_com/pcb_odo_interface.hpp index c8b2834..87697ee 100644 --- a/src/modelec_com/include/modelec_com/pcb_odo_interface.hpp +++ b/src/modelec_com/include/modelec_com/pcb_odo_interface.hpp @@ -25,137 +25,139 @@ #include #include -namespace Modelec { - -class PCBOdoInterface : public rclcpp::Node { -public: - PCBOdoInterface(); - - rclcpp::CallbackGroup::SharedPtr pcb_callback_group_; - std::shared_ptr pcb_executor_; - std::thread pcb_executor_thread_; - ~PCBOdoInterface() override; - - struct OdometryData { - long x; - long y; - double theta; - }; - - struct PIDData +namespace Modelec +{ + class PCBOdoInterface : public rclcpp::Node { - float p; - float i; - float d; + public: + PCBOdoInterface(); + + rclcpp::CallbackGroup::SharedPtr pcb_callback_group_; + std::shared_ptr pcb_executor_; + std::thread pcb_executor_thread_; + ~PCBOdoInterface() override; + + struct OdometryData + { + long x; + long y; + double theta; + }; + + struct PIDData + { + float p; + float i; + float d; + }; + + private: + rclcpp::Publisher::SharedPtr pcb_publisher_; + rclcpp::Subscription::SharedPtr pcb_subscriber_; + + void PCBCallback(const std_msgs::msg::String::SharedPtr msg); + + rclcpp::Publisher::SharedPtr odo_pos_publisher_; + rclcpp::Publisher::SharedPtr odo_speed_publisher_; + rclcpp::Publisher::SharedPtr odo_tof_publisher_; + rclcpp::Publisher::SharedPtr odo_waypoint_reach_publisher_; + rclcpp::Publisher::SharedPtr odo_pid_publisher_; + + rclcpp::Subscription::SharedPtr odo_add_waypoint_subscriber_; + rclcpp::Subscription::SharedPtr odo_set_pos_subscriber_; + rclcpp::Subscription::SharedPtr odo_set_pid_subscriber_; + + void AddWaypointCallback(const modelec_interfaces::msg::OdometryAddWaypoint::SharedPtr msg) const; + void SetPosCallback(const modelec_interfaces::msg::OdometryPos::SharedPtr msg) const; + void SetPIDCallback(const modelec_interfaces::msg::OdometryPid::SharedPtr msg) const; + + rclcpp::Service::SharedPtr get_tof_service_; + rclcpp::Service::SharedPtr get_speed_service_; + rclcpp::Service::SharedPtr get_position_service_; + rclcpp::Service::SharedPtr set_start_service_; + rclcpp::Service::SharedPtr get_pid_service_; + rclcpp::Service::SharedPtr set_pid_service_; + rclcpp::Service::SharedPtr add_waypoint_service_; + + // Promises and mutexes to synchronize service responses asynchronously + std::queue> tof_promises_; + std::mutex tof_mutex_; + + std::queue> speed_promises_; + std::mutex speed_mutex_; + + std::queue> pos_promises_; + std::mutex pos_mutex_; + + std::queue> start_promises_; + std::mutex start_mutex_; + + std::queue> get_pid_promises_; + std::mutex get_pid_mutex_; + + std::queue> set_pid_promises_; + std::mutex set_pid_mutex_; + + std::queue> add_waypoint_promises_; + std::mutex add_waypoint_mutex_; + + // Service handlers using async wait with promises + void HandleGetTof(const std::shared_ptr request, + std::shared_ptr response); + + void HandleGetSpeed(const std::shared_ptr request, + std::shared_ptr response); + + void HandleGetPosition(const std::shared_ptr request, + std::shared_ptr response); + + void HandleGetStart(const std::shared_ptr request, + std::shared_ptr response); + + void HandleGetPID(const std::shared_ptr request, + std::shared_ptr response); + + void HandleSetPID(const std::shared_ptr request, + std::shared_ptr response); + + void HandleAddWaypoint(const std::shared_ptr request, + std::shared_ptr response); + + // Resolving methods called by subscriber callback + void ResolveToFRequest(long distance); + void ResolveSpeedRequest(const OdometryData& speed); + void ResolvePositionRequest(const OdometryData& position); + void ResolveStartRequest(bool start); + void ResolveGetPIDRequest(const PIDData& pid); + void ResolveSetPIDRequest(bool success); + void ResolveAddWaypointRequest(bool success); + + int timeout_ms = 1000; + int attempt = 5; + + public: + void SendToPCB(const std::string& data) const; + void SendToPCB(const std::string& order, const std::string& elem, + const std::vector& data = {}) const; + + void GetData(const std::string& elem, const std::vector& data = {}) const; + void SendOrder(const std::string& elem, const std::vector& data = {}) const; + + void GetPos() const; + void GetSpeed() const; + void GetToF(const int& tof) const; + + void SetRobotPos(const modelec_interfaces::msg::OdometryPos::SharedPtr msg) const; + void SetRobotPos(long x, long y, double theta) const; + + void AddWaypoint(modelec_interfaces::msg::OdometryAddWaypoint::SharedPtr msg) const; + void AddWaypoint(int index, bool IsStopPoint, long x, long y, double theta) const; + + void SetStart(const modelec_interfaces::msg::OdometryStart::SharedPtr msg) const; + void SetStart(bool start) const; + + void GetPID() const; + void SetPID(const modelec_interfaces::msg::OdometryPid::SharedPtr msg) const; + void SetPID(float p, float i, float d) const; }; - -private: - rclcpp::Publisher::SharedPtr pcb_publisher_; - rclcpp::Subscription::SharedPtr pcb_subscriber_; - - void PCBCallback(const std_msgs::msg::String::SharedPtr msg); - - rclcpp::Publisher::SharedPtr odo_pos_publisher_; - rclcpp::Publisher::SharedPtr odo_speed_publisher_; - rclcpp::Publisher::SharedPtr odo_tof_publisher_; - rclcpp::Publisher::SharedPtr odo_waypoint_reach_publisher_; - rclcpp::Publisher::SharedPtr odo_pid_publisher_; - - rclcpp::Subscription::SharedPtr odo_add_waypoint_subscriber_; - rclcpp::Subscription::SharedPtr odo_set_pos_subscriber_; - rclcpp::Subscription::SharedPtr odo_set_pid_subscriber_; - - void AddWaypointCallback(const modelec_interfaces::msg::OdometryAddWaypoint::SharedPtr msg) const; - void SetPosCallback(const modelec_interfaces::msg::OdometryPos::SharedPtr msg) const; - void SetPIDCallback(const modelec_interfaces::msg::OdometryPid::SharedPtr msg) const; - - rclcpp::Service::SharedPtr get_tof_service_; - rclcpp::Service::SharedPtr get_speed_service_; - rclcpp::Service::SharedPtr get_position_service_; - rclcpp::Service::SharedPtr set_start_service_; - rclcpp::Service::SharedPtr get_pid_service_; - rclcpp::Service::SharedPtr set_pid_service_; - rclcpp::Service::SharedPtr add_waypoint_service_; - - // Promises and mutexes to synchronize service responses asynchronously - std::queue> tof_promises_; - std::mutex tof_mutex_; - - std::queue> speed_promises_; - std::mutex speed_mutex_; - - std::queue> pos_promises_; - std::mutex pos_mutex_; - - std::queue> start_promises_; - std::mutex start_mutex_; - - std::queue> get_pid_promises_; - std::mutex get_pid_mutex_; - - std::queue> set_pid_promises_; - std::mutex set_pid_mutex_; - - std::queue> add_waypoint_promises_; - std::mutex add_waypoint_mutex_; - - // Service handlers using async wait with promises - void HandleGetTof(const std::shared_ptr request, - std::shared_ptr response); - - void HandleGetSpeed(const std::shared_ptr request, - std::shared_ptr response); - - void HandleGetPosition(const std::shared_ptr request, - std::shared_ptr response); - - void HandleGetStart(const std::shared_ptr request, - std::shared_ptr response); - - void HandleGetPID(const std::shared_ptr request, - std::shared_ptr response); - - void HandleSetPID(const std::shared_ptr request, - std::shared_ptr response); - - void HandleAddWaypoint(const std::shared_ptr request, - std::shared_ptr response); - - // Resolving methods called by subscriber callback - void ResolveToFRequest(long distance); - void ResolveSpeedRequest(const OdometryData& speed); - void ResolvePositionRequest(const OdometryData& position); - void ResolveStartRequest(bool start); - void ResolveGetPIDRequest(const PIDData& pid); - void ResolveSetPIDRequest(bool success); - void ResolveAddWaypointRequest(bool success); - - int timeout_ms = 1000; - int attempt = 5; - -public: - void SendToPCB(const std::string &data) const; - void SendToPCB(const std::string& order, const std::string& elem, const std::vector& data = {}) const; - - void GetData(const std::string& elem, const std::vector& data = {}) const; - void SendOrder(const std::string &elem, const std::vector &data = {}) const; - - void GetPos() const; - void GetSpeed() const; - void GetToF(const int &tof) const; - - void SetRobotPos(const modelec_interfaces::msg::OdometryPos::SharedPtr msg) const; - void SetRobotPos(long x, long y, double theta) const; - - void AddWaypoint(modelec_interfaces::msg::OdometryAddWaypoint::SharedPtr msg) const; - void AddWaypoint(int index, bool IsStopPoint, long x, long y, double theta) const; - - void SetStart(const modelec_interfaces::msg::OdometryStart::SharedPtr msg) const; - void SetStart(bool start) const; - - void GetPID() const; - void SetPID(const modelec_interfaces::msg::OdometryPid::SharedPtr msg) const; - void SetPID(float p, float i, float d) const; -}; - } // namespace Modelec diff --git a/src/modelec_com/src/multiple_serial_listener.cpp b/src/modelec_com/src/multiple_serial_listener.cpp index 66aede7..5b4340c 100644 --- a/src/modelec_com/src/multiple_serial_listener.cpp +++ b/src/modelec_com/src/multiple_serial_listener.cpp @@ -27,10 +27,14 @@ namespace Modelec read_buffer_.resize(max_message_len_); start_async_read(); - io_thread_ = std::thread([this]() { - try { + io_thread_ = std::thread([this]() + { + try + { io_.run(); - } catch (const std::exception& e) { + } + catch (const std::exception& e) + { RCLCPP_ERROR(rclcpp::get_logger("SerialListener"), "IO thread exception: %s", e.what()); } }); @@ -46,7 +50,8 @@ namespace Modelec void SerialListener::close() { - if (status_) { + if (status_) + { if (port_.is_open()) port_.close(); io_.stop(); if (io_thread_.joinable()) io_thread_.join(); @@ -60,8 +65,10 @@ namespace Modelec port_.async_read_some( boost::asio::buffer(read_buffer_), - [this](const boost::system::error_code& ec, std::size_t bytes_transferred) { - if (!ec && bytes_transferred > 0) { + [this](const boost::system::error_code& ec, std::size_t bytes_transferred) + { + if (!ec && bytes_transferred > 0) + { auto msg = std_msgs::msg::String(); msg.data = std::string(read_buffer_.begin(), read_buffer_.begin() + bytes_transferred); if (publisher_) @@ -69,8 +76,10 @@ namespace Modelec publisher_->publish(msg); } - start_async_read(); // continue reading - } else { + start_async_read(); // continue reading + } + else + { RCLCPP_ERROR(rclcpp::get_logger("SerialListener"), "Async read error: %s", ec.message().c_str()); } }); @@ -82,7 +91,8 @@ namespace Modelec bool write_in_progress = !write_queue_.empty(); write_queue_.push_back(msg->data); - if (!write_in_progress) { + if (!write_in_progress) + { start_async_write(); } } @@ -94,14 +104,19 @@ namespace Modelec boost::asio::async_write( port_, boost::asio::buffer(write_queue_.front()), - [this](const boost::system::error_code& ec, std::size_t /*length*/) { + [this](const boost::system::error_code& ec, std::size_t /*length*/) + { std::lock_guard lock(write_mutex_); - if (!ec) { + if (!ec) + { write_queue_.pop_front(); - if (!write_queue_.empty()) { - start_async_write(); // continue writing + if (!write_queue_.empty()) + { + start_async_write(); // continue writing } - } else { + } + else + { RCLCPP_ERROR(rclcpp::get_logger("SerialListener"), "Async write error: %s", ec.message().c_str()); } }); @@ -157,7 +172,8 @@ namespace Modelec response->publisher = listener->publisher_->get_topic_name(); response->subscriber = listener->subscriber_->get_topic_name(); - RCLCPP_INFO(rclcpp::get_logger("MultipleSerialListener"), "Serial listener %s fully created", request->name.c_str()); + RCLCPP_INFO(rclcpp::get_logger("MultipleSerialListener"), "Serial listener %s fully created", + request->name.c_str()); } rcl_interfaces::msg::SetParametersResult MultipleSerialListener::onParameterChange( @@ -168,7 +184,8 @@ namespace Modelec for (const auto& parameter : parameters) { - if (parameter.get_name() == "bauds" || parameter.get_name() == "serial_port" || parameter.get_name() == "max_message_len") + if (parameter.get_name() == "bauds" || parameter.get_name() == "serial_port" || parameter.get_name() == + "max_message_len") { updateConnection(); } diff --git a/src/modelec_com/src/pca9685_controller.cpp b/src/modelec_com/src/pca9685_controller.cpp index 5d0d842..835bb63 100644 --- a/src/modelec_com/src/pca9685_controller.cpp +++ b/src/modelec_com/src/pca9685_controller.cpp @@ -1,16 +1,19 @@ #include "modelec_com/pca9685_controller.hpp" -namespace Modelec { - - PCA9685Controller::PCA9685Controller() : Node("pca9685_controller") { - if (wiringPiSetup() == -1) { +namespace Modelec +{ + PCA9685Controller::PCA9685Controller() : Node("pca9685_controller") + { + if (wiringPiSetup() == -1) + { RCLCPP_ERROR(this->get_logger(), "WiringPi setup failed."); return; } // Initialize I2C communication with PCA9685 fd = wiringPiI2CSetup(PCA9685_ADDR); - if (fd == -1) { + if (fd == -1) + { RCLCPP_ERROR(this->get_logger(), "Failed to initialize I2C communication with PCA9685."); return; } @@ -19,8 +22,10 @@ namespace Modelec { // Subscribe to topics for servo and solenoid control servo_subscriber_ = this->create_subscription( - "servo_control", 10, [this](const modelec_interfaces::msg::PCA9685Servo::SharedPtr msg) { - if (active_servos.find(msg->pin) == active_servos.end()) { + "servo_control", 10, [this](const modelec_interfaces::msg::PCA9685Servo::SharedPtr msg) + { + if (active_servos.find(msg->pin) == active_servos.end()) + { RCLCPP_ERROR(this->get_logger(), "Servo on pin %d is not active.", msg->pin); return; } @@ -28,17 +33,22 @@ namespace Modelec { }); clear_subscriber_ = this->create_subscription( - "clear_pca9685", 10, [this](const std_msgs::msg::Empty::SharedPtr) { + "clear_pca9685", 10, [this](const std_msgs::msg::Empty::SharedPtr) + { this->clearAllDevices(); }); add_servo_service_ = this->create_service( "add_servo", [this](const modelec_interfaces::srv::AddServoMotor::Request::SharedPtr request, - modelec_interfaces::srv::AddServoMotor::Response::SharedPtr response) { - if (active_servos.find(request->pin) == active_servos.end()) { + modelec_interfaces::srv::AddServoMotor::Response::SharedPtr response) + { + if (active_servos.find(request->pin) == active_servos.end()) + { active_servos.insert(request->pin); response->success = true; - } else { + } + else + { response->success = false; } }); @@ -50,7 +60,8 @@ namespace Modelec { // Handle dynamic parameter changes parameter_callback_handle_ = this->add_on_set_parameters_callback( - [this](const std::vector ¶meters) { + [this](const std::vector& parameters) + { return onParameterChange(parameters); }); @@ -58,19 +69,22 @@ namespace Modelec { } // Initialize PCA9685 in normal operation mode - void PCA9685Controller::initializePCA9685() { + void PCA9685Controller::initializePCA9685() + { writePCA9685Register(0x00, 0x00); } // Configure the PWM frequency - void PCA9685Controller::configurePCA9685Frequency(int frequency) { + void PCA9685Controller::configurePCA9685Frequency(int frequency) + { int prescale = static_cast(PCA9685_FREQUENCY / (PCA9685_RESOLUTION * frequency) - 1); - writePCA9685Register(0x00, 0x10); // Enter sleep mode - writePCA9685Register(0xFE, prescale); // Set prescale value - writePCA9685Register(0x00, 0x00); // Restart PCA9685 + writePCA9685Register(0x00, 0x10); // Enter sleep mode + writePCA9685Register(0xFE, prescale); // Set prescale value + writePCA9685Register(0x00, 0x00); // Restart PCA9685 } - void PCA9685Controller::SetPCA9685PWM(int channel, int on_time, int off_time) { + void PCA9685Controller::SetPCA9685PWM(int channel, int on_time, int off_time) + { wiringPiI2CWriteReg8(fd, 0x06 + 4 * channel, on_time & 0xFF); wiringPiI2CWriteReg8(fd, 0x07 + 4 * channel, on_time >> 8); wiringPiI2CWriteReg8(fd, 0x08 + 4 * channel, off_time & 0xFF); @@ -78,32 +92,41 @@ namespace Modelec { } // Set the servo angle on a specific channel - void PCA9685Controller::setServoPWM(int channel, double angle) { + void PCA9685Controller::setServoPWM(int channel, double angle) + { int on_time = static_cast(SERVO_MIN + (SERVO_MAX - SERVO_MIN) * angle / 180); SetPCA9685PWM(channel, 0, on_time); } // Clear all channels (reset devices) - void PCA9685Controller::clearAllDevices() { - for (int channel = 0; channel < 16; ++channel) { + void PCA9685Controller::clearAllDevices() + { + for (int channel = 0; channel < 16; ++channel) + { SetPCA9685PWM(channel, 0, 0); } } // Write to a specific PCA9685 register - void PCA9685Controller::writePCA9685Register(uint8_t reg, uint8_t value) { - if (wiringPiI2CWriteReg8(fd, reg, value) == -1) { + void PCA9685Controller::writePCA9685Register(uint8_t reg, uint8_t value) + { + if (wiringPiI2CWriteReg8(fd, reg, value) == -1) + { RCLCPP_ERROR(this->get_logger(), "Failed to write to register 0x%02X", reg); } } // Handle dynamic parameter changes - rcl_interfaces::msg::SetParametersResult PCA9685Controller::onParameterChange(const std::vector ¶meters) { + rcl_interfaces::msg::SetParametersResult PCA9685Controller::onParameterChange( + const std::vector& parameters) + { rcl_interfaces::msg::SetParametersResult result; result.successful = true; - for (const auto ¶meter : parameters) { - if (parameter.get_name() == "frequency") { + for (const auto& parameter : parameters) + { + if (parameter.get_name() == "frequency") + { updatePCA9685(); } } @@ -112,18 +135,21 @@ namespace Modelec { } // Update PCA9685 when frequency parameter changes - void PCA9685Controller::updatePCA9685() { + void PCA9685Controller::updatePCA9685() + { int new_frequency = this->get_parameter("frequency").as_int(); - if (new_frequency != frequency) { + if (new_frequency != frequency) + { frequency = new_frequency; configurePCA9685Frequency(frequency); } } } -int main(int argc, char **argv) { +int main(int argc, char** argv) +{ rclcpp::init(argc, argv); rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; -} \ No newline at end of file +} diff --git a/src/modelec_com/src/pcb_action_interface.cpp b/src/modelec_com/src/pcb_action_interface.cpp new file mode 100644 index 0000000..a20d453 --- /dev/null +++ b/src/modelec_com/src/pcb_action_interface.cpp @@ -0,0 +1,137 @@ +#include +#include +#include +#include + +namespace Modelec +{ + PCBActionInterface::PCBActionInterface() : Node("pcb_action_interface") + { + // Service to create a new serial listener + 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()); + } + + auto request = std::make_shared(); + request->name = Config::get("config.usb.pcb.pcb_action.name", "pcb_action"); + request->bauds = Config::get("config.usb.pcb.pcb_action.baudrate", 115200); + request->serial_port = Config::get("config.usb.pcb.pcb_action.port", "/dev/ttyUSB0"); + + auto client = this->create_client("add_serial_listener"); + while (!client->wait_for_service(std::chrono::seconds(1))) + { + if (!rclcpp::ok()) + { + RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service. Exiting."); + return; + } + RCLCPP_INFO(this->get_logger(), "service not available, waiting again..."); + } + auto result = client->async_send_request(request); + if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), result) == + rclcpp::FutureReturnCode::SUCCESS) + { + if (auto res = result.get()) + { + if (res->success) + { + pcb_callback_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + rclcpp::SubscriptionOptions options; + options.callback_group = pcb_callback_group_; + + pcb_subscriber_ = this->create_subscription( + res->publisher, 10, + [this](const std_msgs::msg::String::SharedPtr msg) + { + PCBCallback(msg); + }, options); + + pcb_executor_ = std::make_shared(); + pcb_executor_->add_callback_group(pcb_callback_group_, this->get_node_base_interface()); + + pcb_executor_thread_ = std::thread([this]() + { + pcb_executor_->spin(); + }); + + pcb_publisher_ = this->create_publisher(res->subscriber, 10); + } + else + { + RCLCPP_ERROR(this->get_logger(), "Failed to add serial listener"); + } + } + else + { + RCLCPP_ERROR(this->get_logger(), "Failed to ask for a serial listener"); + } + } + else + { + RCLCPP_ERROR(this->get_logger(), "Service call failed"); + } + } + + PCBActionInterface::~PCBActionInterface() + { + pcb_executor_->cancel(); + if (pcb_executor_thread_.joinable()) + { + pcb_executor_thread_.join(); + } + } + + void PCBActionInterface::PCBCallback(const std_msgs::msg::String::SharedPtr msg) + { + RCLCPP_DEBUG(this->get_logger(), "Received message: '%s'", msg->data.c_str()); + std::vector tokens = split(msg->data, ';'); + } + + void PCBActionInterface::SendToPCB(const std::string& data) const + { + auto message = std_msgs::msg::String(); + message.data = data; + pcb_publisher_->publish(message); + } + + void PCBActionInterface::SendToPCB(const std::string& order, const std::string& elem, + const std::vector& data) const + { + std::string command = order + ";" + elem; + for (const auto& d : data) + { + command += ";" + d; + } + command += "\n"; + + SendToPCB(command); + } + + void PCBActionInterface::GetData(const std::string& elem, const std::vector& data) const + { + SendToPCB("GET", elem, data); + } + + void PCBActionInterface::SendOrder(const std::string& elem, const std::vector& data) const + { + SendToPCB("SET", elem, data); + } +} + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + + // Increase number of threads explicitly! + rclcpp::executors::MultiThreadedExecutor executor( + rclcpp::ExecutorOptions(), 2 /* or more threads! */); + + executor.add_node(node); + executor.spin(); + rclcpp::shutdown(); + return 0; +} diff --git a/src/modelec_com/src/pcb_alim_interface.cpp b/src/modelec_com/src/pcb_alim_interface.cpp index 4cbf22e..ce0c2a6 100644 --- a/src/modelec_com/src/pcb_alim_interface.cpp +++ b/src/modelec_com/src/pcb_alim_interface.cpp @@ -1,6 +1,5 @@ #include #include -#include #include #include @@ -426,7 +425,7 @@ namespace Modelec } void PCBAlimInterface::SendToPCB(const std::string& order, const std::string& elem, - const std::vector& data) const + const std::vector& data) const { std::string command = order + ";" + elem; for (const auto& d : data) diff --git a/src/modelec_com/src/pcb_odo_interface.cpp b/src/modelec_com/src/pcb_odo_interface.cpp index c4bbeab..5da7aa6 100644 --- a/src/modelec_com/src/pcb_odo_interface.cpp +++ b/src/modelec_com/src/pcb_odo_interface.cpp @@ -45,7 +45,8 @@ namespace Modelec pcb_subscriber_ = this->create_subscription( res->publisher, 10, - [this](const std_msgs::msg::String::SharedPtr msg) { + [this](const std_msgs::msg::String::SharedPtr msg) + { PCBCallback(msg); }, options); @@ -53,7 +54,8 @@ namespace Modelec pcb_executor_ = std::make_shared(); pcb_executor_->add_callback_group(pcb_callback_group_, this->get_node_base_interface()); - pcb_executor_thread_ = std::thread([this]() { + pcb_executor_thread_ = std::thread([this]() + { pcb_executor_->spin(); }); @@ -114,7 +116,7 @@ namespace Modelec get_tof_service_ = create_service( "odometry/tof", [this](const std::shared_ptr request, - std::shared_ptr response) + std::shared_ptr response) { HandleGetTof(request, response); }); @@ -122,7 +124,7 @@ namespace Modelec get_speed_service_ = create_service( "odometry/speed", [this](const std::shared_ptr request, - std::shared_ptr response) + std::shared_ptr response) { HandleGetSpeed(request, response); }); @@ -130,7 +132,7 @@ namespace Modelec get_position_service_ = create_service( "odometry/get_position", [this](const std::shared_ptr request, - std::shared_ptr response) + std::shared_ptr response) { HandleGetPosition(request, response); }); @@ -138,7 +140,7 @@ namespace Modelec set_start_service_ = create_service( "odometry/start", [this](const std::shared_ptr request, - std::shared_ptr response) + std::shared_ptr response) { HandleGetStart(request, response); }); @@ -146,7 +148,7 @@ namespace Modelec get_pid_service_ = create_service( "odometry/get_pid", [this](const std::shared_ptr request, - std::shared_ptr response) + std::shared_ptr response) { HandleGetPID(request, response); }); @@ -154,7 +156,7 @@ namespace Modelec set_pid_service_ = create_service( "odometry/set_pid", [this](const std::shared_ptr request, - std::shared_ptr response) + std::shared_ptr response) { HandleSetPID(request, response); }); @@ -162,7 +164,7 @@ namespace Modelec add_waypoint_service_ = create_service( "odometry/add_waypoint", [this](const std::shared_ptr request, - std::shared_ptr response) + std::shared_ptr response) { HandleAddWaypoint(request, response); }); @@ -171,7 +173,8 @@ namespace Modelec PCBOdoInterface::~PCBOdoInterface() { pcb_executor_->cancel(); - if (pcb_executor_thread_.joinable()) { + if (pcb_executor_thread_.joinable()) + { pcb_executor_thread_.join(); } } @@ -378,7 +381,7 @@ namespace Modelec } void PCBOdoInterface::HandleGetStart(const std::shared_ptr request, - std::shared_ptr response) + std::shared_ptr response) { std::promise promise; auto future = promise.get_future(); @@ -393,7 +396,7 @@ namespace Modelec } void PCBOdoInterface::HandleGetPID(const std::shared_ptr, - std::shared_ptr response) + std::shared_ptr response) { std::promise promise; auto future = promise.get_future(); @@ -413,7 +416,7 @@ namespace Modelec } void PCBOdoInterface::HandleSetPID(const std::shared_ptr request, - std::shared_ptr response) + std::shared_ptr response) { std::promise promise; auto future = promise.get_future(); @@ -448,11 +451,14 @@ namespace Modelec void PCBOdoInterface::ResolveToFRequest(const long distance) { std::lock_guard lock(tof_mutex_); - if (!tof_promises_.empty()) { + if (!tof_promises_.empty()) + { auto promise = std::move(tof_promises_.front()); tof_promises_.pop(); promise.set_value(distance); - } else { + } + else + { RCLCPP_DEBUG(get_logger(), "No pending ToF request to resolve."); } } @@ -460,11 +466,14 @@ namespace Modelec void PCBOdoInterface::ResolveSpeedRequest(const OdometryData& speed) { std::lock_guard lock(speed_mutex_); - if (!speed_promises_.empty()) { + if (!speed_promises_.empty()) + { auto promise = std::move(speed_promises_.front()); speed_promises_.pop(); promise.set_value(speed); - } else { + } + else + { RCLCPP_DEBUG(get_logger(), "No pending Speed request to resolve."); } } @@ -472,11 +481,14 @@ namespace Modelec void PCBOdoInterface::ResolvePositionRequest(const OdometryData& position) { std::lock_guard lock(pos_mutex_); - if (!pos_promises_.empty()) { + if (!pos_promises_.empty()) + { auto promise = std::move(pos_promises_.front()); pos_promises_.pop(); promise.set_value(position); - } else { + } + else + { RCLCPP_DEBUG(get_logger(), "No pending Position request to resolve."); } } @@ -484,11 +496,14 @@ namespace Modelec void PCBOdoInterface::ResolveStartRequest(bool start) { std::lock_guard lock(start_mutex_); - if (!start_promises_.empty()) { + if (!start_promises_.empty()) + { auto promise = std::move(start_promises_.front()); start_promises_.pop(); promise.set_value(start); - } else { + } + else + { RCLCPP_DEBUG(get_logger(), "No pending Start request to resolve."); } } @@ -496,11 +511,14 @@ namespace Modelec void PCBOdoInterface::ResolveGetPIDRequest(const PIDData& pid) { std::lock_guard lock(get_pid_mutex_); - if (!get_pid_promises_.empty()) { + if (!get_pid_promises_.empty()) + { auto promise = std::move(get_pid_promises_.front()); get_pid_promises_.pop(); promise.set_value(pid); - } else { + } + else + { RCLCPP_DEBUG(get_logger(), "No pending PID request to resolve."); } } @@ -508,11 +526,14 @@ namespace Modelec void PCBOdoInterface::ResolveSetPIDRequest(bool success) { std::lock_guard lock(set_pid_mutex_); - if (!set_pid_promises_.empty()) { + if (!set_pid_promises_.empty()) + { auto promise = std::move(set_pid_promises_.front()); set_pid_promises_.pop(); promise.set_value(success); - } else { + } + else + { RCLCPP_DEBUG(get_logger(), "No pending Set PID request to resolve."); } } @@ -520,11 +541,14 @@ namespace Modelec void PCBOdoInterface::ResolveAddWaypointRequest(bool success) { std::lock_guard lock(add_waypoint_mutex_); - if (!add_waypoint_promises_.empty()) { + if (!add_waypoint_promises_.empty()) + { auto promise = std::move(add_waypoint_promises_.front()); add_waypoint_promises_.pop(); promise.set_value(success); - } else { + } + else + { RCLCPP_DEBUG(get_logger(), "No pending Add Waypoint request to resolve."); } } @@ -640,10 +664,9 @@ namespace Modelec SendOrder("PID", data); } - } // Modelec -int main(int argc, char **argv) +int main(int argc, char** argv) { rclcpp::init(argc, argv); auto node = std::make_shared(); diff --git a/src/modelec_core/launch/test.modelec.launch.yml b/src/modelec_core/launch/test.modelec.launch.yml index 3d45396..660b776 100644 --- a/src/modelec_core/launch/test.modelec.launch.yml +++ b/src/modelec_core/launch/test.modelec.launch.yml @@ -15,6 +15,11 @@ launch: exec: 'pcb_alim_interface' name: 'pcb_alim_interface' + - node: + pkg: 'modelec_com' + exec: 'pcb_action_interface' + name: 'pcb_action_interface' + - node: pkg: 'modelec_gui' exec: "modelec_gui" diff --git a/src/modelec_core/launch/test.no_gui.modelec.launch.yml b/src/modelec_core/launch/test.no_gui.modelec.launch.yml index 8a20dc6..73d2156 100644 --- a/src/modelec_core/launch/test.no_gui.modelec.launch.yml +++ b/src/modelec_core/launch/test.no_gui.modelec.launch.yml @@ -15,6 +15,11 @@ launch: exec: 'pcb_alim_interface' name: 'pcb_alim_interface' + - node: + pkg: 'modelec_com' + exec: 'pcb_action_interface' + name: 'pcb_action_interface' + - node: pkg: 'modelec_core' exec: 'speed_result' diff --git a/src/modelec_core/launch/test.no_strat.modelec.launch.yml b/src/modelec_core/launch/test.no_strat.modelec.launch.yml index 59ed218..c159f85 100644 --- a/src/modelec_core/launch/test.no_strat.modelec.launch.yml +++ b/src/modelec_core/launch/test.no_strat.modelec.launch.yml @@ -15,6 +15,11 @@ launch: exec: 'pcb_alim_interface' name: 'pcb_alim_interface' + - node: + pkg: 'modelec_com' + exec: 'pcb_action_interface' + name: 'pcb_action_interface' + - node: pkg: 'modelec_gui' exec: "modelec_gui" diff --git a/src/modelec_gui/include/modelec_gui/pages/home_page.hpp b/src/modelec_gui/include/modelec_gui/pages/home_page.hpp index 1336c33..d2e822a 100644 --- a/src/modelec_gui/include/modelec_gui/pages/home_page.hpp +++ b/src/modelec_gui/include/modelec_gui/pages/home_page.hpp @@ -44,6 +44,5 @@ namespace ModelecGUI QPushButton* yellow_button_; QSvgRenderer* renderer_; - }; } diff --git a/src/modelec_gui/include/modelec_gui/pages/odo_page.hpp b/src/modelec_gui/include/modelec_gui/pages/odo_page.hpp index 2611d6b..2160d77 100644 --- a/src/modelec_gui/include/modelec_gui/pages/odo_page.hpp +++ b/src/modelec_gui/include/modelec_gui/pages/odo_page.hpp @@ -16,31 +16,32 @@ #include -namespace ModelecGUI { - +namespace ModelecGUI +{ class OdoPage : public QWidget { Q_OBJECT + public: - OdoPage(rclcpp::Node::SharedPtr node, QWidget *parent = nullptr); + OdoPage(rclcpp::Node::SharedPtr node, QWidget* parent = nullptr); ~OdoPage() override; rclcpp::Node::SharedPtr get_node() const { return node_; } private: - QVBoxLayout *mainLayout_; + QVBoxLayout* mainLayout_; QPushButton* startButton_; QLineEdit *xBox_, *yBox_, *thetaBox_; - QHBoxLayout *posLayout_; + QHBoxLayout* posLayout_; - QPushButton *askPID_; + QPushButton* askPID_; QDoubleSpinBox *pPIDBox_, *iPIDBox_, *dPIDBox_; - QHBoxLayout *pidLayout_; - QPushButton *setPID_; + QHBoxLayout* pidLayout_; + QPushButton* setPID_; - QPushButton *askSpeed_; + QPushButton* askSpeed_; QLineEdit *xSpeedBox_, *ySpeedBox_, *thetaSpeedBox_; - QHBoxLayout *speedLayout_; + QHBoxLayout* speedLayout_; rclcpp::Node::SharedPtr node_; @@ -54,5 +55,4 @@ namespace ModelecGUI { void PositionCallback(const modelec_interfaces::msg::OdometryPos::SharedPtr msg); }; - -} // namespace Modelec \ No newline at end of file +} // namespace Modelec diff --git a/src/modelec_gui/include/modelec_gui/pages/test_map_page.hpp b/src/modelec_gui/include/modelec_gui/pages/test_map_page.hpp index de2aa0b..450ceed 100644 --- a/src/modelec_gui/include/modelec_gui/pages/test_map_page.hpp +++ b/src/modelec_gui/include/modelec_gui/pages/test_map_page.hpp @@ -24,13 +24,14 @@ #include #include -namespace ModelecGUI { +namespace ModelecGUI +{ class TestMapPage : public QWidget { Q_OBJECT public: - TestMapPage(rclcpp::Node::SharedPtr node, QWidget *parent = nullptr); + TestMapPage(rclcpp::Node::SharedPtr node, QWidget* parent = nullptr); rclcpp::Node::SharedPtr get_node() const { return node_; } diff --git a/src/modelec_gui/src/pages/home_page.cpp b/src/modelec_gui/src/pages/home_page.cpp index 379078a..3c9ed8b 100644 --- a/src/modelec_gui/src/pages/home_page.cpp +++ b/src/modelec_gui/src/pages/home_page.cpp @@ -6,14 +6,16 @@ namespace ModelecGUI { HomePage::HomePage(rclcpp::Node::SharedPtr node, QWidget* parent) - : QWidget(parent), node_(node), - renderer_(new QSvgRenderer(QString(":/img/playmat/2025_FINAL.svg"), this)) + : QWidget(parent), node_(node), + renderer_(new QSvgRenderer(QString(":/img/playmat/2025_FINAL.svg"), this)) { yellow_button_ = new QPushButton("Yellow", this); blue_button_ = new QPushButton("Blue", this); - yellow_button_->setStyleSheet("background-color: rgba(255, 255, 0, 128); border: none; color: black; font-size: 24px;"); - blue_button_->setStyleSheet("background-color: rgba(0, 0, 255, 128); border: none; color: white; font-size: 24px;"); + yellow_button_->setStyleSheet( + "background-color: rgba(255, 255, 0, 128); border: none; color: black; font-size: 24px;"); + blue_button_->setStyleSheet( + "background-color: rgba(0, 0, 255, 128); border: none; color: white; font-size: 24px;"); yellow_button_->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); blue_button_->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); diff --git a/src/modelec_gui/src/pages/map_page.cpp b/src/modelec_gui/src/pages/map_page.cpp index f12cfc5..2c7c057 100644 --- a/src/modelec_gui/src/pages/map_page.cpp +++ b/src/modelec_gui/src/pages/map_page.cpp @@ -8,7 +8,10 @@ namespace ModelecGUI { - MapPage::MapPage(rclcpp::Node::SharedPtr node, QWidget* parent) : QWidget(parent), renderer(new QSvgRenderer(QString(":/img/playmat/2025_FINAL.svg"), this)), node_(node) + MapPage::MapPage(rclcpp::Node::SharedPtr node, QWidget* parent) : QWidget(parent), + renderer(new QSvgRenderer( + QString(":/img/playmat/2025_FINAL.svg"), + this)), node_(node) { ratioBetweenMapAndWidgetX_ = width() / 3000.0f; ratioBetweenMapAndWidgetY_ = height() / 2000.0f; @@ -46,14 +49,17 @@ namespace ModelecGUI enemy_length_ = Modelec::Config::get("config.enemy.size.length_mm", 300); enemy_width_ = Modelec::Config::get("config.enemy.size.width_mm", 300); - add_waypoint_sub_ = node_->create_subscription("odometry/add_waypoint", 100, - [this](const modelec_interfaces::msg::OdometryAddWaypoint::SharedPtr msg) { + add_waypoint_sub_ = node_->create_subscription( + "odometry/add_waypoint", 100, + [this](const modelec_interfaces::msg::OdometryAddWaypoint::SharedPtr msg) + { if (lastWapointWasEnd) { qpoints.clear(); lastWapointWasEnd = false; - qpoints.push_back(QPoint(robotPos.x * ratioBetweenMapAndWidgetX_, height() - robotPos.y * ratioBetweenMapAndWidgetY_)); + qpoints.push_back(QPoint(robotPos.x * ratioBetweenMapAndWidgetX_, + height() - robotPos.y * ratioBetweenMapAndWidgetY_)); } if (msg->is_end) @@ -61,31 +67,38 @@ namespace ModelecGUI lastWapointWasEnd = true; } - qpoints.push_back(QPoint(msg->x * ratioBetweenMapAndWidgetX_, height() - msg->y * ratioBetweenMapAndWidgetY_)); + qpoints.push_back(QPoint(msg->x * ratioBetweenMapAndWidgetX_, + height() - msg->y * ratioBetweenMapAndWidgetY_)); update(); - }); + }); odometry_sub_ = node_->create_subscription("odometry/position", 10, - [this](const modelec_interfaces::msg::OdometryPos::SharedPtr msg) { + [this](const modelec_interfaces::msg::OdometryPos::SharedPtr msg) + { robotPos = *msg; update(); - }); + }); score_sub_ = node_->create_subscription("strat/score", 10, - [this](const std_msgs::msg::Int64::SharedPtr msg) { - score_+= msg->data; - score_label_->setText(QString("Score: %1").arg(score_)); - }); + [this](const std_msgs::msg::Int64::SharedPtr msg) + { + score_ += msg->data; + score_label_->setText( + QString("Score: %1").arg(score_)); + }); obstacle_added_sub_ = node_->create_subscription("nav/obstacle/added", 40, - [this](const modelec_interfaces::msg::Obstacle::SharedPtr msg) { + [this](const modelec_interfaces::msg::Obstacle::SharedPtr msg) + { OnObstacleReceived(msg); - }); + }); - obstacle_removed_sub_ = node_->create_subscription("nav/obstacle/removed", 40, - [this](const modelec_interfaces::msg::Obstacle::SharedPtr msg) { + obstacle_removed_sub_ = node_->create_subscription( + "nav/obstacle/removed", 40, + [this](const modelec_interfaces::msg::Obstacle::SharedPtr msg) + { obstacle_.erase(msg->id); - }); + }); enemy_pos_sub_ = node_->create_subscription("enemy/position", 10, [this](const modelec_interfaces::msg::OdometryPos::SharedPtr msg) @@ -97,24 +110,29 @@ namespace ModelecGUI }); strat_start_sub_ = node_->create_subscription("/strat/start_time", 10, - [this](const std_msgs::msg::Int64::SharedPtr msg){ - isGameStarted_ = true; - start_time_ = msg->data; - }); + [this]( + const std_msgs::msg::Int64::SharedPtr msg) + { + isGameStarted_ = true; + start_time_ = msg->data; + }); strat_state_sub_ = node_->create_subscription("/strat/state", 10, - [this](const modelec_interfaces::msg::StratState::SharedPtr msg){ + [this](const modelec_interfaces::msg::StratState::SharedPtr msg) + { if (msg->state == modelec_interfaces::msg::StratState::STOP) { RCLCPP_INFO(node_->get_logger(), "Game stop"); isGameStarted_ = false; } - }); + }); // 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()) { + 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; } @@ -134,8 +152,10 @@ namespace ModelecGUI } 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()) { + 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; } @@ -149,22 +169,24 @@ namespace ModelecGUI void MapPage::AskMap() { reset_timer_ = node_->create_wall_timer( - std::chrono::seconds(1), - [this]() { - - 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; + std::chrono::seconds(1), + [this]() + { + 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..."); } - RCLCPP_INFO(node_->get_logger(), "Waiting for the service..."); - } - ask_map_obstacle_client_->async_send_request(std::make_shared()); + ask_map_obstacle_client_->async_send_request(std::make_shared()); - reset_timer_->cancel(); - }); + reset_timer_->cancel(); + }); } void MapPage::Reset() @@ -202,7 +224,8 @@ namespace ModelecGUI // --- Draw lines --- painter.setPen(QPen(Qt::red, 2)); // Red lines, 2px wide - for (size_t i = 0; i + 1 < qpoints.size(); ++i) { + for (size_t i = 0; i + 1 < qpoints.size(); ++i) + { painter.drawLine(qpoints[i], qpoints[i + 1]); } @@ -211,11 +234,12 @@ namespace ModelecGUI // --- Draw colored points --- const int radius = 5; - for (size_t i = 0; i < qpoints.size(); ++i) { + for (size_t i = 0; i < qpoints.size(); ++i) + { if (i == qpoints.size() - 1) - painter.setBrush(Qt::blue); // Last = blue + painter.setBrush(Qt::blue); // Last = blue else - painter.setBrush(Qt::red); // Middle = red + painter.setBrush(Qt::red); // Middle = red painter.drawEllipse(qpoints[i], radius, radius); } @@ -224,7 +248,7 @@ namespace ModelecGUI if (show_obstacle_) { - for (auto & [index, obs] : obstacle_) + for (auto& [index, obs] : obstacle_) { painter.save(); @@ -233,8 +257,9 @@ namespace ModelecGUI painter.rotate(90 - obs.theta * (180.0 / M_PI)); painter.setBrush(Qt::black); - QRect toDraw(-(obs.width * ratioBetweenMapAndWidgetX_ / 2), -(obs.height * ratioBetweenMapAndWidgetY_ / 2), - obs.width * ratioBetweenMapAndWidgetX_, obs.height * ratioBetweenMapAndWidgetY_); + QRect toDraw(-(obs.width * ratioBetweenMapAndWidgetX_ / 2), + -(obs.height * ratioBetweenMapAndWidgetY_ / 2), + obs.width * ratioBetweenMapAndWidgetX_, obs.height * ratioBetweenMapAndWidgetY_); painter.drawRect(toDraw); @@ -245,7 +270,9 @@ namespace ModelecGUI if (hasEnemy) { painter.setBrush(Qt::red); - painter.drawRect((enemy_pos_.x - (enemy_width_ / 2)) * ratioBetweenMapAndWidgetX_, height() - (enemy_pos_.y + (enemy_length_ / 2)) * ratioBetweenMapAndWidgetY_, enemy_width_*ratioBetweenMapAndWidgetX_, enemy_length_*ratioBetweenMapAndWidgetY_); + painter.drawRect((enemy_pos_.x - (enemy_width_ / 2)) * ratioBetweenMapAndWidgetX_, + height() - (enemy_pos_.y + (enemy_length_ / 2)) * ratioBetweenMapAndWidgetY_, + enemy_width_ * ratioBetweenMapAndWidgetX_, enemy_length_ * ratioBetweenMapAndWidgetY_); } } @@ -271,4 +298,4 @@ namespace ModelecGUI ratioBetweenMapAndWidgetX_ = width() / 3000.0f; ratioBetweenMapAndWidgetY_ = height() / 2000.0f; } -} \ No newline at end of file +} diff --git a/src/modelec_gui/src/pages/odo_page.cpp b/src/modelec_gui/src/pages/odo_page.cpp index 896ba43..48e18b5 100644 --- a/src/modelec_gui/src/pages/odo_page.cpp +++ b/src/modelec_gui/src/pages/odo_page.cpp @@ -4,25 +4,27 @@ namespace ModelecGUI { - OdoPage::OdoPage(rclcpp::Node::SharedPtr node, QWidget* parent) : QWidget(parent), node_(node) { startButton_ = new QPushButton("Start"); - connect(startButton_, &QPushButton::clicked, this, [this]() { + connect(startButton_, &QPushButton::clicked, this, [this]() + { // Create a request for the speed service RCLCPP_INFO(node_->get_logger(), "Start button clicked."); auto request = std::make_shared(); request->start = true; - client_start_->async_send_request(request, [this](rclcpp::Client::SharedFuture response) { - if (response.get()->success) + client_start_->async_send_request( + request, [this](rclcpp::Client::SharedFuture response) { - RCLCPP_INFO(node_->get_logger(), "Start command sent successfully."); - } - else - { - RCLCPP_ERROR(node_->get_logger(), "Failed to send start command."); - } - }); + if (response.get()->success) + { + RCLCPP_INFO(node_->get_logger(), "Start command sent successfully."); + } + else + { + RCLCPP_ERROR(node_->get_logger(), "Failed to send start command."); + } + }); }); // Initialize the UI components @@ -40,25 +42,33 @@ namespace ModelecGUI askPID_ = new QPushButton("Ask PID"); - connect(askPID_, &QPushButton::clicked, this, [this]() { + 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_->setValue(res->p); - iPIDBox_->setValue(res->i); - dPIDBox_->setValue(res->d); - }); - } else { - RCLCPP_ERROR(node_->get_logger(), "Failed to get response for PID request."); - } - }); + 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_->setValue(res->p); + iPIDBox_->setValue(res->i); + dPIDBox_->setValue(res->d); + }); + } + else + { + RCLCPP_ERROR(node_->get_logger(), "Failed to get response for PID request."); + } + }); }); pPIDBox_ = new QDoubleSpinBox(this); pPIDBox_->setMinimum(0); @@ -87,7 +97,8 @@ namespace ModelecGUI pidLayout_->addWidget(dPIDBox_); setPID_ = new QPushButton("Set PID"); - connect(setPID_, &QPushButton::clicked, this, [this]() { + connect(setPID_, &QPushButton::clicked, this, [this]() + { // Create a request for the PID service auto request = std::make_shared(); request->p = pPIDBox_->text().toFloat(); @@ -95,35 +106,44 @@ namespace ModelecGUI request->d = dPIDBox_->text().toFloat(); // Make the asynchronous service call - client_set_pid_->async_send_request(request, [this](rclcpp::Client::SharedFuture response) { - if (response.get()->success) + client_set_pid_->async_send_request( + request, [this](rclcpp::Client::SharedFuture response) { - RCLCPP_INFO(node_->get_logger(), "Set PID command sent successfully."); - } - else - { - RCLCPP_ERROR(node_->get_logger(), "Failed to send set PID command."); - } - }); + if (response.get()->success) + { + RCLCPP_INFO(node_->get_logger(), "Set PID command sent successfully."); + } + else + { + RCLCPP_ERROR(node_->get_logger(), "Failed to send set PID command."); + } + }); }); askSpeed_ = new QPushButton("Ask speed"); - connect(askSpeed_, &QPushButton::clicked, this, [this]() { + connect(askSpeed_, &QPushButton::clicked, this, [this]() + { // Create a request for the speed service auto request = std::make_shared(); // Make the asynchronous service call - client_->async_send_request(request, [this](rclcpp::Client::SharedFuture response) { - if (auto res = response.get()) { - QMetaObject::invokeMethod(this, [this, res]() { - xSpeedBox_->setText(QString("x: %1").arg(res->x)); - ySpeedBox_->setText(QString("y: %1").arg(res->y)); - thetaSpeedBox_->setText(QString("theta: %1").arg(res->theta)); - }); - } else { - RCLCPP_ERROR(node_->get_logger(), "Failed to get response for speed request."); - } - }); + client_->async_send_request( + request, [this](rclcpp::Client::SharedFuture response) + { + if (auto res = response.get()) + { + QMetaObject::invokeMethod(this, [this, res]() + { + xSpeedBox_->setText(QString("x: %1").arg(res->x)); + ySpeedBox_->setText(QString("y: %1").arg(res->y)); + thetaSpeedBox_->setText(QString("theta: %1").arg(res->theta)); + }); + } + else + { + RCLCPP_ERROR(node_->get_logger(), "Failed to get response for speed request."); + } + }); }); xSpeedBox_ = new QLineEdit("x speed: ?"); @@ -154,8 +174,10 @@ namespace ModelecGUI std::bind(&OdoPage::PositionCallback, this, std::placeholders::_1)); client_ = node_->create_client("odometry/speed"); - while (!client_->wait_for_service(std::chrono::seconds(1))) { - if (!rclcpp::ok()) { + while (!client_->wait_for_service(std::chrono::seconds(1))) + { + if (!rclcpp::ok()) + { RCLCPP_ERROR(node_->get_logger(), "Interrupted while waiting for the service. Exiting."); return; } @@ -163,8 +185,10 @@ namespace ModelecGUI } client_start_ = node_->create_client("odometry/start"); - while (!client_start_->wait_for_service(std::chrono::seconds(1))) { - if (!rclcpp::ok()) { + 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."); return; } @@ -173,8 +197,10 @@ namespace ModelecGUI client_get_pid_ = node_->create_client("odometry/get_pid"); - while (!client_get_pid_->wait_for_service(std::chrono::seconds(1))) { - if (!rclcpp::ok()) { + while (!client_get_pid_->wait_for_service(std::chrono::seconds(1))) + { + if (!rclcpp::ok()) + { RCLCPP_ERROR(node_->get_logger(), "Interrupted while waiting for the service. Exiting."); return; } @@ -182,8 +208,10 @@ namespace ModelecGUI } client_set_pid_ = node_->create_client("odometry/set_pid"); - while (!client_set_pid_->wait_for_service(std::chrono::seconds(1))) { - if (!rclcpp::ok()) { + while (!client_set_pid_->wait_for_service(std::chrono::seconds(1))) + { + if (!rclcpp::ok()) + { RCLCPP_ERROR(node_->get_logger(), "Interrupted while waiting for the service. Exiting."); return; } @@ -195,7 +223,8 @@ namespace ModelecGUI void OdoPage::PositionCallback(const modelec_interfaces::msg::OdometryPos::SharedPtr msg) { - QMetaObject::invokeMethod(this, [this, msg]() { + QMetaObject::invokeMethod(this, [this, msg]() + { xBox_->setText(QString("x: %1").arg(msg->x)); yBox_->setText(QString("y: %1").arg(msg->y)); thetaBox_->setText(QString("theta: %1").arg(msg->theta)); diff --git a/src/modelec_gui/src/pages/test_map_page.cpp b/src/modelec_gui/src/pages/test_map_page.cpp index d481b8f..85638d0 100644 --- a/src/modelec_gui/src/pages/test_map_page.cpp +++ b/src/modelec_gui/src/pages/test_map_page.cpp @@ -8,7 +8,11 @@ namespace ModelecGUI { - TestMapPage::TestMapPage(rclcpp::Node::SharedPtr node, QWidget* parent) : QWidget(parent), renderer(new QSvgRenderer(QString(":/img/playmat/2025_FINAL.svg"), this)), node_(node) + TestMapPage::TestMapPage(rclcpp::Node::SharedPtr node, QWidget* parent) : QWidget(parent), + renderer(new QSvgRenderer( + QString( + ":/img/playmat/2025_FINAL.svg"), + this)), node_(node) { ratioBetweenMapAndWidgetX_ = width() / 3000.0f; ratioBetweenMapAndWidgetY_ = height() / 2000.0f; @@ -21,14 +25,17 @@ namespace ModelecGUI enemy_length_ = Modelec::Config::get("config.enemy.size.length_mm", 300); enemy_width_ = Modelec::Config::get("config.enemy.size.width_mm", 300); - add_waypoint_sub_ = node_->create_subscription("odometry/add_waypoint", 100, - [this](const modelec_interfaces::msg::OdometryAddWaypoint::SharedPtr msg) { + add_waypoint_sub_ = node_->create_subscription( + "odometry/add_waypoint", 100, + [this](const modelec_interfaces::msg::OdometryAddWaypoint::SharedPtr msg) + { if (lastWapointWasEnd) { qpoints.clear(); lastWapointWasEnd = false; - qpoints.push_back(QPoint(robotPos.x * ratioBetweenMapAndWidgetX_, height() - robotPos.y * ratioBetweenMapAndWidgetY_)); + qpoints.push_back(QPoint(robotPos.x * ratioBetweenMapAndWidgetX_, + height() - robotPos.y * ratioBetweenMapAndWidgetY_)); } if (msg->is_end) @@ -36,25 +43,30 @@ namespace ModelecGUI lastWapointWasEnd = true; } - qpoints.push_back(QPoint(msg->x * ratioBetweenMapAndWidgetX_, height() - msg->y * ratioBetweenMapAndWidgetY_)); + qpoints.push_back(QPoint(msg->x * ratioBetweenMapAndWidgetX_, + height() - msg->y * ratioBetweenMapAndWidgetY_)); update(); - }); + }); odometry_sub_ = node_->create_subscription("odometry/position", 10, - [this](const modelec_interfaces::msg::OdometryPos::SharedPtr msg) { + [this](const modelec_interfaces::msg::OdometryPos::SharedPtr msg) + { robotPos = *msg; update(); - }); + }); obstacle_added_sub_ = node_->create_subscription("nav/obstacle/added", 40, - [this](const modelec_interfaces::msg::Obstacle::SharedPtr msg) { + [this](const modelec_interfaces::msg::Obstacle::SharedPtr msg) + { OnObstacleReceived(msg); - }); + }); - obstacle_removed_sub_ = node_->create_subscription("nav/obstacle/removed", 40, - [this](const modelec_interfaces::msg::Obstacle::SharedPtr msg) { + obstacle_removed_sub_ = node_->create_subscription( + "nav/obstacle/removed", 40, + [this](const modelec_interfaces::msg::Obstacle::SharedPtr msg) + { obstacle_.erase(msg->id); - }); + }); go_to_pub_ = node_->create_publisher("nav/go_to", 10); @@ -71,8 +83,10 @@ namespace ModelecGUI // 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()) { + 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; } @@ -92,8 +106,10 @@ namespace ModelecGUI } 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()) { + 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; } @@ -138,7 +154,8 @@ namespace ModelecGUI // --- Draw lines --- painter.setPen(QPen(Qt::red, 2)); // Red lines, 2px wide - for (size_t i = 0; i + 1 < qpoints.size(); ++i) { + for (size_t i = 0; i + 1 < qpoints.size(); ++i) + { painter.drawLine(qpoints[i], qpoints[i + 1]); } @@ -147,11 +164,12 @@ namespace ModelecGUI // --- Draw colored points --- const int radius = 5; - for (size_t i = 0; i < qpoints.size(); ++i) { + for (size_t i = 0; i < qpoints.size(); ++i) + { if (i == qpoints.size() - 1) - painter.setBrush(Qt::blue); // Last = blue + painter.setBrush(Qt::blue); // Last = blue else - painter.setBrush(Qt::red); // Middle = red + painter.setBrush(Qt::red); // Middle = red painter.drawEllipse(qpoints[i], radius, radius); } @@ -160,7 +178,7 @@ namespace ModelecGUI if (show_obstacle_) { - for (auto & [index, obs] : obstacle_) + for (auto& [index, obs] : obstacle_) { painter.save(); @@ -169,8 +187,9 @@ namespace ModelecGUI painter.rotate(90 - obs.theta * (180.0 / M_PI)); painter.setBrush(Qt::black); - QRect toDraw(-(obs.width * ratioBetweenMapAndWidgetX_ / 2), -(obs.height * ratioBetweenMapAndWidgetY_ / 2), - obs.width * ratioBetweenMapAndWidgetX_, obs.height * ratioBetweenMapAndWidgetY_); + QRect toDraw(-(obs.width * ratioBetweenMapAndWidgetX_ / 2), + -(obs.height * ratioBetweenMapAndWidgetY_ / 2), + obs.width * ratioBetweenMapAndWidgetX_, obs.height * ratioBetweenMapAndWidgetY_); painter.drawRect(toDraw); @@ -181,7 +200,9 @@ namespace ModelecGUI if (hasEnemy) { painter.setBrush(Qt::red); - painter.drawRect((enemy_pos_.x - (enemy_width_ / 2)) * ratioBetweenMapAndWidgetX_, height() - (enemy_pos_.y + (enemy_length_ / 2)) * ratioBetweenMapAndWidgetY_, enemy_width_*ratioBetweenMapAndWidgetX_, enemy_length_*ratioBetweenMapAndWidgetY_); + painter.drawRect((enemy_pos_.x - (enemy_width_ / 2)) * ratioBetweenMapAndWidgetX_, + height() - (enemy_pos_.y + (enemy_length_ / 2)) * ratioBetweenMapAndWidgetY_, + enemy_width_ * ratioBetweenMapAndWidgetX_, enemy_length_ * ratioBetweenMapAndWidgetY_); } } @@ -212,7 +233,8 @@ namespace ModelecGUI } else { - msg.mask = modelec_interfaces::msg::OdometryGoTo::FREE | modelec_interfaces::msg::OdometryGoTo::WALL | modelec_interfaces::msg::OdometryGoTo::OBSTACLE | modelec_interfaces::msg::OdometryGoTo::ENEMY; + msg.mask = modelec_interfaces::msg::OdometryGoTo::FREE | modelec_interfaces::msg::OdometryGoTo::WALL | + modelec_interfaces::msg::OdometryGoTo::OBSTACLE | modelec_interfaces::msg::OdometryGoTo::ENEMY; } go_to_pub_->publish(msg); diff --git a/src/modelec_strat/data/config.xml b/src/modelec_strat/data/config.xml index 7a91bf0..03b57b1 100644 --- a/src/modelec_strat/data/config.xml +++ b/src/modelec_strat/data/config.xml @@ -32,25 +32,30 @@ - - + + - - + + pcb_odo - /dev/pts/7 + /dev/pts/9 115200 pcb_alim - /dev/pts/11 + /dev/pts/12 115200 + + pcb_action + /dev/pts/12 + 115200 + 300 5 diff --git a/src/modelec_strat/include/modelec_strat/navigation_helper.hpp b/src/modelec_strat/include/modelec_strat/navigation_helper.hpp index 01d61fe..e21cf4a 100644 --- a/src/modelec_strat/include/modelec_strat/navigation_helper.hpp +++ b/src/modelec_strat/include/modelec_strat/navigation_helper.hpp @@ -9,9 +9,10 @@ #include "deposite_zone.hpp" #include "pathfinding.hpp" -namespace Modelec { - - class NavigationHelper { +namespace Modelec +{ + class NavigationHelper + { public: enum { @@ -30,15 +31,15 @@ namespace Modelec { int GetTeamId() const; void SendWaypoint() const; - void SendWaypoint(const std::vector &waypoints) const; + void SendWaypoint(const std::vector& waypoints) const; - void AddWaypoint(const PosMsg &pos, int index); - void AddWaypoint(const WaypointMsg &waypoint); + 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 AddWaypoints(const std::initializer_list& pos_list, int index); + void AddWaypoints(const std::initializer_list& waypoint_list); - void SetWaypoints(const std::list &waypoints); + void SetWaypoints(const std::list& waypoints); bool HasArrived() const; @@ -67,9 +68,10 @@ namespace Modelec { PosMsg::SharedPtr GetCurrentPos() const; - bool LoadDepositeZoneFromXML(const std::string &filename); + bool LoadDepositeZoneFromXML(const std::string& filename); - std::shared_ptr GetClosestDepositeZone(const PosMsg::SharedPtr &pos, int teamId, const std::vector& blacklistedId = {}); + std::shared_ptr GetClosestDepositeZone(const PosMsg::SharedPtr& pos, int teamId, + const std::vector& blacklistedId = {}); PosMsg::SharedPtr GetHomePosition(); @@ -78,7 +80,7 @@ namespace Modelec { void OnEnemyPositionLongTime(const modelec_interfaces::msg::OdometryPos::SharedPtr msg); bool DoesLineIntersectCircle(const Point& start, const Point& end, - const Point& center, float radius); + const Point& center, float radius); bool EnemyOnPath(const modelec_interfaces::msg::OdometryPos msg); @@ -136,4 +138,4 @@ namespace Modelec { bool await_rotate_ = false; std::vector send_back_waypoints_; }; -} \ 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 index aa6248d..303b067 100644 --- a/src/modelec_strat/include/modelec_strat/pathfinding.hpp +++ b/src/modelec_strat/include/modelec_strat/pathfinding.hpp @@ -19,8 +19,8 @@ #include "obstacle/column.hpp" -namespace Modelec { - +namespace Modelec +{ using WaypointMsg = modelec_interfaces::msg::OdometryAddWaypoint; using WaypointReachMsg = modelec_interfaces::msg::OdometryWaypointReach; using PosMsg = modelec_interfaces::msg::OdometryPos; @@ -38,7 +38,8 @@ namespace Modelec { WaypointMsg toMsg() const; }; - class Pathfinding { + class Pathfinding + { public: enum { @@ -83,10 +84,11 @@ namespace Modelec { void AddObstacle(const std::shared_ptr& obstacle); template ::value>> + typename = std::enable_if_t::value>> std::shared_ptr GetClosestObstacle(const PosMsg::SharedPtr& pos) const; - std::shared_ptr GetClosestColumn(const PosMsg::SharedPtr& pos, const std::vector& blacklistedId = {}); + std::shared_ptr GetClosestColumn(const PosMsg::SharedPtr& pos, + const std::vector& blacklistedId = {}); void OnEnemyPosition(const modelec_interfaces::msg::OdometryPos::SharedPtr msg); @@ -145,10 +147,13 @@ namespace Modelec { auto robotPos = Point(pos->x, pos->y, pos->theta); float distance = std::numeric_limits::max(); - for (const auto& obstacle : obstacle_map_) { - if (auto obs = std::dynamic_pointer_cast(obstacle.second)) { + for (const auto& obstacle : obstacle_map_) + { + if (auto obs = std::dynamic_pointer_cast(obstacle.second)) + { auto dist = Point::distance(robotPos, obs->position()); - if (dist < distance) { + if (dist < distance) + { distance = dist; closest_obstacle = obs; } diff --git a/src/modelec_strat/src/enemy_manager.cpp b/src/modelec_strat/src/enemy_manager.cpp index 002a678..9fbf4d7 100644 --- a/src/modelec_strat/src/enemy_manager.cpp +++ b/src/modelec_strat/src/enemy_manager.cpp @@ -21,13 +21,15 @@ namespace Modelec current_pos_sub_ = this->create_subscription( "odometry/position", 10, - [this](const modelec_interfaces::msg::OdometryPos::SharedPtr msg) { + [this](const modelec_interfaces::msg::OdometryPos::SharedPtr msg) + { OnCurrentPos(msg); }); laser_scan_sub_ = this->create_subscription( "scan", 10, - [this](const sensor_msgs::msg::LaserScan::SharedPtr msg) { + [this](const sensor_msgs::msg::LaserScan::SharedPtr msg) + { OnLidarData(msg); }); @@ -35,13 +37,17 @@ namespace Modelec "enemy/position", 10); state_sub_ = create_subscription("/strat/state", 10, - [this](const modelec_interfaces::msg::StratState::SharedPtr msg) - { - if (!game_stated_ && msg->state == modelec_interfaces::msg::StratState::SELECT_MISSION) - { - game_stated_ = true; - } - }); + [this]( + const + modelec_interfaces::msg::StratState::SharedPtr + msg) + { + if (!game_stated_ && msg->state == + modelec_interfaces::msg::StratState::SELECT_MISSION) + { + game_stated_ = true; + } + }); enemy_long_time_pub_ = this->create_publisher( "/enemy/long_time", 10); @@ -142,7 +148,7 @@ namespace Modelec { last_enemy_pos_ = enemy_pos; last_publish_time_ = this->now(); - last_movement_time_ = this->now(); // Mise à jour du dernier vrai mouvement + last_movement_time_ = this->now(); // Mise à jour du dernier vrai mouvement enemy_pos_pub_->publish(enemy_pos); RCLCPP_INFO(this->get_logger(), "Enemy moved: x=%d, y=%d", enemy_pos.x, enemy_pos.y); } @@ -152,7 +158,8 @@ namespace Modelec if ((now - last_movement_time_).seconds() > max_stationary_time_s_) { enemy_long_time_pub_->publish(last_enemy_pos_); - RCLCPP_WARN(this->get_logger(), "Enemy has been stationary for too long at x=%d y=%d", last_enemy_pos_.x, last_enemy_pos_.y); + RCLCPP_WARN(this->get_logger(), "Enemy has been stationary for too long at x=%d y=%d", + last_enemy_pos_.x, last_enemy_pos_.y); last_movement_time_ = now; } @@ -179,9 +186,10 @@ namespace Modelec } } -int main(int argc, char **argv) { +int main(int argc, char** argv) +{ rclcpp::init(argc, argv); rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; -} \ No newline at end of file +} diff --git a/src/modelec_strat/src/missions/banner_mission.cpp b/src/modelec_strat/src/missions/banner_mission.cpp index bd51a57..0bbb199 100644 --- a/src/modelec_strat/src/missions/banner_mission.cpp +++ b/src/modelec_strat/src/missions/banner_mission.cpp @@ -4,7 +4,8 @@ namespace Modelec { - BannerMission::BannerMission(const std::shared_ptr& nav) : step_(GO_TO_FRONT), status_(MissionStatus::READY), nav_(nav) + BannerMission::BannerMission(const std::shared_ptr& nav) : step_(GO_TO_FRONT), + status_(MissionStatus::READY), nav_(nav) { } @@ -41,7 +42,7 @@ namespace Modelec { auto spawn = nav_->GetSpawn(); - nav_->GoTo(spawn.x, (nav_->GetPathfinding()->robot_length_mm_/2) + 150, M_PI_2, true); + nav_->GoTo(spawn.x, (nav_->GetPathfinding()->robot_length_mm_ / 2) + 150, M_PI_2, true); step_ = GO_FORWARD; break; diff --git a/src/modelec_strat/src/navigation_helper.cpp b/src/modelec_strat/src/navigation_helper.cpp index 9a8039c..91120bc 100644 --- a/src/modelec_strat/src/navigation_helper.cpp +++ b/src/modelec_strat/src/navigation_helper.cpp @@ -4,7 +4,8 @@ #include "../../modelec_utils/include/modelec_utils/config.hpp" -namespace Modelec { +namespace Modelec +{ NavigationHelper::NavigationHelper() { } @@ -25,7 +26,8 @@ namespace Modelec { waypoint_reach_sub_ = node_->create_subscription( "odometry/waypoint_reach", 10, - [this](const WaypointReachMsg::SharedPtr msg) { + [this](const WaypointReachMsg::SharedPtr msg) + { OnWaypointReach(msg); }); @@ -33,14 +35,16 @@ namespace Modelec { pos_sub_ = node_->create_subscription( "odometry/position", 20, - [this](const modelec_interfaces::msg::OdometryPos::SharedPtr msg) { + [this](const modelec_interfaces::msg::OdometryPos::SharedPtr msg) + { OnPos(msg); }); pos_pub_ = node_->create_publisher("odometry/set_position", 10); go_to_sub_ = node_->create_subscription( - "nav/go_to", 10, [this](const modelec_interfaces::msg::OdometryGoTo::SharedPtr msg) { + "nav/go_to", 10, [this](const modelec_interfaces::msg::OdometryGoTo::SharedPtr msg) + { GoTo(msg->x, msg->y, msg->theta, msg->close, msg->mask); }); @@ -58,7 +62,8 @@ namespace Modelec { OnEnemyPositionLongTime(msg); }); - std::string deposite_zone_path = ament_index_cpp::get_package_share_directory("modelec_strat") + "/data/deposite_zone.xml"; + std::string deposite_zone_path = ament_index_cpp::get_package_share_directory("modelec_strat") + + "/data/deposite_zone.xml"; if (!LoadDepositeZoneFromXML(deposite_zone_path)) { RCLCPP_ERROR(node_->get_logger(), "Failed to load obstacles from XML"); @@ -82,7 +87,7 @@ namespace Modelec { void NavigationHelper::SendWaypoint() const { - for (auto & w : waypoints_) + for (auto& w : waypoints_) { waypoint_pub_->publish(w.toMsg()); } @@ -90,7 +95,7 @@ namespace Modelec { void NavigationHelper::SendWaypoint(const std::vector& waypoints) const { - for (auto & w : waypoints) + for (auto& w : waypoints) { waypoint_pub_->publish(w); } @@ -178,7 +183,7 @@ namespace Modelec { } waypoints_.back().is_end = false; - for (auto & w : waypoint_list) + for (auto& w : waypoint_list) { waypoints_.emplace_back(w); } @@ -251,7 +256,7 @@ namespace Modelec { waypoints_.clear(); - for (auto & w : wl) + for (auto& w : wl) { waypoints_.emplace_back(w); } @@ -293,7 +298,7 @@ namespace Modelec { send_back_waypoints_.clear(); - for (auto & w : wl) + for (auto& w : wl) { send_back_waypoints_.emplace_back(w); } @@ -302,7 +307,7 @@ namespace Modelec { { waypoints_.clear(); - for (auto & w : wl) + for (auto& w : wl) { waypoints_.emplace_back(w); } @@ -404,7 +409,8 @@ namespace Modelec { return true; } - std::shared_ptr NavigationHelper::GetClosestDepositeZone(const PosMsg::SharedPtr& pos, int teamId, const std::vector& blacklistedId) + std::shared_ptr NavigationHelper::GetClosestDepositeZone( + const PosMsg::SharedPtr& pos, int teamId, const std::vector& blacklistedId) { // TODO : score std::shared_ptr closest_zone = nullptr; @@ -414,7 +420,8 @@ namespace Modelec { for (const auto& [id, zone] : deposite_zones_) { - if (zone->GetTeam() == teamId && zone->RemainingPotPos() > 0 && blacklistedId.end() == std::find(blacklistedId.begin(), blacklistedId.end(), id)) + if (zone->GetTeam() == teamId && zone->RemainingPotPos() > 0 && blacklistedId.end() == std::find( + blacklistedId.begin(), blacklistedId.end(), id)) { double distance = Point::distance(posPoint, zone->GetPosition()); double enemy_distance = Point::distance(enemyPos, zone->GetPosition()); @@ -468,9 +475,11 @@ namespace Modelec { for (auto& [id, zone] : deposite_zones_) { auto zonePos = zone->GetPosition(); - if (Point::distance(enemy_pos, zonePos) < pathfinding_->robot_width_mm_ + (zone->GetWidth()/2) + pathfinding_->enemy_margin_mm_) + if (Point::distance(enemy_pos, zonePos) < pathfinding_->robot_width_mm_ + (zone->GetWidth() / 2) + + pathfinding_->enemy_margin_mm_) { - std::shared_ptr obs = std::make_shared(id, zonePos.x, zonePos.y, zonePos.theta, zone->GetWidth(), zone->GetHeight(), "enemy-zone"); + std::shared_ptr obs = std::make_shared( + id, zonePos.x, zonePos.y, zonePos.theta, zone->GetWidth(), zone->GetHeight(), "enemy-zone"); pathfinding_->AddObstacle(obs); } } @@ -496,10 +505,11 @@ namespace Modelec { auto wp = waypointsList[i]; auto next_wp = waypointsList[i + 1]; if (DoesLineIntersectCircle( - Point(wp.x, wp.y, wp.theta), - Point(next_wp.x, next_wp.y, next_wp.theta), - Point(msg.x, msg.y, msg.theta), - (pathfinding_->enemy_length_mm_ + pathfinding_->robot_length_mm_ + pathfinding_->enemy_margin_mm_) / 2.0f)) + Point(wp.x, wp.y, wp.theta), + Point(next_wp.x, next_wp.y, next_wp.theta), + Point(msg.x, msg.y, msg.theta), + (pathfinding_->enemy_length_mm_ + pathfinding_->robot_length_mm_ + pathfinding_->enemy_margin_mm_) / + 2.0f)) { return true; } @@ -508,9 +518,11 @@ namespace Modelec { return false; } - bool NavigationHelper::DoesLineIntersectCircle(const Point& start, const Point& end, const Point& center, float radius) + bool NavigationHelper::DoesLineIntersectCircle(const Point& start, const Point& end, const Point& center, + float radius) { - float num = std::abs((end.y - start.y) * center.x - (end.x - start.x) * center.y + end.x * start.y - end.y * start.x); + float num = std::abs( + (end.y - start.y) * center.x - (end.x - start.x) * center.y + end.x * start.y - end.y * start.x); float den = std::sqrt((end.y - start.y) * (end.y - start.y) + (end.x - start.x) * (end.x - start.x)); float dist = num / den; return dist < radius; @@ -524,7 +536,8 @@ namespace Modelec { { if (GoTo(last_go_to_.goal, true, last_go_to_.collisionMask) != Pathfinding::FREE) { - GoTo(current_pos_, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE | Pathfinding::ENEMY); + GoTo(current_pos_, true, + Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE | Pathfinding::ENEMY); // TODO : Handle case where no path is found } } @@ -566,18 +579,18 @@ namespace Modelec { { switch (team_id_) { - case YELLOW: - return spawn_yellow_; - case BLUE: - return spawn_blue_; - default: - return spawn_yellow_; + case YELLOW: + return spawn_yellow_; + case BLUE: + return spawn_blue_; + default: + return spawn_yellow_; } } void NavigationHelper::OnWaypointReach(const WaypointReachMsg::SharedPtr msg) { - for (auto & waypoint : waypoints_) + for (auto& waypoint : waypoints_) { if (waypoint.id == msg->id) { @@ -588,7 +601,7 @@ namespace Modelec { await_rotate_ = false; waypoints_.clear(); - for (auto & w : send_back_waypoints_) + for (auto& w : send_back_waypoints_) { waypoints_.emplace_back(w); } @@ -603,5 +616,4 @@ namespace Modelec { current_pos_ = msg; pathfinding_->SetCurrentPos(msg); } - } diff --git a/src/modelec_strat/src/pami_manager.cpp b/src/modelec_strat/src/pami_manager.cpp index 2f079fe..d9fb446 100644 --- a/src/modelec_strat/src/pami_manager.cpp +++ b/src/modelec_strat/src/pami_manager.cpp @@ -56,7 +56,7 @@ namespace Modelec second_goal - now, [this]() { - for (const auto & obs : zones_) + for (const auto& obs : zones_) { add_obs_pub_->publish(obs.toMsg()); @@ -108,8 +108,8 @@ namespace Modelec } for (tinyxml2::XMLElement* elem = root->FirstChildElement("zone"); - elem; - elem = elem->NextSiblingElement("zone")) + elem; + elem = elem->NextSiblingElement("zone")) { zones_.push_back(Obstacle(elem)); } @@ -119,7 +119,8 @@ namespace Modelec } } -int main(int argc, char **argv) { +int main(int argc, char** argv) +{ rclcpp::init(argc, argv); rclcpp::spin(std::make_shared()); rclcpp::shutdown(); diff --git a/src/modelec_strat/src/pathfinding.cpp b/src/modelec_strat/src/pathfinding.cpp index 04f3c47..a39ba9e 100644 --- a/src/modelec_strat/src/pathfinding.cpp +++ b/src/modelec_strat/src/pathfinding.cpp @@ -606,7 +606,8 @@ namespace Modelec { if (auto column = std::dynamic_pointer_cast(obs)) { - if (Point::distance(enemyPos, column->GetPosition()) < enemy_width_mm_ + (column->width() / 2) + enemy_margin_mm_) + if (Point::distance(enemyPos, column->GetPosition()) < enemy_width_mm_ + (column->width() / 2) + + enemy_margin_mm_) { RemoveObstacle(column->id()); } diff --git a/src/modelec_utils/src/point.cpp b/src/modelec_utils/src/point.cpp index 7dc41ba..89b6f71 100644 --- a/src/modelec_utils/src/point.cpp +++ b/src/modelec_utils/src/point.cpp @@ -31,5 +31,4 @@ namespace Modelec { return GetTakePosition(210, theta); } - }