Many changes to actor system.

Changed Actions to fit their integrated functionality.

src/ign_actor_plugin/src/ActorSystem.cpp

@@ -2,71 +2,163 @@
 // Created by bastian on 31.08.22.
 //
 
+#include <chrono>
 #include <cstdio>
+#include <ign_actor_plugin_msgs/action/detail/animation__struct.hpp>
 #include <ignition/common/Console.hh>
 #include <ignition/gazebo/Entity.hh>
 #include <ignition/gazebo/Types.hh>
 #include <ignition/gazebo/components/Component.hh>
 #include <ignition/gazebo/components/Pose.hh>
-#include <ignition/msgs/pose_trajectory.pb.h>
 #include <rclcpp_action/create_server.hpp>
 #include <rclcpp_action/server.hpp>
 #include "ActorSystem.hpp"
 #include <ignition/gazebo/components/Actor.hh>
-#include <ignition/gazebo/components/Name.hh>
 #include <ignition/gazebo/EntityComponentManager.hh>
-#include <ignition/gazebo/Util.hh>
 #include <sdf/Actor.hh>
+#include <ignition/common/MeshManager.hh>
+#include <ignition/common/Skeleton.hh>
+#include <ignition/common/SkeletonAnimation.hh>
+#include <ignition/common/Mesh.hh>
 
 IGNITION_ADD_PLUGIN(
         ignition::gazebo::ActorSystem,
         ignition::gazebo::System,
         ignition::gazebo::ActorSystem::ISystemPreUpdate,
-        ignition::gazebo::ActorSystem::ISystemConfigure);
+        ignition::gazebo::ActorSystem::ISystemConfigure)
-
-using namespace ignition;
-using namespace gazebo;
 
 
+using namespace ignition::gazebo;
+using namespace ign_actor_plugin_msgs;
 
 ActorSystem::ActorSystem() = default;
 
 ActorSystem::~ActorSystem() = default;
 
