diff --git a/doc/fastcat_device_config_parameters.md b/doc/fastcat_device_config_parameters.md index 5a4fd8a..b64b2cc 100644 --- a/doc/fastcat_device_config_parameters.md +++ b/doc/fastcat_device_config_parameters.md @@ -55,6 +55,7 @@ For every `JSD Device` there is an `Offline Device` to emulate the behavior of t | `zero_latency_required` | Controls Manager reaction if circular `Signal` dependencies exist | bool | True | | `actuator_position_directory` | Parent directory of actuator saved position file | string | /tmp/ | | `actuator_fault_on_missing_pos_file` | If true, Fastcat will fail during initialization if a saved pos file does not exist | bool | True | +| `actuator_position_save_settle_sec` | *Optional.* How long all brakes must stay engaged before saved positions are considered settled | double | 0.5 | #### target_loop_rate_hz @@ -68,9 +69,31 @@ During initialization, devices will be reordered by the manager to ensure that t To work seamlessly with actuators, the manager may need to cache the last known position of actuators if they are using any non-absolute position sensor (e.g. incremental encoder, Hall-effect sensor) The Manager will look inside the `actuator_position_directory` for a pre-existing `fastcat_saved_positions.yaml` file to restore position from this file. +The manager maintains this file over the life of the application: + +* Whenever the motors are powered (any actuator reporting `motor_on`), the file is **deleted**. An actuator that is powered may move, so a position captured before the move is no longer trustworthy, and no file at all is safer than a wrong one. +* Once every actuator's brake is engaged again, and has stayed engaged for `actuator_position_save_settle_sec`, the current positions are written back out. +* `Manager::Shutdown()` writes the positions synchronously if the brakes are engaged. If it is called mid-motion the file is left deleted instead — an application that is killed or crashes during a move will therefore find no position file on the next startup. + +Positions are written with an atomic replace (write to a temp file, `fsync`, `rename`), so a reader or a crash never observes a partially written file. The disk I/O runs on a dedicated background thread and never blocks the `Process()` loop. The previous contents are copied to `fastcat_saved_positions_prev.yaml` when a save overwrites an existing file; note that because the file is deleted at the start of every motion, this backup is only produced by a save that was not preceded by motion. + +Topologies with no actuators, or whose actuators all use absolute encoders, bypass this file entirely — they neither read, write, nor delete it, so it is safe for such a topology to share an `actuator_position_directory` with one that does use it. + #### actuator_fault_on_missing_pos_file -The `fastcat_saved_positions.yaml` may not exist for any number of reasons. If this file does not exist, this parameter controls how the manager reacts. if True, then the manager will fault and not initialize. if False, the assumed startup position for all actuators is `0` and when a new `fastcat_saved_positions.yaml` fill will be created when the manager is shutdown by the application. +The `fastcat_saved_positions.yaml` may not exist for any number of reasons — a first-ever run, or an application that was killed mid-motion (see above). If this file does not exist, this parameter controls how the manager reacts. If True, then the manager will fault and not initialize. If False, the assumed startup position for all actuators is `0` and a new `fastcat_saved_positions.yaml` will be created the next time the actuators come to rest. + +Setting this False is intended for demos and testing. Running actuators in production with an assumed startup position of `0` will drive them to the wrong absolute positions. + +#### actuator_position_save_settle_sec + +Optional; defaults to `0.5`. Omitting it is fine and is the recommended configuration. + +Saved positions are captured once all actuators report their brakes engaged (`motor_on == 0`). On a controlled halt the drive decelerates and sets the brake *before* removing power, so the position is already at rest when this is observed. But on an STO, e-stop, or fault, power is cut while the joint is still moving and the joint then coasts to a stop against its brake — capturing on that first brakes-engaged cycle would persist a mid-travel position, and because saving is edge-triggered it would never be corrected. + +This parameter is how long the brakes must stay *continuously* engaged before the positions are treated as settled and written. It should comfortably exceed the worst-case time for a joint to come to rest against its brake after an uncontrolled power cut. Any actuator powering back up resets the window. + +Set it to `0` to save on the first brakes-engaged cycle, which is how Fastcat behaved before this parameter existed. Negative values are rejected during configuration. #### Examples @@ -81,6 +104,7 @@ fastcat: zero_latency_required: True # Always actuator_position_directory: /cal/ # or any other global location on your filesystem actuator_fault_on_missing_pos_file: True # Online - True, Offline - False + actuator_position_save_settle_sec: 0.5 # Optional; omit to accept the 0.5 default ``` ``` yaml @@ -90,6 +114,7 @@ fastcat: zero_latency_required: True # Always actuator_position_directory: /tmp/ # Recommended this is different from the Online path actuator_fault_on_missing_pos_file: False # Let Fastcat Create this for us! + actuator_position_save_settle_sec: 0 # Offline devices stop instantly; no need to wait ``` --- diff --git a/src/manager.cc b/src/manager.cc index 92bbf22..8e8d165 100644 --- a/src/manager.cc +++ b/src/manager.cc @@ -3,13 +3,16 @@ // Include c then c++ libraries #include +#include #include #include #include +#include #include #include #include +#include #include #include @@ -75,6 +78,14 @@ #include "jsd/jsd_sdo_pub.h" #include "jsd/jsd_time.h" +namespace +{ +// Permissions applied to the saved-position files. Previously achieved with a +// process-wide umask(000); applied per-file now so the host application's umask +// is left alone. +constexpr mode_t kPosFileMode = 0666; +} // namespace + fastcat::Manager::Manager() { cmd_queue_ = std::make_shared>(); @@ -83,6 +94,10 @@ fastcat::Manager::Manager() fastcat::Manager::~Manager() { + // Stop the background position writer (drains any final queued request) before + // tearing down the bus. + StopPosWriter(); + for (auto it = jsd_map_.begin(); it != jsd_map_.end(); ++it) { if (it->second != NULL) { jsd_free(it->second); @@ -93,8 +108,57 @@ fastcat::Manager::~Manager() void fastcat::Manager::Shutdown() { - GetActuatorPositions(); - SaveActuatorPosFile(); + SaveActuatorPositions(); +} + +void fastcat::Manager::SaveActuatorPositions() +{ + // Topologies that do not persist positions (no actuators, or only + // absolute-encoder actuators) bypass the position file entirely -- see + // LoadActuatorPosFile(). Without this guard AllBrakesEngaged() would report + // false (it finds no relevant actuator) and we would *delete* a position file + // that this topology never owned in the first place. + if (!pos_file_enabled_) { + return; + } + + // Snapshot the positions AND post the request to the writer while holding + // parameter_mutex_, which serializes against Process(). Posting under the same + // lock that UpdatePositionFileOnBrakeState() runs under is what keeps request + // ordering consistent with the brake edges: if the lock were released before + // posting, the RT loop could observe motion starting and post its invalidate + // first, and our later write would supersede it -- leaving a file full of + // pre-motion positions while the arm is actually moving. + // + // Posting is cheap (it only touches the writer's mailbox mutex, never the + // disk). Only the wait is deferred until after parameter_mutex_ is released, + // because Process() contends on that mutex and holding it across an + // fsync-bound wait would stall the RT loop for the full duration of the + // disk write. + uint64_t seq = 0; + { + std::lock_guard lock(parameter_mutex_); + + // Only persist positions when the arm is safely stopped (all brakes + // engaged). If this save is requested while the arm is in motion (e.g. a + // shutdown that interrupts a move), writing would capture in-motion + // positions. Instead, leave the file invalidated so a stale/wrong position + // can never be loaded -- it is safer to have no saved positions than + // incorrect ones. + if (AllBrakesEngaged()) { + GetActuatorPositions(); + seq = PostPosWriteRequest(BuildActuatorPosYaml(), /*wait=*/false); + } else { + WARNING( + "SaveActuatorPositions requested while not all brakes are engaged; " + "skipping save and leaving position file invalidated"); + seq = PostPosInvalidateRequest(/*wait=*/false); + } + } + + // Wait for the writer to drain our request so the file state is durable before + // a caller on the shutdown path exits the process. + WaitForPosWriter(seq); } bool fastcat::Manager::CreateConfigFromYaml(const YAML::Node& node, @@ -125,6 +189,19 @@ bool fastcat::Manager::CreateConfigFromYaml(const YAML::Node& node, return false; } + // Optional: how long all brakes must stay continuously engaged before the + // positions are considered settled and worth saving. Defaults to a value that + // comfortably covers a joint coasting to rest against its brake after an + // uncontrolled power cut (STO/e-stop/fault). Set to 0 to save on the first + // brakes-engaged cycle, as fastcat did before the debounce was added. + ParseOptVal(fastcat_node, "actuator_position_save_settle_sec", + pos_save_settle_sec_); + if (pos_save_settle_sec_ < 0.0) { + ERROR("actuator_position_save_settle_sec must be >= 0, got %lf", + pos_save_settle_sec_); + return false; + } + // Configure Buses YAML::Node buses_node; if (!ParseList(node, "buses", buses_node)) { @@ -248,6 +325,11 @@ bool fastcat::Manager::InitHardware() // attempt to start in nominal, post-reset state. this->ExecuteAllDeviceResets(); + // Start the background position-file writer now that the bus is up. All + // position saves/invalidations are handed to this thread so disk I/O never + // runs on the RT Process() thread. + StartPosWriter(); + return true; } @@ -353,6 +435,10 @@ bool fastcat::Manager::Process(double external_time) } } + // Persist actuator positions when the arm settles (all brakes engaged) and + // invalidate the saved file when motion resumes. Runs under parameter_mutex_. + UpdatePositionFileOnBrakeState(monotonic_time); + return !faulted_; } @@ -1038,6 +1124,11 @@ bool fastcat::Manager::PopSdoResponseQueue(SdoResponse& res) return true; } +std::string fastcat::Manager::PosFilePath() const +{ + return actuator_position_directory_ + "/fastcat_saved_positions.yaml"; +} + bool fastcat::Manager::LoadActuatorPosFile() { // Look for the existence of at least one actuator in the topology @@ -1075,6 +1166,10 @@ bool fastcat::Manager::LoadActuatorPosFile() return true; } + // Past the bypass checks: this topology owns the position file, so the + // brake-edge save and the invalidate-on-motion paths are live from here on. + pos_file_enabled_ = true; + if (!actuator_fault_on_missing_pos_file_) { WARNING("YAML parameter \'actuator_fault_on_missing_pos_file\' is FALSE"); WARNING("\tThis setting is intended for demo and testing and should"); @@ -1085,23 +1180,30 @@ bool fastcat::Manager::LoadActuatorPosFile() ERROR("actuator_position_directory is empty, check the YAML parameter"); return false; } - std::string pos_file = - actuator_position_directory_ + "/fastcat_saved_positions.yaml"; + std::string pos_file = PosFilePath(); struct stat st; if (0 != stat(pos_file.c_str(), &st)) { if (actuator_fault_on_missing_pos_file_) { - ERROR("Failed to open pos file: %s", strerror(errno)); + ERROR("Failed to open pos file: %s (%s)", pos_file.c_str(), + strerror(errno)); return false; } else { - WARNING("Continuing without pos file: %s", strerror(errno)); + WARNING("Continuing without pos file: %s (%s)", pos_file.c_str(), + strerror(errno)); return true; } } MSG_DEBUG("Opening Pos File: %s", pos_file.c_str()); - YAML::Node node = YAML::LoadFile(pos_file); + YAML::Node node; + try { + node = YAML::LoadFile(pos_file); + } catch (const YAML::Exception& e) { + ERROR("Malformed pos file %s: %s", pos_file.c_str(), e.what()); + return false; + } if (!node) { ERROR("Could not parse pos file YAML: %s", pos_file.c_str()); return false; @@ -1109,6 +1211,17 @@ bool fastcat::Manager::LoadActuatorPosFile() YAML::Node actuators_node; if (!ParseNode(node, "actuators", actuators_node)) { + ERROR("Malformed pos file %s: missing the \'actuators\' node", + pos_file.c_str()); + return false; + } + + // A bare 'actuators:' with nothing under it parses cleanly but yields zero + // entries. Caught here so the operator is told which file is at fault, rather + // than seeing a per-actuator "Missing startup position" error later on. + if (!actuators_node.IsSequence() || actuators_node.size() == 0) { + ERROR("Malformed pos file %s: \'actuators\' has no entries", + pos_file.c_str()); return false; } @@ -1116,11 +1229,15 @@ bool fastcat::Manager::LoadActuatorPosFile() ++act_node) { std::string name; if (!ParseVal(*act_node, "actuator_name", name)) { + ERROR("Malformed pos file %s: entry is missing \'actuator_name\'", + pos_file.c_str()); return false; } ActuatorPosData act_pos_data; if (!ParseVal(*act_node, "position", act_pos_data.position)) { + ERROR("Malformed pos file %s: entry for %s is missing \'position\'", + pos_file.c_str(), name.c_str()); return false; } @@ -1146,8 +1263,8 @@ bool fastcat::Manager::ValidateActuatorPosFile() auto find_pair = device_map_.find(saved_pos_entry->first); if (find_pair == device_map_.end()) { - WARNING("Unused saved position entry found for: %s", - saved_pos_entry->first.c_str()); + WARNING("Unused saved position entry found for %s in pos file %s", + saved_pos_entry->first.c_str(), PosFilePath().c_str()); } } @@ -1176,15 +1293,16 @@ bool fastcat::Manager::ValidateActuatorPosFile() if (find_pos_data == actuator_pos_map_.end()) { if (!actuator_fault_on_missing_pos_file_) { WARNING( - "Missing Startup position for %s, setting starting position to " - "zero", - dev_name.c_str()); + "Missing startup position for %s in pos file %s, setting starting " + "position to zero", + dev_name.c_str(), PosFilePath().c_str()); ActuatorPosData apd = {0}; actuator_pos_map_[dev_name] = apd; } else { - ERROR("Missing startup position for %s", dev_name.c_str()); + ERROR("Missing startup position for %s in pos file %s", dev_name.c_str(), + PosFilePath().c_str()); return false; } } @@ -1259,19 +1377,12 @@ void fastcat::Manager::GetActuatorPositions() } } -void fastcat::Manager::SaveActuatorPosFile() +std::string fastcat::Manager::BuildActuatorPosYaml() { - std::string prev_pos_file = - actuator_position_directory_ + "/fastcat_saved_positions_prev.yaml"; - std::string pos_file = - actuator_position_directory_ + "/fastcat_saved_positions.yaml"; - - MSG("Renaming %s -> %s", pos_file.c_str(), prev_pos_file.c_str()); - - if (0 != rename(pos_file.c_str(), prev_pos_file.c_str())) { - WARNING("Could not move: %s, file may not exist", pos_file.c_str()); - } - + // Pure serialization of the current actuator_pos_map_. Runs on the RT thread + // under parameter_mutex_ (cheap: no syscalls), so the returned string is a + // consistent snapshot that the background writer can persist without any + // access to shared device state. YAML::Node file_node; for (auto pos_pair = actuator_pos_map_.begin(); pos_pair != actuator_pos_map_.end(); ++pos_pair) { @@ -1280,14 +1391,391 @@ void fastcat::Manager::SaveActuatorPosFile() act_node["position"] = pos_pair->second.position; file_node["actuators"].push_back(act_node); } + std::stringstream ss; + ss << file_node; + return ss.str(); +} + +void fastcat::Manager::WritePosFileToDisk(const std::string& contents) +{ + // ALL disk I/O for the position file happens here, on the background writer + // thread only. Never call this from the RT Process() thread. + std::string prev_pos_file = + actuator_position_directory_ + "/fastcat_saved_positions_prev.yaml"; + std::string pos_file = PosFilePath(); + // Temp file lives in the SAME directory as pos_file so the final rename is a + // same-filesystem atomic replace. + std::string tmp_pos_file = + actuator_position_directory_ + "/fastcat_saved_positions.yaml.tmp"; + + // Write the new positions to a temp file, flush it all the way to disk, and + // only then atomically rename it over the canonical file. This guarantees + // that fastcat_saved_positions.yaml is never partially written: at any + // instant a reader (or a crash) sees either the complete old file or the + // complete new one, never a truncated/empty file. This is the crash-safe + // replacement for the previous rename-then-write scheme, which could leave + // no canonical file at all if killed between the rename and the write. + // + // Permissions are applied per-file with chmod() below rather than by clearing + // the process umask: umask is process-wide (NOT per-thread), so setting it + // here would silently make every file the host application creates from now + // on world-writable. + { + std::ofstream file(tmp_pos_file, std::ios::out | std::ios::trunc); + if (!file) { + ERROR("Could not open temp pos file for writing: %s (%s); leaving %s " + "untouched", + tmp_pos_file.c_str(), strerror(errno), pos_file.c_str()); + return; + } + file << contents; + file.flush(); + if (!file) { + ERROR("Failed while writing temp pos file: %s; leaving %s untouched", + tmp_pos_file.c_str(), pos_file.c_str()); + file.close(); + remove(tmp_pos_file.c_str()); + return; + } + file.close(); + } + + // Make the position file group/world writable so it is not owned exclusively + // by whichever user happened to run this process first. Done on the temp file + // before the rename: rename() preserves the inode, so the canonical file lands + // with these permissions already in place and is never briefly unwritable. + if (0 != chmod(tmp_pos_file.c_str(), kPosFileMode)) { + WARNING("Could not chmod %s: %s", tmp_pos_file.c_str(), strerror(errno)); + } + + // fsync the temp file's contents to durable storage before the rename. + int fd = open(tmp_pos_file.c_str(), O_RDONLY); + if (fd >= 0) { + if (0 != fsync(fd)) { + WARNING("fsync failed on %s: %s", tmp_pos_file.c_str(), strerror(errno)); + } + close(fd); + } else { + WARNING("Could not reopen %s to fsync: %s", tmp_pos_file.c_str(), + strerror(errno)); + } + + // Keep a backup of the last-known-good canonical file WITHOUT removing it. + // A plain copy (not rename) means the canonical file always continues to + // exist right up until the atomic replace below. + struct stat st; + if (0 == stat(pos_file.c_str(), &st)) { + std::ifstream src(pos_file, std::ios::binary); + std::ofstream dst(prev_pos_file, std::ios::binary | std::ios::trunc); + if (src && dst) { + dst << src.rdbuf(); + dst.close(); + if (0 != chmod(prev_pos_file.c_str(), kPosFileMode)) { + WARNING("Could not chmod %s: %s", prev_pos_file.c_str(), + strerror(errno)); + } + } else { + WARNING("Could not back up %s -> %s", pos_file.c_str(), + prev_pos_file.c_str()); + } + } + + // Atomic replace: this is the only operation that touches the canonical file. + if (0 != rename(tmp_pos_file.c_str(), pos_file.c_str())) { + ERROR("Atomic rename %s -> %s failed: %s; canonical file left unchanged", + tmp_pos_file.c_str(), pos_file.c_str(), strerror(errno)); + remove(tmp_pos_file.c_str()); + return; + } + + // fsync the CONTAINING DIRECTORY, not just the file. fsync on the temp file + // above only makes its *contents* durable; the rename is a directory-metadata + // operation, so without this a power loss can leave the directory entry still + // pointing at the old file (or at neither) even though the new data is safely + // on the platter. Both fsyncs are required for the atomic-replace guarantee to + // survive an actual power cut. + SyncPosFileDirectory(); - umask(000); - std::ofstream file(pos_file, std::ios::out); - file << file_node; - file.close(); MSG("Successfully wrote Pos File: %s", pos_file.c_str()); } +void fastcat::Manager::StartPosWriter() +{ + if (pos_writer_running_) { + return; + } + { + std::lock_guard lock(pos_writer_mutex_); + pos_writer_stop_ = false; + } + pos_writer_thread_ = std::thread(&Manager::PosWriterLoop, this); + pos_writer_running_ = true; +} + +void fastcat::Manager::StopPosWriter() +{ + if (!pos_writer_running_) { + return; + } + { + std::lock_guard lock(pos_writer_mutex_); + pos_writer_stop_ = true; + } + pos_writer_cv_.notify_one(); + if (pos_writer_thread_.joinable()) { + pos_writer_thread_.join(); + } + pos_writer_running_ = false; +} + +void fastcat::Manager::PosWriterLoop() +{ + // NOTE: deliberately does not touch umask -- umask is process-wide, not + // per-thread, so clearing it here would leak world-writable defaults into + // every file the host application creates. WritePosFileToDisk() chmods the + // files it creates instead. + std::unique_lock lock(pos_writer_mutex_); + while (true) { + pos_writer_cv_.wait(lock, [this]() { + return pos_writer_stop_ || pos_pending_write_ || pos_pending_invalidate_; + }); + + // Snapshot the pending request and clear it while holding the lock. + bool do_write = pos_pending_write_; + bool do_invalidate = pos_pending_invalidate_; + std::string contents = std::move(pos_pending_contents_); + uint64_t handling_seq = pos_request_seq_; + pos_pending_write_ = false; + pos_pending_invalidate_ = false; + pos_pending_contents_.clear(); + + if (do_write || do_invalidate) { + // Release the mailbox lock during disk I/O so the RT thread can post new + // requests without ever blocking on the disk. + lock.unlock(); + // Invalidate wins if both were somehow set (matches "in motion => no + // file"): a later invalidate must not be overridden by a stale write. + if (do_invalidate) { + InvalidateActuatorPosFile(); + } else { + WritePosFileToDisk(contents); + } + lock.lock(); + pos_processed_seq_ = handling_seq; + pos_writer_done_cv_.notify_all(); + } + + if (pos_writer_stop_ && !pos_pending_write_ && !pos_pending_invalidate_) { + break; + } + } +} + +uint64_t fastcat::Manager::PostPosWriteRequest(std::string contents, bool wait) +{ + // Fallback: if the writer thread is not running, do the write inline. This + // only happens off the RT loop (e.g. teardown before StartPosWriter), so + // blocking here is acceptable and avoids waiting forever on a request nobody + // would drain. + if (!pos_writer_running_) { + WritePosFileToDisk(contents); + return 0; + } + + uint64_t seq; + { + std::lock_guard lock(pos_writer_mutex_); + pos_pending_contents_ = std::move(contents); + pos_pending_write_ = true; + pos_pending_invalidate_ = false; // a fresh write supersedes a pending inval + seq = ++pos_request_seq_; + } + pos_writer_cv_.notify_one(); + + if (wait) { + WaitForPosWriter(seq); + } + return seq; +} + +uint64_t fastcat::Manager::PostPosInvalidateRequest(bool wait) +{ + if (!pos_writer_running_) { + InvalidateActuatorPosFile(); + return 0; + } + + uint64_t seq; + { + std::lock_guard lock(pos_writer_mutex_); + pos_pending_invalidate_ = true; + pos_pending_write_ = false; // an invalidate supersedes a pending write + pos_pending_contents_.clear(); + seq = ++pos_request_seq_; + } + pos_writer_cv_.notify_one(); + + if (wait) { + WaitForPosWriter(seq); + } + return seq; +} + +void fastcat::Manager::WaitForPosWriter(uint64_t seq) +{ + // seq == 0 means the request was already serviced inline (writer not running), + // so there is nothing to wait for. + if (seq == 0 || !pos_writer_running_) { + return; + } + std::unique_lock lock(pos_writer_mutex_); + pos_writer_done_cv_.wait( + lock, [this, seq]() { return pos_processed_seq_ >= seq; }); +} + +void fastcat::Manager::InvalidateActuatorPosFile() +{ + // Remove the canonical position file so that a subsequent startup does not + // load stale positions. The _prev.yaml backup (from the last successful save) + // is intentionally left in place for debugging. With + // actuator_fault_on_missing_pos_file_ == true the next startup will fault + // rather than silently trust an out-of-date file, which is the desired + // "no positions is better than wrong positions" behavior. + std::string pos_file = PosFilePath(); + + struct stat st; + if (0 != stat(pos_file.c_str(), &st)) { + // Already absent; nothing to do. + return; + } + + if (0 != remove(pos_file.c_str())) { + WARNING("Could not invalidate (remove) pos file %s: %s", pos_file.c_str(), + strerror(errno)); + } else { + // The unlink is a directory-metadata operation, so it needs the same + // directory fsync as the rename in WritePosFileToDisk(). Without it a power + // cut can resurrect the file we just invalidated, which is precisely the + // stale-position load this function exists to prevent. + SyncPosFileDirectory(); + MSG("Invalidated saved position file (motion started): %s", + pos_file.c_str()); + } +} + +void fastcat::Manager::SyncPosFileDirectory() +{ + // Flush the position directory's entries to durable storage, making the most + // recent rename/unlink survive a power loss. Runs on the writer thread only. + int dir_fd = open(actuator_position_directory_.c_str(), O_RDONLY); + if (dir_fd < 0) { + WARNING("Could not open %s to fsync directory: %s", + actuator_position_directory_.c_str(), strerror(errno)); + return; + } + if (0 != fsync(dir_fd)) { + WARNING("fsync failed on directory %s: %s", + actuator_position_directory_.c_str(), strerror(errno)); + } + close(dir_fd); +} + +bool fastcat::Manager::AllBrakesEngaged() +{ + bool found_actuator = false; + for (auto device = jsd_device_list_.begin(); device != jsd_device_list_.end(); + ++device) { + std::shared_ptr dev_state = (*device)->GetState(); + + if (dev_state->type != GOLD_ACTUATOR_STATE && + dev_state->type != PLATINUM_ACTUATOR_STATE) { + continue; + } + + std::shared_ptr actuator = + std::dynamic_pointer_cast(*device); + + // Absolute-encoder actuators are not persisted, so their motion does not + // affect whether we should save the (incremental-encoder) positions. + if (actuator->HasAbsoluteEncoder()) { + continue; + } + + found_actuator = true; + + // motor_on == 0 means the motor is unpowered and the (spring-applied) brake + // is engaged. Any powered/moving actuator means brakes are not all engaged. + uint8_t motor_on = (dev_state->type == GOLD_ACTUATOR_STATE) + ? dev_state->gold_actuator_state.motor_on + : dev_state->platinum_actuator_state.motor_on; + if (motor_on != 0) { + return false; + } + } + + return found_actuator; +} + +void fastcat::Manager::UpdatePositionFileOnBrakeState(double monotonic_time) +{ + // Called at the end of Process() while parameter_mutex_ is held. + + // Topologies that bypass the position file (no actuators, or only + // absolute-encoder actuators) must not touch it. AllBrakesEngaged() returns + // false for them, which would otherwise look like a falling edge and delete a + // file this topology does not own. + if (!pos_file_enabled_) { + return; + } + + bool all_engaged = AllBrakesEngaged(); + + // Track how long the brakes have been continuously engaged. motor_on drops to + // zero the instant drive power is removed, which on a controlled halt happens + // only after the drive has decelerated and set the brake -- but on an STO, + // e-stop or fault the power is cut while the joint is still moving, and the + // joint then coasts to a stop against the brake. Saving on that first + // brakes-engaged cycle would persist a mid-travel position, and since the edge + // has already been consumed it would never be corrected. Requiring the brakes + // to stay engaged for pos_save_settle_sec_ lets the joint come to rest first. + if (!all_engaged) { + brakes_engaged_since_ = -1.0; + // Motors are powered, so the arm may move: the on-disk file is stale and the + // next settled stop must produce a fresh save. + saw_motion_since_last_save_ = true; + } else if (brakes_engaged_since_ < 0.0 || + monotonic_time < brakes_engaged_since_) { + // Newly engaged, or time ran backwards (an application supplying its own + // external_time may reset it) -- restart the settling window. + brakes_engaged_since_ = monotonic_time; + } + + bool settled = + all_engaged && + (monotonic_time - brakes_engaged_since_) >= pos_save_settle_sec_; + + // saw_motion_since_last_save_ (rather than an edge on `settled`) is what makes + // this fire exactly once per motion->stop cycle. It also means a bus that comes + // up already stopped does not re-save positions it just loaded. + if (settled && saw_motion_since_last_save_) { + // Motion has stopped and the arm has had time to settle -> persist. + // Serialize the snapshot here (cheap, under the RT lock) and hand the disk + // write to the background writer so the RT loop never blocks on I/O. + MSG("All actuator brakes engaged and settled; saving actuator positions"); + GetActuatorPositions(); + PostPosWriteRequest(BuildActuatorPosYaml(), /*wait=*/false); + saw_motion_since_last_save_ = false; + } else if (!all_engaged && prev_all_brakes_engaged_) { + // Falling edge: motion just started -> invalidate the saved file so a stale + // position can never be loaded if we are interrupted before the next stop. + // Deferred to the writer thread (it does the stat/remove). Note this is NOT + // debounced: invalidation must happen as early as possible. + PostPosInvalidateRequest(/*wait=*/false); + } + + prev_all_brakes_engaged_ = all_engaged; +} + bool fastcat::Manager::CheckDeviceNameIsUnique(std::string name) { if (unique_device_map_.end() != unique_device_map_.find(name)) { diff --git a/src/manager.h b/src/manager.h index 21b163e..10f735a 100644 --- a/src/manager.h +++ b/src/manager.h @@ -4,9 +4,14 @@ // Include related header (for cc files) // Include c then c++ libraries +#include +#include +#include #include #include #include +#include +#include #include #include @@ -40,6 +45,26 @@ class Manager */ void Shutdown(); + /** @brief Capture current actuator positions and write them to file. + * + * Intended for callers that want to persist positions without tearing down + * the bus. The write itself is crash-safe (atomic rename), and the positions + * are snapshotted under the same internal mutex used by Process(), so this is + * safe to call from any thread while the process loop is still running. The + * snapshot is taken under that mutex but the disk write is not, so the RT loop + * is not blocked on I/O. + * + * Must NOT be called from a signal handler: it takes a mutex and allocates, + * neither of which is async-signal-safe, and if the signal is delivered on the + * thread already inside Process() it will self-deadlock on that mutex. Set a + * flag in the handler and call this from your normal control flow instead. + * + * No-op for topologies that do not persist positions (no actuators, or only + * actuators with absolute encoders). + * @return void + */ + void SaveActuatorPositions(); + /** @brief Method that accepts a fastcat topology yaml and intializes bus * * @return true on successful initialization. If false, application should @@ -236,9 +261,48 @@ class Manager bool LoadActuatorPosFile(); bool ValidateActuatorPosFile(); + // Full path of the saved positions file, so every message about it can name + // the file the operator has to go look at. + std::string PosFilePath() const; bool SetActuatorPositions(); void GetActuatorPositions(); - void SaveActuatorPosFile(); + // Serialize actuator_pos_map_ to a YAML string. Cheap, pure CPU; called on + // the RT thread under parameter_mutex_ so it sees a consistent snapshot. + std::string BuildActuatorPosYaml(); + // Perform the actual disk write (temp file + fsync + _prev backup + atomic + // rename) for the already-serialized `contents`. Runs ONLY on the background + // writer thread; touches no shared device state, takes no RT lock. + void WritePosFileToDisk(const std::string& contents); + void InvalidateActuatorPosFile(); + // fsync the position directory so the last rename/unlink is durable across a + // power loss. Runs ONLY on the background writer thread. + void SyncPosFileDirectory(); + // Background position-file writer: keeps all disk I/O (fsync, rename, backup + // copy) off the RT Process() thread so a save cannot cause a cycle slip. + void StartPosWriter(); + void StopPosWriter(); + void PosWriterLoop(); + // Post a write (contents) or an invalidate request to the writer thread. If + // `wait` is true, block until the writer has processed it (used on shutdown + // to guarantee durability before exit). Both return the request sequence + // number, which can be handed to WaitForPosWriter() later to defer the block + // until after a caller-held lock is released; 0 means the request was already + // serviced inline because the writer thread is not running. + uint64_t PostPosWriteRequest(std::string contents, bool wait); + uint64_t PostPosInvalidateRequest(bool wait); + // Block until the writer has drained request `seq`. Never call while holding + // parameter_mutex_: the wait is fsync-bound and Process() contends on it. + void WaitForPosWriter(uint64_t seq); + // True iff every GOLD/PLATINUM actuator has its brake engaged (motor_on == 0, + // i.e. unpowered and mechanically held). Actuators with absolute encoders are + // ignored (their positions are not persisted). Returns false if there are no + // relevant actuators. + bool AllBrakesEngaged(); + // Called at the end of each Process() cycle (under parameter_mutex_). Once + // AllBrakesEngaged() has held for pos_save_settle_sec_ it saves the current + // positions; on the falling edge (motion starting) it immediately invalidates + // the saved file so a stale in-motion position can never be loaded. + void UpdatePositionFileOnBrakeState(double monotonic_time); bool CheckDeviceNameIsUnique(std::string name); struct JsdBusInitParams { std::string ifname; @@ -267,6 +331,57 @@ class Manager std::mutex parameter_mutex_; + // Falling-edge tracking for the invalidate-on-motion path. Starts true so a + // bus that comes up already stopped is not treated as a transition. + bool prev_all_brakes_engaged_ = true; + + // Set whenever the motors are powered (brakes not all engaged), cleared once a + // save completes. Gating the save on this, rather than on an edge, guarantees + // exactly one save per motion->stop cycle and means a bus that comes up already + // stopped does not re-save the positions it just loaded. + bool saw_motion_since_last_save_ = false; + + // monotonic_time at which the brakes most recently became fully engaged, or + // -1.0 when they are not. Used to enforce the pos_save_settle_sec_ debounce. + double brakes_engaged_since_ = -1.0; + + // How long all brakes must stay continuously engaged before positions are + // considered settled and saved. Guards against persisting a mid-travel + // position when drive power is cut at speed (STO/e-stop/fault) and the joint + // coasts to rest against its brake. YAML: actuator_position_save_settle_sec. + double pos_save_settle_sec_ = 0.5; + + // True only for topologies that actually persist actuator positions, i.e. + // those with at least one non-absolute-encoder GOLD/PLATINUM actuator. Set by + // LoadActuatorPosFile(), which bypasses all position-file handling otherwise. + // Every save/invalidate path must check this: AllBrakesEngaged() reports false + // when it finds no relevant actuator, which would otherwise be read as "in + // motion" and delete a position file this topology does not own (it may be + // shared with another topology that does use incremental encoders). + bool pos_file_enabled_ = false; + + // ---- Background position-file writer ---- + // A single dedicated thread performs all disk I/O for the position file so + // that no fsync/rename/backup-copy ever runs on the RT Process() thread. The + // RT thread only serializes the (tiny) YAML string under parameter_mutex_ and + // hands it off via the single-slot coalescing mailbox below. + std::thread pos_writer_thread_; + std::mutex pos_writer_mutex_; + std::condition_variable pos_writer_cv_; // RT -> writer: new request + std::condition_variable pos_writer_done_cv_; // writer -> RT: request drained + std::string pos_pending_contents_; // payload for a pending write + bool pos_pending_write_ = false; + bool pos_pending_invalidate_ = false; + bool pos_writer_stop_ = false; // ask writer to exit + // Monotonic counters to let a waiter (shutdown) know its request was handled. + uint64_t pos_request_seq_ = 0; // incremented on each post + uint64_t pos_processed_seq_ = 0; // writer sets = seq handled + // Whether pos_writer_thread_ exists and will drain the mailbox. Atomic because + // it is written by StartPosWriter()/StopPosWriter() on the application thread + // but read by the RT Process() thread (via PostPos*Request) to decide between + // handing off to the writer and writing inline. + std::atomic pos_writer_running_{false}; + }; } // namespace fastcat diff --git a/test/test_unit/CMakeLists.txt b/test/test_unit/CMakeLists.txt index 83c733f..110d85b 100644 --- a/test/test_unit/CMakeLists.txt +++ b/test/test_unit/CMakeLists.txt @@ -8,6 +8,7 @@ set(TEST_SOURCES test_function.cc test_jsd_device_base.cc test_linear_interpolation.cc + test_manager_pos_file.cc test_schmitt_trigger.cc test_signal_generator.cc test_transform_utils.cc diff --git a/test/test_unit/test_manager_pos_file.cc b/test/test_unit/test_manager_pos_file.cc new file mode 100644 index 0000000..c63f395 --- /dev/null +++ b/test/test_unit/test_manager_pos_file.cc @@ -0,0 +1,601 @@ +// Tests for the Manager's saved-actuator-position file: when it is written, +// when it is invalidated, and which topologies own it at all. +// +// These are integration-flavored tests -- they drive a real Manager over an +// offline bus and assert on what lands on disk -- because the behavior under +// test is precisely the interaction between the brake state machine, the +// background writer thread, and the filesystem. +// +// Time is supplied externally (Manager::Process(external_time)) so the settling +// debounce and the actuator's own profile timing advance deterministically +// rather than at wall-clock speed. The fake clock is seeded from real time so it +// only ever moves forward relative to the Process() calls InitHardware() makes +// internally. + +#include + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "fastcat/manager.h" +#include "jsd/jsd_time.h" + +namespace +{ +constexpr double kLoopRateHz = 1000.0; +constexpr double kDt = 1.0 / kLoopRateHz; +constexpr double kSettleSec = 0.5; +constexpr double kStartPosEu = 1.5; +constexpr double kTargetPosEu = 0.5; +// Real-time budget for the background writer thread to service a request. +constexpr double kWriterTimeoutSec = 5.0; +// Real-time grace period granted to the writer before asserting that a file has +// NOT been written, so the assertion cannot pass merely because the writer had +// not been scheduled yet. +constexpr int kNegativeAssertGraceMs = 150; + +std::string PosDirFor(const std::string& test_name) +{ + std::ostringstream ss; + ss << "/tmp/fastcat_pos_file_test_" << getpid() << "_" << test_name; + return ss.str(); +} + +bool FileExists(const std::string& path) +{ + struct stat st; + return 0 == stat(path.c_str(), &st); +} + +std::string ReadFile(const std::string& path) +{ + std::ifstream f(path); + std::stringstream ss; + ss << f.rdbuf(); + return ss.str(); +} + +// Poll until `path` reaches the desired existence state, or the timeout expires. +// Returns true if the desired state was observed. Needed because saves posted +// from Process() are serviced asynchronously by the writer thread. +bool WaitForFileState(const std::string& path, bool should_exist) +{ + const double deadline = jsd_time_get_time_sec() + kWriterTimeoutSec; + while (jsd_time_get_time_sec() < deadline) { + if (FileExists(path) == should_exist) { + return true; + } + std::this_thread::sleep_for(std::chrono::milliseconds(2)); + } + return FileExists(path) == should_exist; +} + +// A GoldActuator on an offline bus, with the position-file knobs parameterized. +std::string ActuatorTopologyYaml(const std::string& pos_dir, double settle_sec) +{ + std::ostringstream ss; + ss << "fastcat:\n" + << " target_loop_rate_hz: " << kLoopRateHz << "\n" + << " zero_latency_required: False\n" + << " actuator_position_directory: " << pos_dir << "\n" + << " actuator_fault_on_missing_pos_file: False\n" + << " actuator_position_save_settle_sec: " << settle_sec << "\n" + << "buses:\n" + << " - type: offline_bus\n" + << " ifname: eth0\n" + << " enable_autorecovery: False\n" + << " devices:\n" + << " - device_class: GoldActuator\n" + << " name: act_1\n" + << " actuator_type: revolute\n" + << " gear_ratio: 100\n" + << " counts_per_rev: 500\n" + << " max_speed_eu_per_sec: 10\n" + << " max_accel_eu_per_sec2: 30\n" + << " over_speed_multiplier: 2\n" + << " vel_tracking_error_eu_per_sec: 1000\n" + << " pos_tracking_error_eu: 1000\n" + << " peak_current_limit_amps: 10\n" + << " peak_current_time_sec: 3.0\n" + << " continuous_current_limit_amps: 5\n" + << " torque_slope_amps_per_sec: 0.5\n" + << " low_pos_cal_limit_eu: -3.2\n" + << " low_pos_cmd_limit_eu: -3.14159\n" + << " high_pos_cmd_limit_eu: 3.14159\n" + << " high_pos_cal_limit_eu: 3.2\n" + // Short so the post-move HOLDING state expires quickly in fake time. + << " holding_duration_sec: 0.05\n" + << " elmo_brake_engage_msec: 10\n" + << " elmo_brake_disengage_msec: 20\n" + << " elmo_crc: 12345\n" + << " elmo_drive_max_current_limit: 10\n" + << " smooth_factor: 0\n"; + return ss.str(); +} + +// A topology with no actuators at all, pointed at the same position directory. +// LoadActuatorPosFile() bypasses the position file for this case, so nothing +// here may read, write, or delete it. +std::string NoActuatorTopologyYaml(const std::string& pos_dir) +{ + std::ostringstream ss; + ss << "fastcat:\n" + << " target_loop_rate_hz: " << kLoopRateHz << "\n" + << " zero_latency_required: False\n" + << " actuator_position_directory: " << pos_dir << "\n" + << " actuator_fault_on_missing_pos_file: False\n" + << "buses:\n" + << " - type: fastcat_bus\n" + << " ifname: fastcat\n" + << " devices:\n" + << " - device_class: SignalGenerator\n" + << " name: sig_gen_1\n" + << " signal_generator_type: SINE_WAVE\n" + << " angular_frequency: 1.0\n" + << " phase: 0\n" + << " amplitude: 1.0\n" + << " offset: 0\n"; + return ss.str(); +} + +} // namespace + +namespace fastcat +{ + +class PosFileTest : public ::testing::Test +{ + protected: + void SetUp() override + { + pos_dir_ = PosDirFor( + ::testing::UnitTest::GetInstance()->current_test_info()->name()); + RemoveDir(); + ASSERT_EQ(0, mkdir(pos_dir_.c_str(), 0777)); + + pos_file_ = pos_dir_ + "/fastcat_saved_positions.yaml"; + prev_pos_file_ = pos_dir_ + "/fastcat_saved_positions_prev.yaml"; + tmp_pos_file_ = pos_dir_ + "/fastcat_saved_positions.yaml.tmp"; + } + + void TearDown() override + { + // Destroy the manager first: its destructor joins the writer thread, so no + // further writes can land in the directory we are about to delete. + manager_.reset(); + RemoveDir(); + } + + void RemoveDir() + { + remove((pos_dir_ + "/fastcat_saved_positions.yaml").c_str()); + remove((pos_dir_ + "/fastcat_saved_positions_prev.yaml").c_str()); + remove((pos_dir_ + "/fastcat_saved_positions.yaml.tmp").c_str()); + rmdir(pos_dir_.c_str()); + } + + void SeedPosFile(double position) + { + std::ofstream f(pos_file_); + ASSERT_TRUE(f.good()); + f << "actuators:\n"; + f << " - actuator_name: act_1\n"; + f << " position: " << position << "\n"; + } + + // Bring up a Manager on `yaml`. The fake clock is seeded from real time so + // that it stays ahead of the Process() calls InitHardware() makes with the + // default (real-time) argument. + void StartManager(const std::string& yaml) + { + manager_ = std::make_unique(); + t_ = jsd_time_get_time_sec(); + YAML::Node node = YAML::Load(yaml); + ASSERT_TRUE(manager_->ConfigFromYaml(node, t_)); + // ConfigFromYaml() runs InitHardware() internally, which itself calls + // Process() twice using real time; advance past that before taking over. + t_ = jsd_time_get_time_sec() + kDt; + } + + void Pump(int cycles) + { + for (int i = 0; i < cycles; ++i) { + t_ += kDt; + manager_->Process(t_); + } + } + + // Advance the fake clock by `seconds` worth of cycles. + void PumpFor(double seconds) { Pump(static_cast(seconds / kDt)); } + + void CommandMoveTo(double target) + { + DeviceCmd cmd{}; + cmd.name = "act_1"; + cmd.type = ACTUATOR_PROF_POS_CMD; + cmd.actuator_prof_pos_cmd.target_position = target; + cmd.actuator_prof_pos_cmd.profile_velocity = 1.0; + cmd.actuator_prof_pos_cmd.end_velocity = 0.0; + cmd.actuator_prof_pos_cmd.profile_accel = 5.0; + cmd.actuator_prof_pos_cmd.relative = 0; + manager_->QueueCommand(cmd); + } + + uint8_t MotorOn() + { + for (const auto& state : manager_->GetDeviceStates()) { + if (state.type == GOLD_ACTUATOR_STATE) { + return state.gold_actuator_state.motor_on; + } + } + ADD_FAILURE() << "no gold actuator in topology"; + return 0; + } + + double ActualPosition() + { + for (const auto& state : manager_->GetDeviceStates()) { + if (state.type == GOLD_ACTUATOR_STATE) { + return state.gold_actuator_state.actual_position; + } + } + ADD_FAILURE() << "no gold actuator in topology"; + return 0.0; + } + + // Pump until the drive reports the brakes engaged (motor power removed), or + // give up after `limit_sec` of fake time. + bool PumpUntilBrakesEngaged(double limit_sec) + { + const int limit_cycles = static_cast(limit_sec / kDt); + for (int i = 0; i < limit_cycles; ++i) { + Pump(1); + if (MotorOn() == 0) { + return true; + } + } + return false; + } + + // Pump until the drive reports motor power applied (brakes releasing). + bool PumpUntilMotorOn(double limit_sec) + { + const int limit_cycles = static_cast(limit_sec / kDt); + for (int i = 0; i < limit_cycles; ++i) { + Pump(1); + if (MotorOn() != 0) { + return true; + } + } + return false; + } + + double SavedPosition() + { + YAML::Node node = YAML::Load(ReadFile(pos_file_)); + return node["actuators"][0]["position"].as(); + } + + std::string SavedActuatorName() + { + YAML::Node node = YAML::Load(ReadFile(pos_file_)); + return node["actuators"][0]["actuator_name"].as(); + } + + // Poll the position file until it holds `expected` (within `tol`), checking + // every observation along the way for completeness. + // + // Existence is not a usable signal for "this cycle's save has landed": a fresh + // write supersedes a pending invalidate in the writer mailbox, so the document + // the previous cycle wrote can stay on disk continuously across a move. Waiting + // on existence and then reading therefore races, and reads the stale position. + // Waiting on the content is what the caller actually means. + // + // Empty reads are skipped rather than failed: the file is legitimately absent + // during the invalidate window, and stat-then-read cannot distinguish absent + // from truncated without a race of its own. Truncation is still caught -- a + // partial document either fails to parse or is missing its fields below. + bool WaitForSavedPosition(double expected, double tol) + { + const double deadline = jsd_time_get_time_sec() + kWriterTimeoutSec; + while (jsd_time_get_time_sec() < deadline) { + const std::string contents = ReadFile(pos_file_); + if (!contents.empty()) { + YAML::Node node; + try { + node = YAML::Load(contents); + } catch (const YAML::Exception& e) { + ADD_FAILURE() << "observed an unparseable position file: " << e.what() + << "\ncontents:\n" + << contents; + return false; + } + if (!node["actuators"] || node["actuators"].size() != 1 || + !node["actuators"][0]["actuator_name"] || + !node["actuators"][0]["position"]) { + ADD_FAILURE() << "observed an incomplete position file:\n" << contents; + return false; + } + EXPECT_EQ("act_1", + node["actuators"][0]["actuator_name"].as()); + if (std::fabs(node["actuators"][0]["position"].as() - expected) <= + tol) { + return true; + } + } + std::this_thread::sleep_for(std::chrono::milliseconds(2)); + } + return false; + } + + std::unique_ptr manager_; + std::string pos_dir_; + std::string pos_file_; + std::string prev_pos_file_; + std::string tmp_pos_file_; + double t_ = 0.0; +}; + +// The startup position is loaded from the file, and a bus that comes up already +// stopped must not rewrite it -- there is nothing new to persist. +TEST_F(PosFileTest, LoadsStartupPositionAndDoesNotResaveWhenNeverMoved) +{ + SeedPosFile(kStartPosEu); + const std::string original = ReadFile(pos_file_); + + StartManager(ActuatorTopologyYaml(pos_dir_, kSettleSec)); + + // SetActuatorPositions() runs after InitHardware()'s internal Process() calls, + // so the applied encoder offset is not visible in published state until the + // next Read(). One cycle is enough. + Pump(1); + + // Position came from the file (within one encoder count). + EXPECT_NEAR(kStartPosEu, ActualPosition(), 1e-3); + + // Idle well past the settling window; the file must be byte-identical and no + // backup should have been created. + PumpFor(3.0 * kSettleSec); + std::this_thread::sleep_for( + std::chrono::milliseconds(kNegativeAssertGraceMs)); + + EXPECT_TRUE(FileExists(pos_file_)); + EXPECT_EQ(original, ReadFile(pos_file_)); + EXPECT_FALSE(FileExists(prev_pos_file_)); +} + +// The core cycle: motion invalidates the file, and it is rewritten with the new +// position once the arm settles. +TEST_F(PosFileTest, InvalidatesOnMotionAndSavesAfterSettling) +{ + SeedPosFile(kStartPosEu); + StartManager(ActuatorTopologyYaml(pos_dir_, kSettleSec)); + + CommandMoveTo(kTargetPosEu); + + // Motor power applied -> the on-disk position is now untrustworthy and must be + // removed before the arm can move anywhere. + ASSERT_TRUE(PumpUntilMotorOn(1.0)); + EXPECT_TRUE(WaitForFileState(pos_file_, /*should_exist=*/false)) + << "position file must be invalidated as soon as motors are powered"; + + // Run the move to completion, through HOLDING and into HALTED. + ASSERT_TRUE(PumpUntilBrakesEngaged(10.0)); + + // Settle, then the save should land. + PumpFor(2.0 * kSettleSec); + ASSERT_TRUE(WaitForFileState(pos_file_, /*should_exist=*/true)) + << "position file must be rewritten after the arm settles"; + + EXPECT_EQ("act_1", SavedActuatorName()); + EXPECT_NEAR(kTargetPosEu, SavedPosition(), 1e-2); + + // Atomic-write bookkeeping: the temp file must not linger. + EXPECT_FALSE(FileExists(tmp_pos_file_)); + + // No _prev backup here, and that is expected: the backup is a copy of the + // canonical file made at write time, and the canonical file had already been + // removed by the invalidate-on-motion step. This is the normal path, so + // _prev.yaml is effectively never produced during ordinary operation -- see + // ShutdownOverExistingFileCreatesBackup for the case that does produce it. + EXPECT_FALSE(FileExists(prev_pos_file_)); +} + +// The _prev backup is only produced when a save overwrites a canonical file that +// is still present -- i.e. a save that was not preceded by an invalidation. +TEST_F(PosFileTest, ShutdownOverExistingFileCreatesBackup) +{ + SeedPosFile(kStartPosEu); + const std::string seeded = ReadFile(pos_file_); + + StartManager(ActuatorTopologyYaml(pos_dir_, kSettleSec)); + Pump(10); // never moves, so the file is never invalidated + + manager_->Shutdown(); + + ASSERT_TRUE(FileExists(prev_pos_file_)); + EXPECT_EQ(seeded, ReadFile(prev_pos_file_)) + << "backup must hold the contents the save replaced"; + EXPECT_NEAR(kStartPosEu, SavedPosition(), 1e-3); + EXPECT_FALSE(FileExists(tmp_pos_file_)); +} + +// Regression test for the settling debounce. On an STO/e-stop/fault, drive power +// is cut while the joint is still moving and the joint coasts to rest against +// the brake -- so a save must NOT happen on the first brakes-engaged cycle. +TEST_F(PosFileTest, SaveIsDeferredUntilSettlingWindowElapses) +{ + SeedPosFile(kStartPosEu); + StartManager(ActuatorTopologyYaml(pos_dir_, kSettleSec)); + + CommandMoveTo(kTargetPosEu); + ASSERT_TRUE(PumpUntilMotorOn(1.0)); + ASSERT_TRUE(WaitForFileState(pos_file_, /*should_exist=*/false)); + + // The instant the brakes engage the position is not yet trustworthy. + ASSERT_TRUE(PumpUntilBrakesEngaged(10.0)); + ASSERT_EQ(0u, MotorOn()); + + // Advance to just short of the settling window: still no file. + PumpFor(0.8 * kSettleSec); + std::this_thread::sleep_for( + std::chrono::milliseconds(kNegativeAssertGraceMs)); + EXPECT_FALSE(FileExists(pos_file_)) + << "save must be deferred until the brakes have been engaged for " + "actuator_position_save_settle_sec"; + + // Cross the threshold: now it saves. + PumpFor(0.4 * kSettleSec); + EXPECT_TRUE(WaitForFileState(pos_file_, /*should_exist=*/true)) + << "save must happen once the settling window has elapsed"; +} + +// settle_sec == 0 restores the pre-debounce behavior, on the first +// brakes-engaged cycle. Documents the escape hatch. +TEST_F(PosFileTest, ZeroSettleSecSavesOnFirstBrakesEngagedCycle) +{ + SeedPosFile(kStartPosEu); + StartManager(ActuatorTopologyYaml(pos_dir_, 0.0)); + + CommandMoveTo(kTargetPosEu); + ASSERT_TRUE(PumpUntilMotorOn(1.0)); + ASSERT_TRUE(WaitForFileState(pos_file_, /*should_exist=*/false)); + + ASSERT_TRUE(PumpUntilBrakesEngaged(10.0)); + // No additional pumping: the save must already have been posted. + EXPECT_TRUE(WaitForFileState(pos_file_, /*should_exist=*/true)); +} + +// Only one save per motion->stop cycle: a long idle must not keep rewriting. +TEST_F(PosFileTest, SavesOncePerMotionCycle) +{ + SeedPosFile(kStartPosEu); + StartManager(ActuatorTopologyYaml(pos_dir_, kSettleSec)); + + CommandMoveTo(kTargetPosEu); + ASSERT_TRUE(PumpUntilMotorOn(1.0)); + ASSERT_TRUE(PumpUntilBrakesEngaged(10.0)); + PumpFor(2.0 * kSettleSec); + // Wait for the new position rather than mere existence: the seeded file is + // still on disk until the writer services this cycle's request, so a snapshot + // taken on existence alone can capture the seed and then differ from the real + // save below. + ASSERT_TRUE(WaitForSavedPosition(kTargetPosEu, 1e-2)); + + const std::string first_contents = ReadFile(pos_file_); + + // Idle for many more settling windows. + PumpFor(10.0 * kSettleSec); + std::this_thread::sleep_for( + std::chrono::milliseconds(kNegativeAssertGraceMs)); + + EXPECT_EQ(first_contents, ReadFile(pos_file_)); +} + +// Shutdown while stopped is the clean path: positions are persisted, and the +// wait is synchronous so the file is on disk by the time Shutdown() returns. +TEST_F(PosFileTest, ShutdownWhileStoppedSavesSynchronously) +{ + SeedPosFile(kStartPosEu); + StartManager(ActuatorTopologyYaml(pos_dir_, kSettleSec)); + + // Move, settle, then invalidate again by starting a second move and letting it + // finish, so there is something new to write at shutdown. + CommandMoveTo(kTargetPosEu); + ASSERT_TRUE(PumpUntilMotorOn(1.0)); + ASSERT_TRUE(PumpUntilBrakesEngaged(10.0)); + PumpFor(2.0 * kSettleSec); + ASSERT_TRUE(WaitForFileState(pos_file_, /*should_exist=*/true)); + + remove(pos_file_.c_str()); + ASSERT_FALSE(FileExists(pos_file_)); + + manager_->Shutdown(); + + // No WaitForFileState(): Shutdown() must block until the write is durable. + EXPECT_TRUE(FileExists(pos_file_)) + << "Shutdown() must not return before the position file is on disk"; + EXPECT_NEAR(kTargetPosEu, SavedPosition(), 1e-2); +} + +// Shutdown mid-motion must leave NO file rather than an in-motion position: the +// next startup is expected to fault rather than trust a bad position. This is a +// deliberate policy choice -- see actuator_fault_on_missing_pos_file. +TEST_F(PosFileTest, ShutdownWhileMovingInvalidatesRatherThanSaving) +{ + SeedPosFile(kStartPosEu); + StartManager(ActuatorTopologyYaml(pos_dir_, kSettleSec)); + + CommandMoveTo(kTargetPosEu); + ASSERT_TRUE(PumpUntilMotorOn(1.0)); + ASSERT_NE(0u, MotorOn()); + + manager_->Shutdown(); + + EXPECT_FALSE(FileExists(pos_file_)) + << "an in-motion shutdown must leave the position file absent, not write " + "a mid-travel position"; +} + +// A topology with no actuators bypasses the position file entirely, so it must +// not delete a file that belongs to some other topology sharing the directory. +TEST_F(PosFileTest, NoActuatorTopologyLeavesPositionFileUntouched) +{ + SeedPosFile(kStartPosEu); + const std::string original = ReadFile(pos_file_); + + StartManager(NoActuatorTopologyYaml(pos_dir_)); + PumpFor(3.0 * kSettleSec); + manager_->Shutdown(); + std::this_thread::sleep_for( + std::chrono::milliseconds(kNegativeAssertGraceMs)); + + ASSERT_TRUE(FileExists(pos_file_)) + << "a topology that bypasses the position file must not delete it"; + EXPECT_EQ(original, ReadFile(pos_file_)); +} + +// A negative settling window is meaningless and must be rejected at +// configuration time rather than silently accepted. +TEST_F(PosFileTest, NegativeSettleSecIsRejected) +{ + SeedPosFile(kStartPosEu); + manager_ = std::make_unique(); + YAML::Node node = YAML::Load(ActuatorTopologyYaml(pos_dir_, -1.0)); + EXPECT_FALSE(manager_->ConfigFromYaml(node, jsd_time_get_time_sec())); +} + +// The canonical file is replaced by an atomic rename, so a reader never observes +// a partial write: every observation is either the old or the new full document. +TEST_F(PosFileTest, PositionFileIsAlwaysCompleteAndParseable) +{ + SeedPosFile(kStartPosEu); + StartManager(ActuatorTopologyYaml(pos_dir_, 0.0)); + + for (int cycle = 0; cycle < 3; ++cycle) { + const double target = (cycle % 2 == 0) ? kTargetPosEu : kStartPosEu; + CommandMoveTo(target); + ASSERT_TRUE(PumpUntilMotorOn(1.0)); + ASSERT_TRUE(PumpUntilBrakesEngaged(10.0)); + + // Whenever the file is present it must be a complete, parseable document + // with a usable position -- never truncated. WaitForSavedPosition() checks + // that of every observation it makes while waiting for this cycle's save. + ASSERT_TRUE(WaitForSavedPosition(target, 1e-2)) + << "cycle " << cycle << ": save of " << target + << " never landed; file holds:\n" + << ReadFile(pos_file_); + + EXPECT_FALSE(FileExists(tmp_pos_file_)) + << "temp file must not survive a completed write"; + } +} + +} // namespace fastcat