Fix OMPL config structure, FK units, firmware query buffer and CAN renaming

piper_cpp/include/piper_cpp/fk.h

@@ -44,8 +44,9 @@ class PiperForwardKinematics
     }
 
     /// Compute forward kinematics for the supplied joint angles (radians). Returns the
-    /// cumulative pose of each link as `[x, y, z, roll, pitch, yaw]` (mm + radians) in the
-    /// base frame. The last entry is the end-effector pose.
+    /// cumulative pose of each link as `[x, y, z, roll, pitch, yaw]` in the base frame, with
+    /// positions in mm and angles in degrees (the convention of the manufacturer's SDK, whose
+    /// home pose is `init_pos`). The last entry is the end-effector pose.
     std::vector<std::array<double, 6>> calcFK(const JointVec& cur_j) const
     {
         std::array<Mat4, 6> Ts;

piper_cpp/include/piper_cpp/interface/piper_interface_v2.h

@@ -220,10 +220,11 @@ class PiperInterfaceV2
     /// Parsed firmware-version string. Looks for the `"S-V"` marker in the streamed firmware
     /// data and returns the 8-byte tag that follows it, e.g. `"S-V1.7-4"`.
     ///
-    /// Returns an empty optional if the marker hasn't been received yet (e.g. you've called
+    /// Returns an empty optional until the whole tag has been received (e.g. you've called
     /// this within the first ~100 ms of `connectPort` and the bytes haven't arrived). If you
     /// need to re-query (after a firmware-update on the arm, say), call
-    /// `requestFirmwareVersion()` and read this again a moment later.
+    /// `requestFirmwareVersion()`, which discards the previous reply, and read this again a
+    /// moment later.
     std::optional<std::string> getFirmwareVersion() const;
 
     /// Re-send the firmware-version query (CAN ID `0x4AF`). The reply trickles in over the next
@@ -232,8 +233,8 @@ class PiperInterfaceV2
     bool requestFirmwareVersion();
 
     /// Compute forward kinematics from the latest measured joint angles. Returns the cumulative
-    /// pose (position + orientation in the base frame) at each link, as a vector of 6-element
-    /// arrays. The last entry is the end-effector pose.
+    /// pose (position in mm + orientation in degrees, in the base frame) at each link, as a
+    /// vector of 6-element arrays. The last entry is the end-effector pose.
     ///
     /// @note This is computed on demand from `getArmJointStates()`; if the joint feedback isn't
     /// valid yet, returns the FK of zero joints and prints a warning to stderr.

piper_cpp/src/can_utils.cpp

@@ -82,6 +82,18 @@ static bool renameInterface(const std::string& old_name, const std::string& new_
     return ok;
 }
 
+// A parking name for an adapter awaiting its final name: short enough for IFNAMSIZ with any
+// prefix, not currently in use, and never one of the final names.
+static std::string freeTemporaryName(const std::vector<std::string>& targets)
+{
+    for (int n = 0;; ++n)
+    {
+        const std::string tmp = "cantmp" + std::to_string(n);
+        if (if_nametoindex(tmp.c_str()) == 0 && std::find(targets.begin(), targets.end(), tmp) == targets.end())
+            return tmp;
+    }
+}
+
 std::vector<PortPair> findPorts() { return getCanInterfacesInternal(); }
 
 std::vector<std::string> activePorts()
@@ -113,25 +125,46 @@ void activate(const std::vector<PortPair>& ports, const std::string& prefix, int
             throw std::runtime_error("Timed out waiting for CAN devices");
     }
     std::sort(list.begin(), list.end(), [](auto& a, auto& b) { return a.second < b.second; });
+
+    // The kernel only renames an interface that is down, and the requested names may already
+    // be taken by the wrong adapters (two adapters swapping names), so: stop everything, park
+    // each adapter that needs renaming under a temporary name, then give out the final names
+    // and bring the interfaces up.
+    std::vector<std::string> targets;
+    for (size_t i = 0; i < list.size(); ++i)
+        targets.push_back(prefix + std::to_string(i));
+    std::vector<std::string> current;
     for (size_t i = 0; i < list.size(); ++i)
     {
         const std::string iface = list[i].first;
-        const std::string target = prefix + std::to_string(i);
-        // stop interface
         if (can_do_stop(iface.c_str()) < 0)
             throw std::runtime_error("Failed to stop interface " + iface);
-        // set bitrate
-        if (can_set_bitrate(iface.c_str(), static_cast<unsigned int>(bitrate)) < 0)
-            throw std::runtime_error("Failed to set bitrate on " + iface);
-        // start interface
-        if (can_do_start(iface.c_str()) < 0)
-            throw std::runtime_error("Failed to start interface " + iface);
-        // rename if necessary
-        if (target != iface)
+        std::string name = iface;
+        if (iface != targets[i])
+        {
+            const std::string tmp = freeTemporaryName(targets);
+            if (!renameInterface(iface, tmp))
+                std::cerr << "[WARN] rename " << iface << "->" << tmp << " failed";
+            else
+                name = tmp;
+        }
+        current.push_back(name);
+    }
+    for (size_t i = 0; i < list.size(); ++i)
+    {
+        std::string name = current[i];
+        const std::string& target = targets[i];
+        if (name != target)
         {
-            if (!renameInterface(iface, target))
-                std::cerr << "[WARN] rename " << iface << "->" << target << " failed";
+            if (!renameInterface(name, target))
+                std::cerr << "[WARN] rename " << name << "->" << target << " failed";
+            else
+                name = target;
         }
+        if (can_set_bitrate(name.c_str(), static_cast<unsigned int>(bitrate)) < 0)
+            throw std::runtime_error("Failed to set bitrate on " + name);
+        if (can_do_start(name.c_str()) < 0)
+            throw std::runtime_error("Failed to start interface " + name);
     }
 }
 

