Propagate feedback and command errors through the ROS hardware interface

piper_cpp/include/piper_cpp/interface/piper_interface_v2.h

@@ -675,6 +675,10 @@ class PiperInterfaceV2
     // (CanCmd / MoveJ / 50% / PositionVelocity / 0 / Invalid).
     mutable std::mutex ctrl_151_tx_mutex_;
     ArmMsgMotionCtrl_2 ctrl_151_tx_;
+
+    // Held for the whole of a multi-frame command so two callers' frames cannot interleave
+    // into a target the arm never received from either.
+    std::mutex multipart_tx_mutex_;
     bool sendCommandedMotionCtrlLocked();
 
     void resetStates();

piper_cpp/src/interface/piper_interface_v2.cpp

@@ -668,6 +668,7 @@ bool PiperInterfaceV2::controlJoints(
         joint_1_millideg, joint_2_millideg, joint_3_millideg,
         joint_4_millideg, joint_5_millideg, joint_6_millideg,
     };
+    std::lock_guard<std::mutex> lock(multipart_tx_mutex_);
     msg.type = ArmMsgType::JointCtrl12;
     if (!sendPiperMsg(msg))
         return false;
@@ -682,6 +683,7 @@ bool PiperInterfaceV2::controlEndPose(int32_t x_um, int32_t y_um, int32_t z_um,
 {
     PiperMessage msg;
     msg.arm_motion_ctrl_cartesian = ArmMsgEndPose{x_um, y_um, z_um, rx_md, ry_md, rz_md};
+    std::lock_guard<std::mutex> lock(multipart_tx_mutex_);
     msg.type = ArmMsgType::MotionCtrlCartesian1;
     if (!sendPiperMsg(msg))
         return false;

piper_cpp/test/piper_interface_v2_test.cpp

@@ -10,8 +10,10 @@
 #include <future>
 #include <initializer_list>
 #include <memory>
+#include <poll.h>
 #include <sys/socket.h>
 #include <thread>
+#include <vector>
 
 using namespace std::chrono_literals;
 using piper_cpp::PiperInterfaceV2;
@@ -202,3 +204,53 @@ TEST(PiperInterfaceV2, DisconnectClearsCachedState)
     EXPECT_FALSE(js.is_valid) << "feedback from a closed session must not look current";
     EXPECT_EQ(js.value.joint_6, 0);
 }
+
+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;
+    auto bus = makeBus();
+    ASSERT_TRUE(bus.piper->connectPort(/*can_init=*/false, /*piper_init=*/false, /*start_threads=*/false));
+
+    auto sender = [&](int32_t value)
+    {
+        for (int i = 0; i < kCallsPerThread; ++i)
+            EXPECT_TRUE(bus.piper->controlJoints(value, value, value, value, value, value));
+    };
+    std::thread a(sender, 1000);
+    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.
+    constexpr size_t kExpectedFrames = 2 * kCallsPerThread * 3;
+    std::vector<can_frame> frames;
+    frames.reserve(kExpectedFrames);
+    bool received_all = true;
+    while (frames.size() < kExpectedFrames)
+    {
+        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)))
+        {
+            received_all = false;
+            break;
+        }
+        frames.push_back(f);
+    }
+    a.join();
+    b.join();
+    ASSERT_TRUE(received_all) << "bus went quiet after " << frames.size() << " frames";
+
+    auto firstJoint = [](const can_frame& f)
+    { return static_cast<int32_t>((f.data[0] << 24) | (f.data[1] << 16) | (f.data[2] << 8) | f.data[3]); };
+    for (size_t i = 0; i < frames.size(); i += 3)
+    {
+        ASSERT_EQ(frames[i].can_id, 0x155u) << "triple " << i / 3 << " does not start with 0x155";
+        ASSERT_EQ(frames[i + 1].can_id, 0x156u);
+        ASSERT_EQ(frames[i + 2].can_id, 0x157u);
+        const auto target = firstJoint(frames[i]);
+        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";
+    }
+}

piper_cpp_ros/CMakeLists.txt

