From 38263dd13fdc89e26a1c1682ede5684fa9d5b3bb Mon Sep 17 00:00:00 2001 From: Bastian Hofmann Date: Tue, 20 Dec 2022 15:34:08 +0000 Subject: [PATCH] Many changes to actor system. Changed Actions to fit their integrated functionality. --- src/ign_actor_plugin/src/ActorSystem.cpp | 302 +++++++++++------- src/ign_actor_plugin/src/ActorSystem.hpp | 39 ++- .../action/Animation.action | 1 - .../action/Movement.action | 1 - 4 files changed, 211 insertions(+), 132 deletions(-) diff --git a/src/ign_actor_plugin/src/ActorSystem.cpp b/src/ign_actor_plugin/src/ActorSystem.cpp index 043abaa..27935d1 100644 --- a/src/ign_actor_plugin/src/ActorSystem.cpp +++ b/src/ign_actor_plugin/src/ActorSystem.cpp @@ -2,71 +2,163 @@ // Created by bastian on 31.08.22. // +#include #include +#include #include #include #include #include #include -#include #include #include #include "ActorSystem.hpp" #include -#include #include -#include #include +#include +#include +#include +#include IGNITION_ADD_PLUGIN( ignition::gazebo::ActorSystem, ignition::gazebo::System, ignition::gazebo::ActorSystem::ISystemPreUpdate, - ignition::gazebo::ActorSystem::ISystemConfigure); - -using namespace ignition; -using namespace gazebo; + ignition::gazebo::ActorSystem::ISystemConfigure) +using namespace ignition::gazebo; +using namespace ign_actor_plugin_msgs; ActorSystem::ActorSystem() = default; ActorSystem::~ActorSystem() = default; -void ActorSystem::PreUpdate(const UpdateInfo &_info, ::EntityComponentManager &_ecm) { - switch(this->actorState){ +void ActorSystem::switchAnimation(EntityComponentManager &_ecm, std::string& animationName){ + igndbg << "Initial setup for new animation." << std::endl; + + auto actorComp = _ecm.Component(entity); + + auto animationNameComp = _ecm.Component(entity); + *animationNameComp = components::AnimationName(animationName); + _ecm.SetChanged(entity, components::AnimationName::typeId, ComponentState::OneTimeChange); + + auto animationTimeComponent = _ecm.Component(entity); + *animationTimeComponent = components::AnimationTime(std::chrono::milliseconds::zero()); + _ecm.SetChanged(entity, components::AnimationTime::typeId, ComponentState::OneTimeChange); + + int foundIndex = -1; + for (int animationIndex = 0 ; animationIndex < actorComp->Data().AnimationCount() ; animationIndex++) { + auto animation = actorComp->Data().AnimationByIndex(animationIndex); + if (animation->Name() == animationName){ + foundIndex = animationIndex; + break; + } + } + + if(foundIndex!=-1){ + igndbg << "Found animation to play" << std::endl; + auto fileName = actorComp->Data().AnimationByIndex(foundIndex)->Filename(); + igndbg << "Path: " << fileName << std::endl; + igndbg << "Name: " << common::MeshManager::Instance()->Load(fileName)->Name() << std::endl; + + auto skeleton = common::MeshManager::Instance()->Load(fileName)->MeshSkeleton(); + + if (skeleton != nullptr){ + duration = skeleton->Animation(0)->Length(); + igndbg << "length: " << duration << " seconds." << std::endl; + } else { + ignerr << "No skeleton found!" << std::endl; + } + } else { + ignerr << "No animation found!" << std::endl; + } +} + +void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ecm) { + threadMutex.lock(); + switch(this->currentState){ + case SETUP: { + igndbg << "Setting up actor plugin" << std::endl; + + auto actorComp = _ecm.Component(entity); + + auto animationNameComp = _ecm.Component(entity); + if (nullptr == animationNameComp) { + _ecm.CreateComponent(entity, components::AnimationName(actorComp->Data().AnimationByIndex(0)->Name())); + } + + auto animTimeComp = _ecm.Component(entity); + if (nullptr == animTimeComp) { + _ecm.CreateComponent(entity, components::AnimationTime()); + } + + ignition::math::Pose3d initialPose; + auto poseComp = _ecm.Component(entity); + if (nullptr == poseComp) { + _ecm.CreateComponent(entity, components::Pose(ignition::math::Pose3d::Zero)); + } else { + initialPose = poseComp->Data(); + + // We'll be setting the actor's X/Y pose with respect to the world. So we + // zero the current values. + auto newPose = initialPose; + newPose.Pos().X(0); + newPose.Pos().Y(0); + *poseComp = components::Pose(newPose); + } + + + + auto trajectoryPoseComp = _ecm.Component(entity); + if (nullptr == trajectoryPoseComp) { + // Leave Z to the pose component, control only 2D with Trajectory + //initialPose.Pos().Z(0); + _ecm.CreateComponent(entity, components::TrajectoryPose(initialPose)); + } + + currentState = ActorSystemState::IDLE; + break; + } case ANIMATION: { - //if (animationTarget == nullptr) { - // return; - //} - auto animationTimeComponent = _ecm.Component(this->entitiy); - + if (lastState == ActorSystemState::IDLE){ + switchAnimation(_ecm, animationTarget.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(); - auto test = std::chrono::duration_cast(newTime); + feedback->set__progress((float) (newTimeSeconds/duration)); + currentAnimationGoalPtr->publish_feedback(feedback); - ignwarn << "Animation state: " << test.count() << "\n"; + if (newTimeSeconds >= duration){ + currentAnimationGoalPtr->succeed(std::make_shared()); + igndbg << "Animation " << animationTarget.animation_name << " finished." << std::endl; + currentState = ActorSystemState::IDLE; + break; + } + *animationTimeComponent = components::AnimationTime(newTime); + _ecm.SetChanged(entity, components::AnimationTime::typeId, ComponentState::OneTimeChange); - _ecm.SetChanged(entitiy, components::AnimationTime::typeId, ComponentState::OneTimeChange); - - return; + break; } case MOVEMENT:{ - if (movementTarget == nullptr) { - return; + if (lastState == ActorSystemState::IDLE){ + switchAnimation(_ecm, movementTarget.animation_name); } - auto trajPoseComp = _ecm.Component(this->entitiy); - - auto actorPose = trajPoseComp->Data(); + + auto trajectoryPoseComp = _ecm.Component(this->entity); + auto actorPose = trajectoryPoseComp->Data(); auto initialPose = actorPose; - - auto targetPose = movementTarget.get()->target_position; + auto targetPose = movementTarget.target_position; double distanceTraveled = (actorPose.Pos() - initialPose.Pos()).Length(); auto animTimeComp = _ecm.Component( - this->entitiy); + this->entity); auto animTime = animTimeComp->Data() + std::chrono::duration_cast( @@ -76,90 +168,30 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, ::EntityComponentManager &_ ) ); *animTimeComp = components::AnimationTime(animTime); - return; + break; } case IDLE: - return; + break; } - + lastState = currentState; + threadMutex.unlock(); } // 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, EntityComponentManager &_ecm, EventManager &) { - igndbg << "Plugin configuring..." << std::endl; - - this->entitiy = _entity; - this->actorState = ActorSystemState::ANIMATION; - + igndbg << "actor plugin configuring..." << std::endl; + auto actorComp = _ecm.Component(_entity); - if (!actorComp) { - ignerr << "No attached actor found, terminating." << std::endl; + if (nullptr == actorComp) { + ignerr << "Plugin not attached to actor, terminating." << std::endl; return; } - - - igndbg << "Found this skin file: " << actorComp->Data().SkinFilename() << std::endl; - - std::string animationName = "go"; - - if (!_sdf->HasElement("animation")) { - if (actorComp->Data().AnimationCount() < 1) { - ignerr << "Actor SDF doesn't have any animations." << std::endl; - return; - } - animationName = actorComp->Data().AnimationByIndex(0)->Name(); - } else { - animationName = _sdf->Get("animation"); - } - igndbg << "Found this animation: " << animationName << std::endl; - - animationName = "walk"; - - auto animationNameComp = _ecm.Component(_entity); - if (nullptr == animationNameComp) { - _ecm.CreateComponent(_entity, components::AnimationName(animationName)); - } else { - *animationNameComp = components::AnimationName(animationName); - } - - - for (const auto& elem: _ecm.ComponentTypes(_entity)) { - igndbg << "Component: " << elem << std::endl; - } - - _ecm.SetChanged(_entity, components::AnimationName::typeId, ComponentState::OneTimeChange); - - auto animTimeComp = _ecm.Component(_entity); - if (nullptr == animTimeComp) { - _ecm.CreateComponent(_entity, components::AnimationTime()); - } - - ignition::math::Pose3d initialPose; - auto poseComp = _ecm.Component(_entity); - if (nullptr == poseComp) { - _ecm.CreateComponent(_entity, components::Pose( - ignition::math::Pose3d::Zero)); - } else { - initialPose = poseComp->Data(); - - // We'll be setting the actor's X/Y pose with respect to the world. So we - // zero the current values. - auto newPose = initialPose; - newPose.Pos().X(0); - newPose.Pos().Y(0); - *poseComp = components::Pose(newPose); - } - - - auto trajPoseComp = _ecm.Component(_entity); - if (nullptr == trajPoseComp) { - // Leave Z to the pose component, control only 2D with Trajectory - initialPose.Pos().Z(0); - _ecm.CreateComponent(_entity, components::TrajectoryPose(initialPose)); - } + this->entity = _entity; + this->currentState = ActorSystemState::SETUP; + this->lastState = ActorSystemState::IDLE; rclcpp::init(0, {}); @@ -167,46 +199,78 @@ void ActorSystem::Configure(const Entity &_entity, const std::shared_ptrHasElement("topic")) { topic = _sdf->Get("topic"); } - std::string name = "Actor"; - if (_sdf->HasElement("actor_name")) { - name = _sdf->Get("actor_name"); - } node = rclcpp::Node::make_shared("moveService", topic); - animationServer = rclcpp_action::create_server( + animationServer = rclcpp_action::create_server( node, "animation", - [](rclcpp_action::GoalUUID goalUuid, - AnimationGoalPtr &animationGoal) { - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; - return rclcpp_action::GoalResponse::REJECT; + [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; + } + }, - [](AnimationServerGoalPtr PH1) { - return rclcpp_action::CancelResponse::ACCEPT; - return rclcpp_action::CancelResponse::REJECT; + [this](const AnimationServerGoalPtr PH1) { + if (this->currentState == ActorSystemState::IDLE) { + return rclcpp_action::CancelResponse::REJECT; + }else{ + threadMutex.lock(); + this->currentState = ActorSystemState::IDLE; + threadMutex.unlock(); + return rclcpp_action::CancelResponse::ACCEPT; + } + }, - [](AnimationServerGoalPtr PH1) { - + [this](const AnimationServerGoalPtr PH1) { + threadMutex.lock(); + currentAnimationGoalPtr = PH1; + currentState = ActorSystemState::ANIMATION; + threadMutex.unlock(); } ); - movementServer = rclcpp_action::create_server( + movementServer = rclcpp_action::create_server( node, "movement", - [](rclcpp_action::GoalUUID goalUuid, MovementGoalPtr &movementGoal ){ - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + [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; + } }, - [](MovementServerGoalPtr goalUuid ){ - return rclcpp_action::CancelResponse::ACCEPT; + [this](const MovementServerGoalPtr goalUuid ){ + if (this->currentState == ActorSystemState::IDLE) { + return rclcpp_action::CancelResponse::REJECT; + }else{ + threadMutex.lock(); + this->currentState = ActorSystemState::IDLE; + threadMutex.unlock(); + return rclcpp_action::CancelResponse::ACCEPT; + } }, - [](MovementServerGoalPtr goalUuid ){} + [this](const MovementServerGoalPtr goalUuid ){ + threadMutex.lock(); + currentMovementGoalPtr = goalUuid; + currentState = ActorSystemState::MOVEMENT; + threadMutex.unlock(); + } ); igndbg << "Spinning node..." << std::endl; std::thread spinThread([this]() { while (true) { - igndbg << "Wheee..." << std::endl; + printf("Spinning...\n"); rclcpp::spin(node); } }); diff --git a/src/ign_actor_plugin/src/ActorSystem.hpp b/src/ign_actor_plugin/src/ActorSystem.hpp index 6fb7ca0..137cc93 100644 --- a/src/ign_actor_plugin/src/ActorSystem.hpp +++ b/src/ign_actor_plugin/src/ActorSystem.hpp @@ -5,26 +5,36 @@ #ifndef BUILD_ACTORSYSTEM_H #define BUILD_ACTORSYSTEM_H +#include #include #include #include +#include #include #include #include #include #include #include +#include +#include -#define AnimationGoalPtr const std::shared_ptr -#define AnimationServerGoalPtr const std::shared_ptr>& +using namespace ign_actor_plugin_msgs; +using rclcpp_action::ServerGoalHandle; -#define MovementGoalPtr const std::shared_ptr -#define MovementServerGoalPtr const std::shared_ptr>& +#define AnimationActionServer rclcpp_action::Server +#define MovementActionServer rclcpp_action::Server + +#define AnimationGoalPtr std::shared_ptr +#define AnimationServerGoalPtr std::shared_ptr> + +#define MovementGoalPtr std::shared_ptr +#define MovementServerGoalPtr std::shared_ptr> namespace ignition { namespace gazebo { enum ActorSystemState{ - IDLE,MOVEMENT,ANIMATION + SETUP,IDLE,MOVEMENT,ANIMATION }; class ActorSystem: @@ -34,12 +44,16 @@ namespace ignition { private: std::shared_ptr node; - std::shared_ptr> animationServer; - std::shared_ptr> movementServer; - ActorSystemState actorState; - AnimationGoalPtr animationTarget; - MovementGoalPtr movementTarget; - Entity entitiy{kNullEntity}; + 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; public: ActorSystem(); @@ -57,6 +71,9 @@ namespace ignition { EntityComponentManager &_ecm, EventManager &/*_eventMgr*/) override; + private: + void switchAnimation(EntityComponentManager &_ecm, std::string& animationName); + }; } diff --git a/src/ign_actor_plugin_msgs/action/Animation.action b/src/ign_actor_plugin_msgs/action/Animation.action index a3b9dc8..11677dd 100644 --- a/src/ign_actor_plugin_msgs/action/Animation.action +++ b/src/ign_actor_plugin_msgs/action/Animation.action @@ -1,6 +1,5 @@ string animation_name float32 animation_speed --- -bool success --- float32 progress diff --git a/src/ign_actor_plugin_msgs/action/Movement.action b/src/ign_actor_plugin_msgs/action/Movement.action index 664f850..5ee69ae 100644 --- a/src/ign_actor_plugin_msgs/action/Movement.action +++ b/src/ign_actor_plugin_msgs/action/Movement.action @@ -3,6 +3,5 @@ float32 animation_speed float32[3] target_position float32[3] target_orientation --- -bool success --- float32 progress