Repository navigation
Propagate feedback and command errors through the ROS hardware interface - #10
Conversation
Reviewer's GuideThe PR hardens the ROS hardware interface by serializing multipart arm commands, validating all outgoing targets before transmission, propagating CAN and stale-feedback failures into lifecycle errors, and releasing the CAN session during error recovery while preserving the arm’s last target. Sequence diagram for validated ROS command transmissionsequenceDiagram
participant ControllerManager
participant PiperHardware
participant PiperInterface
participant CAN
ControllerManager->>PiperHardware: write()
PiperHardware->>PiperHardware: jointCommandToMillideg()
PiperHardware->>PiperHardware: gripperCommandToMicrometres()
alt command validation fails
PiperHardware-->>ControllerManager: ERROR
Note over PiperHardware,CAN: No command is sent
else commands valid
PiperHardware->>PiperInterface: controlJoints()
PiperInterface->>CAN: send joint frames
CAN-->>PiperInterface: success or failure
alt CAN send fails
PiperInterface-->>PiperHardware: false
PiperHardware-->>ControllerManager: ERROR
else joint send succeeds
PiperHardware->>PiperInterface: controlGripper()
PiperInterface->>CAN: send gripper command
CAN-->>PiperInterface: success or failure
PiperHardware-->>ControllerManager: OK or ERROR
end
end
File-Level Changes
Tips and commandsInteracting with Sourcery
Customizing Your ExperienceAccess your dashboard to:
Getting Help
|
|
Important
This repository does not receive automatic reviews because it has fewer than 10 stars. ⚙️ Run configuration
Thanks for using CodeRabbit! It's free for OSS, and your support helps us grow. If you like it, consider giving us a shout-out. Comment |
There was a problem hiding this comment.
🔵 Needs a closer look
Lifecycle recovery and motor-hold behavior require human validation with ros2_control and the physical arm.
0 open findings
What changed in this PR
Brings the previously reviewed #9 changes onto main, hardening the ROS hardware interface’s command handling and error recovery.
Changes:
- Serialize multi-frame commands to prevent concurrent callers from interleaving targets.
- Validate commands, propagate transmission and stale-feedback errors, and release CAN sessions on error.
- Add conversion and concurrency tests, build integration, and behavior documentation.
| File | Description |
|---|---|
| piper_cpp/test/piper_interface_v2_test.cpp | Tests concurrent joint-command serialization. |
| piper_cpp/src/interface/piper_interface_v2.cpp | Locks multi-frame joint and pose commands. |
| piper_cpp/include/piper_cpp/interface/piper_interface_v2.h | Declares the multi-frame command mutex. |
| piper_cpp_ros/test/command_conversion_test.cpp | Tests command conversion and validation. |
| piper_cpp_ros/src/piper_hardware_interface.cpp | Propagates errors and cleans up failed sessions. |
| piper_cpp_ros/README.md | Documents feedback and command error behavior. |
| piper_cpp_ros/package.xml | Adds the GoogleTest dependency. |
| piper_cpp_ros/include/piper_cpp_ros/piper_hardware_interface.hpp | Declares the lifecycle error callback. |
| piper_cpp_ros/include/piper_cpp_ros/command_conversion.hpp | Centralizes validated command conversions. |
| piper_cpp_ros/CMakeLists.txt | Registers conversion tests. |
🧠 Review effort: Balanced
💡 Add a code-review agent skill or configure MCP servers for context-aware, tailored reviews. Learn more in the docs.
![sourcery-ai[bot]](https://avatars.githubusercontent.com/in/48477?s=60&v=4){kind=small}
There was a problem hiding this comment.
Hey - I've found 5 issues
Prompt for AI Agents
Please address the comments from this code review:
## Individual Comments
### Comment 1
<location path="piper_cpp/src/interface/piper_interface_v2.cpp" line_range="671" />
<code_context>
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))
</code_context>
<issue_to_address>
**Failed writes apply partial targets**
When a CAN send fails after an earlier joint frame or actuator target in the same write was accepted, `controlJoints()` can return false after earlier joint frames were sent, and `write()` sends the joints before the gripper. Returning `ERROR` and closing the session cannot undo those sends, so the arm receives a partial command instead of holding its last complete target, contrary to the README contract.
Make failed joint and gripper writes leave or restore the last complete target rather than relying on returning `ERROR` or closing the session.
Also at `piper_cpp_ros/src/piper_hardware_interface.cpp:508`, `piper_cpp_ros/src/piper_hardware_interface.cpp:511-513`, `piper_cpp_ros/src/piper_hardware_interface.cpp:516`, `piper_cpp_ros/src/piper_hardware_interface.cpp:518`, `piper_cpp_ros/README.md:248`.
</issue_to_address>
### Comment 2
<location path="piper_cpp_ros/include/piper_cpp_ros/command_conversion.hpp" line_range="36" />
<code_context>
+/// 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));
</code_context>
<issue_to_address>
**Out-of-range jaw targets are sent**
When a finite gripper command falls outside the configured 0–0.035 m joint limits but remains within ±1 m, `gripperCommandToMicrometres()` accepts the command, including negative positions, and `write()` sends the out-of-range jaw target instead of returning an error.
Validate gripper commands against the configured 0–0.035 m joint limits before converting them.
</issue_to_address>
### Comment 3
<location path="piper_cpp/test/piper_interface_v2_test.cpp" line_range="234-241" />
<code_context>
+ {
+ 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";
</code_context>
<issue_to_address>
**Interleave test can hang on receive error**
When `poll` or `recv` fails before all expected frames have been consumed while sender threads still have frames to write, the receiver breaks out of its loop, then joins both sender threads while their blocking socket writes can still be filling the unread socket buffer. The senders block once that buffer fills, so the joins never return and the test hangs instead of reporting the receive failure.
On receive failure, stop the senders or close the sending socket and ensure blocked writes are released before joining the threads.
</issue_to_address>
### Comment 4
<location path="piper_cpp/test/piper_interface_v2_test.cpp" line_range="234-236" />
<code_context>
+ {
+ 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;
+ }
</code_context>
<issue_to_address>
**Slow sends fail concurrency test**
When the sender threads are not scheduled or produce no frame for more than two seconds, `ConcurrentJointCommandsDoNotInterleave` marks `received_all` false after any two-second gap between frames, then fails at line 243 even if the correct sender threads resume and complete afterward. A heavily loaded runner can therefore fail this test without a command or interleaving defect.
Use a completion-aware wait or a more generous overall deadline so a temporary scheduling pause does not fail the test.
</issue_to_address>
### Comment 5
<location path="piper_cpp/test/piper_interface_v2_test.cpp" line_range="252-254" />
<code_context>
+ 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";
+ }
+}
</code_context>
<issue_to_address>
**Wrong targets pass concurrency test**
When `controlJoints()` sends the same constant value for every request, `ConcurrentJointCommandsDoNotInterleave` derives `target` from the first observed frame and compares only the other frames against it. If `controlJoints()` serializes the same default value for both callers instead of their requested targets, every comparison still passes.
Assert that each triple's target is one of the two requested values, as well as checking consistency across its frames.
</issue_to_address>Sourcery assessment
Approval pending. 5 findings to address first.
Blocking findings: piper_cpp/src/interface/piper_interface_v2.cpp:671, piper_cpp_ros/include/piper_cpp_ros/command_conversion.hpp:36, piper_cpp/test/piper_interface_v2_test.cpp:241, piper_cpp/test/piper_interface_v2_test.cpp:236, piper_cpp/test/piper_interface_v2_test.cpp:254
| 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_); |
There was a problem hiding this comment.
🟡 Medium · Failed writes apply partial targets
When a CAN send fails after an earlier joint frame or actuator target in the same write was accepted, controlJoints() can return false after earlier joint frames were sent, and write() sends the joints before the gripper. Returning ERROR and closing the session cannot undo those sends, so the arm receives a partial command instead of holding its last complete target, contrary to the README contract.
Make failed joint and gripper writes leave or restore the last complete target rather than relying on returning ERROR or closing the session.
Also at piper_cpp_ros/src/piper_hardware_interface.cpp:508, piper_cpp_ros/src/piper_hardware_interface.cpp:511-513, piper_cpp_ros/src/piper_hardware_interface.cpp:516, piper_cpp_ros/src/piper_hardware_interface.cpp:518, piper_cpp_ros/README.md:248.
Prompt for AI agents
In `piper_cpp/src/interface/piper_interface_v2.cpp` at line 671:
**Failed writes apply partial targets**
When a CAN send fails after an earlier joint frame or actuator target in the same write was accepted, `controlJoints()` can return false after earlier joint frames were sent, and `write()` sends the joints before the gripper. Returning `ERROR` and closing the session cannot undo those sends, so the arm receives a partial command instead of holding its last complete target, contrary to the README contract.
Make failed joint and gripper writes leave or restore the last complete target rather than relying on returning `ERROR` or closing the session.
Also at `piper_cpp_ros/src/piper_hardware_interface.cpp:508`, `piper_cpp_ros/src/piper_hardware_interface.cpp:511-513`, `piper_cpp_ros/src/piper_hardware_interface.cpp:516`, `piper_cpp_ros/src/piper_hardware_interface.cpp:518`, `piper_cpp_ros/README.md:248`.| /// is far beyond any jaw. | ||
| inline std::optional<int32_t> gripperCommandToMicrometres(double m) | ||
| { | ||
| if (!std::isfinite(m) || std::fabs(m) > 1.0) |
There was a problem hiding this comment.
🟡 Medium · Out-of-range jaw targets are sent
When a finite gripper command falls outside the configured 0–0.035 m joint limits but remains within ±1 m, gripperCommandToMicrometres() accepts the command, including negative positions, and write() sends the out-of-range jaw target instead of returning an error.
Validate gripper commands against the configured 0–0.035 m joint limits before converting them.
Prompt for AI agents
In `piper_cpp_ros/include/piper_cpp_ros/command_conversion.hpp` at line 36:
**Out-of-range jaw targets are sent**
When a finite gripper command falls outside the configured 0–0.035 m joint limits but remains within ±1 m, `gripperCommandToMicrometres()` accepts the command, including negative positions, and `write()` sends the out-of-range jaw target instead of returning an error.
Validate gripper commands against the configured 0–0.035 m joint limits before converting them.| 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(); |
There was a problem hiding this comment.
🟡 Medium · Interleave test can hang on receive error
When poll or recv fails before all expected frames have been consumed while sender threads still have frames to write, the receiver breaks out of its loop, then joins both sender threads while their blocking socket writes can still be filling the unread socket buffer. The senders block once that buffer fills, so the joins never return and the test hangs instead of reporting the receive failure.
On receive failure, stop the senders or close the sending socket and ensure blocked writes are released before joining the threads.
Prompt for AI agents
In `piper_cpp/test/piper_interface_v2_test.cpp` at lines 234-241:
**Interleave test can hang on receive error**
When `poll` or `recv` fails before all expected frames have been consumed while sender threads still have frames to write, the receiver breaks out of its loop, then joins both sender threads while their blocking socket writes can still be filling the unread socket buffer. The senders block once that buffer fills, so the joins never return and the test hangs instead of reporting the receive failure.
On receive failure, stop the senders or close the sending socket and ensure blocked writes are released before joining the threads.| if (poll(&pfd, 1, 2000) <= 0 || ::recv(bus.peer, &f, sizeof(f), 0) != static_cast<ssize_t>(sizeof(f))) | ||
| { | ||
| received_all = false; |
There was a problem hiding this comment.
🟡 Medium · Slow sends fail concurrency test
When the sender threads are not scheduled or produce no frame for more than two seconds, ConcurrentJointCommandsDoNotInterleave marks received_all false after any two-second gap between frames, then fails at line 243 even if the correct sender threads resume and complete afterward. A heavily loaded runner can therefore fail this test without a command or interleaving defect.
Use a completion-aware wait or a more generous overall deadline so a temporary scheduling pause does not fail the test.
Prompt for AI agents
In `piper_cpp/test/piper_interface_v2_test.cpp` at lines 234-236:
**Slow sends fail concurrency test**
When the sender threads are not scheduled or produce no frame for more than two seconds, `ConcurrentJointCommandsDoNotInterleave` marks `received_all` false after any two-second gap between frames, then fails at line 243 even if the correct sender threads resume and complete afterward. A heavily loaded runner can therefore fail this test without a command or interleaving defect.
Use a completion-aware wait or a more generous overall deadline so a temporary scheduling pause does not fail the test.| 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"; |
There was a problem hiding this comment.
🟡 Medium · Wrong targets pass concurrency test
When controlJoints() sends the same constant value for every request, ConcurrentJointCommandsDoNotInterleave derives target from the first observed frame and compares only the other frames against it. If controlJoints() serializes the same default value for both callers instead of their requested targets, every comparison still passes.
Assert that each triple's target is one of the two requested values, as well as checking consistency across its frames.
Prompt for AI agents
In `piper_cpp/test/piper_interface_v2_test.cpp` at lines 252-254:
**Wrong targets pass concurrency test**
When `controlJoints()` sends the same constant value for every request, `ConcurrentJointCommandsDoNotInterleave` derives `target` from the first observed frame and compares only the other frames against it. If `controlJoints()` serializes the same default value for both callers instead of their requested targets, every comparison still passes.
Assert that each triple's target is one of the two requested values, as well as checking consistency across its frames.
Same six commits as #9, which was reviewed and merged but landed on the freshness-snapshots
branch instead of main. Cherry-picked onto main; the resulting tree is identical.
callers cannot interleave into a target the arm never received from either.
fails. on_activate() checks its sends too.
controller manager then stops the controllers; the arm holds its last target.
Conversions live in piper_cpp_ros/command_conversion.hpp with an ament_cmake_gtest target.
Summary by Sourcery
Harden the ROS hardware interface by propagating feedback, validation, and CAN transmission errors while preserving safe arm behavior and recovery.
New Features:
Bug Fixes:
Enhancements:
Documentation:
Tests: