[Feature] - Use Time Based Interpolation

include/akushon/action/model/action.hpp

@@ -57,6 +57,7 @@ class Action
 
   void reset();
 
+  bool time_based;
 private:
   std::string name;
 

include/akushon/action/model/pose.hpp

@@ -48,6 +48,7 @@ class Pose
 
   void set_target_position(const Pose & target_pose);
 
+  float action_time;
 private:
   float speed;
   float pause;

include/akushon/action/node/action_manager.hpp

@@ -51,7 +51,7 @@ class ActionManager
   void start(std::string action_name, const Pose & initial_pose);
   void start(const Action & action, const Pose & initial_pose);
   void brake();
-  void process(int time);
+  void process(double time);
 
   bool is_playing() const;
 

include/akushon/action/node/action_node.hpp

@@ -61,7 +61,7 @@ class ActionNode
   bool start(const std::string & action_name);
   bool start(const Action & action);
 
-  bool update(int time);
+  bool update(double time);
 
 private:
   void publish_joints();

include/akushon/action/process/interpolator.hpp

@@ -45,7 +45,7 @@ class Interpolator
 
   explicit Interpolator(const std::vector<Action> & actions, const Pose & initial_pose);
 
-  void process(int time);
+  void process(double time);
   bool is_finished() const;
 
   std::vector<tachimawari::joint::Joint> get_joints() const;
@@ -55,7 +55,7 @@ class Interpolator
   const Pose & get_current_pose() const;
 
   bool check_for_next();
-  void next_pose();
+  void next_pose(double time);
 
   void change_state(int state);
 
@@ -64,9 +64,9 @@ class Interpolator
   int state;
   bool init_state;
 
-  int start_stop_time;
+  double start_stop_time;
   bool init_pause;
-  int pause_time;
+  double pause_time;
 
   int current_action_index;
   int current_pose_index;

include/akushon/action/process/joint_process.hpp

@@ -25,6 +25,7 @@
 #include <vector>
 
 #include "tachimawari/joint/model/joint.hpp"
+#include "rclcpp/rclcpp.hpp"
 
 namespace akushon
 {
@@ -35,9 +36,12 @@ class JointProcess
   explicit JointProcess(uint8_t joint_id, float position = 0.0);
 
   void set_target_position(float target_position, float speed = 1.0);
+  void set_target_position_time(float target_position, double time, float action_time = 1.0);
+
   void set_initial_position(float initial_position);
 
   void interpolate();
+  void interpolate_time(double time);
 
   bool is_finished() const;
 
@@ -49,6 +53,8 @@ class JointProcess
   float target_position;
   float initial_position;
   float additional_position;
+  float action_time;
+  float initial_time;
 };
 
 }  // namespace akushon

src/akushon/action/model/action.cpp

@@ -31,7 +31,7 @@ namespace akushon
 {
 
 Action::Action(const std::string & action_name)
-: name(action_name), poses({}), start_delay(0), stop_delay(0), next_action("")
+: name(action_name), poses({}), start_delay(0), stop_delay(0), time_based(false), next_action("")
 {
 }
 

src/akushon/action/model/pose.cpp

@@ -29,7 +29,7 @@ namespace akushon
 {
 
 Pose::Pose(const std::string & pose_name)
-: name(pose_name), speed(0.0), pause(0.0), joints({})
+: name(pose_name), speed(0.0), pause(0.0), action_time(0.0), joints({})
 {
 }
 

src/akushon/action/node/action_manager.cpp

@@ -115,6 +115,7 @@ Action ActionManager::load_action(
 
           pose.set_pause(raw_pose["pause"]);
           pose.set_speed(raw_pose["speed"]);
+          pose.action_time = raw_pose["time"];
           pose.set_joints(joints);
           action.add_pose(pose);
         }
@@ -125,6 +126,8 @@ Action ActionManager::load_action(
       action.set_stop_delay(val);
     } else if (key == "next") {
       action.set_next_action(val);
+    } else if (key == "time_based") {
+      action.time_based = val;
     }
   }
 
@@ -165,7 +168,7 @@ void ActionManager::start(const Action & action, const Pose & initial_pose)
   is_running = true;
 }
 
