From b21ca2bffb1b2e597c69660b83f223cbabffbb9f Mon Sep 17 00:00:00 2001 From: acki Date: Thu, 11 Dec 2025 10:39:54 +0100 Subject: [PATCH 1/6] reset the started variable fix #6 --- src/modelec_strat/src/strat_fms.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modelec_strat/src/strat_fms.cpp b/src/modelec_strat/src/strat_fms.cpp index d782cda..07ca5eb 100644 --- a/src/modelec_strat/src/strat_fms.cpp +++ b/src/modelec_strat/src/strat_fms.cpp @@ -126,6 +126,8 @@ namespace Modelec case State::INIT: if (team_selected_) { + started_ = false; + Transition(State::WAIT_START, "System ready"); } break; From 675a72b506418815db82840b77214666b4842a91 Mon Sep 17 00:00:00 2001 From: acki Date: Thu, 11 Dec 2025 16:01:52 +0100 Subject: [PATCH 2/6] new serial listener --- src/modelec_com/CMakeLists.txt | 10 + .../modelec_com/pcb_odo_interface.new.hpp | 116 +++++ .../include/modelec_com/serial_listener.hpp | 59 +++ src/modelec_com/src/pcb_odo_interface.new.cpp | 469 ++++++++++++++++++ src/modelec_com/src/serial_listener.cpp | 149 ++++++ src/modelec_core/launch/modelec.launch.py | 2 +- 6 files changed, 804 insertions(+), 1 deletion(-) create mode 100644 src/modelec_com/include/modelec_com/pcb_odo_interface.new.hpp create mode 100644 src/modelec_com/include/modelec_com/serial_listener.hpp create mode 100644 src/modelec_com/src/pcb_odo_interface.new.cpp create mode 100644 src/modelec_com/src/serial_listener.cpp diff --git a/src/modelec_com/CMakeLists.txt b/src/modelec_com/CMakeLists.txt index f9150cd..b9bef50 100644 --- a/src/modelec_com/CMakeLists.txt +++ b/src/modelec_com/CMakeLists.txt @@ -35,6 +35,15 @@ 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 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) @@ -67,6 +76,7 @@ install(TARGETS serial_listener pcb_alim_interface pcb_odo_interface + pcb_odo_interface_new pcb_action_interface DESTINATION lib/${PROJECT_NAME} ) 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 new file mode 100644 index 0000000..a0d8893 --- /dev/null +++ b/src/modelec_com/include/modelec_com/pcb_odo_interface.new.hpp @@ -0,0 +1,116 @@ +#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(); + + 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 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; + + bool isOk = false; + + 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/include/modelec_com/serial_listener.hpp b/src/modelec_com/include/modelec_com/serial_listener.hpp new file mode 100644 index 0000000..5078f52 --- /dev/null +++ b/src/modelec_com/include/modelec_com/serial_listener.hpp @@ -0,0 +1,59 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#define MAX_MESSAGE_LEN 1048 + +namespace Modelec +{ + class SerialListener + { + protected: + 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(); + + SerialListener(const std::string& name, int bauds, const std::string& serial_port, + int max_message_len); + + virtual ~SerialListener(); + + void close(); + void open(const std::string& name, int bauds, const std::string& serial_port, + int max_message_len); + + void SetMaxMessageLen(int max_message_len) { max_message_len_ = max_message_len; } + bool IsOk() const { return status_; } + + void SetOk() { status_ = true; } + + virtual void write(const std::string& msg); + + virtual void read(const std::string& msg); + }; +} // namespace Modelec \ 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 new file mode 100644 index 0000000..4c0f792 --- /dev/null +++ b/src/modelec_com/src/pcb_odo_interface.new.cpp @@ -0,0 +1,469 @@ +#include +#include +#include +#include +#include +#include + +namespace Modelec +{ + PCBOdoInterface::PCBOdoInterface() : Node("pcb_odo_interface"), SerialListener() + { + declare_parameter("serial_port", "/tmp/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); + + /*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"); + }*/ + + 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)}); + } + }); + } + + PCBOdoInterface::~PCBOdoInterface() + { + SetStart(false); + + if (pcb_executor_) + { + pcb_executor_->cancel(); + } + if (pcb_executor_thread_.joinable()) + { + pcb_executor_thread_.join(); + } + } + + void PCBOdoInterface::read(const std::string& msg) + { + RCLCPP_INFO(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 (pcb_publisher_) + { + 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); + } + } + + 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_com/src/serial_listener.cpp b/src/modelec_com/src/serial_listener.cpp new file mode 100644 index 0000000..29a8c6e --- /dev/null +++ b/src/modelec_com/src/serial_listener.cpp @@ -0,0 +1,149 @@ +#include +#include + +namespace Modelec +{ + SerialListener::SerialListener() : io_(), port_(io_) + { + } + + SerialListener::SerialListener(const std::string& name, int bauds, const std::string& serial_port, + int max_message_len) : io_(), port_(io_) + { + open(name, bauds, serial_port, max_message_len); + } + + 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::open(const std::string& name, int bauds, const std::string& serial_port, + int max_message_len) { + this->name_ = name; + this->bauds_ = bauds; + this->serial_port_ = serial_port; + this->max_message_len_ = max_message_len; + + 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()); + } + }); + } + + + 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); + }*/ + read(mess); + } + } + + start_async_read(); // continue reading + } + else + { + RCLCPP_ERROR(rclcpp::get_logger("SerialListener"), "Async read error: %s", ec.message().c_str()); + } + }); + } + + void SerialListener::write(const std::string& msg) + { + std::lock_guard lock(write_mutex_); + bool write_in_progress = !write_queue_.empty(); + write_queue_.push_back(msg); + + 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()); + } + }); + } + + void SerialListener::read(const std::string&) { + // Default implementation does nothing + } +} // namespace Modelec \ 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 11fb8f3..feaee78 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', + executable='pcb_odo_interface_new', name='pcb_odo_interface', parameters=[{ 'serial_port': "/dev/USB_ODO", From dc6e9a33ea1b16a871e8745f8b59f5cae7ae1a2b Mon Sep 17 00:00:00 2001 From: acki Date: Thu, 11 Dec 2025 17:02:16 +0100 Subject: [PATCH 3/6] new serial listener --- simulated_pcb/action.py | 8 ++++---- .../modelec_com/pcb_odo_interface.new.hpp | 6 ------ src/modelec_com/src/pcb_odo_interface.new.cpp | 17 +++-------------- src/modelec_core/launch/modelec.launch.py | 2 +- 4 files changed, 8 insertions(+), 25 deletions(-) diff --git a/simulated_pcb/action.py b/simulated_pcb/action.py index 39b93b4..29bad6d 100644 --- a/simulated_pcb/action.py +++ b/simulated_pcb/action.py @@ -133,7 +133,7 @@ if __name__ == '__main__': sim.stop() print("Simulation stopped") -# socat -d -d pty,raw,echo=0,link=/tmp/MARCEL_ACTION_SIM pty,raw,echo=0,link=/tmp/MARCEL_ACTION -# python3 simulated_pcb/action.py --port /tmp/MARCEL_ACTION_SIM -# socat -d -d pty,raw,echo=0,link=/tmp/ENEMY_ACTION_SIM pty,raw,echo=0,link=/tmp/ENEMY_ACTION -# python3 simulated_pcb/action.py --port /tmp/ENEMY_ACTION_SIM \ No newline at end of file +# socat -d -d pty,raw,echo=0,link=/tmp/ACTION_SIM pty,raw,echo=0,link=/tmp/ACTION_USB +# python3 simulated_pcb/action.py --port /tmp/ACTION_SIM +# socat -d -d pty,raw,echo=0,link=/tmp/ODO_SIM pty,raw,echo=0,link=/tmp/ODO_USB +# python3 simulated_pcb/action.py --port /tmp/ODO_SIM 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 index a0d8893..8e496c8 100644 --- a/src/modelec_com/include/modelec_com/pcb_odo_interface.new.hpp +++ b/src/modelec_com/include/modelec_com/pcb_odo_interface.new.hpp @@ -37,9 +37,6 @@ namespace Modelec public: PCBOdoInterface(); - rclcpp::CallbackGroup::SharedPtr pcb_callback_group_; - std::shared_ptr pcb_executor_; - std::thread pcb_executor_thread_; ~PCBOdoInterface() override; struct OdometryData @@ -57,9 +54,6 @@ namespace Modelec }; private: - rclcpp::Publisher::SharedPtr pcb_publisher_; - rclcpp::Subscription::SharedPtr pcb_subscriber_; - void read(const std::string& msg) override; rclcpp::Publisher::SharedPtr odo_pos_publisher_; diff --git a/src/modelec_com/src/pcb_odo_interface.new.cpp b/src/modelec_com/src/pcb_odo_interface.new.cpp index 4c0f792..451cf95 100644 --- a/src/modelec_com/src/pcb_odo_interface.new.cpp +++ b/src/modelec_com/src/pcb_odo_interface.new.cpp @@ -153,20 +153,11 @@ namespace Modelec PCBOdoInterface::~PCBOdoInterface() { SetStart(false); - - if (pcb_executor_) - { - pcb_executor_->cancel(); - } - if (pcb_executor_thread_.joinable()) - { - pcb_executor_thread_.join(); - } } void PCBOdoInterface::read(const std::string& msg) { - RCLCPP_INFO(this->get_logger(), "Received from PCB: %s", msg.c_str()); + RCLCPP_DEBUG(this->get_logger(), "Received from PCB: %s", msg.c_str()); std::vector tokens = split(trim(msg), ';'); if (tokens.size() < 2) { @@ -300,12 +291,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); } } diff --git a/src/modelec_core/launch/modelec.launch.py b/src/modelec_core/launch/modelec.launch.py index feaee78..8022516 100644 --- a/src/modelec_core/launch/modelec.launch.py +++ b/src/modelec_core/launch/modelec.launch.py @@ -109,7 +109,7 @@ def generate_launch_description(): executable='pcb_action_interface', name='pcb_action_interface', parameters=[{ - 'serial_port': "/tmp/USB_ACTION", + 'serial_port': "/dev/USB_ACTION", 'baudrate': 115200, 'name': "pcb_action", }] From b171ad4edd1a39a2b9699a848d0f3729c55b5a6b Mon Sep 17 00:00:00 2001 From: acki Date: Thu, 11 Dec 2025 17:45:46 +0100 Subject: [PATCH 4/6] update action pcb listener --- src/modelec_com/CMakeLists.txt | 10 + .../modelec_com/pcb_action_interface.new.hpp | 76 +++ .../modelec_com/pcb_odo_interface.new.hpp | 2 - .../src/pcb_action_interface.new.cpp | 460 ++++++++++++++++++ src/modelec_com/src/pcb_odo_interface.new.cpp | 75 +-- src/modelec_core/launch/modelec.launch.py | 2 +- 6 files changed, 554 insertions(+), 71 deletions(-) create mode 100644 src/modelec_com/include/modelec_com/pcb_action_interface.new.hpp create mode 100644 src/modelec_com/src/pcb_action_interface.new.cpp diff --git a/src/modelec_com/CMakeLists.txt b/src/modelec_com/CMakeLists.txt index b9bef50..b6db509 100644 --- a/src/modelec_com/CMakeLists.txt +++ b/src/modelec_com/CMakeLists.txt @@ -44,6 +44,15 @@ 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) @@ -78,6 +87,7 @@ install(TARGETS 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/pcb_action_interface.new.hpp b/src/modelec_com/include/modelec_com/pcb_action_interface.new.hpp new file mode 100644 index 0000000..d3199d3 --- /dev/null +++ b/src/modelec_com/include/modelec_com/pcb_action_interface.new.hpp @@ -0,0 +1,76 @@ +#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_odo_interface.new.hpp b/src/modelec_com/include/modelec_com/pcb_odo_interface.new.hpp index 8e496c8..213380d 100644 --- a/src/modelec_com/include/modelec_com/pcb_odo_interface.new.hpp +++ b/src/modelec_com/include/modelec_com/pcb_odo_interface.new.hpp @@ -79,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/src/pcb_action_interface.new.cpp b/src/modelec_com/src/pcb_action_interface.new.cpp new file mode 100644 index 0000000..78ed580 --- /dev/null +++ b/src/modelec_com/src/pcb_action_interface.new.cpp @@ -0,0 +1,460 @@ +#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_DEBUG(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_odo_interface.new.cpp b/src/modelec_com/src/pcb_odo_interface.new.cpp index 451cf95..7d29cac 100644 --- a/src/modelec_com/src/pcb_odo_interface.new.cpp +++ b/src/modelec_com/src/pcb_odo_interface.new.cpp @@ -1,7 +1,5 @@ #include -#include #include -#include #include #include @@ -21,78 +19,13 @@ namespace Modelec this->open(request->name, request->bauds, request->serial_port, MAX_MESSAGE_LEN); - /*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"); - }*/ - 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) + if (IsOk()) { GetPos(); } @@ -148,6 +81,12 @@ 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() diff --git a/src/modelec_core/launch/modelec.launch.py b/src/modelec_core/launch/modelec.launch.py index 8022516..2670d0e 100644 --- a/src/modelec_core/launch/modelec.launch.py +++ b/src/modelec_core/launch/modelec.launch.py @@ -106,7 +106,7 @@ def generate_launch_description(): ), Node( package='modelec_com', - executable='pcb_action_interface', + executable='pcb_action_interface_new', name='pcb_action_interface', parameters=[{ 'serial_port': "/dev/USB_ACTION", From 2fb633ba051f0c084e7b1e283482f946f5c09e4f Mon Sep 17 00:00:00 2001 From: acki Date: Thu, 11 Dec 2025 17:50:40 +0100 Subject: [PATCH 5/6] update action pcb listener --- src/modelec_com/src/pcb_action_interface.new.cpp | 2 +- src/modelec_com/src/pcb_odo_interface.new.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modelec_com/src/pcb_action_interface.new.cpp b/src/modelec_com/src/pcb_action_interface.new.cpp index 78ed580..2875631 100644 --- a/src/modelec_com/src/pcb_action_interface.new.cpp +++ b/src/modelec_com/src/pcb_action_interface.new.cpp @@ -214,7 +214,7 @@ namespace Modelec void PCBActionInterface::read(const std::string& msg) { - RCLCPP_DEBUG(this->get_logger(), "Received message: '%s'", msg.c_str()); + RCLCPP_INFO_ONCE(this->get_logger(), "Received message: '%s'", msg.c_str()); std::vector tokens = split(trim(msg), ';'); if (tokens.size() < 2) diff --git a/src/modelec_com/src/pcb_odo_interface.new.cpp b/src/modelec_com/src/pcb_odo_interface.new.cpp index 7d29cac..cea3bd1 100644 --- a/src/modelec_com/src/pcb_odo_interface.new.cpp +++ b/src/modelec_com/src/pcb_odo_interface.new.cpp @@ -7,7 +7,7 @@ namespace Modelec { PCBOdoInterface::PCBOdoInterface() : Node("pcb_odo_interface"), SerialListener() { - declare_parameter("serial_port", "/tmp/USB_ODO"); + declare_parameter("serial_port", "/dev/USB_ODO"); declare_parameter("baudrate", 115200); declare_parameter("name", "pcb_odo"); @@ -96,7 +96,7 @@ namespace Modelec void PCBOdoInterface::read(const std::string& msg) { - RCLCPP_DEBUG(this->get_logger(), "Received from PCB: %s", msg.c_str()); + RCLCPP_INFO_ONCE(this->get_logger(), "Received from PCB: %s", msg.c_str()); std::vector tokens = split(trim(msg), ';'); if (tokens.size() < 2) { From 88b2e5c418f817262808eb3dbd1c70bafd2e4d95 Mon Sep 17 00:00:00 2001 From: acki Date: Thu, 11 Dec 2025 17:54:20 +0100 Subject: [PATCH 6/6] log pcb data --- src/modelec_com/src/pcb_action_interface.new.cpp | 1 + src/modelec_com/src/pcb_odo_interface.new.cpp | 1 + 2 files changed, 2 insertions(+) diff --git a/src/modelec_com/src/pcb_action_interface.new.cpp b/src/modelec_com/src/pcb_action_interface.new.cpp index 2875631..624c377 100644 --- a/src/modelec_com/src/pcb_action_interface.new.cpp +++ b/src/modelec_com/src/pcb_action_interface.new.cpp @@ -215,6 +215,7 @@ namespace Modelec 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) diff --git a/src/modelec_com/src/pcb_odo_interface.new.cpp b/src/modelec_com/src/pcb_odo_interface.new.cpp index cea3bd1..ec4b871 100644 --- a/src/modelec_com/src/pcb_odo_interface.new.cpp +++ b/src/modelec_com/src/pcb_odo_interface.new.cpp @@ -97,6 +97,7 @@ namespace Modelec 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) {