From 406eec10ee65f128bcc744af2f5fac83d04aca03 Mon Sep 17 00:00:00 2001
From: Vishnuprasad Prachandabhanu <vishnu.pbhat93@gmail.com>
Date: Tue, 6 Jun 2023 11:06:55 +0200
Subject: [PATCH] run interpolated mode in fake slave

---
 .../canopen_fake_slaves/base_slave.hpp        |  1 +
 .../canopen_fake_slaves/cia402_slave.hpp      | 81 +++++++++++++++++++
 2 files changed, 82 insertions(+)

diff --git a/canopen_fake_slaves/include/canopen_fake_slaves/base_slave.hpp b/canopen_fake_slaves/include/canopen_fake_slaves/base_slave.hpp
index 2282c0e2..3fc33efe 100644
--- a/canopen_fake_slaves/include/canopen_fake_slaves/base_slave.hpp
+++ b/canopen_fake_slaves/include/canopen_fake_slaves/base_slave.hpp
@@ -80,6 +80,7 @@ class BaseSlave : public rclcpp_lifecycle::LifecycleNode
     const rclcpp_lifecycle::State &)
   {
     RCLCPP_INFO(this->get_logger(), "Shutdown");
+    return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
   }
 };
 }  // namespace ros2_canopen
diff --git a/canopen_fake_slaves/include/canopen_fake_slaves/cia402_slave.hpp b/canopen_fake_slaves/include/canopen_fake_slaves/cia402_slave.hpp
index 3496e768..83a69055 100644
--- a/canopen_fake_slaves/include/canopen_fake_slaves/cia402_slave.hpp
+++ b/canopen_fake_slaves/include/canopen_fake_slaves/cia402_slave.hpp
@@ -50,6 +50,11 @@ class CIA402MockSlave : public canopen::BasicSlave
       RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Joined cyclic_position_mode thread.");
       cyclic_position_mode.join();
     }
+    if (interpolated_position_mode.joinable())
+    {
+      RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Joined interpolated_position_mode thread.");
+      interpolated_position_mode.join();
+    }
   }
 
 protected:
@@ -136,6 +141,7 @@ class CIA402MockSlave : public canopen::BasicSlave
   std::thread profiled_velocity_mode;
   std::thread cyclic_position_mode;
   std::thread cyclic_velocity_mode;
+  std::thread interpolated_position_mode;
 
   double cycle_time;
 
@@ -248,6 +254,66 @@ class CIA402MockSlave : public canopen::BasicSlave
     }
   }
 
+  void run_interpolated_position_mode()
+  {
+    RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "run_interpolated_position_mode");
+    // Retrieve parameters from the object dictionary
+    double interpolation_period = static_cast<double>((uint8_t)(*this)[0x60C2][1]);
+    double target_position = static_cast<double>((int32_t)(*this)[0x60C1][1]);
+
+    // int32_t offset = (*this)[0x60B0][0];
+
+    // Convert parameters to SI units
+    interpolation_period *= std::pow(10.0, static_cast<double>((int8_t)(*this)[0x60C2][2]));
+    target_position /= 1000.0;
+    double actual_position = static_cast<double>((int32_t)(*this)[0x6064][0]) / 1000.0;
+
+    RCLCPP_INFO(
+      rclcpp::get_logger("cia402_slave"), "Interpolation Period: %f", interpolation_period);
+    RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Target position: %f", target_position);
+
+    while ((state.load() == InternalState::Operation_Enable) &&
+           (operation_mode.load() == Interpolated_Position) && (rclcpp::ok()))
+    {
+      std::this_thread::sleep_for(
+        std::chrono::milliseconds(static_cast<int>(interpolation_period * 1000)));
+
+      target_position = static_cast<double>((int32_t)(*this)[0x60C1][1]) / 1000.0;
+
+      if (target_position != actual_position)
+      {
+        double position_delta = target_position - actual_position;
+        double position_increment = position_delta / 20;
+        clear_status_bit(SW_Operation_mode_specific0);
+        clear_status_bit(SW_Target_reached);
+        {
+          std::scoped_lock<std::mutex> lock(w_mutex);
+          (*this)[0x6041][0] = status_word;
+          this->TpdoEvent(1);
+        }
+
+        while ((std::abs(actual_position - target_position) > 0.001) && (rclcpp::ok()))
+        {
+          std::this_thread::sleep_for(std::chrono::milliseconds(1));
+          actual_position += position_increment;
+          (*this)[0x6064][0] = static_cast<int32_t>(actual_position * 1000);
+          (*this)[0x606C][0] = static_cast<int32_t>(position_increment * 1000);
+        }
+
+        actual_position = target_position;
+
+        RCLCPP_DEBUG(rclcpp::get_logger("cia402_slave"), "Reached target: %f", actual_position);
+        clear_status_bit(SW_Operation_mode_specific0);
+        set_status_bit(SW_Target_reached);
+        {
+          std::lock_guard<std::mutex> lock(w_mutex);
+          (*this)[0x6041][0] = status_word;
+          this->TpdoEvent(1);
+        }
+      }
+    }
+  }
+
   void run_position_mode() {}
 
   void set_new_status_word_and_state()
@@ -432,6 +498,12 @@ class CIA402MockSlave : public canopen::BasicSlave
         RCLCPP_INFO(rclcpp::get_logger("cia402_slave"), "Joined cyclic_position_mode thread.");
         cyclic_position_mode.join();
       }
+      if (interpolated_position_mode.joinable())
+      {
+        RCLCPP_INFO(
+          rclcpp::get_logger("cia402_slave"), "Joined interpolated_position_mode thread.");
+        interpolated_position_mode.join();
+      }
       old_operation_mode.store(operation_mode.load());
       switch (operation_mode.load())
       {
@@ -441,6 +513,9 @@ class CIA402MockSlave : public canopen::BasicSlave
         case Profiled_Position:
           start_profile_pos_mode();
           break;
+        case Interpolated_Position:
+          start_interpolated_pos_mode();
+          break;
         default:
           break;
       }
@@ -458,6 +533,12 @@ class CIA402MockSlave : public canopen::BasicSlave
       std::thread(std::bind(&CIA402MockSlave::run_profiled_position_mode, this));
   }
 
+  void start_interpolated_pos_mode()
+  {
+    interpolated_position_mode =
+      std::thread(std::bind(&CIA402MockSlave::run_interpolated_position_mode, this));
+  }
+
   void on_quickstop_active()
   {
     if (is_enable_operation())