First changes to fix multiple rclcpp instances in same process

src/ign_actor_plugin/src/ActorSystem.cpp

@@ -3,7 +3,6 @@
 //
 
 #include <chrono>
-#include <cstdio>
 #include <ign_actor_plugin_msgs/action/detail/animation__struct.hpp>
 #include <ignition/common/Console.hh>
 #include <ignition/gazebo/Entity.hh>
@@ -19,6 +18,9 @@
 #include <ignition/common/MeshManager.hh>
 #include <ignition/common/SkeletonAnimation.hh>
 #include <ignition/common/Mesh.hh>
+#include <sys/mman.h>
+#include <fcntl.h>
+#include <cstdio>
 
 IGNITION_ADD_PLUGIN(
         ignition::gazebo::ActorSystem,
@@ -241,6 +243,93 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
     threadMutex.unlock();
 }
 
+struct shared_mmap
+{
+    pthread_mutex_t  mutex;
+    ActorSystemState currentState,lastState;
+};
+
+void client(std::string topic,shared_mmap *sharedMmap){
+    rclcpp::init(0, {});
+
+    auto node = rclcpp::Node::make_shared("moveService", topic);
+
+#pragma clang diagnostic push
+#pragma ide diagnostic ignored "UnusedValue"
+    auto animationServer = rclcpp_action::create_server<action::Animation>(
+            node,
+            "animation",
+            [sharedMmap](const rclcpp_action::GoalUUID goalUuid, const AnimationGoalPtr &animationGoal) {
+                if (sharedMmap->currentState == ActorSystemState::IDLE) {
+                    pthread_mutex_lock(&sharedMmap->mutex);
+                    //animationTarget = *animationGoal;
+                    pthread_mutex_unlock(&sharedMmap->mutex);
+                    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+                }else{
+                    return rclcpp_action::GoalResponse::REJECT;
+                }
+
+            },
+            [sharedMmap](const AnimationServerGoalPtr& animationGoalPtr) {
+                if (sharedMmap->currentState == ActorSystemState::IDLE) {
+                    return rclcpp_action::CancelResponse::REJECT;
+                }else{
+                    pthread_mutex_lock(&sharedMmap->mutex);
+                    sharedMmap->currentState = ActorSystemState::IDLE;
+                    pthread_mutex_unlock(&sharedMmap->mutex);
+                    return rclcpp_action::CancelResponse::ACCEPT;
+                }
+
+            },
+            [sharedMmap](const AnimationServerGoalPtr& animationGoalPtr) {
+                pthread_mutex_lock(&sharedMmap->mutex);
+                //currentAnimationGoalPtr = animationGoalPtr;
+                sharedMmap->currentState = ActorSystemState::ANIMATION;
+                pthread_mutex_unlock(&sharedMmap->mutex);
+            }
+    );
+
+
+    auto movementServer = rclcpp_action::create_server<action::Movement>(
+            node,
+            "movement",
+            [sharedMmap](const rclcpp_action::GoalUUID goalUuid, const MovementGoalPtr &movementGoal ){
+
+                if (sharedMmap->currentState == ActorSystemState::IDLE) {
+                    pthread_mutex_lock(&sharedMmap->mutex);
+                    //movementTarget = *movementGoal;
+                    pthread_mutex_unlock(&sharedMmap->mutex);
+                    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+                }else{
+                    return rclcpp_action::GoalResponse::REJECT;
+                }
+            },
+            [sharedMmap](const MovementServerGoalPtr& movementGoalPtr ){
+                if (sharedMmap->currentState == ActorSystemState::IDLE) {
+                    return rclcpp_action::CancelResponse::REJECT;
+                }else{
+                    pthread_mutex_lock(&sharedMmap->mutex);
+                    sharedMmap->currentState = ActorSystemState::IDLE;
+                    pthread_mutex_unlock(&sharedMmap->mutex);
+                    return rclcpp_action::CancelResponse::ACCEPT;
+                }
+            },
+            [sharedMmap](const MovementServerGoalPtr& movementGoalPtr ){
+                pthread_mutex_lock(&sharedMmap->mutex);
+                //currentMovementGoalPtr = movementGoalPtr;
+                sharedMmap->currentState = ActorSystemState::MOVEMENT;
+                pthread_mutex_unlock(&sharedMmap->mutex);
+            }
+    );
+#pragma clang diagnostic pop
+
+    igndbg << "Spinning node..." << std::endl;
+    while (true) {
+        printf("Spinning...\n");
+        rclcpp::spin(node);
+    }
+}
+
 // Found too late: https://github.com/AlanSixth/gazebo_ros_action_tutorial/blob/master/src/gazebo_ros_action_tutorial.cc
 // https://github.com/gazebosim/gz-sim/blob/ign-gazebo6/src/systems/follow_actor/FollowActor.cc
 void ActorSystem::Configure(const Entity &_entity, const std::shared_ptr<const sdf::Element> &_sdf,
@@ -257,90 +346,51 @@ void ActorSystem::Configure(const Entity &_entity, const std::shared_ptr<const s
     this->currentState = ActorSystemState::SETUP;
     this->lastState = ActorSystemState::IDLE;
 
-    rclcpp::init(0, {});
+    int fd = open(".", O_TMPFILE|O_RDWR, 00600);
+    auto structSize = sizeof(struct shared_mmap);
+    if(ftruncate(fd, structSize)!=0){
+        ignerr << "ftruncate" << std::endl;
+        return;
+    }
+    auto *sharedMmap = (struct shared_mmap *) mmap(nullptr,structSize,PROT_READ|PROT_WRITE,MAP_SHARED,fd,0);
+    //close(fd);
+
+    if (sharedMmap == MAP_FAILED){
+        ignerr << "MAP_FAILED" << std::endl;
+        return;
+    }
+
+    pthread_mutexattr_t mutexattr;
+    if(pthread_mutexattr_init(&mutexattr)!=0){
+        ignerr << "pthread_mutexattr_init" << std::endl;
+        return;
+    }
+    if(pthread_mutexattr_setpshared(&mutexattr, PTHREAD_PROCESS_SHARED)!=0){
+        ignerr << "pthread_mutexattr_setpshared" << std::endl;
+        return;
+    }
+    if(pthread_mutex_init(&sharedMmap->mutex, &mutexattr)!=0){
+        ignerr << "pthread_mutex_init" << std::endl;
+        return;
+    }
 
     std::string topic = "ActorPlugin";
     if (_sdf->HasElement("topic")) {
         topic = _sdf->Get<std::string>("topic");
     }
 
-    node = rclcpp::Node::make_shared("moveService", topic);
+    switch(fork()){
-    
+        case -1:
-    #pragma clang diagnostic push
+            ignerr << "Could not fork." << std::endl;
-    #pragma ide diagnostic ignored "UnusedValue"
+            break;
-    animationServer = rclcpp_action::create_server<action::Animation>(
+        case 0:
-            node,
+            ignmsg << "Child fork." << std::endl;
-            "animation",
+            client(topic,sharedMmap);
-            [this](const rclcpp_action::GoalUUID goalUuid, const AnimationGoalPtr &animationGoal) {
+            break;
-                if (this->currentState == ActorSystemState::IDLE) {
+        default:
-                    threadMutex.lock();
+            ignmsg << "Parent fork." << std::endl;
-                    animationTarget = *animationGoal;
+            break;
-                    threadMutex.unlock();
+    }
-                    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
-                }else{
-                    return rclcpp_action::GoalResponse::REJECT;
-                }
-
-            },
-            [this](const AnimationServerGoalPtr& animationGoalPtr) {
-                if (this->currentState == ActorSystemState::IDLE) {
-                    return rclcpp_action::CancelResponse::REJECT;
-                }else{
-                    threadMutex.lock();
-                    this->currentState = ActorSystemState::IDLE;
-                    threadMutex.unlock();
-                    return rclcpp_action::CancelResponse::ACCEPT;
-                }
-
-            },
-            [this](const AnimationServerGoalPtr& animationGoalPtr) {
-                threadMutex.lock();
-                currentAnimationGoalPtr = animationGoalPtr;
-                currentState = ActorSystemState::ANIMATION;
-                threadMutex.unlock();
-            }
-    );
 
 
-    movementServer = rclcpp_action::create_server<action::Movement>(
-            node,
-            "movement",
-            [this](const rclcpp_action::GoalUUID goalUuid, const MovementGoalPtr &movementGoal ){
-
-                if (this->currentState == ActorSystemState::IDLE) {
-                    threadMutex.lock();
-                    movementTarget = *movementGoal;
-                    threadMutex.unlock();
-                    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
-                }else{
-                    return rclcpp_action::GoalResponse::REJECT;
-                }
-            },
-            [this](const MovementServerGoalPtr& movementGoalPtr ){
-                if (this->currentState == ActorSystemState::IDLE) {
-                    return rclcpp_action::CancelResponse::REJECT;
-                }else{
-                    threadMutex.lock();
-                    this->currentState = ActorSystemState::IDLE;
-                    threadMutex.unlock();
-                    return rclcpp_action::CancelResponse::ACCEPT;
-                }
-            },
-            [this](const MovementServerGoalPtr& movementGoalPtr ){
-                threadMutex.lock();
-                currentMovementGoalPtr = movementGoalPtr;
-                currentState = ActorSystemState::MOVEMENT;
-                threadMutex.unlock();
-            }
-    );
-    #pragma clang diagnostic pop
-
-    igndbg << "Spinning node..." << std::endl;
-    std::thread spinThread([this]() {
-        while (true) {
-            printf("Spinning...\n");
-            rclcpp::spin(node);
-        }
-    });
-    spinThread.detach();
 }

src/ign_actor_plugin/src/ActorSystem.hpp

@@ -46,8 +46,8 @@ namespace ignition {
 
         private:
             std::shared_ptr<rclcpp::Node> node;
-            std::shared_ptr<AnimationActionServer > animationServer;
+            std::shared_ptr<AnimationActionServer> animationServer;
-            std::shared_ptr<MovementActionServer > movementServer;
+            std::shared_ptr<MovementActionServer> movementServer;
             ActorSystemState currentState, lastState;
             action::Animation::Goal animationTarget;
             action::Movement::Goal movementTarget;