moteus arm control

src/arm_hardware_interface/CMakeLists.txt

@@ -20,10 +20,21 @@ include_directories(
   ${arm_control_INCLUDE_DIRS}
 )
 
+include(FetchContent)
+FetchContent_Declare(
+  moteus
+  GIT_REPOSITORY https://github.com/mjbots/moteus.git
+  GIT_TAG aa96bf0ae847921a79d2d3d241d65505ee10e209)
+
+FetchContent_MakeAvailable(moteus)
 
 add_library(${PROJECT_NAME} src/armprotocol.cpp include/arm_hardware_interface/ArmSerialProtocol.h)
 
-add_executable(arm_serial_driver src/ArmSerialInterface.cpp include/ArmSerialInterface.h include/arm_hardware_interface/ArmSerialProtocol.h)
+# For old and new arm
+# add_executable(arm_serial_driver src/ArmSerialInterface.cpp include/ArmSerialInterface.h include/arm_hardware_interface/ArmSerialProtocol.h)
+add_executable(arm_serial_driver src/MoteusInterface.cpp include/ArmSerialInterface.h include/arm_hardware_interface/ArmSerialProtocol.h)
+
+target_link_libraries(arm_serial_driver moteus::cpp)
 ament_target_dependencies(arm_serial_driver rclcpp sensor_msgs serial rover_msgs arm_control)
 
 if(BUILD_TESTING)

src/arm_hardware_interface/include/ArmSerialInterface.h

@@ -7,6 +7,7 @@
 #include <thread>
 #include <chrono>
 
+#include "moteus.h"
 #include <arm_hardware_interface/ArmSerialProtocol.h>
 
 
@@ -26,6 +27,7 @@
 #define AXIS_6_DIR 1
 
 using std::string;
+using namespace mjbots;
 
 class ArmSerial : public rclcpp::Node {
 public:
@@ -52,6 +54,11 @@ class ArmSerial : public rclcpp::Node {
     serial::Timeout timeout_uart = serial::Timeout::simpleTimeout(1000); // E.g., 1000 ms or 1 second
 
 
+  std::map<int, std::shared_ptr<moteus::Controller>> controllers;
+  std::map<int, moteus::Query::Result> servo_data;
+  std::shared_ptr<moteus::Transport> transport;
+    bool send_angles = true;
+
     struct Axis{
       float curr_pos;
       float target_pos;