piper_cpp/src/interface/piper_interface_v2.cpp

@@ -6,7 +6,7 @@
 namespace piper_cpp
 {
 
-static constexpr double MILLIDEG2RAD = 0.001 * 3.14 / 180;
+static constexpr double MILLIDEG2RAD = 0.001 * M_PI / 180;
 
 // Implementation of the PiperInterfaceV2 methods
 PiperInterfaceV2::PiperInterfaceV2(
@@ -439,8 +439,11 @@ void PiperInterfaceV2::parseCANFrame(const struct can_frame &frame, double times
         }
         case ArmMsgType::FirmwareRead:
         {
+            // The reply to one query is a few frames; anything beyond this is noise, not the tag.
+            static constexpr size_t kMaxFirmwareBytes = 256;
             std::lock_guard<std::mutex> lock(firmware_data_mutex_);
-            firmware_data_.insert(firmware_data_.end(), msg.firmware_data.begin(), msg.firmware_data.end());
+            if (firmware_data_.size() < kMaxFirmwareBytes)
+                firmware_data_.insert(firmware_data_.end(), msg.firmware_data.begin(), msg.firmware_data.end());
             break;
         }
         case ArmMsgType::InstructionResponseConfig:
@@ -940,6 +943,12 @@ bool PiperInterfaceV2::sendFirmwareQuery()
     std::vector<uint8_t> data{0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
     uint16_t can_id = 0x4AF;
 
+    {
+        // Each query gets its own reply buffer, so a re-query after a firmware update cannot
+        // keep reporting the old tag.
+        std::lock_guard<std::mutex> lock(firmware_data_mutex_);
+        firmware_data_.clear();
+    }
     if (!arm_can_->sendCanMessage(can_id, data))
     {
         std::cerr << "Error: Failed to send firmware query CAN message.\n";
@@ -966,8 +975,10 @@ std::optional<std::string> PiperInterfaceV2::getFirmwareVersion() const
     {
         if (std::memcmp(firmware_data_.data() + i, marker, marker_len) == 0)
         {
-            const size_t end = std::min(i + version_len, firmware_data_.size());
-            return std::string(firmware_data_.begin() + i, firmware_data_.begin() + end);
+            // The tag spans frames; report it only once it is all here.
+            if (i + version_len > firmware_data_.size())
+                return std::nullopt;
+            return std::string(firmware_data_.begin() + i, firmware_data_.begin() + i + version_len);
         }
     }
     return std::nullopt;

piper_cpp/test/CMakeLists.txt

@@ -14,3 +14,4 @@ add_piper_test(std_can_interface_test)
 add_piper_test(piper_interface_v2_test)
 add_piper_test(piper_parser_v2_test)
 add_piper_test(timed_freq_state_test)
+add_piper_test(fk_test)

piper_cpp/test/fk_test.cpp

@@ -0,0 +1,26 @@
+#include "piper_cpp/fk.h"
+
+#include <gtest/gtest.h>
+
+using piper_cpp::PiperForwardKinematics;
+
+// The manufacturer's SDK quotes the home pose of the default (offset) DH set in mm and
+// degrees; the FK must reproduce it at zero joint angles, which pins the output units. The
+// non-offset set's quoted pose is rounded to the millimetre, so it is not asserted.
+TEST(ForwardKinematics, ZeroJointsGiveTheHomePoseInMillimetresAndDegrees)
+{
+    PiperForwardKinematics fk;
+    const auto pose = fk.calcFK({0, 0, 0, 0, 0, 0}).back();
+    for (int i = 0; i < 6; ++i)
+        EXPECT_NEAR(pose[i], fk.init_pos[i], 0.001) << "component " << i;
+}
+
+TEST(ForwardKinematics, InputIsRadians)
+{
+    PiperForwardKinematics fk;
+    // A quarter turn of joint 1 swings the home position from +x onto +y.
+    const auto pose = fk.calcFK({PiperForwardKinematics::PI / 2, 0, 0, 0, 0, 0}).back();
+    EXPECT_NEAR(pose[0], 0.0, 0.01);
+    EXPECT_NEAR(pose[1], fk.init_pos[0], 0.01);
+    EXPECT_NEAR(pose[2], fk.init_pos[2], 0.01);
+}

piper_cpp/test/piper_interface_v2_test.cpp

@@ -7,6 +7,7 @@
 
 #include <algorithm>
 #include <chrono>
+#include <csignal>
 #include <future>
 #include <initializer_list>
 #include <memory>
@@ -210,6 +211,7 @@ TEST(PiperInterfaceV2, ConcurrentJointCommandsDoNotInterleave)
     // Each controlJoints() call is three frames (0x155, 0x156, 0x157). Two callers with
     // different targets must still produce whole triples on the bus, never a mix.
     constexpr int kCallsPerThread = 500;
+    std::signal(SIGPIPE, SIG_IGN); // a sender writing into the shut-down peer must fail, not die
     auto bus = makeBus();
     ASSERT_TRUE(bus.piper->connectPort(/*can_init=*/false, /*piper_init=*/false, /*start_threads=*/false));
 
@@ -222,18 +224,24 @@ TEST(PiperInterfaceV2, ConcurrentJointCommandsDoNotInterleave)
     std::thread b(sender, 2000);
 
     // Receive failures are recorded rather than asserted here so the senders are always
-    // joined; a quiet bus means they have finished, so the joins cannot hang.
+    // joined; shutting the peer down first turns any blocked send into a failure, so the joins
+    // cannot hang. One deadline covers the whole exchange, so a scheduling pause between two
+    // frames does not count as a quiet bus.
     constexpr size_t kExpectedFrames = 2 * kCallsPerThread * 3;
+    const auto deadline = steady_clock::now() + 10s;
     std::vector<can_frame> frames;
     frames.reserve(kExpectedFrames);
     bool received_all = true;
     while (frames.size() < kExpectedFrames)
     {
+        const auto left = std::chrono::duration_cast<std::chrono::milliseconds>(deadline - steady_clock::now());
         pollfd pfd{bus.peer, POLLIN, 0};
         can_frame f{};
-        if (poll(&pfd, 1, 2000) <= 0 || ::recv(bus.peer, &f, sizeof(f), 0) != static_cast<ssize_t>(sizeof(f)))
+        if (left.count() <= 0 || poll(&pfd, 1, static_cast<int>(left.count())) <= 0 ||
+            ::recv(bus.peer, &f, sizeof(f), 0) != static_cast<ssize_t>(sizeof(f)))
         {
             received_all = false;
+            ::shutdown(bus.peer, SHUT_RDWR);
             break;
         }
         frames.push_back(f);
@@ -250,7 +258,32 @@ TEST(PiperInterfaceV2, ConcurrentJointCommandsDoNotInterleave)
         ASSERT_EQ(frames[i + 1].can_id, 0x156u);
         ASSERT_EQ(frames[i + 2].can_id, 0x157u);
         const auto target = firstJoint(frames[i]);
+        ASSERT_TRUE(target == 1000 || target == 2000) << "triple " << i / 3 << " carries neither caller's target";
         ASSERT_EQ(firstJoint(frames[i + 1]), target) << "triple " << i / 3 << " mixes two callers";
         ASSERT_EQ(firstJoint(frames[i + 2]), target) << "triple " << i / 3 << " mixes two callers";
     }
 }
+
+TEST(PiperInterfaceV2, FirmwareVersionNeedsTheWholeTagAndRestartsPerQuery)
+{
+    auto bus = makeBus();
+    ASSERT_TRUE(bus.piper->connectPort(/*can_init=*/false, /*piper_init=*/false, /*start_threads=*/true));
+    const auto deadline = steady_clock::now() + 5s;
+    auto firmwareBytes = [&](size_t n) { return bus.piper->getFirmwareData().size() == n; };
+
+    ASSERT_TRUE(bus.piper->requestFirmwareVersion());
+    sendFrame(bus.peer, frame(0x4AF, {'S', '-', 'V', '1'}), deadline);
+    ASSERT_TRUE(waitFor([&] { return firmwareBytes(4); }));
+    EXPECT_FALSE(bus.piper->getFirmwareVersion()) << "half a tag is not a version";
+
+    sendFrame(bus.peer, frame(0x4AF, {'.', '7', '-', '4'}), deadline);
+    ASSERT_TRUE(waitFor([&] { return firmwareBytes(8); }));
+    EXPECT_EQ(bus.piper->getFirmwareVersion(), "S-V1.7-4");
+
+    // A new query must not keep answering with the previous reply.
+    ASSERT_TRUE(bus.piper->requestFirmwareVersion());
+    EXPECT_FALSE(bus.piper->getFirmwareVersion());
+    sendFrame(bus.peer, frame(0x4AF, {'S', '-', 'V', '2', '.', '0', '-', '1'}), deadline);
+    ASSERT_TRUE(waitFor([&] { return firmwareBytes(8); }));
+    EXPECT_EQ(bus.piper->getFirmwareVersion(), "S-V2.0-1");
+}

piper_cpp_moveit/config/ompl_planning.yaml

@@ -10,17 +10,23 @@ response_adapters:
   - default_planning_response_adapters/ValidateSolution
   - default_planning_response_adapters/DisplayMotionPath
 
+# Planner definitions live at the top level; each group lists the ones it may use by name.
+planner_configs:
+  RRTConnect:
+    type: geometric::RRTConnect
+    range: 0.0
+  RRTstar:
+    type: geometric::RRTstar
+    range: 0.0
+    goal_bias: 0.05
+  PRMstar:
+    type: geometric::PRMstar
+
 manipulator:
   planner_configs:
-    RRTConnect:
-      type: geometric::RRTConnect
-      range: 0.0
-    RRTstar:
-      type: geometric::RRTstar
-      range: 0.0
-      goal_bias: 0.05
-    PRMstar:
-      type: geometric::PRMstar
+    - RRTConnect
+    - RRTstar
+    - PRMstar
   default_planner_config: RRTConnect
   projection_evaluator: joints(joint1,joint2)
   longest_valid_segment_fraction: 0.005