From 4e277ef0ca5e8f8b48ef364e0de6d10136d76e95 Mon Sep 17 00:00:00 2001 From: acki Date: Thu, 11 Dec 2025 18:51:32 +0100 Subject: [PATCH] update new listener --- src/modelec_com/CMakeLists.txt | 59 +- .../modelec_com/multiple_serial_listener.hpp | 76 --- .../modelec_com/pcb_action_interface.hpp | 26 +- .../modelec_com/pcb_action_interface.new.hpp | 76 --- .../modelec_com/pcb_alim_interface.hpp | 109 ---- .../include/modelec_com/pcb_odo_interface.hpp | 14 +- .../modelec_com/pcb_odo_interface.new.hpp | 108 ---- src/modelec_com/package.xml | 2 - .../src/multiple_serial_listener.cpp | 223 -------- src/modelec_com/src/pcb_action_interface.cpp | 101 +--- .../src/pcb_action_interface.new.cpp | 461 ---------------- src/modelec_com/src/pcb_alim_interface.cpp | 516 ------------------ src/modelec_com/src/pcb_odo_interface.cpp | 109 +--- src/modelec_com/src/pcb_odo_interface.new.cpp | 398 -------------- src/modelec_core/launch/modelec.launch.py | 4 +- 15 files changed, 61 insertions(+), 2221 deletions(-) delete mode 100644 src/modelec_com/include/modelec_com/multiple_serial_listener.hpp delete mode 100644 src/modelec_com/include/modelec_com/pcb_action_interface.new.hpp delete mode 100644 src/modelec_com/include/modelec_com/pcb_alim_interface.hpp delete mode 100644 src/modelec_com/include/modelec_com/pcb_odo_interface.new.hpp delete mode 100644 src/modelec_com/src/multiple_serial_listener.cpp delete mode 100644 src/modelec_com/src/pcb_action_interface.new.cpp delete mode 100644 src/modelec_com/src/pcb_alim_interface.cpp delete mode 100644 src/modelec_com/src/pcb_odo_interface.new.cpp diff --git a/src/modelec_com/CMakeLists.txt b/src/modelec_com/CMakeLists.txt index b6db509..b98c1e7 100644 --- a/src/modelec_com/CMakeLists.txt +++ b/src/modelec_com/CMakeLists.txt @@ -9,85 +9,34 @@ endif() find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) -find_package(sensor_msgs REQUIRED) -find_package(geometry_msgs REQUIRED) find_package(Boost REQUIRED COMPONENTS system) find_package(ament_index_cpp REQUIRED) find_package(modelec_interfaces REQUIRED) find_package(modelec_utils REQUIRED) -# 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) -target_include_directories(serial_listener PUBLIC - $ - $ -) - # PCB Odometry Node -add_executable(pcb_odo_interface src/pcb_odo_interface.cpp) +add_executable(pcb_odo_interface src/pcb_odo_interface.cpp src/serial_listener.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) +target_link_libraries(pcb_odo_interface Boost::system modelec_utils::utils modelec_utils::config) target_include_directories(pcb_odo_interface PUBLIC $ $ ) -# PCB Odometry Node -add_executable(pcb_odo_interface_new src/pcb_odo_interface.new.cpp src/serial_listener.cpp) -ament_target_dependencies(pcb_odo_interface_new rclcpp std_msgs modelec_interfaces ament_index_cpp) -target_link_libraries(pcb_odo_interface_new modelec_utils::utils modelec_utils::config) -target_include_directories(pcb_odo_interface_new PUBLIC - $ - $ -) - # PCB Action Node -add_executable(pcb_action_interface_new src/pcb_action_interface.new.cpp src/serial_listener.cpp) -ament_target_dependencies(pcb_action_interface_new rclcpp std_msgs modelec_interfaces ament_index_cpp) -target_link_libraries(pcb_action_interface_new modelec_utils::utils modelec_utils::config) -target_include_directories(pcb_action_interface_new PUBLIC - $ - $ -) - -# 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) -target_include_directories(pcb_alim_interface PUBLIC - $ - $ -) - -# PCB Action Node -add_executable(pcb_action_interface src/pcb_action_interface.cpp) +add_executable(pcb_action_interface src/pcb_action_interface.cpp src/serial_listener.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_link_libraries(pcb_action_interface Boost::system modelec_utils::utils modelec_utils::config) target_include_directories(pcb_action_interface PUBLIC $ $ ) -# Enable testing and linting -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() - - # Install targets install(TARGETS - serial_listener - pcb_alim_interface pcb_odo_interface - pcb_odo_interface_new pcb_action_interface - pcb_action_interface_new DESTINATION lib/${PROJECT_NAME} ) diff --git a/src/modelec_com/include/modelec_com/multiple_serial_listener.hpp b/src/modelec_com/include/modelec_com/multiple_serial_listener.hpp deleted file mode 100644 index 37cba05..0000000 --- a/src/modelec_com/include/modelec_com/multiple_serial_listener.hpp +++ /dev/null @@ -1,76 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include -#include - -#define MAX_MESSAGE_LEN 1048 - -namespace Modelec -{ - 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_; - - void start_async_read(); - void start_async_write(); - - public: - std::string name_; - boost::asio::serial_port port_; - - 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(); - - void close(); - - void SetMaxMessageLen(int max_message_len) { max_message_len_ = max_message_len; } - bool IsOk() const { return status_; } - - void SetOk() { status_ = true; } - - void write(std_msgs::msg::String::SharedPtr msg); - }; - - class MultipleSerialListener : public rclcpp::Node - { - public: - MultipleSerialListener(); - ~MultipleSerialListener() override; - - private: - int default_max_message_len_ = MAX_MESSAGE_LEN; - - std::map> serial_listeners; - - 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); - - 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/pcb_action_interface.hpp b/src/modelec_com/include/modelec_com/pcb_action_interface.hpp index 494ab8e..d3199d3 100644 --- a/src/modelec_com/include/modelec_com/pcb_action_interface.hpp +++ b/src/modelec_com/include/modelec_com/pcb_action_interface.hpp @@ -1,5 +1,7 @@ #pragma once +#include + #include #include #include @@ -11,14 +13,11 @@ namespace Modelec { - class PCBActionInterface : public rclcpp::Node + class PCBActionInterface : public rclcpp::Node, public SerialListener { public: PCBActionInterface(); - rclcpp::CallbackGroup::SharedPtr pcb_callback_group_; - std::shared_ptr pcb_executor_; - std::thread pcb_executor_thread_; ~PCBActionInterface() override; @@ -31,10 +30,7 @@ namespace Modelec std::map relay_value_; private: - rclcpp::Publisher::SharedPtr pcb_publisher_; - rclcpp::Subscription::SharedPtr pcb_subscriber_; - - void PCBCallback(const std_msgs::msg::String::SharedPtr msg); + void read(const std::string& msg) override; rclcpp::Subscription::SharedPtr asc_get_sub_; rclcpp::Subscription::SharedPtr servo_get_sub_; @@ -67,16 +63,14 @@ namespace Modelec rclcpp::Subscription::SharedPtr tir_disarm_sub_; rclcpp::Subscription::SharedPtr tir_arm_set_sub_; - - bool isOk = false; public: - void SendToPCB(const std::string& data) const; + void SendToPCB(const std::string& data); void SendToPCB(const std::string& order, const std::string& elem, - const std::vector& data = {}) const; + const std::vector& data = {}); - void GetData(const std::string& elem, const std::vector& data = {}) const; - void SendOrder(const std::string& elem, const std::vector& data = {}) const; - void SendMove(const std::string& elem, const std::vector& data = {}) const; - void RespondEvent(const std::string& elem, const std::vector& data = {}) const; + void GetData(const std::string& elem, const std::vector& data = {}); + void SendOrder(const std::string& elem, const std::vector& data = {}); + void SendMove(const std::string& elem, const std::vector& data = {}); + void RespondEvent(const std::string& elem, const std::vector& data = {}); }; } \ No newline at end of file diff --git a/src/modelec_com/include/modelec_com/pcb_action_interface.new.hpp b/src/modelec_com/include/modelec_com/pcb_action_interface.new.hpp deleted file mode 100644 index d3199d3..0000000 --- a/src/modelec_com/include/modelec_com/pcb_action_interface.new.hpp +++ /dev/null @@ -1,76 +0,0 @@ -#pragma once - -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -namespace Modelec -{ - class PCBActionInterface : public rclcpp::Node, public SerialListener - { - public: - - PCBActionInterface(); - - ~PCBActionInterface() override; - - protected: - std::map asc_value_mapper_; - std::map> servo_pos_mapper_; - - int asc_state_ = 0; - std::map servo_value_; - std::map relay_value_; - - private: - void read(const std::string& msg) override; - - rclcpp::Subscription::SharedPtr asc_get_sub_; - rclcpp::Subscription::SharedPtr servo_get_sub_; - rclcpp::Subscription::SharedPtr relay_get_sub_; - - rclcpp::Publisher::SharedPtr asc_get_res_pub_; - rclcpp::Publisher::SharedPtr servo_get_res_pub_; - rclcpp::Publisher::SharedPtr relay_get_res_pub_; - - rclcpp::Subscription::SharedPtr asc_set_sub_; - rclcpp::Subscription::SharedPtr servo_set_sub_; - - rclcpp::Publisher::SharedPtr asc_set_res_pub_; - rclcpp::Publisher::SharedPtr servo_set_res_pub_; - - rclcpp::Subscription::SharedPtr asc_move_sub_; - rclcpp::Subscription::SharedPtr servo_move_sub_; - rclcpp::Subscription::SharedPtr relay_move_sub_; - - rclcpp::Publisher::SharedPtr asc_move_res_pub_; - rclcpp::Publisher::SharedPtr servo_move_res_pub_; - rclcpp::Publisher::SharedPtr relay_move_res_pub_; - - rclcpp::Publisher::SharedPtr tir_start_pub_; - rclcpp::Publisher::SharedPtr tir_arm_pub_; - rclcpp::Publisher::SharedPtr tir_disarm_pub_; - - rclcpp::Subscription::SharedPtr tir_start_sub_; - rclcpp::Subscription::SharedPtr tir_arm_sub_; - rclcpp::Subscription::SharedPtr tir_disarm_sub_; - rclcpp::Subscription::SharedPtr tir_arm_set_sub_; - - public: - void SendToPCB(const std::string& data); - void SendToPCB(const std::string& order, const std::string& elem, - const std::vector& data = {}); - - void GetData(const std::string& elem, const std::vector& data = {}); - void SendOrder(const std::string& elem, const std::vector& data = {}); - void SendMove(const std::string& elem, const std::vector& data = {}); - void RespondEvent(const std::string& elem, const std::vector& data = {}); - }; -} \ 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 deleted file mode 100644 index 3f47b42..0000000 --- a/src/modelec_com/include/modelec_com/pcb_alim_interface.hpp +++ /dev/null @@ -1,109 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -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 - { - 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); - - bool isOk = false; - - 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 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 1a14e24..213380d 100644 --- a/src/modelec_com/include/modelec_com/pcb_odo_interface.hpp +++ b/src/modelec_com/include/modelec_com/pcb_odo_interface.hpp @@ -1,5 +1,7 @@ #pragma once +#include + #include #include @@ -30,14 +32,11 @@ namespace Modelec { - class PCBOdoInterface : public rclcpp::Node + class PCBOdoInterface : public rclcpp::Node, public SerialListener { public: PCBOdoInterface(); - rclcpp::CallbackGroup::SharedPtr pcb_callback_group_; - std::shared_ptr pcb_executor_; - std::thread pcb_executor_thread_; ~PCBOdoInterface() override; struct OdometryData @@ -55,10 +54,7 @@ namespace Modelec }; private: - rclcpp::Publisher::SharedPtr pcb_publisher_; - rclcpp::Subscription::SharedPtr pcb_subscriber_; - - void PCBCallback(const std_msgs::msg::String::SharedPtr msg); + void read(const std::string& msg) override; rclcpp::Publisher::SharedPtr odo_pos_publisher_; rclcpp::Subscription::SharedPtr odo_get_pos_sub_; @@ -83,8 +79,6 @@ namespace Modelec int timeout_ms = 1000; int attempt = 5; - bool isOk = false; - public: void SendToPCB(const std::string& data); void SendToPCB(const std::string& order, const std::string& elem, diff --git a/src/modelec_com/include/modelec_com/pcb_odo_interface.new.hpp b/src/modelec_com/include/modelec_com/pcb_odo_interface.new.hpp deleted file mode 100644 index 213380d..0000000 --- a/src/modelec_com/include/modelec_com/pcb_odo_interface.new.hpp +++ /dev/null @@ -1,108 +0,0 @@ -#pragma once - -#include - -#include - -#include -#include -#include - -#include - -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -namespace Modelec -{ - class PCBOdoInterface : public rclcpp::Node, public SerialListener - { - public: - PCBOdoInterface(); - - ~PCBOdoInterface() override; - - struct OdometryData - { - long x; - long y; - double theta; - }; - - struct PIDData - { - float p; - float i; - float d; - }; - - private: - void read(const std::string& msg) override; - - rclcpp::Publisher::SharedPtr odo_pos_publisher_; - rclcpp::Subscription::SharedPtr odo_get_pos_sub_; - 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_add_waypoints_subscriber_; - rclcpp::Subscription::SharedPtr odo_set_pos_subscriber_; - rclcpp::Subscription::SharedPtr odo_set_pid_subscriber_; - - void AddWaypointCallback(const modelec_interfaces::msg::OdometryWaypoint::SharedPtr msg); - void AddWaypointsCallback(const modelec_interfaces::msg::OdometryWaypoints::SharedPtr msg); - void SetPosCallback(const modelec_interfaces::msg::OdometryPos::SharedPtr msg); - void SetPIDCallback(const modelec_interfaces::msg::OdometryPid::SharedPtr msg); - - rclcpp::Subscription::SharedPtr start_odo_sub_; - bool start_odo_ = false; - - int timeout_ms = 1000; - int attempt = 5; - - public: - void SendToPCB(const std::string& data); - void SendToPCB(const std::string& order, const std::string& elem, - const std::vector& data = {}); - - void GetData(const std::string& elem, const std::vector& data = {}); - void SendOrder(const std::string& elem, const std::vector& data = {}); - - void GetPos(); - void GetSpeed(); - void GetToF(const int& tof); - - void SetRobotPos(const modelec_interfaces::msg::OdometryPos::SharedPtr msg); - void SetRobotPos(long x, long y, double theta); - - void AddWaypoints(modelec_interfaces::msg::OdometryWaypoints::SharedPtr msg); - void AddWaypoint(modelec_interfaces::msg::OdometryWaypoint::SharedPtr msg); - void AddWaypoint(int index, bool IsStopPoint, long x, long y, double theta); - - void SetStart(const modelec_interfaces::msg::OdometryStart::SharedPtr msg); - void SetStart(bool start); - - void GetPID(); - void SetPID(const modelec_interfaces::msg::OdometryPid::SharedPtr msg); - void SetPID(std::string name, float p, float i, float d, std::optional min = std::nullopt, std::optional max = std::nullopt); - }; -} // namespace Modelec diff --git a/src/modelec_com/package.xml b/src/modelec_com/package.xml index 85df8e6..ebb96cd 100644 --- a/src/modelec_com/package.xml +++ b/src/modelec_com/package.xml @@ -13,8 +13,6 @@ modelec_core rclcpp std_msgs - sensor_msgs - geometry_msgs boost boost diff --git a/src/modelec_com/src/multiple_serial_listener.cpp b/src/modelec_com/src/multiple_serial_listener.cpp deleted file mode 100644 index c50e43e..0000000 --- a/src/modelec_com/src/multiple_serial_listener.cpp +++ /dev/null @@ -1,223 +0,0 @@ -#include "modelec_com/multiple_serial_listener.hpp" -#include -#include -#include -#include -#include -#include - -namespace Modelec -{ - SerialListener::SerialListener(const std::string& name, int bauds, const std::string& serial_port, - int max_message_len, boost::asio::io_service& io) - : bauds_(bauds), serial_port_(serial_port), max_message_len_(max_message_len), io_(io), name_(name), port_(io) - { - try - { - port_.open(serial_port_); - port_.set_option(boost::asio::serial_port_base::baud_rate(bauds_)); - status_ = true; - } - catch (boost::system::system_error& e) - { - RCLCPP_ERROR(rclcpp::get_logger("SerialListener"), "Failed to open serial port: %s", e.what()); - status_ = false; - return; - } - - read_buffer_.resize(max_message_len_); - start_async_read(); - - io_thread_ = std::thread([this]() - { - try - { - io_.run(); - } - catch (const std::exception& e) - { - RCLCPP_ERROR(rclcpp::get_logger("SerialListener"), "IO thread exception: %s", e.what()); - } - }); - } - - SerialListener::~SerialListener() - { - if (status_) - { - close(); - } - } - - void SerialListener::close() - { - if (status_) - { - if (port_.is_open()) port_.close(); - io_.stop(); - if (io_thread_.joinable()) io_thread_.join(); - status_ = false; - } - } - - void SerialListener::start_async_read() - { - if (!status_) start_async_read(); - - 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) - { - std::string d = std::string(read_buffer_.begin(), read_buffer_.begin() + bytes_transferred); - - auto allMess = Modelec::split(d, '\n'); - for (const auto& mess : allMess) - { - if (!mess.empty()) - { - // RCLCPP_INFO(rclcpp::get_logger("SerialListener"), "Received message: %s", mess.c_str()); - auto msg = std_msgs::msg::String(); - msg.data = mess; - if (publisher_) - { - publisher_->publish(msg); - } - } - } - - start_async_read(); // continue reading - } - else - { - RCLCPP_ERROR(rclcpp::get_logger("SerialListener"), "Async read error: %s", ec.message().c_str()); - } - }); - } - - void SerialListener::write(std_msgs::msg::String::SharedPtr msg) - { - std::lock_guard lock(write_mutex_); - bool write_in_progress = !write_queue_.empty(); - write_queue_.push_back(msg->data); - - if (!write_in_progress) - { - start_async_write(); - } - } - - void SerialListener::start_async_write() - { - if (write_queue_.empty()) return; - - boost::asio::async_write( - port_, - boost::asio::buffer(write_queue_.front()), - [this](const boost::system::error_code& ec, std::size_t /*length*/) - { - std::lock_guard lock(write_mutex_); - if (!ec) - { - write_queue_.pop_front(); - if (!write_queue_.empty()) - { - start_async_write(); // continue writing - } - } - else - { - RCLCPP_ERROR(rclcpp::get_logger("SerialListener"), "Async write error: %s", ec.message().c_str()); - } - }); - } - - MultipleSerialListener::MultipleSerialListener() - : Node("multiple_serial_listener"), io() - { - add_serial_listener_service_ = create_service( - "add_serial_listener", std::bind(&MultipleSerialListener::add_serial_listener, this, std::placeholders::_1, - std::placeholders::_2)); - } - - MultipleSerialListener::~MultipleSerialListener() - { - for (auto& listener : serial_listeners) - { - listener.second->close(); - } - } - - void MultipleSerialListener::add_serial_listener( - const std::shared_ptr request, - std::shared_ptr response) - { - if (serial_listeners.find(request->name) != serial_listeners.end()) - { - response->success = true; - response->publisher = serial_listeners[request->name]->publisher_->get_topic_name(); - response->subscriber = serial_listeners[request->name]->subscriber_->get_topic_name(); - return; - } - - RCLCPP_INFO(rclcpp::get_logger("MultipleSerialListener"), "Adding serial listener: %s", request->name.c_str()); - - auto listener = std::make_shared(request->name, request->bauds, request->serial_port, - MAX_MESSAGE_LEN, io); - - if (!listener->IsOk()) - { - response->success = false; - return; - } - - listener->publisher_ = create_publisher("raw_data/" + request->name, 10); - listener->subscriber_ = create_subscription( - "send_to_serial/" + request->name, rclcpp::QoS(10).reliability(rclcpp::ReliabilityPolicy::BestEffort), - std::bind(&SerialListener::write, listener.get(), std::placeholders::_1)); - - serial_listeners.insert({request->name, listener}); - - response->success = true; - 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()); - } - - rcl_interfaces::msg::SetParametersResult MultipleSerialListener::onParameterChange( - const std::vector& parameters) - { - rcl_interfaces::msg::SetParametersResult result; - result.successful = true; - - for (const auto& parameter : parameters) - { - if (parameter.get_name() == "bauds" || parameter.get_name() == "serial_port" || parameter.get_name() == - "max_message_len") - { - updateConnection(); - } - } - - return result; - } - - void MultipleSerialListener::updateConnection() - { - for (auto& listener : serial_listeners) - { - listener.second->SetMaxMessageLen(get_parameter("max_message_len").as_int()); - } - } -} - -int main(int argc, char** argv) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/src/modelec_com/src/pcb_action_interface.cpp b/src/modelec_com/src/pcb_action_interface.cpp index f39cdce..6c91aa8 100644 --- a/src/modelec_com/src/pcb_action_interface.cpp +++ b/src/modelec_com/src/pcb_action_interface.cpp @@ -1,11 +1,11 @@ -#include #include +#include #include #include namespace Modelec { - PCBActionInterface::PCBActionInterface() : Node("pcb_action_interface") + PCBActionInterface::PCBActionInterface() : Node("pcb_action_interface"), SerialListener() { // Service to create a new serial listener declare_parameter("serial_port", "/dev/USB_ACTION"); @@ -17,62 +17,7 @@ namespace Modelec request->bauds = get_parameter("baudrate").as_int(); request->serial_port = get_parameter("serial_port").as_string(); - 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); - - isOk = true; - } - 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"); - } + this->open(request->name, request->bauds, request->serial_port, MAX_MESSAGE_LEN); asc_get_sub_ = this->create_subscription( "action/get/asc", 10, @@ -265,20 +210,13 @@ namespace Modelec PCBActionInterface::~PCBActionInterface() { - if (pcb_executor_) - { - pcb_executor_->cancel(); - } - if (pcb_executor_thread_.joinable()) - { - pcb_executor_thread_.join(); - } } - void PCBActionInterface::PCBCallback(const std_msgs::msg::String::SharedPtr msg) + void PCBActionInterface::read(const std::string& msg) { - RCLCPP_DEBUG(this->get_logger(), "Received message: '%s'", msg->data.c_str()); - std::vector tokens = split(trim(msg->data), ';'); + RCLCPP_INFO_ONCE(this->get_logger(), "Received message: '%s'", msg.c_str()); + RCLCPP_DEBUG_SKIPFIRST(this->get_logger(), "Received message: '%s'", msg.c_str()); + std::vector tokens = split(trim(msg), ';'); if (tokens.size() < 2) { @@ -393,7 +331,7 @@ namespace Modelec } else { - RCLCPP_WARN(this->get_logger(), "Unknown message format for OK response: '%s'", msg->data.c_str()); + RCLCPP_WARN(this->get_logger(), "Unknown message format for OK response: '%s'", msg.c_str()); } } else if (tokens[0] == "KO") @@ -436,7 +374,7 @@ namespace Modelec } else { - RCLCPP_WARN(this->get_logger(), "Unknown message format: '%s'", msg->data.c_str()); + RCLCPP_WARN(this->get_logger(), "Unknown message format: '%s'", msg.c_str()); } } else if (tokens[0] == "EVENT") @@ -462,18 +400,17 @@ namespace Modelec } } - void PCBActionInterface::SendToPCB(const std::string& data) const + void PCBActionInterface::SendToPCB(const std::string& data) { - if (pcb_publisher_) + if (IsOk()) { - auto message = std_msgs::msg::String(); - message.data = data; - pcb_publisher_->publish(message); + RCLCPP_DEBUG(this->get_logger(), "Sending to PCB: %s", data.c_str()); + this->write(data); } } void PCBActionInterface::SendToPCB(const std::string& order, const std::string& elem, - const std::vector& data) const + const std::vector& data) { std::string command = order + ";" + elem; for (const auto& d : data) @@ -485,27 +422,28 @@ namespace Modelec SendToPCB(command); } - void PCBActionInterface::GetData(const std::string& elem, const std::vector& data) const + void PCBActionInterface::GetData(const std::string& elem, const std::vector& data) { SendToPCB("GET", elem, data); } - void PCBActionInterface::SendOrder(const std::string& elem, const std::vector& data) const + void PCBActionInterface::SendOrder(const std::string& elem, const std::vector& data) { SendToPCB("SET", elem, data); } - void PCBActionInterface::SendMove(const std::string& elem, const std::vector& data) const + void PCBActionInterface::SendMove(const std::string& elem, const std::vector& data) { SendToPCB("MOV", elem, data); } - void PCBActionInterface::RespondEvent(const std::string& elem, const std::vector& data) const + void PCBActionInterface::RespondEvent(const std::string& elem, const std::vector& data) { SendToPCB("ACK", elem, data); } } +#ifndef MODELEC_COM_TESTING int main(int argc, char** argv) { rclcpp::init(argc, argv); @@ -520,3 +458,4 @@ int main(int argc, char** argv) rclcpp::shutdown(); return 0; } +#endif diff --git a/src/modelec_com/src/pcb_action_interface.new.cpp b/src/modelec_com/src/pcb_action_interface.new.cpp deleted file mode 100644 index 624c377..0000000 --- a/src/modelec_com/src/pcb_action_interface.new.cpp +++ /dev/null @@ -1,461 +0,0 @@ -#include -#include -#include -#include - -namespace Modelec -{ - PCBActionInterface::PCBActionInterface() : Node("pcb_action_interface"), SerialListener() - { - // Service to create a new serial listener - declare_parameter("serial_port", "/dev/USB_ACTION"); - declare_parameter("baudrate", 115200); - declare_parameter("name", "pcb_action"); - - auto request = std::make_shared(); - request->name = get_parameter("name").as_string(); - request->bauds = get_parameter("baudrate").as_int(); - request->serial_port = get_parameter("serial_port").as_string(); - - this->open(request->name, request->bauds, request->serial_port, MAX_MESSAGE_LEN); - - asc_get_sub_ = this->create_subscription( - "action/get/asc", 10, - [this](const modelec_interfaces::msg::ActionAscPos::SharedPtr) - { - GetData("ASC", {"POS"}); - }); - - servo_get_sub_ = this->create_subscription( - "action/get/servo", 10, - [this](const modelec_interfaces::msg::ActionServoPos::SharedPtr msg) - { - GetData("SERVO" + std::to_string(msg->id), {"POS"}); - }); - - relay_get_sub_ = this->create_subscription( - "action/get/relay", 10, - [this](const modelec_interfaces::msg::ActionRelayState::SharedPtr msg) - { - GetData("RELAY" + std::to_string(msg->id), {"STATE"}); - }); - - asc_get_res_pub_ = this->create_publisher( - "action/get/asc/res", 10); - servo_get_res_pub_ = this->create_publisher( - "action/get/servo/res", 10); - relay_get_res_pub_ = this->create_publisher( - "action/get/relay/res", 10); - - asc_set_sub_ = this->create_subscription( - "action/set/asc", 10, - [this](const modelec_interfaces::msg::ActionAscPos::SharedPtr msg) - { - SendOrder("ASC", {std::to_string(msg->pos), std::to_string(msg->value)}); - }); - - servo_set_sub_ = this->create_subscription( - "action/set/servo", 10, - [this](const modelec_interfaces::msg::ActionServoPos::SharedPtr msg) - { - SendOrder("SERVO" + std::to_string(msg->id), { - "POS" + std::to_string(msg->pos), std::to_string(static_cast(msg->angle * 100)) - }); - }); - - asc_set_res_pub_ = this->create_publisher( - "action/set/asc/res", 10); - - servo_set_res_pub_ = this->create_publisher( - "action/set/servo/res", 10); - - asc_move_sub_ = this->create_subscription( - "action/move/asc", 10, - [this](const modelec_interfaces::msg::ActionAscPos::SharedPtr msg) - { - SendMove("ASC", {std::to_string(msg->pos)}); - }); - - servo_move_sub_ = this->create_subscription( - "action/move/servo", 10, - [this](const modelec_interfaces::msg::ActionServoPos::SharedPtr msg) - { - SendMove("SERVO" + std::to_string(msg->id), {"POS" + std::to_string(msg->pos)}); - }); - - relay_move_sub_ = this->create_subscription( - "action/move/relay", 10, - [this](const modelec_interfaces::msg::ActionRelayState::SharedPtr msg) - { - SendMove("RELAY" + std::to_string(msg->id), {std::to_string(msg->state)}); - }); - - asc_move_res_pub_ = this->create_publisher( - "action/move/asc/res", 10); - - servo_move_res_pub_ = this->create_publisher( - "action/move/servo/res", 10); - - relay_move_res_pub_ = this->create_publisher( - "action/move/relay/res", 10); - - tir_start_pub_ = this->create_publisher( - "action/tir/start", 10); - - tir_start_sub_ = this->create_subscription( - "action/tir/start/res", 10, - [this](const std_msgs::msg::Empty::SharedPtr) - { - RespondEvent("TIR", {"START"}); - }); - - tir_arm_pub_ = this->create_publisher( - "action/tir/arm", 10); - - tir_arm_sub_ = this->create_subscription( - "action/tir/arm/res", 10, - [this](const std_msgs::msg::Empty::SharedPtr) - { - RespondEvent("TIR", {"ARM"}); - }); - - tir_disarm_pub_ = this->create_publisher( - "action/tir/disarm", 10); - - tir_disarm_sub_ = this->create_subscription( - "action/tir/disarm/res", 10, - [this](const std_msgs::msg::Empty::SharedPtr) - { - RespondEvent("TIR", {"DIS"}); - }); - - tir_arm_set_sub_ = this->create_subscription( - "action/tir/arm/set", 10, - [this](const std_msgs::msg::Bool::SharedPtr msg) - { - SendOrder("TIR", {"ARM", msg->data ? "1" : "0"}); - }); - - - // TODO : check for real value there - asc_value_mapper_ = { - {0, 0}, - {1, 100}, - {2, 200}, - {3, 300} - }; - /*for (auto & [id, v] : asc_value_mapper_) - { - SendOrder("ASC", {std::to_string(id), std::to_string(v)}); - }*/ - - asc_state_ = 3; - - // SendMove("ASC", {std::to_string(asc_state_)}); - - servo_pos_mapper_ = { - {0, {{0, 0.55}, {1, 0}}}, - {1, {{0, 0}, {1, 0.4}}}, - {2, {{0, M_PI_2}}}, - {3, {{0, M_PI_2}}}, - {4, {{0, 1.25}, {1, 0.45}}}, - {5, {{0, 0}, {1, M_PI}}}, - }; - - for (auto & [id, v] : servo_pos_mapper_) - { - if (id == 5) continue; - - for (auto & [key, angle] : v) - { - rclcpp::sleep_for(std::chrono::milliseconds(100)); - - SendOrder("SERVO" + std::to_string(id), {"POS" + std::to_string(key), std::to_string(static_cast(angle * 100))}); - } - } - - servo_value_ = { - {0, 1}, - {1, 1}, - {2, 0}, - {3, 0}, - {4, 1}, - {5, 0} - }; - - for (auto & [id, v] : servo_value_) - { - rclcpp::sleep_for(std::chrono::milliseconds(100)); - - SendMove("SERVO" + std::to_string(id), {"POS" + std::to_string(v)}); - } - - relay_value_ = { - {1, false}, - {2, false}, - {3, false}, - }; - - for (auto & [id, v] : relay_value_) - { - rclcpp::sleep_for(std::chrono::milliseconds(100)); - - SendMove("RELAY" + std::to_string(id), {std::to_string(v)}); - } - - rclcpp::sleep_for(std::chrono::milliseconds(100)); - - SendOrder("TIR", {"ARM", "1"}); - } - - PCBActionInterface::~PCBActionInterface() - { - } - - void PCBActionInterface::read(const std::string& msg) - { - RCLCPP_INFO_ONCE(this->get_logger(), "Received message: '%s'", msg.c_str()); - RCLCPP_DEBUG_SKIPFIRST(this->get_logger(), "Received message: '%s'", msg.c_str()); - std::vector tokens = split(trim(msg), ';'); - - if (tokens.size() < 2) - { - RCLCPP_ERROR(this->get_logger(), "Invalid message format"); - return; - } - - if (tokens[0] == "SET") - { - if (tokens[1] == "ASC") - { - asc_state_ = std::stoi(tokens[3]); - - modelec_interfaces::msg::ActionAscPos asc_msg; - asc_msg.pos = asc_state_; - asc_msg.value = asc_value_mapper_[asc_state_]; - asc_msg.success = true; - asc_get_res_pub_->publish(asc_msg); - } - else if (startsWith(tokens[1], "SERVO")) - { - int servo_id = std::stoi(tokens[1].substr(5)); - int v = std::stoi(tokens[3]); - servo_value_[servo_id] = v; - - modelec_interfaces::msg::ActionServoPos servo_msg; - servo_msg.id = servo_id; - servo_msg.pos = v; - servo_msg.angle = servo_pos_mapper_[servo_id][v]; - servo_msg.success = true; - servo_get_res_pub_->publish(servo_msg); - } - else if (startsWith(tokens[1], "RELAY")) - { - int relay_id = std::stoi(tokens[1].substr(5)); - bool state = (tokens[3] == "1"); - relay_value_[relay_id] = state; - - modelec_interfaces::msg::ActionRelayState relay_msg; - relay_msg.id = relay_id; - relay_msg.state = state; - relay_msg.success = true; - relay_get_res_pub_->publish(relay_msg); - } - } - else if (tokens[0] == "OK") - { - if (tokens.size() == 4) - { - if (tokens[1] == "ASC") - { - int pos = std::stoi(tokens[2]); - int v = std::stoi(tokens[3]); - asc_value_mapper_[pos] = v; - - modelec_interfaces::msg::ActionAscPos asc_msg; - asc_msg.pos = pos; - asc_msg.value = v; - asc_msg.success = true; - asc_set_res_pub_->publish(asc_msg); - } - else if (startsWith(tokens[1], "SERVO")) - { - int servo_id = std::stoi(tokens[1].substr(5)); - int key = std::stoi(tokens[2].substr(3)); - int v = std::stoi(tokens[3]); - servo_pos_mapper_[servo_id][key] = v; - - modelec_interfaces::msg::ActionServoPos servo_msg; - servo_msg.id = servo_id; - servo_msg.pos = key; - servo_msg.angle = v; - servo_msg.success = true; - servo_set_res_pub_->publish(servo_msg); - } - } - else if (tokens.size() == 3) - { - if (tokens[1] == "ASC") - { - asc_state_ = std::stoi(tokens[2]); - - modelec_interfaces::msg::ActionAscPos asc_msg; - asc_msg.pos = asc_state_; - asc_msg.success = true; - asc_move_res_pub_->publish(asc_msg); - } - else if (startsWith(tokens[1], "SERVO")) - { - int servo_id = std::stoi(tokens[1].substr(5)); - int key = std::stoi(tokens[2].substr(3)); - servo_value_[servo_id] = key; - - modelec_interfaces::msg::ActionServoPos servo_msg; - servo_msg.id = servo_id; - servo_msg.pos = key; - servo_msg.success = true; - servo_move_res_pub_->publish(servo_msg); - } - else if (startsWith(tokens[1], "RELAY")) - { - int relay_id = std::stoi(tokens[1].substr(5)); - bool state = (tokens[2] == "1"); - relay_value_[relay_id] = state; - - modelec_interfaces::msg::ActionRelayState relay_msg; - relay_msg.id = relay_id; - relay_msg.state = state; - relay_msg.success = true; - relay_move_res_pub_->publish(relay_msg); - } - } - else - { - RCLCPP_WARN(this->get_logger(), "Unknown message format for OK response: '%s'", msg.c_str()); - } - } - else if (tokens[0] == "KO") - { - if (tokens.size() == 4) - { - if (tokens[1] == "ASC") - { - modelec_interfaces::msg::ActionAscPos asc_msg; - asc_msg.success = false; - asc_set_res_pub_->publish(asc_msg); - } - else if (startsWith(tokens[1], "SERVO")) - { - modelec_interfaces::msg::ActionServoPos servo_msg; - servo_msg.success = false; - servo_set_res_pub_->publish(servo_msg); - } - } - else if (tokens.size() == 3) - { - if (tokens[1] == "ASC") - { - modelec_interfaces::msg::ActionAscPos asc_msg; - asc_msg.success = false; - asc_move_res_pub_->publish(asc_msg); - } - else if (startsWith(tokens[1], "SERVO")) - { - modelec_interfaces::msg::ActionServoPos servo_msg; - servo_msg.success = false; - servo_move_res_pub_->publish(servo_msg); - } - else if (startsWith(tokens[1], "RELAY")) - { - modelec_interfaces::msg::ActionRelayState relay_msg; - relay_msg.success = false; - relay_move_res_pub_->publish(relay_msg); - } - } - else - { - RCLCPP_WARN(this->get_logger(), "Unknown message format: '%s'", msg.c_str()); - } - } - else if (tokens[0] == "EVENT") - { - if (tokens[1] == "TIR") - { - if (tokens[2] == "START") - { - tir_start_pub_->publish(std_msgs::msg::Empty()); - RespondEvent(tokens[1], {tokens[2]}); - } - else if (tokens[2] == "ARM") - { - tir_arm_pub_->publish(std_msgs::msg::Empty()); - RespondEvent(tokens[1], {tokens[2]}); - } - else if (tokens[2] == "DIS") - { - tir_disarm_pub_->publish(std_msgs::msg::Empty()); - RespondEvent(tokens[1], {tokens[2]}); - } - } - } - } - - void PCBActionInterface::SendToPCB(const std::string& data) - { - if (IsOk()) - { - RCLCPP_DEBUG(this->get_logger(), "Sending to PCB: %s", data.c_str()); - this->write(data); - } - } - - void PCBActionInterface::SendToPCB(const std::string& order, const std::string& elem, - const std::vector& data) - { - 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) - { - SendToPCB("GET", elem, data); - } - - void PCBActionInterface::SendOrder(const std::string& elem, const std::vector& data) - { - SendToPCB("SET", elem, data); - } - - void PCBActionInterface::SendMove(const std::string& elem, const std::vector& data) - { - SendToPCB("MOV", elem, data); - } - - void PCBActionInterface::RespondEvent(const std::string& elem, const std::vector& data) - { - SendToPCB("ACK", elem, data); - } -} - -#ifndef MODELEC_COM_TESTING -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; -} -#endif diff --git a/src/modelec_com/src/pcb_alim_interface.cpp b/src/modelec_com/src/pcb_alim_interface.cpp deleted file mode 100644 index d461e36..0000000 --- a/src/modelec_com/src/pcb_alim_interface.cpp +++ /dev/null @@ -1,516 +0,0 @@ -#include -#include -#include -#include - -namespace Modelec -{ - PCBAlimInterface::PCBAlimInterface() : Node("pcb_alim_interface") - { - // Service to create a new serial listener - declare_parameter("serial_port", "/dev/ttyUSB0"); - declare_parameter("baudrate", 115200); - declare_parameter("name", "pcb_alim"); - - auto request = std::make_shared(); - request->name = get_parameter("name").as_string(); - request->bauds = get_parameter("baudrate").as_int(); - request->serial_port = get_parameter("serial_port").as_string(); - - RCLCPP_INFO(this->get_logger(), "Requesting serial listener with parameters: " - "name='%s', bauds=%ld, serial_port='%s'", - request->name.c_str(), request->bauds, request->serial_port.c_str()); - - 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); - - isOk = true; - } - 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"); - } - - pcb_out_service_ = create_service( - "alim/out", - [this](const std::shared_ptr request, - std::shared_ptr response) - { - if (request->enable != -1 && request->type == modelec_interfaces::srv::AlimOut::Request::STATE) - { - HandleSetPCBOutData(request, response); - } - else - { - HandleGetPCBOutData(request, response); - } - }); - - pcb_in_service_ = create_service( - "alim/in", - [this](const std::shared_ptr request, - std::shared_ptr response) - { - HandleGetPCBInData(request, response); - }); - - pcb_bau_service_ = create_service( - "alim/bau", - [this](const std::shared_ptr request, - std::shared_ptr response) - { - HandleGetPCBBauData(request, response); - }); - - pcb_emg_service_ = create_service( - "alim/emg", - [this](const std::shared_ptr request, - std::shared_ptr response) - { - HandleSetPCBEmgData(request, response); - }); - - pcb_temp_service_ = create_service( - "alim/temp", - [this](const std::shared_ptr request, - std::shared_ptr response) - { - HandleGetPCBTempData(request, response); - }); - - pcb_emg_subscriber_ = this->create_subscription( - "alim/emg", 10, - [this](const modelec_interfaces::msg::AlimEmg::SharedPtr msg) - { - PCBEmgCallback(msg); - }); - } - - PCBAlimInterface::~PCBAlimInterface() - { - if (pcb_executor_) - { - pcb_executor_->cancel(); - } - if (pcb_executor_thread_.joinable()) - { - pcb_executor_thread_.join(); - } - } - - void PCBAlimInterface::PCBCallback(const std_msgs::msg::String::SharedPtr msg) - { - RCLCPP_DEBUG(this->get_logger(), "Received message: '%s'", msg->data.c_str()); - std::vector tokens = split(trim(msg->data), ';'); - if (tokens.size() < 2) - { - RCLCPP_ERROR(this->get_logger(), "Invalid message format: %s", msg->data.c_str()); - return; - } - - if (tokens[0] == "SET") - { - if (startsWith(tokens[1], "OUT")) - { - bool success = true; - int value = std::stoi(tokens[3]); - ResolveGetPCBOutRequest({success, value}); - } - else if (startsWith(tokens[1], "IN")) - { - bool success = true; - int value = std::stoi(tokens[3]); - ResolveGetPCBInRequest({success, value}); - } - else if (tokens[1] == "BAU") - { - bool success = true; - bool activate = (tokens[3] == "1"); - ResolveGetPCBBauRequest({success, activate}); - } - else if (tokens[1] == "TEMP") - { - bool success = true; - int value = std::stoi(tokens[3]); - ResolveGetPCBTempRequest({success, value}); - } - } - else if (tokens[0] == "OK") - { - if (startsWith(tokens[1], "OUT")) - { - bool success = true; - int value = std::stoi(tokens[3]); - - ResolveSetPCBOutRequest({success, value}); - } - else if (tokens[1] == "EMG") - { - bool success = true; - ResolveSetPCBEmgRequest(success); - } - } - else if (tokens[0] == "KO") - { - if (startsWith(tokens[1], "OUT")) - { - bool success = false; - int value = -1; - - ResolveSetPCBOutRequest({success, value}); - } - else if (tokens[1] == "EMG") - { - bool success = false; - - ResolveSetPCBEmgRequest(success); - } - } - else - { - RCLCPP_ERROR(this->get_logger(), "Unknown message type: %s", tokens[0].c_str()); - } - } - - void PCBAlimInterface::PCBEmgCallback(const modelec_interfaces::msg::AlimEmg::SharedPtr msg) const - { - SendOrder("EMG", {"STATE", msg->activate == true ? "1" : "0"}); - } - - void PCBAlimInterface::HandleGetPCBOutData(const std::shared_ptr request, - std::shared_ptr response) - { - if (!isOk) - { - RCLCPP_ERROR(get_logger(), "PCB not initialized"); - response->success = false; - return; - } - - std::promise promise; - auto future = promise.get_future(); - - { - std::lock_guard lock(pcb_out_mutex_); - pcb_out_promises_.push(std::move(promise)); - } - - GetData(request->out, {request->type}); - - PCBData result = future.get(); - - response->success = result.success; - response->result = result.value; - } - - void PCBAlimInterface::HandleSetPCBOutData(const std::shared_ptr request, - std::shared_ptr response) - { - if (!isOk) - { - RCLCPP_ERROR(get_logger(), "PCB not initialized"); - response->success = false; - return; - } - - std::promise promise; - auto future = promise.get_future(); - - { - std::lock_guard lock(pcb_out_mutex_); - pcb_out_promises_.push(std::move(promise)); - } - - SendOrder(request->out, {request->type, request->enable == true ? "1" : "0"}); - - PCBData result = future.get(); - - response->success = result.success; - response->result = result.value; - } - - void PCBAlimInterface::HandleGetPCBInData(const std::shared_ptr request, - std::shared_ptr response) - { - if (!isOk) - { - RCLCPP_ERROR(get_logger(), "PCB not initialized"); - response->success = false; - return; - } - - std::promise promise; - auto future = promise.get_future(); - - { - std::lock_guard lock(pcb_out_mutex_); - pcb_out_promises_.push(std::move(promise)); - } - - GetData("IN" + request->input, {request->type}); - - PCBData result = future.get(); - - response->success = result.success; - response->result = result.value; - } - - void PCBAlimInterface::HandleGetPCBBauData(const std::shared_ptr, - std::shared_ptr response) - { - if (!isOk) - { - RCLCPP_ERROR(get_logger(), "PCB not initialized"); - response->success = false; - return; - } - - std::promise promise; - auto future = promise.get_future(); - - { - std::lock_guard lock(pcb_bau_mutex_); - pcb_bau_promises_.push(std::move(promise)); - } - - GetData("BAU", {"STATE"}); - - PCBBau result = future.get(); - response->success = result.success; - response->activate = result.activate; - } - - void PCBAlimInterface::HandleSetPCBEmgData(const std::shared_ptr request, - std::shared_ptr response) - { - if (!isOk) - { - RCLCPP_ERROR(get_logger(), "PCB not initialized"); - response->success = false; - return; - } - - std::promise promise; - auto future = promise.get_future(); - - { - std::lock_guard lock(pcb_emg_mutex_); - pcb_emg_promises_.push(std::move(promise)); - } - - SendOrder("EMG", {"STATE", request->activate == true ? "1" : "0"}); - - response->success = future.get(); - } - - void PCBAlimInterface::HandleGetPCBTempData( - const std::shared_ptr, - std::shared_ptr response) - { - if (!isOk) - { - RCLCPP_ERROR(get_logger(), "PCB not initialized"); - response->success = false; - return; - } - - std::promise promise; - auto future = promise.get_future(); - - { - std::lock_guard lock(pcb_temp_mutex_); - pcb_temp_promises_.push(std::move(promise)); - } - - SendOrder("TEMP", {"CELS"}); - - PCBData result = future.get(); - response->success = result.success; - response->value = result.value; - } - - void PCBAlimInterface::ResolveGetPCBOutRequest(const PCBData& value) - { - std::lock_guard lock(pcb_out_mutex_); - if (!pcb_out_promises_.empty()) - { - auto promise = std::move(pcb_out_promises_.front()); - pcb_out_promises_.pop(); - promise.set_value(value); - } - else - { - RCLCPP_DEBUG(this->get_logger(), "No pending request for PCB out data"); - } - } - - void PCBAlimInterface::ResolveSetPCBOutRequest(const PCBData& value) - { - std::lock_guard lock(pcb_out_mutex_); - if (!pcb_out_promises_.empty()) - { - auto promise = std::move(pcb_out_promises_.front()); - pcb_out_promises_.pop(); - promise.set_value(value); - } - else - { - RCLCPP_DEBUG(this->get_logger(), "No pending request for PCB out data"); - } - } - - void PCBAlimInterface::ResolveGetPCBInRequest(const PCBData& value) - { - std::lock_guard lock(pcb_in_mutex_); - if (!pcb_in_promises_.empty()) - { - auto promise = std::move(pcb_in_promises_.front()); - pcb_in_promises_.pop(); - promise.set_value(value); - } - else - { - RCLCPP_DEBUG(this->get_logger(), "No pending request for PCB in data"); - } - } - - void PCBAlimInterface::ResolveGetPCBBauRequest(const PCBBau& value) - { - std::lock_guard lock(pcb_bau_mutex_); - if (!pcb_bau_promises_.empty()) - { - auto promise = std::move(pcb_bau_promises_.front()); - pcb_bau_promises_.pop(); - promise.set_value(value); - } - else - { - RCLCPP_DEBUG(this->get_logger(), "No pending request for PCB bau data"); - } - } - - void PCBAlimInterface::ResolveSetPCBEmgRequest(bool value) - { - std::lock_guard lock(pcb_emg_mutex_); - if (!pcb_emg_promises_.empty()) - { - auto promise = std::move(pcb_emg_promises_.front()); - pcb_emg_promises_.pop(); - promise.set_value(value); - } - else - { - RCLCPP_DEBUG(this->get_logger(), "No pending request for PCB emg data"); - } - } - - void PCBAlimInterface::ResolveGetPCBTempRequest(const PCBData& value) - { - std::lock_guard lock(pcb_temp_mutex_); - if (!pcb_temp_promises_.empty()) - { - auto promise = std::move(pcb_temp_promises_.front()); - pcb_temp_promises_.pop(); - promise.set_value(value); - } - else - { - RCLCPP_DEBUG(this->get_logger(), "No pending request for PCB temp data"); - } - } - - void PCBAlimInterface::SendToPCB(const std::string& data) const - { - if (pcb_publisher_) - { - auto message = std_msgs::msg::String(); - message.data = data; - pcb_publisher_->publish(message); - } - } - - void PCBAlimInterface::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 PCBAlimInterface::GetData(const std::string& elem, const std::vector& data) const - { - SendToPCB("GET", elem, data); - } - - void PCBAlimInterface::SendOrder(const std::string& elem, const std::vector& data) const - { - SendToPCB("SET", elem, data); - } -} // namespace Modelec - -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_odo_interface.cpp b/src/modelec_com/src/pcb_odo_interface.cpp index c10193b..452e7fe 100644 --- a/src/modelec_com/src/pcb_odo_interface.cpp +++ b/src/modelec_com/src/pcb_odo_interface.cpp @@ -1,12 +1,11 @@ #include #include -#include #include #include namespace Modelec { - PCBOdoInterface::PCBOdoInterface() : Node("pcb_odo_interface") + PCBOdoInterface::PCBOdoInterface() : Node("pcb_odo_interface"), SerialListener() { declare_parameter("serial_port", "/dev/USB_ODO"); declare_parameter("baudrate", 115200); @@ -18,70 +17,7 @@ namespace Modelec request->bauds = get_parameter("baudrate").as_int(); request->serial_port = get_parameter("serial_port").as_string(); - 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); - - isOk = true; - - SetStart(true); - - SetPID("THETA", 14, 0, 0); - SetPID("POS", 10, 0, 0); - SetPID("LEFT", 5, 0, 0); - SetPID("RIGHT", 5, 0, 0); - } - 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"); - } + this->open(request->name, request->bauds, request->serial_port, MAX_MESSAGE_LEN); odo_pos_publisher_ = this->create_publisher( "odometry/position", 10); @@ -89,7 +25,7 @@ namespace Modelec odo_get_pos_sub_ = this->create_subscription( "odometry/get/pos", 30, [this](const std_msgs::msg::Empty::SharedPtr) { - if (isOk) + if (IsOk()) { GetPos(); } @@ -145,29 +81,27 @@ namespace Modelec SendOrder("START", {std::to_string(msg->data)}); } }); + + + SetPID("THETA", 14, 0, 0); + SetPID("POS", 10, 0, 0); + SetPID("LEFT", 5, 0, 0); + SetPID("RIGHT", 5, 0, 0); } PCBOdoInterface::~PCBOdoInterface() { SetStart(false); - - if (pcb_executor_) - { - pcb_executor_->cancel(); - } - if (pcb_executor_thread_.joinable()) - { - pcb_executor_thread_.join(); - } } - void PCBOdoInterface::PCBCallback(const std_msgs::msg::String::SharedPtr msg) + void PCBOdoInterface::read(const std::string& msg) { - RCLCPP_DEBUG(this->get_logger(), "Received from PCB: %s", msg->data.c_str()); - std::vector tokens = split(trim(msg->data), ';'); + RCLCPP_INFO_ONCE(this->get_logger(), "Received from PCB: %s", msg.c_str()); + RCLCPP_DEBUG_SKIPFIRST(this->get_logger(), "Received from PCB: %s", msg.c_str()); + std::vector tokens = split(trim(msg), ';'); if (tokens.size() < 2) { - RCLCPP_ERROR(this->get_logger(), "Invalid message format: %s", msg->data.c_str()); + RCLCPP_ERROR(this->get_logger(), "Invalid message format: %s", msg.c_str()); return; } @@ -254,7 +188,7 @@ namespace Modelec } else { - RCLCPP_INFO(this->get_logger(), "PCB response: %s", msg->data.c_str()); + RCLCPP_INFO(this->get_logger(), "PCB response: %s", msg.c_str()); } } else if (tokens[0] == "KO") @@ -271,7 +205,7 @@ namespace Modelec } else { - RCLCPP_WARN(this->get_logger(), "PCB error: %s", msg->data.c_str()); + RCLCPP_WARN(this->get_logger(), "PCB error: %s", msg.c_str()); } } } @@ -297,12 +231,10 @@ namespace Modelec void PCBOdoInterface::SendToPCB(const std::string& data) { - if (pcb_publisher_) + if (IsOk()) { RCLCPP_DEBUG(this->get_logger(), "Sending to PCB: %s", data.c_str()); - auto message = std_msgs::msg::String(); - message.data = data; - pcb_publisher_->publish(message); + this->write(data); } } @@ -449,17 +381,18 @@ namespace Modelec } } // Modelec +#ifndef MODELEC_COM_TESTING 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! */); + rclcpp::ExecutorOptions(), 2); executor.add_node(node); executor.spin(); rclcpp::shutdown(); return 0; } +#endif \ No newline at end of file diff --git a/src/modelec_com/src/pcb_odo_interface.new.cpp b/src/modelec_com/src/pcb_odo_interface.new.cpp deleted file mode 100644 index ec4b871..0000000 --- a/src/modelec_com/src/pcb_odo_interface.new.cpp +++ /dev/null @@ -1,398 +0,0 @@ -#include -#include -#include -#include - -namespace Modelec -{ - PCBOdoInterface::PCBOdoInterface() : Node("pcb_odo_interface"), SerialListener() - { - declare_parameter("serial_port", "/dev/USB_ODO"); - declare_parameter("baudrate", 115200); - declare_parameter("name", "pcb_odo"); - - // Service to create a new serial listener - auto request = std::make_shared(); - request->name = get_parameter("name").as_string(); - request->bauds = get_parameter("baudrate").as_int(); - request->serial_port = get_parameter("serial_port").as_string(); - - this->open(request->name, request->bauds, request->serial_port, MAX_MESSAGE_LEN); - - odo_pos_publisher_ = this->create_publisher( - "odometry/position", 10); - - odo_get_pos_sub_ = this->create_subscription( - "odometry/get/pos", 30, [this](const std_msgs::msg::Empty::SharedPtr) - { - if (IsOk()) - { - GetPos(); - } - }); - - odo_speed_publisher_ = this->create_publisher( - "odometry/speed", 10); - - odo_tof_publisher_ = this->create_publisher( - "odometry/tof", 10); - - odo_waypoint_reach_publisher_ = this->create_publisher( - "odometry/waypoint_reach", 10); - - odo_pid_publisher_ = this->create_publisher( - "odometry/get_pid", 10); - - odo_add_waypoint_subscriber_ = this->create_subscription( - "odometry/add_waypoint", 30, - [this](const modelec_interfaces::msg::OdometryWaypoint::SharedPtr msg) - { - AddWaypointCallback(msg); - }); - - odo_add_waypoints_subscriber_ = this->create_subscription( - "odometry/add_waypoints", 30, - [this](const modelec_interfaces::msg::OdometryWaypoints::SharedPtr msg) - { - AddWaypointsCallback(msg); - }); - - odo_set_pos_subscriber_ = this->create_subscription( - "odometry/set_position", 10, - [this](const modelec_interfaces::msg::OdometryPos::SharedPtr msg) - { - SetPosCallback(msg); - }); - - odo_set_pid_subscriber_ = this->create_subscription( - "odometry/set_pid", 10, - [this](const modelec_interfaces::msg::OdometryPid::SharedPtr msg) - { - SetPIDCallback(msg); - }); - - start_odo_sub_ = this->create_subscription( - "odometry/start", 10, - [this](const std_msgs::msg::Bool::SharedPtr msg) - { - if (msg->data != start_odo_) - { - start_odo_ = msg->data; - SendOrder("START", {std::to_string(msg->data)}); - } - }); - - - SetPID("THETA", 14, 0, 0); - SetPID("POS", 10, 0, 0); - SetPID("LEFT", 5, 0, 0); - SetPID("RIGHT", 5, 0, 0); - } - - PCBOdoInterface::~PCBOdoInterface() - { - SetStart(false); - } - - void PCBOdoInterface::read(const std::string& msg) - { - RCLCPP_INFO_ONCE(this->get_logger(), "Received from PCB: %s", msg.c_str()); - RCLCPP_DEBUG_SKIPFIRST(this->get_logger(), "Received from PCB: %s", msg.c_str()); - std::vector tokens = split(trim(msg), ';'); - if (tokens.size() < 2) - { - RCLCPP_ERROR(this->get_logger(), "Invalid message format: %s", msg.c_str()); - return; - } - - if (tokens[0] == "SET") - { - if (tokens[1] == "POS") - { - long x = std::stol(tokens[2]); - long y = std::stol(tokens[3]); - double theta = std::stod(tokens[4]); - - auto message = modelec_interfaces::msg::OdometryPos(); - message.x = x; - message.y = y; - message.theta = theta; - - if (odo_pos_publisher_) { - odo_pos_publisher_->publish(message); - } - } - else if (tokens[1] == "SPEED") - { - long x = std::stol(tokens[2]); - long y = std::stol(tokens[3]); - double theta = std::stod(tokens[4]); - - auto message = modelec_interfaces::msg::OdometrySpeed(); - message.x = x; - message.y = y; - message.theta = theta; - - odo_speed_publisher_->publish(message); - } - else if (tokens[1] == "DIST") - { - int n = std::stoi(tokens[2]); - long dist = std::stol(tokens[3]); - - auto message = modelec_interfaces::msg::OdometryToF(); - message.n = n; - message.distance = dist; - - odo_tof_publisher_->publish(message); - } - else if (tokens[1] == "WAYPOINT") - { - int id = std::stoi(tokens[2]); - - auto message = modelec_interfaces::msg::OdometryWaypoint(); - message.id = id; - - odo_waypoint_reach_publisher_->publish(message); - } - else if (tokens[1] == "PID") - { - std::string name = tokens[2]; - float p = std::stof(tokens[3]); - float i = std::stof(tokens[4]); - float d = std::stof(tokens[5]); - - auto message = modelec_interfaces::msg::OdometryPid(); - message.name = name; - message.p = p; - message.i = i; - message.d = d; - - odo_pid_publisher_->publish(message); - } - } - else if (tokens[0] == "OK") - { - if (tokens[1] == "START") - { - start_odo_ = tokens[2] == "1"; - } - else if (tokens[1] == "WAYPOINT") - { - } - else if (tokens[1] == "PID") - { - } - else if (tokens[1] == "POS") - { - } - else - { - RCLCPP_INFO(this->get_logger(), "PCB response: %s", msg.c_str()); - } - } - else if (tokens[0] == "KO") - { - if (tokens[1] == "START") - { - } - else if (tokens[1] == "WAYPOINT") - { - } - - else if (tokens[1] == "PID") - { - } - else - { - RCLCPP_WARN(this->get_logger(), "PCB error: %s", msg.c_str()); - } - } - } - - void PCBOdoInterface::AddWaypointsCallback(const modelec_interfaces::msg::OdometryWaypoints::SharedPtr msg) { - AddWaypoints(msg); - } - - void PCBOdoInterface::AddWaypointCallback(const modelec_interfaces::msg::OdometryWaypoint::SharedPtr msg) - { - AddWaypoint(msg); - } - - void PCBOdoInterface::SetPosCallback(const modelec_interfaces::msg::OdometryPos::SharedPtr msg) - { - SetRobotPos(msg); - } - - void PCBOdoInterface::SetPIDCallback(const modelec_interfaces::msg::OdometryPid::SharedPtr msg) - { - SetPID(msg); - } - - void PCBOdoInterface::SendToPCB(const std::string& data) - { - if (IsOk()) - { - RCLCPP_DEBUG(this->get_logger(), "Sending to PCB: %s", data.c_str()); - this->write(data); - } - } - - void PCBOdoInterface::SendToPCB(const std::string& order, const std::string& elem, - const std::vector& data) - { - std::string command = order + ";" + elem; - for (const auto& d : data) - { - command += ";" + d; - } - command += "\n"; - - SendToPCB(command); - } - - void PCBOdoInterface::GetData(const std::string& elem, const std::vector& data) - { - SendToPCB("GET", elem, data); - } - - void PCBOdoInterface::SendOrder(const std::string& elem, const std::vector& data) - { - SendToPCB("SET", elem, data); - } - - void PCBOdoInterface::GetPos() - { - GetData("POS"); - } - - void PCBOdoInterface::GetSpeed() - { - GetData("SPEED"); - } - - void PCBOdoInterface::GetToF(const int& tof) - { - GetData("DIST", {std::to_string(tof)}); - } - - void PCBOdoInterface::SetRobotPos(const modelec_interfaces::msg::OdometryPos::SharedPtr msg) - { - SetRobotPos(msg->x, msg->y, msg->theta); - } - - void PCBOdoInterface::SetRobotPos(const long x, const long y, const double theta) - { - std::vector data = { - std::to_string(x), - std::to_string(y), - std::to_string(theta) - }; - - SendOrder("POS", data); - } - - void PCBOdoInterface::AddWaypoints(modelec_interfaces::msg::OdometryWaypoints::SharedPtr msg) - { - if (!start_odo_) - { - SendOrder("START", {std::to_string(true)}); - } - - std::vector data; - - for (const auto& wp : msg->waypoints) - { - data.push_back(std::to_string(wp.id)); - data.push_back(std::to_string(wp.is_end)); - data.push_back(std::to_string(wp.x)); - data.push_back(std::to_string(wp.y)); - data.push_back(std::to_string(wp.theta)); - } - - SendOrder("WAYPOINT", data); - } - - void PCBOdoInterface::AddWaypoint( - const modelec_interfaces::msg::OdometryWaypoint::SharedPtr msg) - { - AddWaypoint(msg->id, msg->is_end, msg->x, msg->y, msg->theta); - } - - void PCBOdoInterface::AddWaypoint(const int index, const bool IsStopPoint, const long x, const long y, - const double theta) - { - if (!start_odo_) - { - SendOrder("START", {std::to_string(true)}); - } - - std::vector data = { - std::to_string(index), - std::to_string(IsStopPoint), - std::to_string(x), - std::to_string(y), - std::to_string(theta) - }; - - SendOrder("WAYPOINT", data); - } - - void PCBOdoInterface::SetStart(const modelec_interfaces::msg::OdometryStart::SharedPtr msg) - { - SetStart(msg->start); - } - - void PCBOdoInterface::SetStart(bool start) - { - SendOrder("START", {std::to_string(start)}); - } - - void PCBOdoInterface::GetPID() - { - GetData("PID"); - } - - void PCBOdoInterface::SetPID(const modelec_interfaces::msg::OdometryPid::SharedPtr msg) - { - SetPID(msg->name, msg->p, msg->i, msg->d); - } - - void PCBOdoInterface::SetPID(std::string name, float p, float i, float d, std::optional min, std::optional max) - { - std::vector data = { - name, - std::to_string(p), - std::to_string(i), - std::to_string(d) - }; - - if (min.has_value()) - { - data.push_back(std::to_string(min.value())); - } - - if (max.has_value()) - { - data.push_back(std::to_string(max.value())); - } - - SendOrder("PID", data); - } -} // Modelec - -#ifndef MODELEC_COM_TESTING -int main(int argc, char** argv) -{ - rclcpp::init(argc, argv); - auto node = std::make_shared(); - - rclcpp::executors::MultiThreadedExecutor executor( - rclcpp::ExecutorOptions(), 2); - - executor.add_node(node); - executor.spin(); - rclcpp::shutdown(); - return 0; -} -#endif \ No newline at end of file diff --git a/src/modelec_core/launch/modelec.launch.py b/src/modelec_core/launch/modelec.launch.py index 2670d0e..2bd954c 100644 --- a/src/modelec_core/launch/modelec.launch.py +++ b/src/modelec_core/launch/modelec.launch.py @@ -96,7 +96,7 @@ def generate_launch_description(): Node(package='modelec_com', executable='serial_listener', name='serial_listener'), Node( package='modelec_com', - executable='pcb_odo_interface_new', + executable='pcb_odo_interface', name='pcb_odo_interface', parameters=[{ 'serial_port': "/dev/USB_ODO", @@ -106,7 +106,7 @@ def generate_launch_description(): ), Node( package='modelec_com', - executable='pcb_action_interface_new', + executable='pcb_action_interface', name='pcb_action_interface', parameters=[{ 'serial_port': "/dev/USB_ACTION",