fixed broken code

docker-compose.yml

@@ -115,24 +115,4 @@ services:
               capabilities: [gpu]
     tty: true
     stdin_open: true
-    network_mode: host
-  rover_x11: # Do not use if you don't have a nvidia GPU with supported cuda cores
-    build:
-      context: .
-      dockerfile: Dockerfile
-    image: roverflake2:dev
-    container_name: rover_dev
-    working_dir: /RoverFlake2
-    tty: true
-    stdin_open: true
-    volumes: # Purpose: docker env watches for changes in repo and updates
-      # preferred method is to specify a directory
-      # e.g. '- ./node_practice:/RoverFlake2/node_practice'
-      # Bind-mount the whole repo into the container
-      - ./:/RoverFlake2
-      - /tmp/.X11-unix:/tmp/.X11-unix # GUI compatibility
-    environment:
-      - DISPLAY=${DISPLAY}
-      - ROVERFLAKE_ROOT=/RoverFlake2
-      - MUJOCO_GL=glfw # or egl (headless GUI)
-    network_mode: host
+    network_mode: host

src/arm_control/include/controller_config.h

@@ -268,8 +268,4 @@ namespace ArmControllerConfig { // Can make into a class later?
     //     return true;
     //     }
     // }
-}
-
-#else
-#pragma warning "Warning: No valid ACTIVE_CONTROLLER defined in controller_config.h"
-#endif
+}

src/arm_control/src/joy_arm_control.cpp

@@ -1,6 +1,30 @@
 #include "joy_arm_control.h"
 #include <cmath>
 
+// Cartesian / IK-mode constants (used by MoveIt Servo twist path)
+namespace ControllerConfig {
+    constexpr const char* CART_FRAME_ID    = "base_link";
+    constexpr double      CART_BUTTON_SPEED = 0.5;
+    constexpr double      ROT_STICK_SPEED  = 1.0;
+    constexpr double      AXIS_DEADZONE    = 0.1;
+
+    constexpr int BTN_CART_POS_X = switch_index::buttons::DPAD_UP;
+    constexpr int BTN_CART_NEG_X = switch_index::buttons::DPAD_DOWN;
+    constexpr int BTN_CART_POS_Y = switch_index::buttons::DPAD_RIGHT;
+    constexpr int BTN_CART_NEG_Y = switch_index::buttons::DPAD_LEFT;
+    constexpr int BTN_CART_POS_Z = switch_index::buttons::Y;
+    constexpr int BTN_CART_NEG_Z = switch_index::buttons::A;
+
+    constexpr int  AXIS_ROLL    = switch_index::axes::RIGHT_JOYSTICK_X;
+    constexpr bool INVERT_ROLL  = false;
+    constexpr int  AXIS_PITCH   = switch_index::axes::RIGHT_JOYSTICK_Y;
+    constexpr bool INVERT_PITCH = false;
+    constexpr int  AXIS_YAW     = switch_index::axes::LEFT_JOYSTICK_X;
+    constexpr bool INVERT_YAW   = false;
+
+    constexpr int BTN_GRIPPER_TOGGLE = switch_index::buttons::R1;
+}
+
 // Constructor
 ArmJoy::ArmJoy() : 
 Node("arm_joy_control") 
@@ -37,13 +61,6 @@ Node("arm_joy_control")
     };
 
     RCLCPP_INFO(this->get_logger(), "=== ArmJoy started ===");
-#if ACTIVE_CONTROLLER == CONTROLLER_PRO_CONTROLLER
-    RCLCPP_INFO(this->get_logger(), "Active controller: Nintendo Switch Pro Controller");
-#elif ACTIVE_CONTROLLER == CONTROLLER_CYBORG_STICK
-    RCLCPP_INFO(this->get_logger(), "Active controller: Saitek Cyborg USB Stick");
-#else
-    RCLCPP_INFO(this->get_logger(), "Active controller: Unknown");
-#endif
     RCLCPP_INFO(this->get_logger(), "Cartesian frame: %s  |  Button speed: %.2f",
         ControllerConfig::CART_FRAME_ID, ControllerConfig::CART_BUTTON_SPEED);
 }

src/arm_hardware_interface/CMakeLists.txt

