feat(odrive_can_driver): Lifecycle management in can threads

rxsio · Dec 4, 2024 · 9e254b3 · 9e254b3
1 parent 33d47f2
commit 9e254b3
Show file tree

Hide file tree

Showing 7 changed files with 220 additions and 48 deletions.
diff --git a/odrive_can_driver/include/odrive_can_driver/can/can.hpp b/odrive_can_driver/include/odrive_can_driver/can/can.hpp
@@ -15,6 +15,8 @@
 #include <ros2_socketcan/socket_can_id.hpp>
 #include <ros2_socketcan/socket_can_receiver.hpp>
 #include <ros2_socketcan/socket_can_sender.hpp>
+
+#include "odrive_can_driver/can/parsing.hpp"
 namespace odrive_can_driver
 {
 
@@ -29,6 +31,12 @@ class Can
     sender_.reset();
     return hardware_interface::CallbackReturn::SUCCESS;
   };
+  void Configure(const rclcpp::Time & time)
+  {
+    sender_->Notify(time, HardwareState::kConfigure);
+    receiver_->Notify(time, HardwareState::kConfigure);
+  };
+  void Activate(const rclcpp::Time & time) { sender_->Notify(time, HardwareState::kActivate); };
   void Write(const rclcpp::Time & time, const rclcpp::Duration & period)
   {
     sender_->Notify(time, period);
@@ -38,6 +46,13 @@ class Can
   {
     receiver_->Notify(time, period);
   };
+  void Deactivate(const rclcpp::Time & time) { sender_->Notify(time, HardwareState::kDeactivate); };
+  void Cleanup(const rclcpp::Time & time) { sender_->Notify(time, HardwareState::kCleanup); };
+  void Error(const rclcpp::Time & time)
+  {
+    sender_->Notify(time, HardwareState::kError);
+    receiver_->Notify(time, HardwareState::kError);
+  };
 
 private:
   std::unique_ptr<CanReadThread> receiver_;

diff --git a/odrive_can_driver/include/odrive_can_driver/can/can_read_thread.hpp b/odrive_can_driver/include/odrive_can_driver/can/can_read_thread.hpp
@@ -40,7 +40,11 @@ class CanReadThread
   }
   ~CanReadThread();
   void operator()();
-  void Notify(const rclcpp::Time & time, const rclcpp::Duration & period);
+
+  void Notify(
+    const rclcpp::Time & time, const rclcpp::Duration & period,
+    HardwareState state = HardwareState::kRun);
+  void Notify(const rclcpp::Time & time, HardwareState state = HardwareState::kRun);
 
 private:
   std::reference_wrapper<std::vector<MotorAxis>> motor_axis_;
@@ -49,24 +53,30 @@ class CanReadThread
   drivers::socketcan::SocketCanSender sender_;
   rclcpp::Time time_{0};
   rclcpp::Duration period_{0, 0};
-  rclcpp::Time last_response_time_{0, 0};
-  std::mutex timestamp_mutex_;
-  std::condition_variable wait_for_next_read_;
+  std::mutex step_mutex_;
+  std::condition_variable step_;
   std::atomic<bool> run_{true};
   const rclcpp::Duration k_min_timeout_{0, 1000000};
+  HardwareState state_{};
 
   void SendRTR(const uint8_t node_id, CommandId command, const rclcpp::Time & deadline);
+  void Receive(const rclcpp::Time & deadline);
 
-  rclcpp::Time GetDeadline()
+  std::pair<rclcpp::Time, rclcpp::Duration> Time()
   {
-    std::lock_guard<std::mutex> lock(timestamp_mutex_);
-    return time_ + period_;
+    std::lock_guard<std::mutex> lock(step_mutex_);
+    return std::make_pair(time_, period_);
   }
+
+  std::pair<rclcpp::Time, rclcpp::Duration> Wait(const rclcpp::Time & previous_time);
+
   static std::chrono::nanoseconds GetTimeout(const rclcpp::Time & deadline)
   {
     return (deadline - rclcpp::Clock().now()).to_chrono<std::chrono::nanoseconds>();
   }
-  void Receive(const rclcpp::Time & deadline);
+  void Configure(const rclcpp::Time & time);
+  void Read(const rclcpp::Time & time, const rclcpp::Duration & period);
+  void Error(const rclcpp::Time & time);
 };
 
 template <odrive_can_driver::CommandId C>

