From 4cd994cf7accc1adbb8f6cebe33815e99b12213b Mon Sep 17 00:00:00 2001
From: Pimpest <82343504+Pimpest@users.noreply.github.com>
Date: Fri, 28 Feb 2025 19:43:00 +0100
Subject: [PATCH] Fixed services for parameter adjusting not working

---
 mg_odometry/src/mg_odom_publisher.cpp | 7 ++++---
 1 file changed, 4 insertions(+), 3 deletions(-)

diff --git a/mg_odometry/src/mg_odom_publisher.cpp b/mg_odometry/src/mg_odom_publisher.cpp
index bceac11..3c5d0ff 100644
--- a/mg_odometry/src/mg_odom_publisher.cpp
+++ b/mg_odometry/src/mg_odom_publisher.cpp
@@ -6,6 +6,7 @@
 #include <rclcpp/service.hpp>
 #include <string>
 #include <libserial/SerialStream.h>
+#include <sys/types.h>
 
 #include "mg_msgs/srv/set_width.hpp"
 #include "std_srvs/srv/empty.hpp"
@@ -56,9 +57,9 @@ class MgOdomPublisher : public rclcpp::Node {
     void set_width(const std::shared_ptr<SetWidthSrv::Request> request) {
         RCLCPP_INFO(get_logger(), "Setting width to: %lf", request->width);
 
-        if (!enc_serial_port_.IsOpen()) {
+        if (enc_serial_port_.IsOpen()) {
             union {
-                std::array<double, sizeof(double)> bytes;
+                std::array<u_char, sizeof(double)> bytes;
                 double                             data;
             } value{};
             value.data = request->width;
@@ -71,7 +72,7 @@ class MgOdomPublisher : public rclcpp::Node {
     void zero(const std::shared_ptr<ZeroSrv::Request> /*request*/) {
         RCLCPP_INFO(get_logger(), "Zeroing odometry");
 
-        if (!enc_serial_port_.IsOpen()) {
+        if (enc_serial_port_.IsOpen()) {
             enc_serial_port_ << "z;";
         }
     }