@@ -44,6 +44,12 @@ install(TARGETS piper_cpp_ros
 install(DIRECTORY include/ DESTINATION include)
 install(DIRECTORY config launch urdf DESTINATION share/${PROJECT_NAME})
 
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+  ament_add_gtest(command_conversion_test test/command_conversion_test.cpp)
+  target_include_directories(command_conversion_test PRIVATE include)
+endif()
+
 ament_export_targets(export_piper_cpp_ros HAS_LIBRARY_TARGET)
 ament_export_dependencies(rclcpp rclcpp_lifecycle hardware_interface pluginlib piper_cpp)
 

piper_cpp_ros/README.md

@@ -242,6 +242,10 @@ shows the pattern.
   `on_deactivate()`, joint motors stay enabled and hold the last commanded pose -- disabling
   motors here would drop the arm. If you want to deactivate, use `piper_cpp` API directly,
   or the shipped `piper_enable --disable` command from CLI (shipped with `piper_cpp`).
+- While active, `read()` returns an error if joint (or gripper) feedback stops arriving for
+  a second, and `write()` returns an error if any command is not a usable position or a CAN
+  send fails; nothing is sent when any part of the command set is invalid. The controller
+  manager then stops the controllers and the arm holds its last target, as on deactivation.
 - `update_rate` in [piper_controllers.yaml](config/piper_controllers.yaml) sets the
   read/write rate. 200 Hz is used, this is what the robot seem to expect; the CAN bus and
   arm firmware comfortably handle this and higher.

piper_cpp_ros/include/piper_cpp_ros/command_conversion.hpp

@@ -0,0 +1,41 @@
+#pragma once
+
+#include <cmath>
+#include <cstdint>
+#include <optional>
+
+namespace piper_cpp_ros
+{
+
+constexpr double kRadToMd = 1000.0 * 180.0 / M_PI;
+constexpr double kMdToRad = 1.0 / kRadToMd;
+
+// piper_cpp's high-speed feedback reports motor_speed in 0.001 rad/s and effort in 0.001 N*m.
+constexpr double kMilliToUnit = 1.0e-3;
+
+// Gripper feedback / command: jaw stroke is in 0.001 mm = micrometers (1 um = 1e-6 m).
+constexpr double kUmToM = 1.0e-6;
+constexpr double kMToUm = 1.0e6;
+
+inline double mdToRad(int32_t md) { return static_cast<double>(md) * kMdToRad; }
+
+/// Joint position command in rad to the arm's 0.001 deg units, or nullopt when the value is not
+/// finite or lies beyond a full turn. Real joint limits are the controller's job; this only
+/// refuses values that have no sensible wire representation.
+inline std::optional<int32_t> jointCommandToMillideg(double rad)
+{
+    if (!std::isfinite(rad) || std::fabs(rad) > 2.0 * M_PI)
+        return std::nullopt;
+    return static_cast<int32_t>(std::lround(rad * kRadToMd));
+}
+
+/// Gripper stroke command in metres to micrometres, with the same acceptance rule; one metre
+/// is far beyond any jaw.
+inline std::optional<int32_t> gripperCommandToMicrometres(double m)
+{
+    if (!std::isfinite(m) || std::fabs(m) > 1.0)
+        return std::nullopt;
+    return static_cast<int32_t>(std::lround(m * kMToUm));
+}
+
+} // namespace piper_cpp_ros

piper_cpp_ros/include/piper_cpp_ros/piper_hardware_interface.hpp

@@ -68,6 +68,7 @@ class PiperHardware : public hardware_interface::SystemInterface
     hardware_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override;
     hardware_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override;
     hardware_interface::CallbackReturn on_cleanup(const rclcpp_lifecycle::State& previous_state) override;
+    hardware_interface::CallbackReturn on_error(const rclcpp_lifecycle::State& previous_state) override;
 
     std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
     std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;

piper_cpp_ros/package.xml

@@ -26,6 +26,7 @@
   <exec_depend>xacro</exec_depend>
   <exec_depend>piper_description</exec_depend>
 
+  <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>