diff --git a/odrive_can_driver/include/odrive_can_driver/can/can_write_thread.hpp b/odrive_can_driver/include/odrive_can_driver/can/can_write_thread.hpp
@@ -33,11 +33,14 @@ class CanWriteThread
   : motor_axis_(motor_axis), sender_(can_interface)
   {
     // We can't pass this object directly, because it's not copyable
-    // (drivers::socketcan::SocketCanReceiver and other data members are not copyable)
+    // (drivers::socketcan::SocketCanReceiver, std::thread itself and other data members are not copyable)
     thread_ = std::thread(std::ref(*this));
   }
   void operator()();
-  void Notify(const rclcpp::Time & time, const rclcpp::Duration & period);
+  void Notify(
+    const rclcpp::Time & time, const rclcpp::Duration & period,
+    HardwareState state = HardwareState::kRun);
+  void Notify(const rclcpp::Time & time, HardwareState state = HardwareState::kRun);
   ~CanWriteThread();
 
 private:
@@ -46,10 +49,13 @@ class CanWriteThread
   drivers::socketcan::SocketCanSender sender_;
   rclcpp::Time time_{0};
   rclcpp::Duration period_{0, 0};
-  std::mutex timestamp_mutex_;
-  std::condition_variable wait_for_next_write_;
+  std::mutex step_mutex_;
+  std::condition_variable step_;
   std::atomic<bool> run_{true};
   const rclcpp::Duration k_min_timeout_{0, 1000000};
+  HardwareState state_{};
+
+  std::pair<rclcpp::Time, rclcpp::Duration> Wait(const rclcpp::Time & previous_time);
 
   template <typename... T>
   bool Send(
@@ -86,7 +92,12 @@ class CanWriteThread
     }
   }
   bool Send(const uint8_t node_id, CommandId command, const rclcpp::Time & deadline);
-  void Write(const rclcpp::Time & deadline);
+  void Configure(const rclcpp::Time & time);
+  void Activate(const rclcpp::Time & time);
+  void Write(const rclcpp::Time & time, const rclcpp::Duration & period);
+  void Deactivate(const rclcpp::Time & time);
+  void Cleanup(const rclcpp::Time & time);
+  void Error(const rclcpp::Time & time);
 };
 
 }  // namespace odrive_can_driver

diff --git a/odrive_can_driver/include/odrive_can_driver/can/parsing.hpp b/odrive_can_driver/include/odrive_can_driver/can/parsing.hpp
@@ -141,5 +141,15 @@ constexpr std::tuple<T...> UnpackFromLittleEndian(
     bus_time, frame_type, drivers::socketcan::StandardFrame};
 };
 
+// enum for lifecycle transitions
+enum class HardwareState : uint8_t {
+  kConfigure = 1,
+  kActivate = 2,
+  kRun = 3,
+  kDeactivate = 4,
+  kCleanup = 5,
+  kError = 0
+};
+
 }  // namespace odrive_can_driver
 #endif /* ODRIVE_CAN_DRIVER_CAN_PARSING_H_ */
