Reapply "review: revert heart node, drop moteus_ros2 + cal log, restore reviewed arm headers (PR #187 feedback)"

This reverts commit d5c5b0b.

Author: aaronrhim

.gitmodules

@@ -7,9 +7,6 @@
 [submodule "src/external_pkgs/moteus"]
 	path = src/external_pkgs/moteus
 	url = https://github.com/mjbots/moteus.git
-[submodule "src/external_pkgs/moteus_ros2"]
-	path = src/external_pkgs/moteus_ros2
-	url = https://github.com/mjbots/moteus_ros2.git
 [submodule "src/external_pkgs/ffmpeg_image_transport"]
 	path = src/external_pkgs/ffmpeg_image_transport
 	url = https://github.com/ros-misc-utilities/ffmpeg_image_transport.git

moteus-cal-AD8ANVQwUBggOTNO-20260804T014146.313271.log

@@ -75,17 +75,18 @@ struct JointMap {
     double      direction;        // +1 or -1: sign between output revolutions and URDF angle
 };
 
-//!? this is wrong?? where is A4?
-// Should only be needed for IK/rviz. Leave be for now.
+// urdf joint names only needed for IK/rviz. The URDF still describes the old
+// 6-joint arm (a4_rotation..a6_rotation) — a5_differential/a6_differential/
+// ee_prismatic don't exist there yet.
 static const JointMap ARM_JOINTS[NUM_MOTORS] = {
     //  id   hardware label    urdf joint name    boot angle (rad)   direction
-    {  1,   "Base",           "shoulder_joint",       -1.57,          -1.0  },
-    {  2,   "Shoulder",       "link_1_joint",         -1.57,          -1.0  },
-    {  3,   "Elbow",          "link1_link2",           0.9,           -1.0  },
-    {  4,   "BALLS",          "MEOWMEOW",           0.9,           -1.0  }, // IDK
-    {  5,   "Wrist Pitch",    "a4_rotation",           0.0,           -1.0  },
-    {  6,   "Wrist Roll",     "a5_rotation",           1.2,           -1.0  },
-    {  7,   "End Effector",   "a6_rotation",           0.0,           -1.0  },
+    {  1,   "A1",   "shoulder_joint",       -1.57,          -1.0  },
+    {  2,   "A2",   "link_1_joint",         -1.57,          -1.0  },
+    {  3,   "A3",   "link1_link2",           0.9,           -1.0  },
+    {  4,   "A4",   "a4_rotation",           0.9,           -1.0  },
+    {  5,   "A5",   "a5_differential",       0.0,           -1.0  },
+    {  6,   "A6",   "a6_differential",       1.2,           -1.0  },
+    {  7,   "EE",   "ee_prismatic",          0.0,           -1.0  },
 };
 
 

src/external_pkgs/moteus_ros2

@@ -3,6 +3,7 @@
 #include "motor_addressing.h"  // NUM_MOTORS
 
 #include <cmath>
+#include <cstdint>
 #include <limits>
 #include <string>
 #include <vector>
