Fix save positions

fcat/CMakeLists.txt

@@ -23,7 +23,7 @@ endif()
 include(FetchContent)
 FetchContent_Declare(fastcat
   GIT_REPOSITORY https://github.com/nasa-jpl/fastcat.git
-  GIT_TAG v0.13.15
+  GIT_TAG v0.13.20
 )
 FetchContent_MakeAvailable(fastcat)
 

fcat/src/fcat.cpp

@@ -21,7 +21,28 @@ using namespace std::chrono_literals;
 using std::placeholders::_1;
 using std::placeholders::_2;
 
-Fcat::~Fcat() { fcat_manager_.Shutdown(); }
+Fcat::~Fcat() {
+  // Best-effort deregistration so the callback is not left pointing at a
+  // destroyed node if the context outlives this object.
+  auto context = this->get_node_base_interface()->get_context();
+  if (context) {
+    context->remove_pre_shutdown_callback(pre_shutdown_cb_handle_);
+  }
+  // Fallback for a destruction that is not preceded by rclcpp::shutdown().
+  // Normally the pre-shutdown callback has already saved and call_once makes
+  // this a no-op.
+  SaveState();
+}
+
+void Fcat::SaveState() {
+  // Runs at most once, whether triggered by the pre-shutdown callback or the
+  // destructor. The manager serializes the capture+write against Process()
+  // internally.
+  std::call_once(save_state_flag_, [this]() {
+    RCLCPP_INFO(this->get_logger(), "Saving actuator positions on shutdown");
+    fcat_manager_.SaveActuatorPositions();
+  });
+}
 
 Fcat::Fcat(const rclcpp::NodeOptions& options)
     : FcatNode("fcat", "fcat", options),
@@ -248,6 +269,15 @@ Fcat::Fcat(const rclcpp::NodeOptions& options)
   fcat_state_ = FcatState::INACTIVE;
   StartProcessTimer();
   fcat_state_ = FcatState::ACTIVE;
+
+  // Persist actuator positions on shutdown. Runs on whichever thread initiates
+  // shutdown (the signal thread for SIGINT/SIGTERM), before the context is torn
+  // down, so the save does not depend on destructor ordering.
+  // SaveActuatorPositions() takes the manager's process mutex, so it is safe
+  // even though the process loop may still be spinning here.
+  pre_shutdown_cb_handle_ =
+      this->get_node_base_interface()->get_context()->add_pre_shutdown_callback(
+          [this]() { SaveState(); });
 }
 
 void Fcat::SetRealtimePreempt(int scheduler_priority) {
@@ -1448,7 +1478,6 @@ void Fcat::SetCpuAffinity() {
 }
 
 void Fcat::Process() {
-  fprintf(stderr, "Handling Process() loop\n");
   auto now = this->get_clock()->now();
 
   bool report_cycle_slips = this->get_parameter("report_cycle_slips").as_bool();
@@ -1609,8 +1638,6 @@ void Fcat::PublishAsyncSdoResponse() {
 
     msg.data = jsd_sdo_data_to_string(sdo_resp.response.data_type, sdo_resp.response.data);
 
-    fprintf(stderr, "Publishing new AsyncSdoResponse\n");
-
     async_sdo_response_pub_->publish(msg);
   }
 }

fcat/src/fcat.hpp

@@ -8,6 +8,7 @@
 #include <any>
 #include <functional>
 #include <memory>
+#include <mutex>
 #include <string>
 #include <unordered_map>
 #include <vector>
@@ -109,6 +110,9 @@ class Fcat : public FcatNode {
 
  private:
   void Process() override;
+  // Capture + persist actuator positions on shutdown. Idempotent (guarded by
+  // save_state_flag_); invoked by both the pre-shutdown callback and the dtor.
+  void SaveState();
   void SetRealtimePreempt(int scheduler_priority);
   void PopulateDeviceStateFields();
   void SetCpuAffinity();
@@ -476,5 +480,8 @@ class Fcat : public FcatNode {
   bool reset_in_progress_ = false;
   FcatState fcat_state_ = FcatState::UNCONFIGURED;
   std::vector<rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr> param_cb_handles_;
+
+  std::once_flag save_state_flag_;
+  rclcpp::PreShutdownCallbackHandle pre_shutdown_cb_handle_;
 };
 #endif  // FCAT_HPP_