Keep motor indices in the order passed to init_arm_motors()

include/openarm/canbus/can_device_collection.hpp

@@ -33,6 +33,12 @@ class CANDeviceCollection {
     void dispatch_frame_callback(can_frame& frame);
     void dispatch_frame_callback(canfd_frame& frame);
     const std::map<canid_t, std::shared_ptr<CANDevice>>& get_devices() const { return devices_; }
+    // The same devices in the order they were added. DMDeviceCollection's
+    // index i is the i-th device here, so for an arm it is the i-th motor
+    // passed to init_arm_motors(), whatever order the ids are in.
+    const std::vector<std::shared_ptr<CANDevice>>& get_devices_in_order() const {
+        return ordered_devices_;
+    }
     canbus::CANSocket& get_can_socket() const { return can_socket_; }
     int get_socket_fd() const { return can_socket_.get_socket_fd(); }
 
@@ -47,7 +53,8 @@ class CANDeviceCollection {
 
 private:
     canbus::CANSocket& can_socket_;
-    std::map<canid_t, std::shared_ptr<CANDevice>> devices_;
+    std::map<canid_t, std::shared_ptr<CANDevice>> devices_;    // recv-id lookup
+    std::vector<std::shared_ptr<CANDevice>> ordered_devices_;  // insertion order
     std::map<canid_t, uint64_t> unmatched_frames_;
 };
 }  // namespace openarm::canbus

include/openarm/damiao_motor/dm_motor_device_collection.hpp

@@ -24,6 +24,10 @@
 
 namespace openarm::damiao_motor {
 
+// The motor index i taken by the *_one(i, ...) methods, get_motor(i) and
+// get_link_stats(i), and the position in the *_all(...) vectors, is the order
+// the motors were added: for an arm, the order of the ids passed to
+// init_arm_motors(), whether or not they are ascending.
 class DMDeviceCollection {
 public:
     DMDeviceCollection(canbus::CANSocket& can_socket);

src/openarm/canbus/can_device_collection.cpp

@@ -12,6 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include <algorithm>
 #include <iostream>
 #include <openarm/canbus/can_device_collection.hpp>
 #include <openarm/canbus/can_socket.hpp>
@@ -25,9 +26,17 @@ CANDeviceCollection::~CANDeviceCollection() {}
 void CANDeviceCollection::add_device(const std::shared_ptr<CANDevice>& device) {
     if (!device) return;
 
-    // Add device to our collection
     canid_t device_id = device->get_recv_can_id();
+    auto it = devices_.find(device_id);
+    if (it != devices_.end()) {
+        // Same recv id again: the new device takes the old one's place, so the
+        // order of the others does not shift.
+        std::replace(ordered_devices_.begin(), ordered_devices_.end(), it->second, device);
+        it->second = device;
+        return;
+    }
     devices_[device_id] = device;
+    ordered_devices_.push_back(device);
 }
 
 void CANDeviceCollection::remove_device(const std::shared_ptr<CANDevice>& device) {
@@ -36,7 +45,9 @@ void CANDeviceCollection::remove_device(const std::shared_ptr<CANDevice>& device
     canid_t device_id = device->get_recv_can_id();
     auto it = devices_.find(device_id);
     if (it != devices_.end()) {
-        // Remove from our collection
+        ordered_devices_.erase(
+            std::remove(ordered_devices_.begin(), ordered_devices_.end(), it->second),
+            ordered_devices_.end());
         devices_.erase(it);
     }
 }

src/openarm/damiao_motor/dm_motor_device_collection.cpp

@@ -196,8 +196,10 @@ const MotorLinkStats& DMDeviceCollection::get_link_stats(int i) const {
 }
 
 std::vector<std::shared_ptr<DMCANDevice>> DMDeviceCollection::get_dm_devices() const {
+    // Insertion order, not recv-id order: index i is the i-th motor the caller
+    // passed in, so control modes and *_all() vectors line up with that list.
     std::vector<std::shared_ptr<DMCANDevice>> dm_devices;
-    for (const auto& [id, device] : device_collection_->get_devices()) {
+    for (const auto& device : device_collection_->get_devices_in_order()) {
         auto dm_device = std::dynamic_pointer_cast<DMCANDevice>(device);
         if (dm_device) {
             dm_devices.push_back(dm_device);