@@ -33,13 +33,18 @@ add_executable(
     src/moteus_CAN_interface/MoteusInterface.cpp
     src/moteus_CAN_interface/MoteusIO.cpp)
 
+add_executable(moteus_poll src/poll.cpp)
+
 target_compile_definitions(moteus_arm_driver PRIVATE BUILDING_NEW_DRIVER=1) #??
 
 target_link_libraries(moteus_arm_driver moteus::cpp)
+target_link_libraries(moteus_poll moteus::cpp)
 ament_target_dependencies(serial_arm_driver rclcpp sensor_msgs serial
                           rover_msgs arm_control)
 ament_target_dependencies(moteus_arm_driver rclcpp sensor_msgs serial
-                          rover_msgs arm_control std_msgs) 
+                          rover_msgs arm_control std_msgs)
+
+                          ament_target_dependencies(moteus_poll rclcpp)
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
@@ -58,7 +63,7 @@ install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}/launch)
 install(DIRECTORY include/ DESTINATION include/)
 
 install(
-  TARGETS serial_arm_driver moteus_arm_driver ${PROJECT_NAME}
+  TARGETS serial_arm_driver moteus_arm_driver moteus_poll ${PROJECT_NAME}
   DESTINATION lib/${PROJECT_NAME}
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib)

src/arm_hardware_interface/include/moteus_poll.h

@@ -0,0 +1,24 @@
+#pragma once
+
+#include <chrono>
+#include <memory>
+#include <vector>
+#include "rclcpp/rclcpp.hpp"
+
+#include "moteus.h"
+
+namespace moteus = mjbots::moteus;
+
+#define NUM_JOINTS 6
+
+class MoteusPoll : public rclcpp::Node {
+public:
+    MoteusPoll();
+
+private:
+    void poll();
+
+    std::vector<std::shared_ptr<moteus::Controller>> controllers_;
+    std::shared_ptr<moteus::Transport> transport_;
+    rclcpp::TimerBase::SharedPtr timer_;
+};

src/arm_hardware_interface/src/poll.cpp

@@ -0,0 +1,58 @@
+#include "moteus_poll.h"
+
+MoteusPoll::MoteusPoll() : Node("moteus_poll") {
+    transport_ = moteus::Controller::MakeSingletonTransport({});
+
+    for (int id = 1; id <= NUM_JOINTS; id++) {
+        moteus::Controller::Options opts;
+        opts.id = id;
+        auto controller = std::make_shared<moteus::Controller>(opts);
+        controllers_.push_back(controller);
+    }
+
+    timer_ = this->create_wall_timer(
+        std::chrono::milliseconds(100),
+        std::bind(&MoteusPoll::poll, this)
+    );
+
+    RCLCPP_INFO(this->get_logger(), "Polling %d motors at 10 Hz", NUM_JOINTS);
+}
+
+void MoteusPoll::poll() {
+    std::vector<moteus::CanFdFrame> command_frames;
+
+    for (auto& controller : controllers_) {
+        command_frames.push_back(controller->MakeQuery());
+    }
+
+    std::vector<moteus::CanFdFrame> replies;
+    transport_->BlockingCycle(command_frames.data(), command_frames.size(), &replies);
+
+    for (const auto& frame : replies) {
+        int motor_id = frame.source;
+        auto result = moteus::Query::Parse(frame.data, frame.size);
+
+        RCLCPP_INFO(this->get_logger(),
+            "ID: %d | Mode: %2d | Fault: %2d | "
+            "Pos: %.3f rev | Vel: %.3f rev/s | Torque: %.3f Nm | "
+            "Voltage: %.1f V | Temp: %.1f C",
+            motor_id,
+            static_cast<int>(result.mode),
+            static_cast<int>(result.fault),
+            result.position,
+            result.velocity,
+            result.torque,
+            result.voltage,
+            result.temperature
+        );
+    }
+
+    RCLCPP_INFO(this->get_logger(), "---");
+}
+
+int main(int argc, char* argv[]) {
+    rclcpp::init(argc, argv);
+    rclcpp::spin(std::make_shared<MoteusPoll>());
+    rclcpp::shutdown();
+    return 0;
+}