From 329d3fe6ec133ea29fa5b7101cca8bcc0ec08876 Mon Sep 17 00:00:00 2001
From: Bastian Hofmann <bastian.hofmann@xitaso.com>
Date: Thu, 23 Feb 2023 13:58:02 +0000
Subject: [PATCH] Fixed actor plugin deadlock.

---
 src/ign_actor_plugin/src/ActorSystem.cpp      | 29 ++++++++++++-------
 src/ign_actor_plugin/src/ActorSystem.hpp      |  2 +-
 .../launch/gazebo_controller_launch.py        | 13 +++++----
 src/moveit_test/src/test.cpp                  | 14 ++++-----
 4 files changed, 33 insertions(+), 25 deletions(-)

diff --git a/src/ign_actor_plugin/src/ActorSystem.cpp b/src/ign_actor_plugin/src/ActorSystem.cpp
index e6093dc..106419a 100644
--- a/src/ign_actor_plugin/src/ActorSystem.cpp
+++ b/src/ign_actor_plugin/src/ActorSystem.cpp
@@ -94,7 +94,11 @@ void ActorSystem::switchAnimation(EntityComponentManager &_ecm, char *animationN
 void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ecm) {
     threadMutex.lock();
 
-    
+    if(nextState != currentState){
+        igndbg << "State change before: " << currentState << " -> " << nextState << std::endl;
+        currentState = nextState;
+        sendFeedback(0.0);
+    }
 
     switch (currentState) {
     case SETUP: {
@@ -122,7 +126,7 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
             _ecm.CreateComponent(entity, components::TrajectoryPose(ignition::math::Pose3d::Zero));
         }
 
-        currentState = IDLE;
+        nextState = IDLE;
         break;
     }
     case ANIMATION: {
@@ -138,7 +142,8 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
 
         if (newTimeSeconds >= duration) {
             igndbg << "Animation " << animation_name << " finished." << std::endl;
-            currentState = IDLE;
+            nextState = IDLE;
+            sendFeedback(0.0);
             break;
         }
 
@@ -177,8 +182,9 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
         auto targetDirection = math::Vector3d(targetPose[0], targetPose[1], targetPose[2]) - actorPose.Pos();
 
         if (targetDirection.Length() < 0.05) {
+            igndbg << "Finished Movement..." << std::endl;
             sendFeedback(1.0);
-            currentState = IDLE;
+            nextState = IDLE;
             break;
         }
 
@@ -231,12 +237,8 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
         break;
     }
 
-    if (lastState != currentState) {
-        igndbg << "State change: " << lastState << " -> " << currentState << std::endl;
-        sendFeedback(0.0);
-    }
-    
     lastState = currentState;
+
     threadMutex.unlock();
 }
 
@@ -276,7 +278,7 @@ void ActorSystem::messageQueueInterface(const char name[256]) {
             // animation_speed = receivedAction.animationSpeed;
             animation_distance = receivedAction.animationDistance;
             target_position = math::Vector3d(receivedAction.positionX, receivedAction.positionY, receivedAction.positionZ);
-            currentState = receivedAction.state;
+            nextState = receivedAction.state;
 
             std::cout << "Got Action" << std::endl;
             threadMutex.unlock();
@@ -288,7 +290,11 @@ void ActorSystem::sendFeedback(double progress){
     FeedbackMessage message;
     message.progress = progress;
     message.state = currentState;
-    mq_send(feedbackQueue,(char *)&message,sizeof(FeedbackMessage),0);
+    if(mq_send(feedbackQueue,(char *)&message,sizeof(FeedbackMessage),0)==0){
+        igndbg << "Sent feedback. (State: " << lastState << "->" << currentState << " | Progress: "<< progress <<")" << std::endl;
+    }else{
+        ignerr << "Error " << errno << " sending feedback." << std::endl;
+    }
 }
 
 // Found too late: https://github.com/AlanSixth/gazebo_ros_action_tutorial/blob/master/src/gazebo_ros_action_tutorial.cc
@@ -318,6 +324,7 @@ void ActorSystem::Configure(const Entity &_entity, const std::shared_ptr<const s
 
     messageQueueInterface(topic);
 
+    nextState = SETUP;
     currentState = SETUP;
     lastState = SETUP;
 }
\ No newline at end of file
diff --git a/src/ign_actor_plugin/src/ActorSystem.hpp b/src/ign_actor_plugin/src/ActorSystem.hpp
index d282fad..030a91f 100644
--- a/src/ign_actor_plugin/src/ActorSystem.hpp
+++ b/src/ign_actor_plugin/src/ActorSystem.hpp
@@ -58,7 +58,7 @@ namespace ignition {
             math::Vector3d target_position;
             double animation_distance;
             char animation_name[256];
-            ActorPluginState currentState,lastState;
+            ActorPluginState nextState,currentState,lastState;
             double duration{};
             Entity entity{kNullEntity};
             std::mutex threadMutex;
diff --git a/src/ign_world/launch/gazebo_controller_launch.py b/src/ign_world/launch/gazebo_controller_launch.py
index 85bec5d..7f7082a 100644
--- a/src/ign_world/launch/gazebo_controller_launch.py
+++ b/src/ign_world/launch/gazebo_controller_launch.py
@@ -49,7 +49,8 @@ def generate_launch_description():
 
         Node(
             package='ros_actor_action_server',
-            executable='ros_actor_action_server'
+            executable='ros_actor_action_server',
+            output='both',
         ),
 
         IncludeLaunchDescription(
@@ -58,11 +59,11 @@ def generate_launch_description():
             ),
         ),
 
-        #IncludeLaunchDescription(
-        #    PythonLaunchDescriptionSource(
-        #        str(moveit_config.package_path / "launch/move_group.launch.py")
-        #    ),
-        #),
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                str(moveit_config.package_path / "launch/move_group.launch.py")
+            ),
+        ),
 
         RegisterEventHandler(
             event_handler=OnProcessExit(
diff --git a/src/moveit_test/src/test.cpp b/src/moveit_test/src/test.cpp
index 966dd58..b0624db 100644
--- a/src/moveit_test/src/test.cpp
+++ b/src/moveit_test/src/test.cpp
@@ -16,7 +16,7 @@ int main(int argc, char * argv[])
 
     // Create the MoveIt MoveGroup Interface
     using moveit::planning_interface::MoveGroupInterface;
-    auto move_group_interface = MoveGroupInterface(node, "iisy");
+    auto move_group_interface = MoveGroupInterface(node, "arm");
 
     // Set a target Pose
     auto const target_pose = []{
@@ -24,16 +24,16 @@ int main(int argc, char * argv[])
         msg.orientation.w = 1.0;
         msg.position.x = 0.25;
         msg.position.y = 0.25;
-        msg.position.z = 1.0;
+        msg.position.z = 0.0;
         return msg;
     }();
-    //move_group_interface.setPoseTarget(target_pose);
+    move_group_interface.setPoseTarget(target_pose);
 
-    std::map<std::string,double> states = {
-        {"base_rot",10.0}
-    };
+    //std::map<std::string,double> states = {
+    //    {"base_rot",10.0}
+    //};
 
-    move_group_interface.setJointValueTarget(states);
+    //move_group_interface.setJointValueTarget(states);
 
     // Create a plan to that target pose
     auto const [success, plan] = [&move_group_interface]{