-void ActionManager::process(int time)
+void ActionManager::process(double time)
 {
   if (interpolator) {
     interpolator->process(time);

src/akushon/action/node/action_node.cpp

@@ -116,7 +116,7 @@ bool ActionNode::start(const Action & action)
   return true;
 }
 
-bool ActionNode::update(int time)
+bool ActionNode::update(double time)
 {
   if (action_manager->is_playing()) {
     action_manager->process(time);

src/akushon/action/process/interpolator.cpp

@@ -46,7 +46,7 @@ Interpolator::Interpolator(const std::vector<Action> & actions, const Pose & ini
   }
 }
 
-void Interpolator::process(int time)
+void Interpolator::process(double time)
 {
   switch (state) {
     case START_DELAY:
@@ -75,7 +75,7 @@ void Interpolator::process(int time)
             change_state(STOP_DELAY);
             init_pause = true;
           } else if ((time - pause_time) > (get_current_pose().get_pause())) {
-            next_pose();
+            next_pose(time);
             init_pause = true;
           }
         }
@@ -106,7 +106,10 @@ void Interpolator::process(int time)
   }
 
   for (const auto & [id, joint] : joint_processes) {
-    joint_processes.at(id).interpolate();
+    if (get_current_action().time_based)
+      joint_processes.at(id).interpolate_time(time);
+    else
+      joint_processes.at(id).interpolate();
   }
 }
 
@@ -115,13 +118,15 @@ bool Interpolator::is_finished() const
   return state == END;
 }
 
-void Interpolator::next_pose()
+void Interpolator::next_pose(double time)
 {
   auto current_pose = get_current_pose();
   for (const auto & joint : current_pose.get_joints()) {
     if (joint_processes.find(joint.get_id()) != joint_processes.end()) {
-      joint_processes.at(joint.get_id()).set_target_position(
-        joint.get_position(), current_pose.get_speed());
+      if(get_current_action().time_based)
+        joint_processes.at(joint.get_id()).set_target_position_time(joint.get_position(), time, current_pose.action_time);
+      else
+        joint_processes.at(joint.get_id()).set_target_position(joint.get_position(), current_pose.get_speed());
     }
   }
   ++current_pose_index;

src/akushon/action/process/joint_process.cpp

@@ -31,10 +31,17 @@ namespace akushon
 
 JointProcess::JointProcess(uint8_t joint_id, float position)
 : joint(tachimawari::joint::Joint(joint_id, position)), initial_position(position),
-  target_position(position), additional_position(0.0)
+  target_position(position), additional_position(0.0), initial_time(0.0), action_time(0.0)
 {
 }
 
+void JointProcess::set_target_position_time(float target_position, double time, float action_time)
+{
+  this->target_position = target_position;
+  this->action_time = action_time;
+  this->initial_time = time;
+}
+
 void JointProcess::set_target_position(float target_position, float speed)
 {
   float filtered_speed = (speed > 1.0) ? 1.0 : speed;
@@ -50,6 +57,21 @@ void JointProcess::set_initial_position(float initial_position)
   this->initial_position = initial_position;
 }
 
+void JointProcess::interpolate_time(double time)
+{
+  double passed_time = time - initial_time;
+  double divider = passed_time / action_time;
+
+  if (divider >= 1.0) {
+      joint.set_position(target_position);
+      additional_position = 0.0;
+      initial_position = target_position;
+  } else {
+      additional_position = (target_position - initial_position) * divider;
+      joint.set_position(initial_position + additional_position);
+  }
+}
+
 void JointProcess::interpolate()
 {
   bool target_position_is_reached = false;
@@ -69,7 +91,7 @@ void JointProcess::interpolate()
 
 bool JointProcess::is_finished() const
 {
-  return (initial_position == target_position) || (additional_position == 0.0);
+  return (initial_position == target_position);
 }
 
 JointProcess::operator tachimawari::joint::Joint() const

src/interpolator_main.cpp

@@ -64,6 +64,7 @@ int main(int argc, char * argv[])
     }
     pose.set_pause(0.0);
     pose.set_speed(0.0);
+    pose.action_time = 0.0;
     pose.set_joints(joints);
   }