From fc7a74a8e89cbd16ba34acb266cfab2caf27b3a7 Mon Sep 17 00:00:00 2001 From: Bastian Hofmann Date: Tue, 31 Jan 2023 19:37:45 +0000 Subject: [PATCH] First changes to fix multiple rclcpp instances in same process --- src/ign_actor_plugin/src/ActorSystem.cpp | 208 ++++++++++++++--------- src/ign_actor_plugin/src/ActorSystem.hpp | 4 +- 2 files changed, 131 insertions(+), 81 deletions(-) diff --git a/src/ign_actor_plugin/src/ActorSystem.cpp b/src/ign_actor_plugin/src/ActorSystem.cpp index a3e2ef8..07835d2 100644 --- a/src/ign_actor_plugin/src/ActorSystem.cpp +++ b/src/ign_actor_plugin/src/ActorSystem.cpp @@ -3,7 +3,6 @@ // #include -#include #include #include #include @@ -19,6 +18,9 @@ #include #include #include +#include +#include +#include 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( + 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( + 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 &_sdf, @@ -257,90 +346,51 @@ void ActorSystem::Configure(const Entity &_entity, const std::shared_ptrcurrentState = 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("topic"); } - - node = rclcpp::Node::make_shared("moveService", topic); - - #pragma clang diagnostic push - #pragma ide diagnostic ignored "UnusedValue" - animationServer = rclcpp_action::create_server( - node, - "animation", - [this](const rclcpp_action::GoalUUID goalUuid, const AnimationGoalPtr &animationGoal) { - if (this->currentState == ActorSystemState::IDLE) { - threadMutex.lock(); - animationTarget = *animationGoal; - 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(); - } - ); + switch(fork()){ + case -1: + ignerr << "Could not fork." << std::endl; + break; + case 0: + ignmsg << "Child fork." << std::endl; + client(topic,sharedMmap); + break; + default: + ignmsg << "Parent fork." << std::endl; + break; + } - movementServer = rclcpp_action::create_server( - 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(); } \ 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 dbbaeb4..1d2a034 100644 --- a/src/ign_actor_plugin/src/ActorSystem.hpp +++ b/src/ign_actor_plugin/src/ActorSystem.hpp @@ -46,8 +46,8 @@ namespace ignition { private: std::shared_ptr node; - std::shared_ptr animationServer; - std::shared_ptr movementServer; + std::shared_ptr animationServer; + std::shared_ptr movementServer; ActorSystemState currentState, lastState; action::Animation::Goal animationTarget; action::Movement::Goal movementTarget;