-void ActorSystem::PreUpdate(const UpdateInfo &_info, ::EntityComponentManager &_ecm) {
+void ActorSystem::switchAnimation(EntityComponentManager &_ecm, std::string& animationName){
-    switch(this->actorState){
+    igndbg << "Initial setup for new animation." << std::endl;
+
+    auto actorComp = _ecm.Component<components::Actor>(entity);
+
+    auto animationNameComp = _ecm.Component<components::AnimationName>(entity);
+    *animationNameComp = components::AnimationName(animationName);
+    _ecm.SetChanged(entity, components::AnimationName::typeId, ComponentState::OneTimeChange);
+
+    auto animationTimeComponent = _ecm.Component<components::AnimationTime>(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<components::Actor>(entity);
+
+            auto animationNameComp = _ecm.Component<components::AnimationName>(entity);
+            if (nullptr == animationNameComp) {
+                _ecm.CreateComponent(entity, components::AnimationName(actorComp->Data().AnimationByIndex(0)->Name()));
+            }
+
+            auto animTimeComp = _ecm.Component<components::AnimationTime>(entity);
+            if (nullptr == animTimeComp) {
+                _ecm.CreateComponent(entity, components::AnimationTime());
+            }
+
+            ignition::math::Pose3d initialPose;
+            auto poseComp = _ecm.Component<components::Pose>(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<components::TrajectoryPose>(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) {
+            if (lastState == ActorSystemState::IDLE){
-            //    return;
+                switchAnimation(_ecm, animationTarget.animation_name);
-            //}
+            }
-            auto animationTimeComponent = _ecm.Component<components::AnimationTime>(this->entitiy);
+            auto animationTimeComponent = _ecm.Component<components::AnimationTime>(this->entity);
-            
             auto newTime = animationTimeComponent->Data() + _info.dt;
+            auto newTimeSeconds = std::chrono::duration<double>(newTime).count();
+            auto feedback = std::make_shared<action::Animation::Feedback>();
             
-            auto test = std::chrono::duration_cast<std::chrono::milliseconds>(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<action::Animation::Result>());
+                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);
+            break;
-            
-            return;
         }
         case MOVEMENT:{
-            if (movementTarget == nullptr) {
+            if (lastState == ActorSystemState::IDLE){
-                return;
+                switchAnimation(_ecm, movementTarget.animation_name);
             }
-            auto trajPoseComp = _ecm.Component<components::TrajectoryPose>(this->entitiy);
+
-    
+            auto trajectoryPoseComp = _ecm.Component<components::TrajectoryPose>(this->entity);
-            auto actorPose = trajPoseComp->Data();
+            auto actorPose = trajectoryPoseComp->Data();
             auto initialPose = actorPose;
-    
+            auto targetPose = movementTarget.target_position;
-            auto targetPose = movementTarget.get()->target_position;
     
             double distanceTraveled = (actorPose.Pos() - initialPose.Pos()).Length();
     
             auto animTimeComp = _ecm.Component<components::AnimationTime>(
-                this->entitiy);
+                this->entity);
 
             auto animTime = animTimeComp->Data() +
                     std::chrono::duration_cast<std::chrono::steady_clock::duration>(
@@ -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<const sdf::Element> &_sdf,
                             EntityComponentManager &_ecm, EventManager &) {
-    igndbg << "Plugin configuring..." << std::endl;
+    igndbg << "actor plugin configuring..." << std::endl;
-    
+
-    this->entitiy = _entity;
-    this->actorState = ActorSystemState::ANIMATION;
-    
     auto actorComp = _ecm.Component<components::Actor>(_entity);
-    if (!actorComp) {
+    if (nullptr == actorComp) {
-        ignerr << "No attached actor found, terminating." << std::endl;
+        ignerr << "Plugin not attached to actor, terminating." << std::endl;
         return;
     }
     
-    
+    this->entity = _entity;
-    
+    this->currentState = ActorSystemState::SETUP;
-    igndbg << "Found this skin file: " << actorComp->Data().SkinFilename() << std::endl;
+    this->lastState = ActorSystemState::IDLE;
-    
-    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<std::string>("animation");
-    }
-    igndbg << "Found this animation: " << animationName << std::endl;
-    
-    animationName = "walk";
-    
-    auto animationNameComp = _ecm.Component<components::AnimationName>(_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<components::AnimationTime>(_entity);
-    if (nullptr == animTimeComp) {
-        _ecm.CreateComponent(_entity, components::AnimationTime());
-    }
-    
-    ignition::math::Pose3d initialPose;
-    auto poseComp = _ecm.Component<components::Pose>(_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<components::TrajectoryPose>(_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));
-    }
     
     rclcpp::init(0, {});
 
@@ -167,46 +199,78 @@ void ActorSystem::Configure(const Entity &_entity, const std::shared_ptr<const s
     if (_sdf->HasElement("topic")) {
         topic = _sdf->Get<std::string>("topic");
     }
-    std::string name = "Actor";
-    if (_sdf->HasElement("actor_name")) {
-        name = _sdf->Get<std::string>("actor_name");
-    }
     
     node = rclcpp::Node::make_shared("moveService", topic);
     
-    animationServer = rclcpp_action::create_server<ign_actor_plugin_msgs::action::Animation>(
+    animationServer = rclcpp_action::create_server<action::Animation>(
             node,
             "animation",
-            [](rclcpp_action::GoalUUID goalUuid,
+            [this](const rclcpp_action::GoalUUID goalUuid, const AnimationGoalPtr &animationGoal) {
-                AnimationGoalPtr &animationGoal) {
+                if (this->currentState == ActorSystemState::IDLE) {
-                return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+                    threadMutex.lock();
-                return rclcpp_action::GoalResponse::REJECT;
+                    animationTarget = *animationGoal;
+                    threadMutex.unlock();
+                    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+                }else{
+                    return rclcpp_action::GoalResponse::REJECT;
+                }
+
             },
-            [](AnimationServerGoalPtr PH1) {
+            [this](const AnimationServerGoalPtr PH1) {
-                return rclcpp_action::CancelResponse::ACCEPT;
+                if (this->currentState == ActorSystemState::IDLE) {
-                return rclcpp_action::CancelResponse::REJECT;
+                    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<ign_actor_plugin_msgs::action::Movement>(
+    movementServer = rclcpp_action::create_server<action::Movement>(
             node,
             "movement",
-            [](rclcpp_action::GoalUUID goalUuid, MovementGoalPtr &movementGoal ){
+            [this](const rclcpp_action::GoalUUID goalUuid, const MovementGoalPtr &movementGoal ){
-                return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+
+                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 ){
+            [this](const MovementServerGoalPtr goalUuid ){
-                return rclcpp_action::CancelResponse::ACCEPT;
+                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);
         }
     });

src/ign_actor_plugin/src/ActorSystem.hpp

@@ -5,26 +5,36 @@
 #ifndef BUILD_ACTORSYSTEM_H
 #define BUILD_ACTORSYSTEM_H
 
+#include <cstddef>
 #include <ignition/gazebo/Entity.hh>
 #include <ignition/gazebo/System.hh>
 #include <ignition/plugin/Register.hh>
+#include <optional>
 #include <queue>
 #include <rclcpp/node.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp_action/server.hpp>
 #include <ign_actor_plugin_msgs/action/animation.hpp>
 #include <ign_actor_plugin_msgs/action/movement.hpp>
+#include <rclcpp_action/server_goal_handle.hpp>
+#include <string>
 
-#define AnimationGoalPtr const std::shared_ptr<const ign_actor_plugin_msgs::action::Animation::Goal>
+using namespace ign_actor_plugin_msgs;
-#define AnimationServerGoalPtr const std::shared_ptr<rclcpp_action::ServerGoalHandle<ign_actor_plugin_msgs::action::Animation>>&
+using rclcpp_action::ServerGoalHandle;
 
-#define MovementGoalPtr const std::shared_ptr<const ign_actor_plugin_msgs::action::Movement::Goal>
+#define AnimationActionServer rclcpp_action::Server<action::Animation>
-#define MovementServerGoalPtr const std::shared_ptr<rclcpp_action::ServerGoalHandle<ign_actor_plugin_msgs::action::Movement>>&
+#define MovementActionServer rclcpp_action::Server<action::Movement>
+
+#define AnimationGoalPtr std::shared_ptr<const action::Animation::Goal>
+#define AnimationServerGoalPtr std::shared_ptr<ServerGoalHandle<action::Animation>>
+
+#define MovementGoalPtr std::shared_ptr<const action::Movement::Goal>
+#define MovementServerGoalPtr std::shared_ptr<ServerGoalHandle<action::Movement>>
 
 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<rclcpp::Node> node;
-                std::shared_ptr<rclcpp_action::Server<ign_actor_plugin_msgs::action::Animation>> animationServer;
+                std::shared_ptr<AnimationActionServer> animationServer;
-                std::shared_ptr<rclcpp_action::Server<ign_actor_plugin_msgs::action::Movement>> movementServer;
+                std::shared_ptr<MovementActionServer> movementServer;
-                ActorSystemState actorState;
+                ActorSystemState currentState,lastState;
-                AnimationGoalPtr animationTarget;
+                action::Animation::Goal animationTarget;
-                MovementGoalPtr movementTarget;
+                action::Movement::Goal movementTarget;
-                Entity entitiy{kNullEntity};
+                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);
+            
 
         };
     }

src/ign_actor_plugin_msgs/action/Animation.action

@@ -1,6 +1,5 @@
 string animation_name
 float32 animation_speed
 ---
-bool success
 ---
 float32 progress

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