@@ -63,6 +64,14 @@ struct MotorConfig {
     // Stored here so the HMI can display it in the Motor Config panel.
     float gear_reduction = 1.0f;
 
+    // Datasheet identity (Maxon).  poles/kv feed calibration flags — these are
+    // PER MOTOR: calibrating with the wrong pole count mis-commutates the
+    // motor (low torque at max current, siren whine, angle-dependent gains).
+    int         poles             = 16;    // pole count (pole pairs x 2)
+    float       kv                = 265.0f;
+    float       nominal_current_A = 1.0f;  // continuous thermal rating
+    const char* part_no           = "?";
+
     // -------------------------------------------------------------------------
     // Helpers used by configureMotor()
     // -------------------------------------------------------------------------
@@ -99,14 +108,18 @@ struct MotorConfig {
 inline std::vector<MotorConfig> get_arm_configuration() {
     std::vector<MotorConfig> motors(NUM_MOTORS);
 
+    // --- Synced from controller flash 2026-08-02 (conf get readback) so the
+    //     repo matches the bench tuning.  A7/EE was not replying; it keeps
+    //     struct defaults until it can be read.
+
     // --- PID gains (hand-tuned per axis) ---
     //     Higher kp = stiffer.  Add kd to dampen oscillation.
-    motors[0].kp = 180.0f;   motors[0].kd =  40.0f;   // Base
-    motors[1].kp = 2100.0f;  motors[1].kd = 100.0f;   // Shoulder
-    motors[2].kp = 4000.0f;  motors[2].kd = 750.0f;   // Elbow
-    motors[3].kp =  50.0f;   motors[3].kd =   0.0f;   // Wrist Pitch
-    motors[4].kp = 600.0f;   motors[4].kd = 100.0f;   // Wrist Roll
-    motors[5].kp = 600.0f;   motors[5].kd = 100.0f;   // End Effector
+    motors[0].kp =  4000.0f;  motors[0].kd =  600.0f;   // A1
+    motors[1].kp = 30000.0f;  motors[1].kd = 6000.0f;   // A2
+    motors[2].kp = 40000.0f;  motors[2].kd = 2000.0f;   // A3
+    motors[3].kp =   550.0f;  motors[3].kd =   10.0f;   // A4
+    motors[4].kp = 17000.0f;  motors[4].kd = 3500.0f;   // A5
+    motors[5].kp = 17000.0f;  motors[5].kd = 3500.0f;   // A6
 
     // --- Gear reductions (for display in HMI only — firmware holds the real value) ---
     motors[0].gear_reduction = 1.0f / 190.0f;
@@ -115,23 +128,56 @@ inline std::vector<MotorConfig> get_arm_configuration() {
     motors[3].gear_reduction = 1.0f / 190.0f;
     motors[4].gear_reduction = 1.0f /  66.0f;
     motors[5].gear_reduction = 1.0f /  66.0f;
+    motors[6].gear_reduction = 1.0f / 190.0f;   // EE
+
+    // --- Datasheet identity (bench notes / Maxon, 2026-08) ---
+    // A2/A3 are 22-pole kv=134 — calibration flags MUST come from here, not
+    // the old hardcoded 16/265 (that mis-commutates them).
+    const char* parts[NUM_MOTORS] = {"339281","607942","515458","339281","651607","651607","339281"};
+    const int   poles[NUM_MOTORS] = { 16,      22,      22,      16,      16,      16,      16     };
+    const float kvs  [NUM_MOTORS] = { 265,     134,     134,     187,     265,     265,     265    };
+    const float noms [NUM_MOTORS] = { 1.01f,   9.5f,    6.44f,   1.01f,   2.52f,   2.52f,   1.01f  };
+    for (int i = 0; i < NUM_MOTORS; i++) {
+        motors[i].part_no = parts[i];
+        motors[i].poles   = poles[i];
+        motors[i].kv      = kvs[i];
+        motors[i].nominal_current_A = noms[i];
+    }
 
     // --- Current limits (A) ---
-    motors[0].max_current_A = 1.0f;
-    motors[1].max_current_A = 8.0f;
-    motors[2].max_current_A = 5.5f;
-    motors[3].max_current_A = 0.5f;
-    motors[4].max_current_A = 2.5f;
-    motors[5].max_current_A = 2.5f;
-
-    // --- Software position limits (output-shaft revolutions) ---
-    motors[0].position_min = -0.3f;     motors[0].position_max =   0.3f;
-    motors[1].position_min = -0.47f;    motors[1].position_max =  -0.05f;
-    motors[2].position_min =  0.01f;    motors[2].position_max =   0.4f;
-    // motors[3]: uses default (-1.0, 1.0)
-    //TODO this is positions for motors, not motors
-    motors[4].position_min = -999.01f;  motors[4].position_max = 999.4f;  // continuous
-    motors[5].position_min = -999.01f;  motors[5].position_max = 999.4f;  // continuous
+    motors[0].max_current_A =  2.0f;   // back to 2.0 (2026-08-03); was 3.0 after the 08-02 stall
+    motors[1].max_current_A = 14.0f;
+    motors[2].max_current_A = 14.0f;
+    motors[3].max_current_A =  1.01f;   // 200142 nominal 2.14 A; 0.3 starved it (bench 2026-08-03)
+    motors[4].max_current_A =  2.0f;
+    motors[5].max_current_A =  2.0f;
+
+    // --- Voltage / power limits ---
+    const float fnan = std::numeric_limits<float>::quiet_NaN();
+    motors[0].max_voltage = 30.0f;  motors[0].max_power_W = 200.0f;
+    motors[1].max_voltage = 30.0f;  motors[1].max_power_W = fnan;
+    motors[2].max_voltage = 32.0f;  motors[2].max_power_W = 250.0f;
+    motors[3].max_voltage = 24.0f;  motors[3].max_power_W = 30.0f;
+    motors[4].max_voltage = 30.0f;  motors[4].max_power_W = fnan;
+    motors[5].max_voltage = 30.0f;  motors[5].max_power_W = fnan;
+
+    // --- Motion profile ---
+    motors[0].max_acceleration = 0.5f;  motors[0].max_velocity = 0.01f;
+    motors[1].max_acceleration = 0.5f;  motors[1].max_velocity = 0.03f;
+    motors[2].max_acceleration = 0.5f;  motors[2].max_velocity = 0.05f;
+    motors[3].max_acceleration = 0.5f;  motors[3].max_velocity = 0.10f;
+    motors[4].max_acceleration = 1.0f;  motors[4].max_velocity = 0.05f;
+    motors[5].max_acceleration = 1.0f;  motors[5].max_velocity = 0.05f;
+
+    // --- Position limits (output-shaft revolutions) ---
+    //     Flash has servopos unbounded (nan) everywhere except A4 — adopted
+    //     as-is; homing/soft-stop bounds live in AxisConfig, not here.
+    for (int i : {0, 1, 2, 4, 5}) {
+        motors[i].position_min = fnan;
+        motors[i].position_max = fnan;
+    }
+    motors[3].position_min = -0.5f;  motors[3].position_max = 0.5f;
+    motors[3].def_timeout  = 0.1f;
 
     return motors;
 }
@@ -176,7 +222,7 @@ namespace AxisConfig {
         /*AXIS 1 */   0.1,
         /*AXIS 2 */   0.1,
         /*AXIS 3 */   0.1,
-        /*AXIS 4 */   0.1, // DISABLED
+        /*AXIS 4 */   0.1,
         /*AXIS 5 */   0.1,
         /*AXIS 6 */   0.1,
         /*AXIS EE */  0.5 };
@@ -190,14 +236,33 @@ namespace AxisConfig {
         /*AXIS 6 */   0.5,
         /*AXIS EE */  0.5 };
 
-    float idle_position[NUM_AXES] = { // Rev/s Direction Independent. 
+    float idle_position[NUM_AXES] = { // Rev/s Direction Independent.
         /*AXIS 1 */   0.1,
         /*AXIS 2 */   0.1,
         /*AXIS 3 */   0.1,
         /*AXIS 4 */   0.1,
         /*AXIS 5 */   -0.1,
         /*AXIS 6 */   0.1,
         /*AXIS EE */  0.1 };
-    
 
-};
+    // Limit switches — AUX2 digital input, see setup_limit_switches.py.
+    // mask: aux2 GPIO status bitfield, bit0 = aux2.pins.0 (ABS connector pin 2).
+    // inverted=false: NC switch to GND (hard 2k pullup) -> pin HIGH = pressed.
+    bool has_limit_switch[NUM_AXES] = {
+        /*AXIS 1 */   true,
+        /*AXIS 2 */   true,
+        /*AXIS 3 */   true,
+        /*AXIS 4 */   true,
+        /*AXIS 5 */   false,
+        /*AXIS 6 */   false,
+        /*AXIS EE */  false };
+
+    // Per-axis, measured with DEBUG_LIMIT_SWITCH_RAW_REPLY (2026-08-02).
+    // All four: pressed = HIGH; only the pin differs (A1/A3 pin 0, A2/A4 pin 1).
+    // A1's first capture was misread as inverted — the arm was resting on its
+    // switch, so the observed transitions were releases, not presses.
+    // A3's pin has no pull; if it flakes: conf set aux2.pins.0.pull 2 on motor 3.
+    uint8_t limit_switch_mask[NUM_AXES]     = {1, 2, 1, 2, 0, 0, 0};
+    bool    limit_switch_inverted[NUM_AXES] = {false, false, false, false, false, false, false};
+
+};

src/rover_arm_common/include/rover_arm_common/motor_addressing.h

@@ -1,40 +1,105 @@
-//* Per-computer supervisor: starts/stops the subsystems listed in heart.yaml and
-//* reports their OBSERVED state (reaped via waitpid, never a stored belief).
+//* Meant to be like, the heart of the rover, each subsystem can be thought of as a muscle or organ. the heart can start/shutoff the flow of blood to each subsystem
+//* One of these will run on each computer, and run/manage all nodes on that device
+
 #include <rclcpp/rclcpp.hpp>
 #include <rover_utils/include/roverCommon.h>
-#include <rover_msgs/msg/subsystem_command.hpp>
-#include <rover_msgs/msg/heart_status.hpp>
-#include <chrono>
-#include <sys/types.h>
-
+#include <rover_msgs/msg/heart_request.hpp>
+using namespace ConsoleFormat;
 class HeartNode : public rclcpp::Node
 {
 public:
-    HeartNode();
-
-private:
-    using State = rover_msgs::msg::SubsystemState;
-    struct SubSystem {
-        std::string name;
-        std::string exec_command;
-        pid_t pid = -1;              // == pgid/sid (child calls setsid)
-        uint8_t state = State::STOPPED;
-        int exit_code = 0;
-        bool pending_restart = false;
-        int stop_stage = 0;          // 1=SIGINT sent, 2=SIGTERM, 3=SIGKILL
-        std::chrono::steady_clock::time_point started_at, escalate_at;
-    };
-    std::map<std::string, SubSystem> subsystems;  // ordered -> stable msg order
-    std::string my_host_id;
-
-    void startSubsystem(SubSystem& s);
-    void stopSubsystem(SubSystem& s);
-    void superviseTick();            // 250 ms: reap children, escalate stops
-    void publishStatus();
-    void commandCallback(rover_msgs::msg::SubsystemCommand::SharedPtr cmd);
-    void signalGroup(pid_t pgid, int sig);
-
-    rclcpp::TimerBase::SharedPtr heartbeat_timer, supervise_timer;
-    rclcpp::Subscription<rover_msgs::msg::SubsystemCommand>::SharedPtr command_sub;
-    rclcpp::Publisher<rover_msgs::msg::HeartStatus>::SharedPtr status_pub;
+    HeartNode() : Node("broken_heart", rclcpp::NodeOptions().allow_undeclared_parameters(true).automatically_declare_parameters_from_overrides(true))
+    {    
+        std::string my_name = this->get_name();
+    
+        if(my_name == "broken_heart"){
+        RCLCPP_ERROR(this->get_logger(), "Sorry, you need to launch me with a different name (use a launch file with name= ). I'm going to get confused otherwise.");
+            rclcpp::shutdown();
+    }
+    rclcpp::Parameter topic_param;
+    rclcpp::Parameter feedback_topic_param;
+    rclcpp::Parameter heartbeat_rate_param;
+    this->get_parameter("request_topic", topic_param);
+    this->get_parameter("heart_feedback_topic", feedback_topic_param);
+    this->get_parameter("heartbeat_rate", heartbeat_rate_param);
+
+    // std::string request_topic = my_name + "/request";
+    my_host_id = my_name;
+    my_host_id.erase(0, 6);
+    std::string request_topic = topic_param.value_to_string();
+    std::string feedback_topic = feedback_topic_param.value_to_string();
+    this->get_parameters("subsystems", params);
+    
+    
+    RCLCPP_INFO(this->get_logger(), "I am %s, on topic %s", this->get_name(), request_topic.c_str());
+
+    //* Construct Subsystems from Params
+    { //just making a little scope for i, sure theres a better way
+    int i = 0;
+    for (const auto& [key, param] : params) {
+        RCLCPP_INFO(this->get_logger(), "Got Subystem %s%s%s = %s%s%s", 
+                    green(), key.c_str(), reset(), bright_blue(), param.value_to_string().c_str(), reset());
+            SubSystem new_subsys;
+            new_subsys.name = key;
+            new_subsys.exec_command = param.value_to_string();
+            new_subsys.index = i;
+            subsystems_vector.push_back(new_subsys);
+            subsystems[key] = new_subsys;
+            i++;
+    }
+    if(i == 0){
+        RCLCPP_WARN(this->get_logger(), "Warning, no subsystems defined. You sure you loaded parameters right? I'm useless without params, exiting.");
+        rclcpp::shutdown();
+    }
+    }
+    heart_request_sub = this->create_subscription<rover_msgs::msg::HeartRequest>(
+        request_topic, 10, std::bind(&HeartNode::heartRequestCallback, this, std::placeholders::_1));
+    heart_feedback_pub = this->create_publisher<rover_msgs::msg::HeartRequest>(
+        feedback_topic, 20);
+
+        double heartbeat_rate = heartbeat_rate_param.as_double();
+        double heartbeat_period = 1.0/heartbeat_rate;
+
+        heartbeat_timer = this->create_wall_timer(
+            std::chrono::duration<double>(heartbeat_period), std::bind(&HeartNode::heartbeat, this));
+
+
+    // runSubSystem(subsystems_vector[0]);
+    }
+private: 
+struct SubSystem{
+    std::string name;
+    std::string exec_command;
+    int index = -1;
+    pid_t pid;
+    pid_t gpid;
+    pid_t sid;
+    bool online = false;
+};
+std::map<std::string, rclcpp::Parameter> params;
+std::unordered_map<std::string, SubSystem> subsystems;
+
+// std::unordered_map<std::string, std::SubSystem>;
+
+std::string my_host_id;
+
+std::vector<SubSystem> subsystems_vector;
+
+void runSubSystem(SubSystem& subsys);
+void killSubSystem(SubSystem& subsys);
+void heartbeat();
+
+// void runChildNode(std::string pkg, std::string node_or_launch_file, std::string subsytem_name, int type = NODE, bool kill_orphan = true);
+void killProcessGroup(pid_t pgid); //! Should prob have a error return
+
+    rclcpp::TimerBase::SharedPtr heartbeat_timer;
+
+    rclcpp::Subscription<rover_msgs::msg::HeartRequest>::SharedPtr heart_request_sub;
+        void heartRequestCallback(const rover_msgs::msg::HeartRequest::SharedPtr request);
+    
+    rclcpp::Publisher<rover_msgs::msg::HeartRequest>::SharedPtr heart_feedback_pub;
 };
+
+
+
+