cheng
48 lines
2.4 KiB
Python
48 lines
2.4 KiB
Python
"""Compile/run the actual hardware guard without ROS or a robot connection."""
|
|
import subprocess
|
|
import tempfile
|
|
from pathlib import Path
|
|
from unittest import TestCase
|
|
|
|
class PositionCommandFenceTest(TestCase):
|
|
def test_reject_old_targets_after_enable_and_native_movel(self):
|
|
root = Path(__file__).resolve().parents[2]
|
|
include = root / 'src/flexiv_ros2/flexiv_hardware/include'
|
|
code = r"""
|
|
#include <cassert>
|
|
#include <limits>
|
|
#include "flexiv_hardware/position_command_fence.hpp"
|
|
int main() {
|
|
flexiv_hardware::MotionExecutionLatch latch;
|
|
assert(!latch.observe(true, true, false)); // not activated
|
|
latch.activate(); assert(latch.observe(true, true, false));
|
|
assert(!latch.observe(true, false, false)); // physical E-stop
|
|
for (int i=0; i<2000; ++i) assert(!latch.observe(true, true, false)); // Enable cannot resume
|
|
latch.activate(); assert(latch.observe(true, true, false));
|
|
assert(!latch.observe(true, true, true)); // fault
|
|
assert(!latch.observe(true, true, false)); // clear fault cannot resume
|
|
latch.activate(); assert(!latch.observe(false, true, false)); // disconnect
|
|
assert(!latch.observe(true, true, false));
|
|
flexiv_hardware::PositionCommandFence fence;
|
|
std::vector<double> current(7, .74), old(7, 0.);
|
|
for (int i=0; i<2000; ++i) assert(!fence.accept(old, current));
|
|
assert(!fence.accept({}, current));
|
|
auto invalid = current; invalid[0] = std::numeric_limits<double>::quiet_NaN();
|
|
assert(!fence.accept(invalid, current));
|
|
assert(fence.accept(current, current));
|
|
assert(fence.accept(old, current)); // normal trajectory after handshake
|
|
fence.reset(); // native MoveL / controller activation
|
|
for (int i=0; i<2000; ++i) assert(!fence.accept(old, current));
|
|
auto start = current; start[0] += .0001;
|
|
assert(fence.accept(start, current));
|
|
assert(fence.accept(old, current));
|
|
}
|
|
"""
|
|
with tempfile.TemporaryDirectory() as folder:
|
|
source = Path(folder)/'fence.cpp'; binary = Path(folder)/'fence'
|
|
source.write_text(code)
|
|
built = subprocess.run(['g++','-std=c++17','-I',str(include),str(source),'-o',str(binary)],capture_output=True,text=True,timeout=30)
|
|
self.assertEqual(built.returncode,0,built.stderr)
|
|
result = subprocess.run([str(binary)],capture_output=True,text=True,timeout=5)
|
|
self.assertEqual(result.returncode,0,result.stderr)
|