diff --git a/mg_control/src/mg_control.cpp b/mg_control/src/mg_control.cpp index 26ef4e7..625e96b 100644 --- a/mg_control/src/mg_control.cpp +++ b/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 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; }