diff --git a/odrive_can_driver/src/can/can_read_thread.cpp b/odrive_can_driver/src/can/can_read_thread.cpp
@@ -51,12 +51,10 @@ void CanReadThread::Receive(const rclcpp::Time & deadline)
   switch (command_id) {
     case CommandId::kEncoderEstimates: {
       odrive_can_driver::Receive<CommandId::kEncoderEstimates>(data, length, motor_axis);
-      last_response_time_ = rclcpp::Clock().now();
       break;
     }
     case CommandId::kIq: {
       odrive_can_driver::Receive<CommandId::kIq>(data, length, motor_axis);
-      last_response_time_ = rclcpp::Clock().now();
       break;
     }
     case CommandId::kControllerError: {
@@ -86,32 +84,85 @@ CanReadThread::~CanReadThread()
 
 void CanReadThread::operator()()
 {
+  rclcpp::Time time;
+  rclcpp::Duration period{0, 0};
   while (rclcpp::ok() && run_.load(std::memory_order_relaxed)) {
-    std::unique_lock<std::mutex> lock(timestamp_mutex_);
-    wait_for_next_read_.wait(lock);
-    lock.unlock();
-    auto deadline = GetDeadline();
-    for (auto & motor_axis : motor_axis_.get()) {
-      const auto node_id = motor_axis.NodeId();
-      SendRTR(node_id, CommandId::kEncoderEstimates, deadline);
-      SendRTR(node_id, CommandId::kIq, deadline);
-      SendRTR(node_id, CommandId::kControllerError, deadline);
-      SendRTR(node_id, CommandId::kMotorError, deadline);
-      SendRTR(node_id, CommandId::kEncoderError, deadline);
-    }
-    while (deadline > rclcpp::Clock().now()) {
-      Receive(deadline);
+    std::tie(time, period) = Wait(time);
+    switch (state_) {
+      case HardwareState::kConfigure: {
+        Configure(time);
+        break;
+      }
+      case HardwareState::kRun: {
+        Read(time, period);
+        break;
+      }
+      case HardwareState::kError: {
+        Error(time);
+        break;
+      }
+      default:
+        break;
     }
   }
 }
 
-void CanReadThread::Notify(const rclcpp::Time & time, const rclcpp::Duration & period)
+void CanReadThread::Notify(
+  const rclcpp::Time & time, const rclcpp::Duration & period, HardwareState state)
 {
-  std::lock_guard<std::mutex> lock(timestamp_mutex_);
+  std::lock_guard<std::mutex> lock(step_mutex_);
   time_ = time;
   period_ = period;
-  wait_for_next_read_.notify_one();
+  state_ = state;
+  step_.notify_one();
 }
+void CanReadThread::Notify(const rclcpp::Time & time, HardwareState state)
+{
+  std::lock_guard<std::mutex> lock(step_mutex_);
+  time_ = time;
+  period_ = rclcpp::Duration(0, 0);
+  state_ = state;
+  step_.notify_one();
+}
+std::pair<rclcpp::Time, rclcpp::Duration> CanReadThread::Wait(const rclcpp::Time & previous_time)
+{
+  std::unique_lock<std::mutex> lock(step_mutex_);
+  step_.wait(lock, [this, &previous_time]() {
+    return !run_.load(std::memory_order_relaxed) || time_ != previous_time;
+  });
+  return std::make_pair(time_, period_);
+}
+
+void CanReadThread::Configure(const rclcpp::Time & time)
+{
+  for (auto & motor_axis : motor_axis_.get()) {
+    const auto node_id = motor_axis.NodeId();
+    SendRTR(node_id, CommandId::kControllerError, time);
+    SendRTR(node_id, CommandId::kMotorError, time);
+    SendRTR(node_id, CommandId::kEncoderError, time);
+  }
+  while (time == Time().first) {
+    Receive(time);
+  }
+}
+
+void CanReadThread::Read(const rclcpp::Time & time, const rclcpp::Duration & period)
+{
+  auto deadline = time + period;
+  for (auto & motor_axis : motor_axis_.get()) {
+    const auto node_id = motor_axis.NodeId();
+    SendRTR(node_id, CommandId::kEncoderEstimates, deadline);
+    SendRTR(node_id, CommandId::kIq, deadline);
+    SendRTR(node_id, CommandId::kControllerError, deadline);
+    SendRTR(node_id, CommandId::kMotorError, deadline);
+    SendRTR(node_id, CommandId::kEncoderError, deadline);
+  }
+  while (deadline > rclcpp::Clock().now() && time == Time().first) {
+    Receive(deadline);
+  }
+}
+
+void CanReadThread::Error(const rclcpp::Time & /*time*/) {}
 
 void CanReadThread::SendRTR(const uint8_t node_id, CommandId command, const rclcpp::Time & deadline)
 {

diff --git a/odrive_can_driver/src/can/can_write_thread.cpp b/odrive_can_driver/src/can/can_write_thread.cpp
@@ -5,35 +5,103 @@
 #include <stdexcept>
 namespace odrive_can_driver
 {
-
-void CanWriteThread::operator()()
+void CanWriteThread::Configure(const rclcpp::Time & time)
 {
   // TODO: Parametrize init timeout duration
-  auto deadline = rclcpp::Clock().now() + rclcpp::Duration(5, 0);
+  auto deadline = time + rclcpp::Duration(5, 0);
   // TODO: Synchronize with read loop so initial errors are either read first
   // or they are not read at all before initial ClearErrors
   for (auto & motor_axis : motor_axis_.get()) {
     auto node_id = motor_axis.NodeId();
     Send(node_id, CommandId::kClearErrors, deadline);
+    // Idle
+    Send(node_id, CommandId::kAxisRequestedState, deadline, uint32_t(1));
   }
-  while (rclcpp::ok() && run_.load(std::memory_order_relaxed)) {
-    std::unique_lock<std::mutex> lock(timestamp_mutex_);
-    wait_for_next_write_.wait(lock);
-    Write(time_ + period_);
+}
+void CanWriteThread::Activate(const rclcpp::Time & time)
+{
+  auto deadline = time + rclcpp::Duration(5, 0);
+  for (auto & motor_axis : motor_axis_.get()) {
+    // Startup sequence
+    Send(motor_axis.NodeId(), CommandId::kAxisRequestedState, deadline, uint32_t(2));
+  }
+}
+void CanWriteThread::Deactivate(const rclcpp::Time & time)
+{
+  auto deadline = time + rclcpp::Duration(5, 0);
+  for (auto & motor_axis : motor_axis_.get()) {
+    // Idle
+    Send(motor_axis.NodeId(), CommandId::kAxisRequestedState, deadline, uint32_t(1));
   }
+}
+void CanWriteThread::Cleanup(const rclcpp::Time & time)
+{
   // TODO: Parametrize cleanup timeout duration
-  deadline = rclcpp::Clock().now() + rclcpp::Duration(5, 0);
+  auto deadline = time + rclcpp::Duration(5, 0);
   for (auto & motor_axis : motor_axis_.get()) {
-    auto node_id = motor_axis.NodeId();
-    Send(node_id, CommandId::kControllerModes, deadline, uint32_t(1), uint32_t(0));
+    Send(motor_axis.NodeId(), CommandId::kControllerModes, deadline, uint32_t(1), uint32_t(0));
+  }
+}
+void CanWriteThread::Error(const rclcpp::Time & /*time*/) {}
+
+void CanWriteThread::operator()()
+{
+  rclcpp::Time time;
+  rclcpp::Duration period{0, 0};
+  while (rclcpp::ok() && run_.load(std::memory_order_relaxed)) {
+    std::tie(time, period) = Wait(time);
+    switch (state_) {
+      case HardwareState::kConfigure: {
+        Configure(time);
+        break;
+      }
+      case HardwareState::kActivate: {
+        Activate(time);
+        break;
+      }
+      case HardwareState::kRun: {
+        Write(time, period);
+        break;
+      }
+      case HardwareState::kDeactivate: {
+        Deactivate(time);
+        break;
+      }
+      case HardwareState::kCleanup: {
+        Cleanup(time);
+        break;
+      }
+      case HardwareState::kError: {
+        Error(time);
+        break;
+      }
+    }
   }
 };
-void CanWriteThread::Notify(const rclcpp::Time & time, const rclcpp::Duration & period)
+void CanWriteThread::Notify(
+  const rclcpp::Time & time, const rclcpp::Duration & period, HardwareState state)
 {
-  std::lock_guard<std::mutex> lock(timestamp_mutex_);
+  std::lock_guard<std::mutex> lock(step_mutex_);
   time_ = time;
   period_ = period;
-  wait_for_next_write_.notify_one();
+  state_ = state;
+  step_.notify_one();
+}
+void CanWriteThread::Notify(const rclcpp::Time & time, HardwareState state)
+{
+  std::lock_guard<std::mutex> lock(step_mutex_);
+  time_ = time;
+  period_ = rclcpp::Duration(0, 0);
+  state_ = state;
+  step_.notify_one();
+}
+std::pair<rclcpp::Time, rclcpp::Duration> CanWriteThread::Wait(const rclcpp::Time & previous_time)
+{
+  std::unique_lock<std::mutex> lock(step_mutex_);
+  step_.wait(lock, [this, &previous_time]() {
+    return !run_.load(std::memory_order_relaxed) || time_ != previous_time;
+  });
+  return std::make_pair(time_, period_);
 }
 CanWriteThread::~CanWriteThread()
 {
@@ -72,8 +140,9 @@ bool CanWriteThread::Send(const uint8_t node_id, CommandId command, const rclcpp
   }
 }
 
-void CanWriteThread::Write(const rclcpp::Time & deadline)
+void CanWriteThread::Write(const rclcpp::Time & time, const rclcpp::Duration & period)
 {
+  auto deadline = time + period;
   for (auto & motor_axis : motor_axis_.get()) {
     auto command_id = motor_axis.command.load();
     const auto node_id = motor_axis.NodeId();