Fixed services for parameter adjusting not working

mg_control/src/mg_control.cpp

@@ -66,14 +66,18 @@ hardware_interface::return_type mg::MgStepperInterface::read(const rclcpp::Time&
 }
 
 hardware_interface::return_type mg::MgStepperInterface::write(const rclcpp::Time&, const rclcpp::Duration&) {
-    std::string cmd_left;
-    std::string cmd_right;
-
-    cmd_left  = std::to_string(left_wheel_vel_cmd / (2 * M_PI)) + " ";
-    cmd_right = std::to_string(-right_wheel_vel_cmd / (2 * M_PI));
+    union {
+        std::array<u_char, sizeof(double)> bytes;
+        double                             data;
+    } value{};
 
     try {
-        odrive_serial_interface.Write(cmd_left + cmd_right);
+        odrive_serial_interface.Write("s;");
+        value.data = left_wheel_vel_cmd / (2 * M_PI);
+        for (const auto& bt : value.bytes) { odrive_serial_interface.WriteByte(bt); }
+        value.data = -right_wheel_vel_cmd / (2 * M_PI);
+        for (const auto& bt : value.bytes) { odrive_serial_interface.WriteByte(bt); }
+
     } catch (const std::runtime_error& e) { return hardware_interface::return_type::ERROR; }
     return hardware_interface::return_type::OK;
 }

mg_navigation/include/handlers/dwm.hpp

@@ -4,3 +4,4 @@
 #include "handlers/dwm_point.hpp"
 #include "handlers/dwm_forward.hpp"
 #include "handlers/dwm_lookat.hpp"
+#include "handlers/dwm_rotate.hpp"

mg_navigation/src/mg_navigation.cpp

@@ -76,10 +76,10 @@ namespace mg {
 
         sv_rotate = rclcpp_action::create_server<Rotate>(
           this,
-          "MoveStraight",
-          std::bind(&MgNavigationServer::handle_goal<Rotate>, this, _1, _2, "MoveStraight"),
-          std::bind(&MgNavigationServer::handle_cancel<Rotate>, this, _1, "MoveStraight"),
-          std::bind(&MgNavigationServer::handle_accepted<Rotate>, this, _1, "MoveStraight"));
+          "Rotate",
+          std::bind(&MgNavigationServer::handle_goal<Rotate>, this, _1, _2, "Rotate"),
+          std::bind(&MgNavigationServer::handle_cancel<Rotate>, this, _1, "Rotate"),
+          std::bind(&MgNavigationServer::handle_accepted<Rotate>, this, _1, "Rotate"));
     }
 
 };

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;";
         }
     }