Debugging and spacing.

src/ign_actor_plugin/src/ActorSystem.cpp

@@ -21,6 +21,7 @@
 #include <sys/mman.h>
 #include <fcntl.h>
 #include <cstdio>
+#include <unistd.h>
 
 IGNITION_ADD_PLUGIN(
         ignition::gazebo::ActorSystem,
@@ -244,7 +245,7 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
 }
 
 
-void ActorSystem::client(const std::string& topic){
+void ActorSystem::rclcppServer(const std::string& topic){
     rclcpp::init(0, {});
 
     auto node = rclcpp::Node::make_shared("moveService", topic);
@@ -258,84 +259,108 @@ void ActorSystem::client(const std::string& topic){
             node,
             "animation",
             [this](const rclcpp_action::GoalUUID goalUuid, const AnimationGoalPtr &animationGoal) {
+                igndbg << "goal" << shared_mmap->currentState << std::endl;
                 if (shared_mmap->currentState == ActorSystemState::IDLE) {
                     pthread_mutex_lock(&shared_mmap->mutex);
-                    //animationTarget = *animationGoal;
+                    igndbg << "goal l" << shared_mmap->currentState << std::endl;
+
                     strcpy(shared_mmap->animation_name,animationGoal->animation_name.data());
+
                     pthread_mutex_unlock(&shared_mmap->mutex);
-                    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+                    return rclcpp_action::GoalResponse::ACCEPT_AND_DEFER;
                 }else{
                     return rclcpp_action::GoalResponse::REJECT;
                 }
             },
-            [this,&currentAnimationGoalPtr](const AnimationServerGoalPtr& animationGoalPtr) {
+            [this](const AnimationServerGoalPtr& animationGoalPtr) {
-                currentAnimationGoalPtr = nullptr;
+                igndbg << "cancel" << shared_mmap->currentState << std::endl;
                 if (shared_mmap->currentState == ActorSystemState::IDLE) {
                     return rclcpp_action::CancelResponse::REJECT;
                 }else{
                     pthread_mutex_lock(&shared_mmap->mutex);
+
                     shared_mmap->currentState = ActorSystemState::IDLE;
                     pthread_mutex_unlock(&shared_mmap->mutex);
+
                     return rclcpp_action::CancelResponse::ACCEPT;
                 }
             },
             [this,&currentAnimationGoalPtr](const AnimationServerGoalPtr& animationGoalPtr) {
+                igndbg << "accepted" << shared_mmap->currentState << std::endl;
                 pthread_mutex_lock(&shared_mmap->mutex);
+                igndbg << "accepted l" << shared_mmap->currentState << std::endl;
+
+                igndbg << "got animation handle" << std::endl;
                 currentAnimationGoalPtr = animationGoalPtr;
+
                 shared_mmap->currentState = ActorSystemState::ANIMATION;
+                currentAnimationGoalPtr->execute();
+
                 pthread_mutex_unlock(&shared_mmap->mutex);
+
             }
     );
 
-
     auto movementServer = rclcpp_action::create_server<action::Movement>(
             node,
             "movement",
             [this](const rclcpp_action::GoalUUID goalUuid, const MovementGoalPtr &movementGoal ){
-
                 if (shared_mmap->currentState == ActorSystemState::IDLE) {
                     pthread_mutex_lock(&shared_mmap->mutex);
-                    //movementTarget = *movementGoal;
+
                     shared_mmap->target_position = math::Vector3d(
                             movementGoal->target_position[0],
                             movementGoal->target_position[1],
                             movementGoal->target_position[2]);
                     shared_mmap->animation_distance = movementGoal->animation_distance;
                     strcpy(shared_mmap->animation_name,movementGoal->animation_name.data());
+
                     pthread_mutex_unlock(&shared_mmap->mutex);
                     return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
                 }else{
                     return rclcpp_action::GoalResponse::REJECT;
                 }
             },
-            [this,&currentMovementGoalPtr](const MovementServerGoalPtr& movementGoalPtr ){
+            [this](const MovementServerGoalPtr& movementGoalPtr ){
-                currentMovementGoalPtr = nullptr;
                 if (shared_mmap->currentState == ActorSystemState::IDLE) {
                     return rclcpp_action::CancelResponse::REJECT;
                 }else{
                     pthread_mutex_lock(&shared_mmap->mutex);
+
                     shared_mmap->currentState = ActorSystemState::IDLE;
+
                     pthread_mutex_unlock(&shared_mmap->mutex);
                     return rclcpp_action::CancelResponse::ACCEPT;
                 }
             },
             [this,&currentMovementGoalPtr](const MovementServerGoalPtr& movementGoalPtr ){
                 pthread_mutex_lock(&shared_mmap->mutex);
+
+                igndbg << "got movement handle" << std::endl;
                 currentMovementGoalPtr = movementGoalPtr;
                 shared_mmap->currentState = ActorSystemState::MOVEMENT;
+
                 pthread_mutex_unlock(&shared_mmap->mutex);
             }
     );
 #pragma clang diagnostic pop
 
-
     std::thread t([this,&currentAnimationGoalPtr,&currentMovementGoalPtr]() {
         float progress;
         while(true) {
             read(feedback_pipe[0], &progress, sizeof(float));
 
-            pthread_mutex_lock(&shared_mmap->mutex);
+            igndbg << progress << std::endl;
 
+            if(progress >= 1.0f){
+                if(currentAnimationGoalPtr!=nullptr){
+
+                    currentAnimationGoalPtr->succeed(std::make_shared<action::Animation::Result>());
+                }
+                if(currentMovementGoalPtr!=nullptr){
+                    currentMovementGoalPtr->succeed(std::make_shared<action::Movement::Result>());
+                }
+            }else{
                 if(currentAnimationGoalPtr!=nullptr){
                     auto feedback = std::make_shared<action::Animation::Feedback>();
                     feedback->set__progress(progress);
@@ -346,21 +371,10 @@ void ActorSystem::client(const std::string& topic){
                     feedback->set__progress(progress);
                     currentMovementGoalPtr->publish_feedback(feedback);
                 }
-
-            if(progress == 1.0){
-                if(currentAnimationGoalPtr!=nullptr){
-                    currentAnimationGoalPtr->succeed(std::make_shared<action::Animation::Result>());
             }
-                if(currentMovementGoalPtr!=nullptr){
+            usleep(500);
-                    currentMovementGoalPtr->succeed(std::make_shared<action::Movement::Result>());
-                }
-                currentMovementGoalPtr = nullptr;
-                currentAnimationGoalPtr = nullptr;
-            }
-            pthread_mutex_unlock(&shared_mmap->mutex);
         }
     });
-
     t.detach();
 
     igndbg << "Spinning node..." << std::endl;
@@ -423,7 +437,7 @@ void ActorSystem::Configure(const Entity &_entity, const std::shared_ptr<const s
 
     igndbg << "pipe setup" << std::endl;
 
-    if (pipe(feedback_pipe) < 0){
+    if (pipe(feedback_pipe)!=0){
         igndbg << "pipe failed." << std::endl;
         return;
     }
@@ -434,7 +448,7 @@ void ActorSystem::Configure(const Entity &_entity, const std::shared_ptr<const s
             break;
         case 0:
             ignmsg << "Child fork." << std::endl;
-            client(topic);
+            rclcppServer(topic);
             break;
         default:
             ignmsg << "Parent fork." << std::endl;

src/ign_actor_plugin/src/ActorSystem.hpp

@@ -88,7 +88,7 @@ namespace ignition {
 
         private:
             void switchAnimation(EntityComponentManager &_ecm, char *animationName);
-            void client(const std::string& topic);
+            void rclcppServer(const std::string& topic);
 
 
         };

test.sh

@@ -1,2 +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: 'walk', animation_speed: 1.0}"
-ros2 action send_goal /ActorPlugin/animation ign_actor_plugin_msgs/action/Animation "{animation_name: 'go', animation_speed: 1.0}"
+ros2 action send_goal /ActorPlugin/movement ign_actor_plugin_msgs/action/Movement "{animation_name: 'walk', animation_speed: 1.0, animation_distance: 1.5, target_position: [1,1,0], target_orientation: [0,0,0]}"