From e2088630161aa52490a5b070ffdac068ecf6a55a Mon Sep 17 00:00:00 2001 From: acki Date: Sun, 6 Apr 2025 20:39:03 -0400 Subject: [PATCH] rework pcb interface + begin of Qt6 --- src/modelec/launch/test.modelec.launch.yml | 10 +- src/modelec/src/gamecontroller_listener.cpp | 205 ++++++++++++------ src/modelec/src/multiple_serial_listener.cpp | 110 +++++++--- src/modelec/src/pcb_alim_interface.cpp | 26 ++- src/modelec/src/pcb_odo_interface.cpp | 52 +++-- src/modelec_gui/CMakeLists.txt | 2 + .../include/modelec_gui/modelec_gui.hpp | 25 ++- src/modelec_gui/src/main.cpp | 27 ++- src/modelec_gui/src/modelec_gui.cpp | 88 ++++++-- 9 files changed, 379 insertions(+), 166 deletions(-) diff --git a/src/modelec/launch/test.modelec.launch.yml b/src/modelec/launch/test.modelec.launch.yml index 9081cef..f7e57fa 100644 --- a/src/modelec/launch/test.modelec.launch.yml +++ b/src/modelec/launch/test.modelec.launch.yml @@ -5,12 +5,12 @@ launch: exec: "serial_listener" name: "serial_listener" -- node: - pkg: 'modelec' - exec: 'usb_arduino_logic_processor' - name: 'usb_arduino_logic_processor' - - node: pkg: 'modelec' exec: 'pcb_odo_interface' name: 'pcb_odo_interface' + +- node: + pkg: 'modelec_gui' + exec: "modelec_gui" + name: "modelec_gui" \ No newline at end of file diff --git a/src/modelec/src/gamecontroller_listener.cpp b/src/modelec/src/gamecontroller_listener.cpp index 74dfb5a..7aacbae 100644 --- a/src/modelec/src/gamecontroller_listener.cpp +++ b/src/modelec/src/gamecontroller_listener.cpp @@ -2,7 +2,8 @@ #include "modelec/utils.hpp" #include -namespace Modelec { +namespace Modelec +{ ControllerListener::ControllerListener() : Node("controller_listener") { // Subscribe to the 'joy' topic @@ -12,58 +13,85 @@ namespace Modelec { servo_publisher_ = this->create_publisher("arm_control", 10); - // Service to create a new serial listener + // Service to create a new serial listener auto request = std::make_shared(); request->name = "odometry"; request->bauds = 115200; request->serial_port = "/dev/ttyACM0"; 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; + 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 (!result.get()->success) { - RCLCPP_ERROR(this->get_logger(), "Failed to add serial listener"); - } else { - RCLCPP_INFO(this->get_logger(), "Added serial listener"); + if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), result) == + rclcpp::FutureReturnCode::SUCCESS) + { + if (auto res = result.get()) + { + if (res->success) + { + RCLCPP_INFO(this->get_logger(), "Serial listener created"); + + odometry_publisher_ = this->create_publisher(result.get()->subscriber, 10); + + clear_pca_publisher_ = this->create_publisher("clear_pca9685", 10); + + pca9685_publisher_ = this->create_publisher( + "servo_control", 10); + } + else + { + RCLCPP_ERROR(this->get_logger(), "Failed to create serial listener"); + } } - } else { + else + { + RCLCPP_ERROR(this->get_logger(), "Failed to get result from service"); + } + } + else + { RCLCPP_ERROR(this->get_logger(), "Service call failed"); } - odometry_publisher_ = this->create_publisher(result.get()->subscriber, 10); - - clear_pca_publisher_ = this->create_publisher("clear_pca9685", 10); - - pca9685_publisher_ = this->create_publisher("servo_control", 10); - client_ = this->create_client("add_servo"); // ensure the server is up - while (!client_->wait_for_service(std::chrono::seconds(1))) { - if (!rclcpp::ok()) { + while (!client_->wait_for_service(std::chrono::seconds(1))) + { + if (!rclcpp::ok()) + { RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service. Exiting."); return; } RCLCPP_INFO(this->get_logger(), "service not available, waiting again..."); } - for (auto servo : solarPannelServos) { + for (auto servo : solarPannelServos) + { auto request = std::make_shared(); request->pin = servo.pin; auto res = client_->async_send_request(request); - if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), res) == rclcpp::FutureReturnCode::SUCCESS) { - if (!res.get()->success) { + if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), res) == + rclcpp::FutureReturnCode::SUCCESS) + { + if (!res.get()->success) + { RCLCPP_ERROR(this->get_logger(), "Failed to add servo on pin %d", servo.pin); - } else { + } + else + { RCLCPP_INFO(this->get_logger(), "Added servo on pin %d", servo.pin); } - } else { + } + else + { RCLCPP_ERROR(this->get_logger(), "Service call failed"); } } @@ -79,96 +107,132 @@ namespace Modelec { void ControllerListener::CheckButton(const sensor_msgs::msg::Joy::SharedPtr msg) { - if (msg->buttons[2] == 1) { - if (button_2_was_pressed) { + if (msg->buttons[2] == 1) + { + if (button_2_was_pressed) + { return; } auto message = ServoMode(); message.pin = 0; - if (pinces[0] == ServoMode::PINCE_CLOSED) { + if (pinces[0] == ServoMode::PINCE_CLOSED) + { pinces[0] = ServoMode::PINCE_OPEN; - } else { + } + else + { pinces[0] = ServoMode::PINCE_CLOSED; } message.mode = pinces[0]; servo_publisher_->publish(message); button_2_was_pressed = true; - } else { + } + else + { button_2_was_pressed = false; } - if (msg->buttons[3] == 1) { - if (button_3_was_pressed) { + if (msg->buttons[3] == 1) + { + if (button_3_was_pressed) + { return; } auto message = ServoMode(); message.pin = 1; - if (pinces[1] == ServoMode::PINCE_CLOSED) { + if (pinces[1] == ServoMode::PINCE_CLOSED) + { pinces[1] = ServoMode::PINCE_OPEN; - } else { + } + else + { pinces[1] = ServoMode::PINCE_CLOSED; } message.mode = pinces[1]; servo_publisher_->publish(message); button_3_was_pressed = true; - } else { + } + else + { button_3_was_pressed = false; } - if (msg->buttons[1] == 1) { - if (button_1_was_pressed) { + if (msg->buttons[1] == 1) + { + if (button_1_was_pressed) + { return; } auto message = ServoMode(); message.pin = 2; - if (pinces[2] == ServoMode::PINCE_CLOSED) { + if (pinces[2] == ServoMode::PINCE_CLOSED) + { pinces[2] = ServoMode::PINCE_OPEN; - } else { + } + else + { pinces[2] = ServoMode::PINCE_CLOSED; } message.mode = pinces[2]; servo_publisher_->publish(message); button_1_was_pressed = true; - } else { + } + else + { button_1_was_pressed = false; } // arm - if (msg->buttons[0] == 1) { - if (button_0_was_pressed) { + if (msg->buttons[0] == 1) + { + if (button_0_was_pressed) + { return; } auto message = ServoMode(); - if (arm == ServoMode::ARM_BOTTOM) { + if (arm == ServoMode::ARM_BOTTOM) + { arm = ServoMode::ARM_TOP; - } else { + } + else + { arm = ServoMode::ARM_BOTTOM; } message.mode = arm; message.is_arm = true; servo_publisher_->publish(message); button_0_was_pressed = true; - } else { + } + else + { button_0_was_pressed = false; } - if (msg->buttons[4]) { - if (button_4_was_pressed) { + if (msg->buttons[4]) + { + if (button_4_was_pressed) + { return; } auto message = std_msgs::msg::String(); message.data = "W"; odometry_publisher_->publish(message); button_4_was_pressed = true; - } else { + } + else + { button_4_was_pressed = false; } - if (msg->buttons[5]) { - if (button_5_was_pressed) { + if (msg->buttons[5]) + { + if (button_5_was_pressed) + { return; } clear_pca_publisher_->publish(std_msgs::msg::Empty()); button_5_was_pressed = true; - } else { + } + else + { button_5_was_pressed = false; } } @@ -177,33 +241,45 @@ namespace Modelec { { auto message = std_msgs::msg::String(); int speed = 0; - if (msg->axes[1] < 0.1 && msg->axes[1] > -0.1) { + if (msg->axes[1] < 0.1 && msg->axes[1] > -0.1) + { speed = 0; - } else { + } + else + { speed = static_cast(Modelec::mapValue(static_cast(msg->axes[1]), -1.0f, 1.0f, -310.0f, 310.0f)); } - if (speed != last_speed) { + if (speed != last_speed) + { message.data = "V " + std::to_string(speed); odometry_publisher_->publish(message); last_speed = speed; } int rotation = 0; - if (msg->axes[3] < 0.1 && msg->axes[3] > -0.1) { + if (msg->axes[3] < 0.1 && msg->axes[3] > -0.1) + { rotation = 0; - } else { - rotation = static_cast(Modelec::mapValue(static_cast(-msg->axes[3]), -1.0f, 1.0f, -310.0f, 310.0f)); + } + else + { + rotation = static_cast(Modelec::mapValue(static_cast(-msg->axes[3]), -1.0f, 1.0f, -310.0f, + 310.0f)); } - if (rotation != last_rotation) { + if (rotation != last_rotation) + { message.data = "R " + std::to_string(rotation); odometry_publisher_->publish(message); last_rotation = rotation; } - if (msg->axes[2] != last_solar_1_angle) { - int solarPannelAngle = static_cast(Modelec::mapValue(static_cast(msg->axes[2]), -1.0f, 1.0f, solarPannelServos[0].startAngle, solarPannelServos[0].endAngle)); + if (msg->axes[2] != last_solar_1_angle) + { + int solarPannelAngle = static_cast(Modelec::mapValue(static_cast(msg->axes[2]), -1.0f, 1.0f, + solarPannelServos[0].startAngle, + solarPannelServos[0].endAngle)); auto solarPannelAngleMessage = modelec_interface::msg::PCA9685Servo(); solarPannelAngleMessage.pin = solarPannelServos[0].pin; solarPannelAngleMessage.angle = solarPannelAngle; @@ -211,8 +287,11 @@ namespace Modelec { last_solar_1_angle = msg->axes[2]; } - if (msg->axes[5] != last_solar_2_angle) { - int solarPannelAngle = static_cast(Modelec::mapValue(static_cast(msg->axes[5]), -1.0f, 1.0f, solarPannelServos[1].startAngle, solarPannelServos[1].endAngle)); + if (msg->axes[5] != last_solar_2_angle) + { + int solarPannelAngle = static_cast(Modelec::mapValue(static_cast(msg->axes[5]), -1.0f, 1.0f, + solarPannelServos[1].startAngle, + solarPannelServos[1].endAngle)); auto solarPannelAngleMessage = modelec_interface::msg::PCA9685Servo(); solarPannelAngleMessage.pin = solarPannelServos[1].pin; solarPannelAngleMessage.angle = solarPannelAngle; @@ -222,7 +301,7 @@ namespace Modelec { } } -int main(int argc, char **argv) +int main(int argc, char** argv) { rclcpp::init(argc, argv); rclcpp::spin(std::make_shared()); diff --git a/src/modelec/src/multiple_serial_listener.cpp b/src/modelec/src/multiple_serial_listener.cpp index 81a0ea3..1f05be1 100644 --- a/src/modelec/src/multiple_serial_listener.cpp +++ b/src/modelec/src/multiple_serial_listener.cpp @@ -1,45 +1,63 @@ #include -namespace Modelec { - 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)); - timer = create_wall_timer(std::chrono::milliseconds(READ_REFRESH_RATE), [this]() { - for (auto &listener : serial_listeners) { +namespace Modelec +{ + 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)); + timer = create_wall_timer(std::chrono::milliseconds(READ_REFRESH_RATE), [this]() + { + for (auto& listener : serial_listeners) + { if (listener.second->IsOk()) listener.second->read(); } }); parameter_callback_handle_ = this->add_on_set_parameters_callback( - [this](const std::vector ¶meters) { + [this](const std::vector& parameters) + { return onParameterChange(parameters); }); } - MultipleSerialListener::~MultipleSerialListener() { - for (auto &listener : serial_listeners) { + MultipleSerialListener::~MultipleSerialListener() + { + for (auto& listener : serial_listeners) + { listener.second->port_.close(); } } - void MultipleSerialListener::add_serial_listener(const std::shared_ptr request, std::shared_ptr response) { - if (serial_listeners.find(request->name) != serial_listeners.end()) { + 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; } - auto listener = std::make_shared(request->name, request->bauds, request->serial_port, MAX_MESSAGE_LEN, io); + RCLCPP_INFO(rclcpp::get_logger("MultipleSerialListener"), "Adding serial listener: %s", request->name.c_str()); - if (!listener->IsOk()) { + 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/" + listener->name_, 10); - listener->subscriber_ = create_subscription( - "send_to_serial/" + listener->name_, 10, [listener](std_msgs::msg::String::SharedPtr msg) { + listener->subscriber_ = create_subscription( + "send_to_serial/" + listener->name_, 10, [listener](std_msgs::msg::String::SharedPtr msg) + { listener->write(msg->data); }); @@ -50,12 +68,17 @@ namespace Modelec { response->subscriber = listener->subscriber_->get_topic_name(); } - rcl_interfaces::msg::SetParametersResult MultipleSerialListener::onParameterChange(const std::vector ¶meters) { + rcl_interfaces::msg::SetParametersResult MultipleSerialListener::onParameterChange( + const std::vector& parameters) + { rcl_interfaces::msg::SetParametersResult result; result.successful = true; - for (const auto ¶meter : parameters) { - if (parameter.get_name() == "bauds" || parameter.get_name() == "serial_port" || parameter.get_name() == "max_message_len") { + for (const auto& parameter : parameters) + { + if (parameter.get_name() == "bauds" || parameter.get_name() == "serial_port" || parameter.get_name() == + "max_message_len") + { updateConnection(); } } @@ -63,64 +86,83 @@ namespace Modelec { return result; } - void MultipleSerialListener::updateConnection() { - for (auto &listener : serial_listeners) { + void MultipleSerialListener::updateConnection() + { + for (auto& listener : serial_listeners) + { listener.second->SetMaxMessageLen(get_parameter("max_message_len").as_int()); } read_refresh_rate_ = get_parameter("read_refresh_rate").as_int(); timer->cancel(); - timer = create_wall_timer(std::chrono::milliseconds(read_refresh_rate_), [this]() { - for (auto &listener : serial_listeners) { + timer = create_wall_timer(std::chrono::milliseconds(read_refresh_rate_), [this]() + { + for (auto& listener : serial_listeners) + { listener.second->read(); } }); } - MultipleSerialListener::SerialListener::SerialListener(const std::string& name, int bauds, const std::string& serial_port, int max_message_len, boost::asio::io_service& io) + MultipleSerialListener::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 { + try + { port_.open(serial_port_); port_.set_option(boost::asio::serial_port_base::baud_rate(bauds_)); status_ = true; - } catch (boost::system::system_error &e) { + } + catch (boost::system::system_error& e) + { RCLCPP_ERROR(rclcpp::get_logger("MultipleSerialListener"), "Failed to open serial port: %s", e.what()); status_ = false; return; } } - void MultipleSerialListener::SerialListener::read() { + void MultipleSerialListener::SerialListener::read() + { if (!status_) return; std::vector data(max_message_len_); - try { + try + { // Attempt to read data from the serial port - + size_t len = port_.read_some(boost::asio::buffer(data.data(), max_message_len_)); - if (len > 0) { + if (len > 0) + { // Prepare and publish the message auto msg = std_msgs::msg::String(); msg.data = std::string(data.begin(), data.begin() + len); publisher_->publish(msg); } - } catch (boost::system::system_error &e) { + } + catch (boost::system::system_error& e) + { RCLCPP_ERROR(rclcpp::get_logger("SerialListener"), "Read error: %s", e.what()); } } - - void MultipleSerialListener::SerialListener::write(const std::string& data) { - try { + + void MultipleSerialListener::SerialListener::write(const std::string& data) + { + try + { boost::asio::write(port_, boost::asio::buffer(data)); - } catch (boost::system::system_error &e) { + } + catch (boost::system::system_error& e) + { RCLCPP_ERROR(rclcpp::get_logger("SerialListener"), "Write error: %s", e.what()); } } } -int main(int argc, char **argv) { +int main(int argc, char** argv) +{ rclcpp::init(argc, argv); rclcpp::spin(std::make_shared()); rclcpp::shutdown(); diff --git a/src/modelec/src/pcb_alim_interface.cpp b/src/modelec/src/pcb_alim_interface.cpp index 66bc377..1314974 100644 --- a/src/modelec/src/pcb_alim_interface.cpp +++ b/src/modelec/src/pcb_alim_interface.cpp @@ -24,24 +24,32 @@ namespace Modelec if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), result) == rclcpp::FutureReturnCode::SUCCESS) { - if (!result.get()->success) + if (auto res = result.get()) { - RCLCPP_ERROR(this->get_logger(), "Failed to add serial listener"); + if (res->success) + { + RCLCPP_INFO(this->get_logger(), "Publisher: %s", res->publisher.c_str()); + RCLCPP_INFO(this->get_logger(), "Subscriber: %s", res->subscriber.c_str()); + + pcb_publisher_ = this->create_publisher(result.get()->subscriber, 10); + pcb_subscriber_ = this->create_subscription( + result.get()->publisher, 10, + std::bind(&PCBAlimInterface::PCBCallback, this, std::placeholders::_1)); + } + else + { + RCLCPP_ERROR(this->get_logger(), "Failed to add serial listener"); + } } else { - RCLCPP_INFO(this->get_logger(), "Added serial listener"); + RCLCPP_ERROR(this->get_logger(), "Failed to ask for a serial listener"); } } else { RCLCPP_ERROR(this->get_logger(), "Service call failed"); } - - pcb_publisher_ = this->create_publisher(result.get()->subscriber, 10); - pcb_subscriber_ = this->create_subscription( - result.get()->publisher, 10, - std::bind(&PCBAlimInterface::PCBCallback, this, std::placeholders::_1)); } void PCBAlimInterface::PCBCallback(const std_msgs::msg::String::SharedPtr msg) @@ -57,7 +65,7 @@ namespace Modelec } void PCBAlimInterface::SendToPCB(const std::string& order, const std::string& elem, const std::string& data, - const std::string& value) + const std::string& value) { std::string command = order + ";" + elem; if (!data.empty()) diff --git a/src/modelec/src/pcb_odo_interface.cpp b/src/modelec/src/pcb_odo_interface.cpp index 5bab028..f0ab82a 100644 --- a/src/modelec/src/pcb_odo_interface.cpp +++ b/src/modelec/src/pcb_odo_interface.cpp @@ -10,7 +10,7 @@ namespace Modelec auto request = std::make_shared(); request->name = "pcb_odo"; request->bauds = 115200; - request->serial_port = "/dev/serial1"; // TODO : check the real serial port + request->serial_port = "/dev/pts/6"; // TODO : check the real serial port auto client = this->create_client("add_serial_listener"); while (!client->wait_for_service(std::chrono::seconds(1))) { @@ -25,13 +25,29 @@ namespace Modelec if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), result) == rclcpp::FutureReturnCode::SUCCESS) { - if (!result.get()->success) + if (auto res = result.get()) { - RCLCPP_ERROR(this->get_logger(), "Failed to add serial listener"); + if (res->success) + { + RCLCPP_INFO(this->get_logger(), "Publisher: %s", res->publisher.c_str()); + RCLCPP_INFO(this->get_logger(), "Subscriber: %s", res->subscriber.c_str()); + + pcb_subscriber_ = this->create_subscription( + res->publisher, 10, [this](std_msgs::msg::String::SharedPtr msg) + { + PCBCallback(msg); + }); + + pcb_publisher_ = this->create_publisher(res->subscriber, 10); + } + else + { + RCLCPP_ERROR(this->get_logger(), "Failed to add serial listener"); + } } else { - RCLCPP_INFO(this->get_logger(), "Added serial listener"); + RCLCPP_ERROR(this->get_logger(), "Failed to ask for a serial listener"); } } else @@ -39,11 +55,6 @@ namespace Modelec RCLCPP_ERROR(this->get_logger(), "Service call failed"); } - pcb_publisher_ = this->create_publisher(result.get()->subscriber, 10); - pcb_subscriber_ = this->create_subscription( - result.get()->publisher, 10, - std::bind(&PCBOdoInterface::PCBCallback, this, std::placeholders::_1)); - odo_pos_publisher_ = this->create_publisher( "odometry/position", 10); @@ -54,14 +65,14 @@ namespace Modelec "odometry/tof", 10); odo_waypoint_reach_publisher_ = this->create_publisher( - "odometry/waypoint-reach", 10); + "odometry/waypoint_reach", 10); odo_add_waypoint_subscriber_ = this->create_subscription( - "odometry/add-waypoint", 10, + "odometry/add_waypoint", 10, std::bind(&PCBOdoInterface::AddWaypointCallback, this, std::placeholders::_1)); odo_set_pos_subscriber_ = this->create_subscription( - "odometry/set-position", 10, + "odometry/set_position", 10, std::bind(&PCBOdoInterface::SetPosCallback, this, std::placeholders::_1)); // Services @@ -101,7 +112,7 @@ namespace Modelec message.theta = theta; odo_pos_publisher_->publish(message); - ResolvePositionRequest({ x, y, theta }); + ResolvePositionRequest({x, y, theta}); } else if (tokens[1] == "SPEED") { @@ -115,7 +126,7 @@ namespace Modelec message.theta = theta; odo_speed_publisher_->publish(message); - ResolveSpeedRequest({ x, y, theta }); + ResolveSpeedRequest({x, y, theta}); } else if (tokens[1] == "DIST") { @@ -175,7 +186,7 @@ namespace Modelec } void PCBOdoInterface::HandleGetSpeed(const std::shared_ptr, - std::shared_ptr response) + std::shared_ptr response) { std::promise promise; auto future = promise.get_future(); @@ -252,10 +263,6 @@ namespace Modelec pos_promises_.pop(); promise.set_value(position); } - else - { - RCLCPP_WARN(this->get_logger(), "Received Position response but no promise waiting"); - } } void PCBOdoInterface::SendToPCB(const std::string& data) const @@ -266,7 +273,7 @@ namespace Modelec } void PCBOdoInterface::SendToPCB(const std::string& order, const std::string& elem, - const std::vector& data) const + const std::vector& data) const { std::string command = order + ";" + elem; for (const auto& d : data) @@ -299,7 +306,7 @@ namespace Modelec void PCBOdoInterface::GetToF(const int& tof) const { - GetData("DIST", { std::to_string(tof) }); + GetData("DIST", {std::to_string(tof)}); } void PCBOdoInterface::SetRobotPos(const modelec_interface::msg::OdometryPos::SharedPtr msg) const @@ -324,7 +331,8 @@ namespace Modelec 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 long theta) const + void PCBOdoInterface::AddWaypoint(const int index, const bool IsStopPoint, const long x, const long y, + const long theta) const { std::vector data = { std::to_string(index), diff --git a/src/modelec_gui/CMakeLists.txt b/src/modelec_gui/CMakeLists.txt index 25e365f..216ae06 100644 --- a/src/modelec_gui/CMakeLists.txt +++ b/src/modelec_gui/CMakeLists.txt @@ -12,6 +12,7 @@ set(CMAKE_AUTOMOC ON) find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) +find_package(modelec_interface REQUIRED) find_package(Qt6 COMPONENTS Core Gui @@ -23,6 +24,7 @@ add_executable(modelec_gui src/modelec_gui.cpp src/main.cpp include/modelec_gui/ ament_target_dependencies(modelec_gui rclcpp std_msgs + modelec_interface ) target_link_libraries(modelec_gui Qt6::Core diff --git a/src/modelec_gui/include/modelec_gui/modelec_gui.hpp b/src/modelec_gui/include/modelec_gui/modelec_gui.hpp index c7823fd..6a78003 100644 --- a/src/modelec_gui/include/modelec_gui/modelec_gui.hpp +++ b/src/modelec_gui/include/modelec_gui/modelec_gui.hpp @@ -7,20 +7,31 @@ #include #include #include +#include +#include class ROS2QtGUI : public QWidget { Q_OBJECT public: - explicit ROS2QtGUI(std::shared_ptr node, QWidget *parent = nullptr); + explicit ROS2QtGUI(QWidget *parent = nullptr); ~ROS2QtGUI() override; // Explicitly declare destructor -private slots: - void onSendClicked(); + rclcpp::Node::SharedPtr get_node() const { return node_; } private: - QLineEdit *textBox_; - QPushButton *sendButton_; - std::shared_ptr node_; - rclcpp::Publisher::SharedPtr publisher_; + QLineEdit *xBox_, *yBox_, *thetaBox_; + QVBoxLayout *mainLayout_; + QHBoxLayout *posLayout_; + QPushButton *askSpeed_; + QLineEdit *xSpeedBox_, *ySpeedBox_, *thetaSpeedBox_; + QHBoxLayout *speedLayout_; + + rclcpp::Node::SharedPtr node_; + rclcpp::Subscription::SharedPtr sub_; + + // client + rclcpp::Client::SharedPtr client_; + + void PositionCallback(const modelec_interface::msg::OdometryPos::SharedPtr msg); }; diff --git a/src/modelec_gui/src/main.cpp b/src/modelec_gui/src/main.cpp index 4e100eb..a2bbc3c 100644 --- a/src/modelec_gui/src/main.cpp +++ b/src/modelec_gui/src/main.cpp @@ -1,17 +1,24 @@ #include "modelec_gui/modelec_gui.hpp" int main(int argc, char *argv[]) { - // Initialize ROS 2 rclcpp::init(argc, argv); - auto node = std::make_shared("ros2_qt_gui_node"); - - // Initialize Qt QApplication app(argc, argv); - ROS2QtGUI gui(node); - gui.setWindowTitle("ROS 2 Qt GUI"); - gui.resize(400, 200); - gui.show(); - // Run the Qt event loop - return app.exec(); + // Create the ROS2QtGUI instance, which is the Qt GUI and ROS node + auto gui = std::make_shared(); + gui->show(); + + // Create and start the ROS 2 executor in a separate thread + auto exec = std::make_shared(); + exec->add_node(gui->get_node()); // Ensure this is the correct way to add your node + + std::thread spin_thread([exec]() { exec->spin(); }); + + int result = app.exec(); + + // Shutdown ROS 2 and join the thread + exec->cancel(); + spin_thread.join(); + rclcpp::shutdown(); + return result; } \ No newline at end of file diff --git a/src/modelec_gui/src/modelec_gui.cpp b/src/modelec_gui/src/modelec_gui.cpp index 3265c07..d1e595c 100644 --- a/src/modelec_gui/src/modelec_gui.cpp +++ b/src/modelec_gui/src/modelec_gui.cpp @@ -1,25 +1,81 @@ #include "modelec_gui/modelec_gui.hpp" -ROS2QtGUI::ROS2QtGUI(std::shared_ptr node, QWidget *parent) - : QWidget(parent), node_(node) { - publisher_ = node_->create_publisher("ui_topic", 10); +ROS2QtGUI::ROS2QtGUI(QWidget *parent) + : QWidget(parent), node_(rclcpp::Node::make_shared("qt_gui_node")) { - QVBoxLayout *layout = new QVBoxLayout(this); - textBox_ = new QLineEdit(this); - sendButton_ = new QPushButton("Send Message", this); + xBox_ = new QLineEdit("x: ?"); + yBox_ = new QLineEdit("y: ?"); + thetaBox_ = new QLineEdit("theta: ?"); + xBox_->setReadOnly(true); + yBox_->setReadOnly(true); + thetaBox_->setReadOnly(true); - layout->addWidget(textBox_); - layout->addWidget(sendButton_); - setLayout(layout); + posLayout_ = new QHBoxLayout; + posLayout_->addWidget(xBox_); + posLayout_->addWidget(yBox_); + posLayout_->addWidget(thetaBox_); - connect(sendButton_, &QPushButton::clicked, this, &ROS2QtGUI::onSendClicked); + askSpeed_ = new QPushButton("Ask speed"); + connect(askSpeed_, &QPushButton::clicked, this, [this]() { + auto request = std::make_shared(); + + auto future = client_->async_send_request(request); + if (rclcpp::spin_until_future_complete(node_, future) != + rclcpp::FutureReturnCode::SUCCESS) { + RCLCPP_ERROR(node_->get_logger(), "Failed to call service"); + return; + } + + if (auto response = future.get()) { + xSpeedBox_->setText(QString("x speed: %1").arg(response->x)); + ySpeedBox_->setText(QString("y speed: %1").arg(response->y)); + thetaSpeedBox_->setText(QString("theta speed: %1").arg(response->theta)); + } else { + RCLCPP_ERROR(node_->get_logger(), "Failed to call service"); + } + }); + + xSpeedBox_ = new QLineEdit("x speed: ?"); + ySpeedBox_ = new QLineEdit("y speed: ?"); + thetaSpeedBox_ = new QLineEdit("theta speed: ?"); + xSpeedBox_->setReadOnly(true); + ySpeedBox_->setReadOnly(true); + thetaSpeedBox_->setReadOnly(true); + + speedLayout_ = new QHBoxLayout; + speedLayout_->addWidget(xSpeedBox_); + speedLayout_->addWidget(ySpeedBox_); + speedLayout_->addWidget(thetaSpeedBox_); + + mainLayout_ = new QVBoxLayout(this); + mainLayout_->addLayout(posLayout_); + mainLayout_->addWidget(askSpeed_); + mainLayout_->addLayout(speedLayout_); + setLayout(mainLayout_); + + sub_ = node_->create_subscription( + "/odometry/position", 10, + std::bind(&ROS2QtGUI::PositionCallback, this, std::placeholders::_1)); + + client_ = node_->create_client("odometry/speed"); + + // ensure the server is up + while (!client_->wait_for_service(std::chrono::seconds(1))) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(node_->get_logger(), "Interrupted while waiting for the service. Exiting."); + return; + } + RCLCPP_INFO(node_->get_logger(), "service not available, waiting again..."); + } } ROS2QtGUI::~ROS2QtGUI() = default; -void ROS2QtGUI::onSendClicked() { - std_msgs::msg::String message; - message.data = textBox_->text().toStdString(); - publisher_->publish(message); - RCLCPP_INFO(node_->get_logger(), "Published: '%s'", message.data.c_str()); -} \ No newline at end of file +void ROS2QtGUI::PositionCallback(const modelec_interface::msg::OdometryPos::SharedPtr msg) +{ + QMetaObject::invokeMethod(this, [this, msg]() { + xBox_->setText(QString("x: %1").arg(msg->x)); + yBox_->setText(QString("y: %1").arg(msg->y)); + thetaBox_->setText(QString("theta: %1").arg(msg->theta)); + }); +}