Added dwm rotate

mg_control/src/mg_control.cpp

@@ -66,18 +66,14 @@ hardware_interface::return_type mg::MgStepperInterface::read(const rclcpp::Time&
 }
 
 hardware_interface::return_type mg::MgStepperInterface::write(const rclcpp::Time&, const rclcpp::Duration&) {
-    union {
-        std::array<u_char, sizeof(double)> bytes;
-        double                             data;
-    } value{};
+    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));
 
     try {
-        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); }
-
+        odrive_serial_interface.Write(cmd_left + cmd_right);
     } 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,4 +4,3 @@
 #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,
-          "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"));
+          "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"));
     }
 
 };

mg_odometry/src/mg_odom_publisher.cpp

@@ -6,7 +6,6 @@
 #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"
@@ -57,9 +56,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<u_char, sizeof(double)> bytes;
+                std::array<double, sizeof(double)> bytes;
                 double                             data;
             } value{};
             value.data = request->width;
@@ -72,7 +71,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;";
         }
     }