mg_odometry/src/mg_odom_publisher.cpp

@@ -1,9 +1,15 @@
+#include <array>
 #include <chrono>
+#include <cstddef>
 #include <functional>
 #include <memory>
+#include <rclcpp/service.hpp>
 #include <string>
 #include <libserial/SerialStream.h>
 
+#include "mg_msgs/srv/set_width.hpp"
+#include "std_srvs/srv/empty.hpp"
+
 #include "geometry_msgs/msg/transform_stamped.hpp"
 #include "tf2_ros/transform_broadcaster.h"
 
@@ -18,6 +24,9 @@
 
 constexpr const char* ENCODER_SERIAL_PATH_DEFAULT = "/dev/ttyACM1";
 
+using SetWidthSrv = mg_msgs::srv::SetWidth;
+using ZeroSrv     = std_srvs::srv::Empty;
+
 class MgOdomPublisher : public rclcpp::Node {
   public:
     MgOdomPublisher() : Node("mg_odom_publisher") {
@@ -29,13 +38,44 @@ class MgOdomPublisher : public rclcpp::Node {
 
         timer_ = this->create_wall_timer(std::chrono::milliseconds(TIMEOUT),
                                          std::bind(&MgOdomPublisher::timer_callback, this));
+
+        set_width_service_ = create_service<SetWidthSrv>(
+          "/set_width", std::bind(&MgOdomPublisher::set_width, this, std::placeholders::_1));
+
+        zero_service_
+          = create_service<ZeroSrv>("/zero", std::bind(&MgOdomPublisher::zero, this, std::placeholders::_1));
     }
 
   private:
     std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
+    rclcpp::Service<SetWidthSrv>::SharedPtr        set_width_service_;
+    rclcpp::Service<ZeroSrv>::SharedPtr            zero_service_;
     rclcpp::TimerBase::SharedPtr                   timer_;
     LibSerial::SerialStream                        enc_serial_port_;
 
+    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()) {
+            union {
+                std::array<double, sizeof(double)> bytes;
+                double                             data;
+            } value{};
+            value.data = request->width;
+
+            enc_serial_port_ << "w;";
+            for (const auto& byte : value.bytes) { enc_serial_port_ << byte; }
+        }
+    }
+
+    void zero(const std::shared_ptr<ZeroSrv::Request> /*request*/) {
+        RCLCPP_INFO(get_logger(), "Zeroing odometry");
+
+        if (!enc_serial_port_.IsOpen()) {
+            enc_serial_port_ << "z;";
+        }
+    }
+
     void timer_callback() {
         double _x     = NAN;
         double _y     = NAN;