From 16e33e647d6a556e9a546131acb3816155b116f1 Mon Sep 17 00:00:00 2001 From: Bastian Hofmann Date: Thu, 2 Feb 2023 06:18:24 +0000 Subject: [PATCH] Added full shmem implementation, working without feedback. --- src/ign_actor_plugin/src/ActorSystem.cpp | 191 ++++++++++++++--------- src/ign_actor_plugin/src/ActorSystem.hpp | 31 ++-- 2 files changed, 140 insertions(+), 82 deletions(-) diff --git a/src/ign_actor_plugin/src/ActorSystem.cpp b/src/ign_actor_plugin/src/ActorSystem.cpp index 07835d2..e9e2f60 100644 --- a/src/ign_actor_plugin/src/ActorSystem.cpp +++ b/src/ign_actor_plugin/src/ActorSystem.cpp @@ -36,7 +36,7 @@ ActorSystem::ActorSystem() = default; ActorSystem::~ActorSystem() = default; -void ActorSystem::switchAnimation(EntityComponentManager &_ecm, std::string& animationName){ +void ActorSystem::switchAnimation(EntityComponentManager &_ecm, char* animationName){ igndbg << "Initial setup for new animation." << std::endl; auto actorComp = _ecm.Component(entity); @@ -83,8 +83,6 @@ void ActorSystem::switchAnimation(EntityComponentManager &_ecm, std::string& ani ignerr << "No skeleton found!" << std::endl; } - - auto initialPose = currentRootNodeAnimation -> FrameAt(0).Pose(); auto poseComponent = _ecm.Component(entity); @@ -99,8 +97,8 @@ void ActorSystem::switchAnimation(EntityComponentManager &_ecm, std::string& ani } void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ecm) { - threadMutex.lock(); - switch(this->currentState){ + pthread_mutex_lock(&shared_mmap->mutex); + switch(shared_mmap->currentState){ case SETUP: { igndbg << "Setting up actor plugin" << std::endl; @@ -126,25 +124,25 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec _ecm.CreateComponent(entity, components::TrajectoryPose(ignition::math::Pose3d::Zero)); } - currentState = ActorSystemState::IDLE; + shared_mmap->currentState = ActorSystemState::IDLE; break; } case ANIMATION: { - if (lastState == ActorSystemState::IDLE){ - switchAnimation(_ecm, animationTarget.animation_name); + if (shared_mmap->lastState == ActorSystemState::IDLE){ + switchAnimation(_ecm, shared_mmap->animation_name); } auto animationTimeComponent = _ecm.Component(this->entity); auto newTime = animationTimeComponent->Data() + _info.dt; auto newTimeSeconds = std::chrono::duration(newTime).count(); - auto feedback = std::make_shared(); - - feedback->set__progress((float) (newTimeSeconds/duration)); - currentAnimationGoalPtr->publish_feedback(feedback); + + auto feedback = (float) (newTimeSeconds/duration); + write(feedback_pipe[1],&feedback,sizeof(float)); + //currentAnimationGoalPtr->publish_feedback(feedback); if (newTimeSeconds >= duration){ - currentAnimationGoalPtr->succeed(std::make_shared()); - igndbg << "Animation " << animationTarget.animation_name << " finished." << std::endl; - currentState = ActorSystemState::IDLE; + //currentAnimationGoalPtr->succeed(std::make_shared()); + igndbg << "Animation " << shared_mmap->animation_name << " finished." << std::endl; + shared_mmap->currentState = ActorSystemState::IDLE; break; } @@ -172,20 +170,22 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec break; } case MOVEMENT:{ - if (lastState == ActorSystemState::IDLE){ + if (shared_mmap->lastState == ActorSystemState::IDLE){ igndbg << "Starting Movement..." << std::endl; - switchAnimation(_ecm, movementTarget.animation_name); + switchAnimation(_ecm, shared_mmap->animation_name); } auto trajectoryPoseComp = _ecm.Component(this->entity); auto actorPose = trajectoryPoseComp->Data(); - auto targetPose = movementTarget.target_position; + auto targetPose = shared_mmap->target_position; auto targetDirection = math::Vector3d(targetPose[0],targetPose[1],targetPose[2]) - actorPose.Pos(); if (targetDirection.Length()<0.05){ - currentMovementGoalPtr->succeed(std::make_shared()); - currentState = ActorSystemState::IDLE; + float feedback = 1.0f; + write(feedback_pipe[1],&feedback,sizeof(float)); + //currentMovementGoalPtr->succeed(std::make_shared()); + shared_mmap->currentState = ActorSystemState::IDLE; break; } @@ -220,7 +220,7 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec break; } - double distance = (deltaTimeSeconds/duration)*movementTarget.animation_distance; + double distance = (deltaTimeSeconds/duration)*shared_mmap->animation_distance; actorPose.Pos() += targetDirection.Normalize() * distance; @@ -234,58 +234,56 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec break; } case IDLE: - if (lastState != currentState){ + if (shared_mmap->lastState != shared_mmap->currentState){ igndbg << "Now idling..." << std::endl; } break; } - lastState = currentState; - threadMutex.unlock(); + shared_mmap->lastState = shared_mmap->currentState; + pthread_mutex_unlock(&shared_mmap->mutex); } -struct shared_mmap -{ - pthread_mutex_t mutex; - ActorSystemState currentState,lastState; -}; -void client(std::string topic,shared_mmap *sharedMmap){ +void ActorSystem::client(const std::string& topic){ rclcpp::init(0, {}); auto node = rclcpp::Node::make_shared("moveService", topic); + AnimationServerGoalPtr currentAnimationGoalPtr; + MovementServerGoalPtr currentMovementGoalPtr; + #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); + [this](const rclcpp_action::GoalUUID goalUuid, const AnimationGoalPtr &animationGoal) { + if (shared_mmap->currentState == ActorSystemState::IDLE) { + pthread_mutex_lock(&shared_mmap->mutex); //animationTarget = *animationGoal; - pthread_mutex_unlock(&sharedMmap->mutex); + strcpy(shared_mmap->animation_name,animationGoal->animation_name.data()); + pthread_mutex_unlock(&shared_mmap->mutex); return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; }else{ return rclcpp_action::GoalResponse::REJECT; } - }, - [sharedMmap](const AnimationServerGoalPtr& animationGoalPtr) { - if (sharedMmap->currentState == ActorSystemState::IDLE) { + [this,¤tAnimationGoalPtr](const AnimationServerGoalPtr& animationGoalPtr) { + currentAnimationGoalPtr = nullptr; + if (shared_mmap->currentState == ActorSystemState::IDLE) { return rclcpp_action::CancelResponse::REJECT; }else{ - pthread_mutex_lock(&sharedMmap->mutex); - sharedMmap->currentState = ActorSystemState::IDLE; - pthread_mutex_unlock(&sharedMmap->mutex); + pthread_mutex_lock(&shared_mmap->mutex); + shared_mmap->currentState = ActorSystemState::IDLE; + pthread_mutex_unlock(&shared_mmap->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); + [this,¤tAnimationGoalPtr](const AnimationServerGoalPtr& animationGoalPtr) { + pthread_mutex_lock(&shared_mmap->mutex); + currentAnimationGoalPtr = animationGoalPtr; + shared_mmap->currentState = ActorSystemState::ANIMATION; + pthread_mutex_unlock(&shared_mmap->mutex); } ); @@ -293,36 +291,78 @@ void client(std::string topic,shared_mmap *sharedMmap){ auto movementServer = rclcpp_action::create_server( node, "movement", - [sharedMmap](const rclcpp_action::GoalUUID goalUuid, const MovementGoalPtr &movementGoal ){ + [this](const rclcpp_action::GoalUUID goalUuid, const MovementGoalPtr &movementGoal ){ - if (sharedMmap->currentState == ActorSystemState::IDLE) { - pthread_mutex_lock(&sharedMmap->mutex); + if (shared_mmap->currentState == ActorSystemState::IDLE) { + pthread_mutex_lock(&shared_mmap->mutex); //movementTarget = *movementGoal; - pthread_mutex_unlock(&sharedMmap->mutex); + 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; } }, - [sharedMmap](const MovementServerGoalPtr& movementGoalPtr ){ - if (sharedMmap->currentState == ActorSystemState::IDLE) { + [this,¤tMovementGoalPtr](const MovementServerGoalPtr& movementGoalPtr ){ + currentMovementGoalPtr = nullptr; + if (shared_mmap->currentState == ActorSystemState::IDLE) { return rclcpp_action::CancelResponse::REJECT; }else{ - pthread_mutex_lock(&sharedMmap->mutex); - sharedMmap->currentState = ActorSystemState::IDLE; - pthread_mutex_unlock(&sharedMmap->mutex); + pthread_mutex_lock(&shared_mmap->mutex); + shared_mmap->currentState = ActorSystemState::IDLE; + pthread_mutex_unlock(&shared_mmap->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); + [this,¤tMovementGoalPtr](const MovementServerGoalPtr& movementGoalPtr ){ + pthread_mutex_lock(&shared_mmap->mutex); + currentMovementGoalPtr = movementGoalPtr; + shared_mmap->currentState = ActorSystemState::MOVEMENT; + pthread_mutex_unlock(&shared_mmap->mutex); } ); #pragma clang diagnostic pop + + std::thread t([this,¤tAnimationGoalPtr,¤tMovementGoalPtr]() { + float progress; + while(true) { + read(feedback_pipe[0], &progress, sizeof(float)); + + pthread_mutex_lock(&shared_mmap->mutex); + + if(currentAnimationGoalPtr!=nullptr){ + auto feedback = std::make_shared(); + feedback->set__progress(progress); + currentAnimationGoalPtr->publish_feedback(feedback); + } + if(currentMovementGoalPtr!=nullptr){ + auto feedback = std::make_shared(); + feedback->set__progress(progress); + currentMovementGoalPtr->publish_feedback(feedback); + } + + if(progress == 1.0){ + if(currentAnimationGoalPtr!=nullptr){ + currentAnimationGoalPtr->succeed(std::make_shared()); + } + if(currentMovementGoalPtr!=nullptr){ + currentMovementGoalPtr->succeed(std::make_shared()); + } + currentMovementGoalPtr = nullptr; + currentAnimationGoalPtr = nullptr; + } + pthread_mutex_unlock(&shared_mmap->mutex); + } + }); + + t.detach(); + igndbg << "Spinning node..." << std::endl; while (true) { printf("Spinning...\n"); @@ -341,21 +381,25 @@ void ActorSystem::Configure(const Entity &_entity, const std::shared_ptrHasElement("topic")) { + topic = _sdf->Get("topic"); + } this->entity = _entity; - this->currentState = ActorSystemState::SETUP; - this->lastState = ActorSystemState::IDLE; + igndbg << "shmem setup" << std::endl; int fd = open(".", O_TMPFILE|O_RDWR, 00600); - auto structSize = sizeof(struct shared_mmap); + auto structSize = sizeof(struct sharedMmap); 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); + shared_mmap = (struct sharedMmap *) mmap(nullptr,structSize,PROT_READ|PROT_WRITE,MAP_SHARED,fd,0); //close(fd); - if (sharedMmap == MAP_FAILED){ + if (shared_mmap == MAP_FAILED){ ignerr << "MAP_FAILED" << std::endl; return; } @@ -369,14 +413,19 @@ void ActorSystem::Configure(const Entity &_entity, const std::shared_ptrmutex, &mutexattr)!=0){ + if(pthread_mutex_init(&shared_mmap->mutex, &mutexattr)!=0){ ignerr << "pthread_mutex_init" << std::endl; return; } - std::string topic = "ActorPlugin"; - if (_sdf->HasElement("topic")) { - topic = _sdf->Get("topic"); + shared_mmap->currentState = ActorSystemState::SETUP; + shared_mmap->lastState = ActorSystemState::IDLE; + + igndbg << "pipe setup" << std::endl; + + if (pipe(feedback_pipe) < 0){ + igndbg << "pipe failed." << std::endl; + return; } switch(fork()){ @@ -385,12 +434,10 @@ void ActorSystem::Configure(const Entity &_entity, const std::shared_ptr node; - std::shared_ptr animationServer; - std::shared_ptr movementServer; - ActorSystemState currentState, lastState; - action::Animation::Goal animationTarget; - action::Movement::Goal movementTarget; - AnimationServerGoalPtr currentAnimationGoalPtr; - MovementServerGoalPtr currentMovementGoalPtr; + //std::shared_ptr node; + //std::shared_ptr animationServer; + //std::shared_ptr movementServer; + //ActorSystemState currentState, lastState; + //action::Animation::Goal animationTarget; + //action::Movement::Goal movementTarget; + //AnimationServerGoalPtr currentAnimationGoalPtr; + //MovementServerGoalPtr currentMovementGoalPtr; double duration{}; Entity entity{kNullEntity}; - std::mutex threadMutex; + //std::mutex threadMutex; std::shared_ptr currentSkeleton; common::NodeAnimation* currentRootNodeAnimation; math::Matrix4 currentRotationAlign, currentTranslationAlign; + struct sharedMmap* shared_mmap; + int feedback_pipe[2]; public: ActorSystem(); @@ -77,7 +87,8 @@ namespace ignition { EventManager &/*_eventMgr*/) override; private: - void switchAnimation(EntityComponentManager &_ecm, std::string &animationName); + void switchAnimation(EntityComponentManager &_ecm, char *animationName); + void client(const std::string& topic); };