From 3199b400d1cfe5dbdb487461fd7e7fdc4e17d7f7 Mon Sep 17 00:00:00 2001
From: Bastian Hofmann <bastian.hofmann@xitaso.com>
Date: Mon, 16 Jan 2023 12:41:34 +0000
Subject: [PATCH] Clarify movement code

---
 src/gaz_simulation/world/gaz_new_test.sdf     | 13 +++++---
 src/ign_actor_plugin/src/ActorSystem.cpp      | 32 +++----------------
 .../action/Movement.action                    |  1 +
 test.sh                                       |  2 ++
 4 files changed, 16 insertions(+), 32 deletions(-)
 create mode 100644 test.sh

diff --git a/src/gaz_simulation/world/gaz_new_test.sdf b/src/gaz_simulation/world/gaz_new_test.sdf
index c565fd7..923ba91 100644
--- a/src/gaz_simulation/world/gaz_new_test.sdf
+++ b/src/gaz_simulation/world/gaz_new_test.sdf
@@ -37,17 +37,20 @@
     
     <actor name="actor_walking">
       <plugin name="ignition::gazebo::ActorSystem" filename="/home/ros/workspace/install/ign_actor_plugin/lib/ign_actor_plugin/libActorPlugin.so">
-
       </plugin>
       <skin>
-        <filename>https://fuel.ignitionrobotics.org/1.0/Mingfei/models/actor/tip/files/meshes/walk.dae</filename>
+        <filename>/home/ros/walk.dae</filename>
         <scale>1.0</scale>
       </skin>
       <animation name='walk'>
-        <filename>https://fuel.ignitionrobotics.org/1.0/Mingfei/models/actor/tip/files/meshes/walk.dae</filename>
+        <filename>/home/ros/walk.dae</filename>
+        <interpolate_x>true</interpolate_x>
       </animation>
-      <animation name='go'>
-        <filename>/home/ros/go.dae</filename>
+      <animation name='standing_extend_arm'>
+        <filename>/home/ros/standing_extend_arm.dae</filename>
+      </animation>
+      <animation name='standing_retract_arm'>
+        <filename>/home/ros/standing_retract_arm.dae</filename>
       </animation>
       
     </actor>
diff --git a/src/ign_actor_plugin/src/ActorSystem.cpp b/src/ign_actor_plugin/src/ActorSystem.cpp
index cd3cbc7..a3e2ef8 100644
--- a/src/ign_actor_plugin/src/ActorSystem.cpp
+++ b/src/ign_actor_plugin/src/ActorSystem.cpp
@@ -17,7 +17,6 @@
 #include <ignition/gazebo/EntityComponentManager.hh>
 #include <sdf/Actor.hh>
 #include <ignition/common/MeshManager.hh>
-#include <ignition/common/Skeleton.hh>
 #include <ignition/common/SkeletonAnimation.hh>
 #include <ignition/common/Mesh.hh>
 
@@ -180,7 +179,7 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
             auto actorPose = trajectoryPoseComp->Data();
             auto targetPose = movementTarget.target_position;
 
-            auto targetDirection = actorPose.Pos() - math::Vector3d(targetPose[0],targetPose[1],targetPose[2]);
+            auto targetDirection = math::Vector3d(targetPose[0],targetPose[1],targetPose[2]) - actorPose.Pos();
 
             if (targetDirection.Length()<0.05){
                 currentMovementGoalPtr->succeed(std::make_shared<action::Movement::Result>());
@@ -200,15 +199,9 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
             }
 
             auto targetYaw = atan2(targetDirection.Y(),targetDirection.X());
-            auto currentYaw = actorPose.Rot().Yaw();
+            auto currentYaw = fmod(actorPose.Rot().Yaw(),M_PI);
 
-            auto angularDirection = fmod((targetYaw - currentYaw), 2*M_PI);
-            if (angularDirection>M_PI){
-                angularDirection-=M_PI;
-            }
-            if (angularDirection<=-M_PI){
-                angularDirection+=M_PI;
-            }
+            auto angularDirection = fmod((targetYaw - currentYaw), M_PI+0.001); // additional 0.001 rad to prevent instant flip through 180 rotation
             auto turnSpeed = 1.0;
 
             if (angularDirection < 0){
@@ -225,25 +218,10 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
                     break;
                 }
 
-                double distance;
-                if (oldTimeSeconds<=newTimeSeconds){
-                    auto oldPos = currentRootNodeAnimation->FrameAt(oldTimeSeconds).Pose().X();
-                    auto newPos = currentRootNodeAnimation->FrameAt(newTimeSeconds).Pose().X();
-                    distance = oldPos - newPos;
-                }else{
-                    auto oldPos = currentRootNodeAnimation->FrameAt(oldTimeSeconds).Pose().X();
-                    // \/
-                    auto endPos = currentRootNodeAnimation->KeyFrame(currentRootNodeAnimation->FrameCount()-1).second.Pose().X();
-                    // Big Distance
-                    auto startPos = currentRootNodeAnimation->KeyFrame(0).second.Pose().X();
-                    // \/
-                    auto newPos = currentRootNodeAnimation->FrameAt(newTimeSeconds).Pose().X();
-
-                    distance = (oldPos - endPos) + (startPos - newPos);
-                    igndbg << (oldPos - endPos) << "|" << (startPos - newPos) << std::endl;
-                }
+                double distance = (deltaTimeSeconds/duration)*movementTarget.animation_distance;
 
                 actorPose.Pos() += targetDirection.Normalize() * distance;
+
                 actorPose.Rot().Euler(0,0,targetYaw);
                 *trajectoryPoseComp = components::TrajectoryPose(actorPose);
                 *animationTimeComponent = components::AnimationTime(newTime);
diff --git a/src/ign_actor_plugin_msgs/action/Movement.action b/src/ign_actor_plugin_msgs/action/Movement.action
index 5ee69ae..9d75095 100644
--- a/src/ign_actor_plugin_msgs/action/Movement.action
+++ b/src/ign_actor_plugin_msgs/action/Movement.action
@@ -1,5 +1,6 @@
 string animation_name
 float32 animation_speed
+float32 animation_distance
 float32[3] target_position
 float32[3] target_orientation
 ---
diff --git a/test.sh b/test.sh
new file mode 100644
index 0000000..ce5d789
--- /dev/null
+++ b/test.sh
@@ -0,0 +1,2 @@
+ros2 action send_goal /ActorPlugin/movement ign_actor_plugin_msgs/action/Movement "{animation_name: 'walk', animation_speed: 1.0, target_position: [1,1,0], target_orientation: [0,0,0]}"
+ros2 action send_goal /ActorPlugin/animation ign_actor_plugin_msgs/action/Animation "{animation_name: 'go', animation_speed: 1.0}"
\ No newline at end of file