Fixes to new actor server and tree nodes

src/btree_nodes/CMakeLists.txt

@@ -50,6 +50,7 @@ set(CPP_FILES
         src/nodes/InAreaTest.cpp
         src/nodes/InterruptableSequence.cpp
         src/nodes/SetRobotVelocity.cpp
+        src/nodes/ActorAnimation.cpp
         src/nodes/ActorMovement.cpp
         )
 

src/btree_nodes/nodes.xml

@@ -19,6 +19,15 @@
             <output_port name="pose">Generated pose in area</output_port>
         </Action>
         
+        <Action ID="ActorMovement">
+            <input_port name="target">Target to move actor to</input_port>
+            <input_port name="animation">Animation name to use</input_port>
+        </Action>
+
+        <Action ID="ActorAnimation">
+            <input_port name="animation">Animation name to use</input_port>
+        </Action>
+
         <Action ID="MoveActorToTarget">
             <input_port name="current">Current actor position</input_port>
             <input_port name="target">Target to move actor to</input_port>

src/btree_nodes/src/Tree.cpp

@@ -14,6 +14,7 @@
 #include "nodes/InterruptableSequence.h"
 #include "nodes/SetRobotVelocity.h"
 #include "nodes/ActorMovement.h"
+#include "nodes/ActorAnimation.h"
 #include <chrono>
 #include <thread>
 
@@ -25,33 +26,31 @@ int main(int argc, char **argv) {
     node_options.automatically_declare_parameters_from_overrides(true);
 
     auto mainNode = rclcpp::Node::make_shared("tree_general_node", node_options);
-    auto moveNode = rclcpp::Node::make_shared("tree_moveit_node", node_options);
+
 
     rclcpp::executors::SingleThreadedExecutor executor;
-    executor.add_node(mainNode);
-    std::thread([&executor]() { executor.spin(); }).detach();
 
     BehaviorTreeFactory factory;
 
     const std::shared_ptr<Area> safeArea = std::make_shared<Area>();
-    safeArea->triangles = {std::vector<Position2D>({
+    safeArea->triangles = std::vector<Position2D>({
             {   1,     3.5 },
             {   1,     7   },
             {   3,     3.5 },
             {   7,     7   },
             {   3,    -1   },
             {   7,    -1   }
-    })};
+    });
 
     const std::shared_ptr<Area> warningArea = std::make_shared<Area>();
-    warningArea->triangles = {std::vector<Position2D>({
+    warningArea->triangles = std::vector<Position2D>({
             {  -1,     2.5 },
             {  -1,     3.5 },
             {   2,     2.5 },
             {   3,     3.5 },
             {   2,    -1   },
             {   3,    -1   }
-    })};
+    });
 
     const std::shared_ptr<Area> unsafeArea = std::make_shared<Area>();
     unsafeArea->triangles = std::vector<Position2D>({
@@ -82,7 +81,6 @@ int main(int argc, char **argv) {
     factory.registerNodeType<GenerateXYPose>("GenerateXYPose");
     factory.registerNodeType<AmICalled>("AmICalled");
 
-    //factory.registerNodeType<ActorMovement>("ActorMovement");
     factory.registerNodeType<MoveActorToTarget>("MoveActorToTarget");
     factory.registerNodeType<WeightedRandomNode>("WeightedRandom");
     factory.registerNodeType<OffsetPose>("OffsetPose");
@@ -91,17 +89,25 @@ int main(int argc, char **argv) {
 
     factory.registerNodeType<InterruptableSequence>("InterruptableSequence");
 
+    auto moveNode = rclcpp::Node::make_shared("tree_moveit_node", node_options);
     auto connection = std::make_shared<MoveConnection>(moveNode, "arm");
+    executor.add_node(moveNode);
 
-    NodeBuilder builderIisyMove = [&connection](const std::string &name, const NodeConfiguration &config) {
+    factory.registerBuilder<RobotMove>("RobotMove", [&connection](const std::string &name, const NodeConfiguration &config) {
         return std::make_unique<RobotMove>(name, config, &connection);
-    };
+    });
-    factory.registerBuilder<RobotMove>("RobotMove", builderIisyMove);
 
-    NodeBuilder builderIisyVelocity = [&connection](const std::string &name, const NodeConfiguration &config) {
+    factory.registerBuilder<SetRobotVelocity>("SetRobotVelocity", [&connection](const std::string &name, const NodeConfiguration &config) {
         return std::make_unique<SetRobotVelocity>(name, config, &connection);
+    });
+
+    
+    auto actorAnimationNode = rclcpp::Node::make_shared("actorAnimationNode",node_options);
+    NodeBuilder builderActorAnimation = [&actorAnimationNode](const std::string &name, const NodeConfiguration &config) {
+        return std::make_unique<ActorAnimation>(name, config, actorAnimationNode, "/ActorPlugin/animation");
     };
-    factory.registerBuilder<SetRobotVelocity>("SetRobotVelocity", builderIisyVelocity);
+    executor.add_node(actorAnimationNode);
+
 
     bool called;
     auto IsCalled = [&called](__attribute__((unused)) TreeNode& parent_node) -> NodeStatus{
@@ -142,6 +148,25 @@ int main(int argc, char **argv) {
 
     std::cout << "Starting subscriber." << std::endl;
 
+    std::thread([&executor]() {
+        while(rclcpp::ok()){
+            executor.spin();
+        }
+    }).detach();
+
+
+    auto actorMovementNode = rclcpp::Node::make_shared("actorMovementNode",node_options);
+    NodeBuilder builderActorMovement = [&actorMovementNode, &trees](const std::string &name, const NodeConfiguration &config) {
+        return std::make_unique<ActorMovement>(name, config, actorMovementNode, "/ActorPlugin/movement",[&trees](std::shared_ptr<const Movement::Feedback> feedback){
+            for (const auto &item : trees){
+                item->set("actorPos", feedback);
+            }
+        });
+    };
+
+    executor.add_node(actorMovementNode);
+
+
     MinimalSubscriber<geometry_msgs::msg::Pose>::createAsThread("tree_get_currentPose", "currentPose",
             [&trees](geometry_msgs::msg::Pose pose) {
                 auto sharedPose = std::make_shared<geometry_msgs::msg::Pose>(pose);

src/btree_nodes/src/nodes/ActorAnimation.cpp

@@ -0,0 +1,60 @@
+//
+// Created by bastian on 26.03.22.
+//
+
+#include "ActorAnimation.h"
+
+
+ActorAnimation::ActorAnimation(const std::string &name, const BT::NodeConfiguration &config, std::shared_ptr<rclcpp::Node> node, const std::string &actionName)
+        : BT::StatefulActionNode(name, config) {
+            client = rclcpp_action::create_client<Animation>(node,actionName);
+        }
+
+BT::PortsList ActorAnimation::providedPorts() {
+    return {
+        BT::InputPort<std::string>("animation"),
+    };
+}
+
+BT::NodeStatus ActorAnimation::onStart() {
+    std::cout << "started animation" << std::endl;
+
+    ros_actor_action_server_msgs::action::Animation::Goal goal;
+
+    auto res = getInput<std::string>("animation", goal.animation_name);
+
+    if (!res) {
+        std::cerr << "[ no animation name specified ]" << std::endl;
+        std::cout << res.error() << std::endl;
+        return BT::NodeStatus::FAILURE;
+    }
+
+    goal.animation_speed=1.0;
+
+    auto send_goal_options = Client<Animation>::SendGoalOptions();
+    send_goal_options.result_callback = [=](const ClientGoalHandle<Animation>::WrappedResult & parameter) {
+        mutex.lock();
+        if(parameter.code == ResultCode::SUCCEEDED){
+            result = BT::NodeStatus::SUCCESS;
+        }else{
+            result = BT::NodeStatus::FAILURE;
+        }
+        mutex.unlock();
+    };
+
+    client->async_send_goal(goal,send_goal_options);
+
+    return BT::NodeStatus::RUNNING;
+}
+
+BT::NodeStatus ActorAnimation::onRunning() {
+    mutex.lock();
+    auto status = result;
+    mutex.unlock();
+    return status;
+}
+
+void ActorAnimation::onHalted() {
+    std::cout << "halted animation" << std::endl;
+    client->async_cancel_all_goals();
+}

src/btree_nodes/src/nodes/ActorAnimation.h

@@ -0,0 +1,40 @@
+//
+// Created by bastian on 26.03.22.
+//
+
+#ifndef BUILD_ACTORANIMATION_H
+#define BUILD_ACTORANIMATION_H
+
+#include <behaviortree_cpp_v3/action_node.h>
+#include <behaviortree_cpp_v3/basic_types.h>
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp_action/rclcpp_action.hpp>
+#include <rclcpp_action/client_goal_handle.hpp>
+#include <geometry_msgs/msg/pose.hpp>
+#include <ros_actor_action_server_msgs/action/animation.hpp>
+#include <memory>
+#include <cstdlib>
+
+using Animation = ros_actor_action_server_msgs::action::Animation;
+using namespace rclcpp_action;
+
+class ActorAnimation : public BT::StatefulActionNode {
+    public:
+        ActorAnimation(const std::string &name, const BT::NodeConfiguration &config, std::shared_ptr<rclcpp::Node> node, const std::string &actionName);
+
+        static BT::PortsList providedPorts();
+
+        BT::NodeStatus onStart() override;
+
+        BT::NodeStatus onRunning() override;
+
+        void onHalted() override;
+
+    private:
+        BT::NodeStatus result;
+        std::mutex mutex;
+        std::shared_ptr<Client<Animation>> client;
+};
+
+
+#endif //BUILD_ACTORANIMATION_H

src/btree_nodes/src/nodes/ActorMovement.cpp

@@ -4,18 +4,17 @@
 
 #include "ActorMovement.h"
 
-#include <memory>
 
-
+ActorMovement::ActorMovement(const std::string &name, const BT::NodeConfiguration &config, std::shared_ptr<rclcpp::Node> node, const std::string &actionName, std::function<void(std::shared_ptr<const Movement::Feedback>)> positionCallback)
-ActorMovement::ActorMovement(const std::string &name, const BT::NodeConfiguration &config)
         : BT::StatefulActionNode(name, config) {
-            //this->client=client;
+            client = rclcpp_action::create_client<Movement>(node,actionName);
+            this->positionCallback = positionCallback;
         }
 
 BT::PortsList ActorMovement::providedPorts() {
     return {
-        BT::OutputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("current"),
+        BT::InputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("target"),
-        BT::InputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("target")
+        BT::InputPort<std::string>("animation"),
     };
 }
 
@@ -24,6 +23,7 @@ BT::NodeStatus ActorMovement::onStart() {
 
     ros_actor_action_server_msgs::action::Movement::Goal goal;
 
+    std::shared_ptr<geometry_msgs::msg::Pose> target;
     auto res = getInput<std::shared_ptr<geometry_msgs::msg::Pose>>("target", target);
 
     if (!res) {
@@ -37,9 +37,11 @@ BT::NodeStatus ActorMovement::onStart() {
     goal.animation_speed=1.0;
     goal.target = *target;
 
+
+
     auto send_goal_options = Client<Movement>::SendGoalOptions();
     send_goal_options.feedback_callback = [=](__attribute__((unused)) std::shared_ptr<ClientGoalHandle<Movement>> goal_handle, const std::shared_ptr<const Movement::Feedback> feedback) {
-        setOutput("current",std::make_shared<geometry_msgs::msg::Pose>(feedback->current));
+        positionCallback(feedback);
     };
     send_goal_options.result_callback = [=](const ClientGoalHandle<Movement>::WrappedResult & parameter) {
         mutex.lock();
@@ -51,8 +53,6 @@ BT::NodeStatus ActorMovement::onStart() {
         mutex.unlock();
     };
 
-    auto node = rclcpp::Node::make_shared("actorMovementPublisher");
-    client = rclcpp_action::create_client<Movement>(node,"/actorPlugin/movement");
     client->async_send_goal(goal,send_goal_options);
 
     return BT::NodeStatus::RUNNING;

src/btree_nodes/src/nodes/ActorMovement.h

@@ -20,12 +20,10 @@ using namespace rclcpp_action;
 
 class ActorMovement : public BT::StatefulActionNode {
     public:
-        ActorMovement(const std::string &name, const BT::NodeConfiguration &config);
+        ActorMovement(const std::string &name, const BT::NodeConfiguration &config, std::shared_ptr<rclcpp::Node> node, const std::string &actionName, std::function<void(std::shared_ptr<const Movement::Feedback>)> positionCallback);
 
         static BT::PortsList providedPorts();
 
-        std::shared_ptr<geometry_msgs::msg::Pose> target;
-
         BT::NodeStatus onStart() override;
 
         BT::NodeStatus onRunning() override;
@@ -33,6 +31,7 @@ class ActorMovement : public BT::StatefulActionNode {
         void onHalted() override;
 
     private:
+        std::function<void(std::shared_ptr<const Movement::Feedback>)> positionCallback;
         BT::NodeStatus result;
         std::mutex mutex;
         std::shared_ptr<Client<Movement>> client;

src/ign_actor_plugin/src/ActorSystem.cpp

@@ -3,24 +3,6 @@
 //
 
 #include "ActorSystem.hpp"
-#include <chrono>
-#include <cstdio>
-#include <fcntl.h>
-#include <ignition/common/Console.hh>
-#include <ignition/common/Mesh.hh>
-#include <ignition/common/MeshManager.hh>
-#include <ignition/common/SkeletonAnimation.hh>
-#include <ignition/gazebo/Entity.hh>
-#include <ignition/gazebo/EntityComponentManager.hh>
-#include <ignition/gazebo/Types.hh>
-#include <ignition/gazebo/components/Actor.hh>
-#include <ignition/gazebo/components/Component.hh>
-#include <ignition/gazebo/components/Pose.hh>
-#include <rclcpp_action/create_server.hpp>
-#include <rclcpp_action/server.hpp>
-#include <sdf/Actor.hh>
-#include <sys/mman.h>
-#include <unistd.h>
 
 IGNITION_ADD_PLUGIN(ignition::gazebo::ActorSystem, ignition::gazebo::System, ignition::gazebo::ActorSystem::ISystemPreUpdate,
                     ignition::gazebo::ActorSystem::ISystemConfigure)
@@ -66,11 +48,6 @@ void ActorSystem::switchAnimation(EntityComponentManager &_ecm, char *animationN
         auto rootNode = currentSkeleton->RootNode()->Name();
 
         currentRootNodeAnimation = currentSkeleton->Animation(0)->NodeAnimationByName(rootNode);
-        currentTranslationAlign = currentSkeleton->AlignTranslation(0, rootNode);
-        currentRotationAlign = currentSkeleton->AlignRotation(0, rootNode);
-
-        igndbg << "Translation: " << currentTranslationAlign << std::endl;
-        igndbg << "Rotation: " << currentRotationAlign << std::endl;
 
         if (currentSkeleton != nullptr) {
             duration = currentSkeleton->Animation(0)->Length();
@@ -91,6 +68,15 @@ void ActorSystem::switchAnimation(EntityComponentManager &_ecm, char *animationN
     }
 }
 
+double Angle(ignition::math::Quaterniond a,ignition::math::Quaterniond b){
+    auto dot = fmin(abs(a.Dot(b)),1.0);
+    if(dot > 0.999999){
+        return 0.0;
+    }else{
+        return acos(dot) * 2;
+    }
+}
+
 void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ecm) {
     threadMutex.lock();
 
@@ -127,6 +113,8 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
             _ecm.CreateComponent(entity, components::TrajectoryPose(startPose));
         }
 
+        addPoseToFeedback(startPose);
+
         nextState = IDLE;
         break;
     }
@@ -152,44 +140,11 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
         *animationTimeComponent = components::AnimationTime(newTime);
         _ecm.SetChanged(entity, components::AnimationTime::typeId, ComponentState::OneTimeChange);
 
-        /*
-        if(currentRootNodeAnimation == nullptr){
-            break;
-        }
-
-        auto rootTransformation = currentRootNodeAnimation->FrameAt(newTimeSeconds,false);
-        math::Matrix4d totalTf = currentTranslationAlign * rootTransformation * currentRotationAlign;
-
-        auto translation = totalTf.Translation();
-        translation[0] = 0;
-        totalTf.SetTranslation(translation);
-
-        auto poseComp = _ecm.Component<components::Pose>(entity);
-        *poseComp = components::Pose (totalTf.Pose());
-        _ecm.SetChanged(entity, components::Pose::typeId, ComponentState::OneTimeChange);
-        */
-
         break;
     }
     case MOVEMENT: {
-        if (lastState == IDLE) {
-            igndbg << "Starting Movement..." << std::endl;
-            switchAnimation(_ecm, animation_name);
-        }
-
         auto trajectoryPoseComp = _ecm.Component<components::TrajectoryPose>(this->entity);
         auto actorPose = trajectoryPoseComp->Data();
-        auto targetPose = target_position;
-
-        auto targetDirection = math::Vector3d(targetPose[0], targetPose[1], targetPose[2]) - actorPose.Pos();
-
-        if (targetDirection.Length() < 0.05) {
-            igndbg << "Finished Movement..." << std::endl;
-            feedback.progress = 1.0f;
-            sendFeedback();
-            nextState = IDLE;
-            break;
-        }
 
         auto animationTimeComponent = _ecm.Component<components::AnimationTime>(this->entity);
         auto oldTimeSeconds = std::chrono::duration<double>(animationTimeComponent->Data()).count();
@@ -197,44 +152,95 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
         auto newTimeSeconds = oldTimeSeconds + deltaTimeSeconds;
         auto newTime = animationTimeComponent->Data() + _info.dt;
 
-        if (newTimeSeconds >= duration) {
+        auto turnSpeed = 1;
-            newTimeSeconds -= duration;
+
-            newTime = std::chrono::duration_cast<std::chrono::steady_clock::duration>(std::chrono::duration<double>(newTimeSeconds));
+        if (lastState == IDLE) {
+            igndbg << "Starting Movement..." << std::endl;
+            switchAnimation(_ecm, animation_name);
+            
+            movementDetails.state = movementDetails.ROTATE_TO_TARGET;
+            movementDetails.time = 0.0;
+            movementDetails.stepTime = 0.0;
+            movementDetails.start = actorPose;
+            movementDetails.targetDiff.Pos() = movementDetails.target.Pos() - movementDetails.start.Pos();
+
+            if(movementDetails.targetDiff.Pos().Length() >= 0.001){
+                movementDetails.targetDiff.Rot() = ignition::math::Quaterniond::EulerToQuaternion(0.0, 0.0, atan2(movementDetails.targetDiff.Pos().Y(), movementDetails.targetDiff.Pos().X()));
+                movementDetails.rotateToTargetDuration = Angle(actorPose.Rot(),movementDetails.targetDiff.Rot()) / turnSpeed;
+                movementDetails.moveDuration = movementDetails.targetDiff.Pos().Length() / animation_distance * duration;
+            }else{
+                movementDetails.targetDiff.Rot() = movementDetails.start.Rot();
+                movementDetails.rotateToTargetDuration = 0.0;
+                movementDetails.moveDuration = 0.0;
             }
 
-        auto targetYaw = atan2(targetDirection.Y(), targetDirection.X());
+            movementDetails.rotateToEndDuration = Angle(movementDetails.targetDiff.Rot(),movementDetails.target.Rot()) / turnSpeed;
-        auto currentYaw = fmod(actorPose.Rot().Yaw(), M_PI);
-
-        auto angularDirection = fmod((targetYaw - currentYaw),
-                                     M_PI + 0.001); // additional 0.001 rad to prevent instant flip through 180 rotation
-        auto turnSpeed = 1.0;
-
-        if (angularDirection < 0) {
-            turnSpeed = -turnSpeed;
         }
         
-        if (abs(angularDirection) > 0.01) {
+        movementDetails.time += deltaTimeSeconds;
-            actorPose.Rot().Euler(0, 0, actorPose.Rot().Yaw() + (turnSpeed * deltaTimeSeconds));
+        movementDetails.stepTime += deltaTimeSeconds;
+
+        if (movementDetails.state == movementDetails.ROTATE_TO_TARGET){
+            if(movementDetails.stepTime<=movementDetails.rotateToTargetDuration){
+                actorPose.Rot() = ignition::math::Quaterniond::Slerp(movementDetails.stepTime/movementDetails.rotateToTargetDuration,movementDetails.start.Rot(),movementDetails.targetDiff.Rot(),true);
+            }else{
+                actorPose.Rot() = movementDetails.targetDiff.Rot();
+                movementDetails.state = movementDetails.MOVE;
+                movementDetails.stepTime -= movementDetails.rotateToTargetDuration;
+            }
+        }
+
+        if (movementDetails.state == movementDetails.MOVE){
+            if(movementDetails.stepTime<=movementDetails.moveDuration){
+                newTime = std::chrono::duration_cast<std::chrono::steady_clock::duration>(std::chrono::duration<double>(fmod(movementDetails.stepTime,duration)));
+                actorPose.Pos()=movementDetails.start.Pos()+(movementDetails.targetDiff.Pos()*(movementDetails.stepTime/movementDetails.moveDuration));
+            }else{
+                actorPose.Pos()=movementDetails.target.Pos();
+                movementDetails.state = movementDetails.ROTATE_TO_END;
+                movementDetails.stepTime -= movementDetails.moveDuration;
+            }
+        }
+
+        if (movementDetails.state == movementDetails.ROTATE_TO_END){
+            if(movementDetails.stepTime<=movementDetails.rotateToEndDuration){
+                actorPose.Rot() = ignition::math::Quaterniond::Slerp(movementDetails.stepTime/movementDetails.rotateToEndDuration,movementDetails.targetDiff.Rot(),movementDetails.target.Rot(),true);
+            }else{
+                actorPose.Rot() = movementDetails.target.Rot();
+                movementDetails.state = movementDetails.END;
+                movementDetails.stepTime -= movementDetails.rotateToEndDuration;
+            }
+        }
+
+        if (movementDetails.state == movementDetails.END || movementDetails.state  == movementDetails.ROTATE_TO_END){
+            if (newTime.count() < duration / 2){
+                newTime = std::chrono::duration_cast<std::chrono::steady_clock::duration>(std::chrono::duration<double>(newTimeSeconds-(deltaTimeSeconds)));
+                if(newTime.count()<=0){
+                    newTime = std::chrono::duration_cast<std::chrono::steady_clock::duration>(std::chrono::duration<double>(0.0));
+                    if(movementDetails.state==movementDetails.END){
+                        nextState = IDLE;
+                    }
+                }
+            }else{
+                newTime = std::chrono::duration_cast<std::chrono::steady_clock::duration>(std::chrono::duration<double>(newTimeSeconds+(deltaTimeSeconds)));
+                if(newTime.count()>=duration){
+                    newTime = std::chrono::duration_cast<std::chrono::steady_clock::duration>(std::chrono::duration<double>(duration));
+                    if(movementDetails.state == movementDetails.END){
+                        nextState = IDLE;
+                    }
+                }
+            }
+        }
+
+        addPoseToFeedback(actorPose);
+        feedback.progress = movementDetails.time / (movementDetails.rotateToTargetDuration + movementDetails.moveDuration + movementDetails.rotateToEndDuration);
+        sendFeedback();
 
         *trajectoryPoseComp = components::TrajectoryPose(actorPose);
         _ecm.SetChanged(entity, components::TrajectoryPose::typeId, ComponentState::OneTimeChange);
-        } else {
-            if (nullptr == currentRootNodeAnimation) {
-                ignerr << "Current animation doesn't move root node, this is unsupported for movement animations." << std::endl;
-                break;
-            }
 
-            double distance = (deltaTimeSeconds / duration) * animation_distance;
-
-            actorPose.Pos() += targetDirection.Normalize() * distance;
-
-            actorPose.Rot().Euler(0, 0, targetYaw);
-            *trajectoryPoseComp = components::TrajectoryPose(actorPose);
         *animationTimeComponent = components::AnimationTime(newTime);
-
-            _ecm.SetChanged(entity, components::TrajectoryPose::typeId, ComponentState::OneTimeChange);
         _ecm.SetChanged(entity, components::AnimationTime::typeId, ComponentState::OneTimeChange);
-        }
+
         break;
     }
     case IDLE:
@@ -294,26 +300,35 @@ void ActorSystem::messageQueueInterface(const char name[256]) {
 
             mq_receive(actionQueue, (char *)&receivedAction, sizeof(ActionMessage), nullptr);
 
-            igndbg << "Got Action" << std::endl;
-
             threadMutex.lock();
 
             strcpy(animation_name, receivedAction.animationName);
             // animation_speed = receivedAction.animationSpeed;
             animation_distance = receivedAction.animationDistance;
-            target_position = math::Vector3d(receivedAction.target.position.x, receivedAction.target.position.y, receivedAction.target.position.z);
+            movementDetails.target.Pos().Set(receivedAction.target.position.x, receivedAction.target.position.y, receivedAction.target.position.z);
+            movementDetails.target.Rot().Set(receivedAction.target.orientation.w, receivedAction.target.orientation.x, receivedAction.target.orientation.y, receivedAction.target.orientation.z);
             nextState = receivedAction.state;
             
+            igndbg << "Received Action; State: " << nextState << " Target: " << movementDetails.target.Pos() << " | " << movementDetails.target.Rot().Z() << std::endl;
+            
             threadMutex.unlock();
         }
     }).detach();
 }
 
+void ActorSystem::addPoseToFeedback(ignition::math::Pose3<double> pose){
+    feedback.current.position.x=pose.X();
+    feedback.current.position.y=pose.Y();
+    feedback.current.position.z=pose.Z();
+    feedback.current.orientation.w=pose.Rot().W();
+    feedback.current.orientation.x=pose.Rot().X();
+    feedback.current.orientation.y=pose.Rot().Y();
+    feedback.current.orientation.z=pose.Rot().Z();
+}
+
 void ActorSystem::sendFeedback(){
     feedback.state = currentState;
-    if(mq_send(feedbackQueue,(char *)&feedback,sizeof(FeedbackMessage),0)==0){
+    if(mq_send(feedbackQueue,(char *)&feedback,sizeof(FeedbackMessage),0)!=0){
-        igndbg << "Sent feedback. (State: " << lastState << "->" << currentState << " | Progress: "<< feedback.progress <<")" << std::endl;
-    }else{
         ignerr << "Error " << errno << " sending feedback." << std::endl;
     }
 }

src/ign_actor_plugin/src/ActorSystem.hpp

@@ -6,24 +6,44 @@
 #define BUILD_ACTORSYSTEM_H
 
 #include <cstddef>
-#include <ignition/gazebo/Entity.hh>
+#include <fcntl.h>
-#include <ignition/gazebo/System.hh>
+#include <mqueue.h>
-#include <ignition/plugin/Register.hh>
+#include <chrono>
+#include <cstdio>
+#include <fcntl.h>
+#include <unistd.h>
 #include <optional>
 #include <queue>
+#include <string>
+#include <cmath>
+
+#include <sys/stat.h>
+#include <sys/mman.h>
+
+#include <ignition/gazebo/System.hh>
+#include <ignition/plugin/Register.hh>
+#include <ignition/common/Console.hh>
+#include <ignition/common/Mesh.hh>
+#include <ignition/common/MeshManager.hh>
+#include <ignition/common/SkeletonAnimation.hh>
+#include <ignition/common/Skeleton.hh>
+#include <ignition/common/NodeAnimation.hh>
+#include <ignition/gazebo/Entity.hh>
+#include <ignition/gazebo/EntityComponentManager.hh>
+#include <ignition/gazebo/Types.hh>
+#include <ignition/gazebo/components/Actor.hh>
+#include <ignition/gazebo/components/Component.hh>
+#include <ignition/gazebo/components/Pose.hh>
+#include <sdf/Actor.hh>
+
 #include <rclcpp/node.hpp>
 #include <rclcpp/rclcpp.hpp>
+#include <rclcpp_action/create_server.hpp>
 #include <rclcpp_action/server.hpp>
+#include <rclcpp_action/server_goal_handle.hpp>
 #include <ros_actor_action_server_msgs/action/animation.hpp>
 #include <ros_actor_action_server_msgs/action/movement.hpp>
 #include <ros_actor_message_queue_msgs/MessageTypes.hpp>
-#include <rclcpp_action/server_goal_handle.hpp>
-#include <string>
-#include <ignition/common/Skeleton.hh>
-#include <ignition/common/NodeAnimation.hh>
-#include <fcntl.h>
-#include <sys/stat.h>
-#include <mqueue.h>
 
 using namespace ros_actor_message_queue_msgs;
 using namespace ros_actor_action_server_msgs;
@@ -41,22 +61,13 @@ using rclcpp_action::ServerGoalHandle;
 namespace ignition {
 namespace gazebo {
 
-class ActorSystem :
+class ActorSystem : public System,
-                public System,
                     public ISystemPreUpdate,
                     public ISystemConfigure {
 
     private:
-    //std::shared_ptr<rclcpp::Node> node;
+        ignition::math::Pose3d startPose = ignition::math::Pose3d::Zero;
-    //std::shared_ptr<AnimationActionServer> animationServer;
+        ignition::math::Pose3d target_pose;
-    //std::shared_ptr<MovementActionServer> movementServer;
-    //ActorSystemState currentState, lastState;
-    //action::Animation::Goal animationTarget;
-    //action::Movement::Goal movementTarget;
-    //AnimationServerGoalPtr currentAnimationGoalPtr;
-    //MovementServerGoalPtr currentMovementGoalPtr;
-    ignition::math::v6::Pose3d startPose = ignition::math::Pose3d::Zero;
-    math::Vector3d target_position;
         double animation_distance;
         char animation_name[256];
         ActorPluginState nextState,currentState,lastState;
@@ -65,22 +76,22 @@ class ActorSystem :
         std::mutex threadMutex;
         std::shared_ptr<common::Skeleton> currentSkeleton;
         common::NodeAnimation* currentRootNodeAnimation;
-    math::Matrix4<double> currentRotationAlign, currentTranslationAlign;
         mqd_t feedbackQueue;
         mqd_t actionQueue;
         FeedbackMessage feedback;
 
+        struct {
+            enum {ROTATE_TO_TARGET,MOVE,ROTATE_TO_END,END} state;
+            ignition::math::Pose3d start;
+            ignition::math::Pose3d targetDiff;
+            ignition::math::Pose3d target;
+            double rotateToTargetDuration,moveDuration,rotateToEndDuration,time,stepTime;
+        } movementDetails;
+
     public:
         ActorSystem();
-
-  public:
         ~ActorSystem() override;
-
+        void PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ecm) override;
-  public:
-    void PreUpdate(const UpdateInfo &_info,
-                   EntityComponentManager &_ecm) override;
-
-  public:
         void Configure(const ignition::gazebo::Entity &animationGoalPtr,
                        const std::shared_ptr<const sdf::Element> &_sdf,
                        EntityComponentManager &_ecm,
@@ -92,9 +103,8 @@ class ActorSystem :
         void sendFeedback();
         void addPoseToFeedback(ignition::math::Pose3<double> pose);
 
-
+};
-        };
+}
-    }
 }
 
 #endif //BUILD_ACTORSYSTEM_H

src/ros_actor_action_server/src/Server.cpp

@@ -86,6 +86,7 @@ int main(int argc, char **argv) {
                     currentAnimationGoalPtr->publish_feedback(feedback);
                 }else{
                     currentAnimationGoalPtr->succeed(std::make_shared<ros_actor_action_server_msgs::action::Animation_Result>());
+                    currentAnimationGoalPtr == nullptr;
                 }
             }
             if(currentAction.state==MOVEMENT && currentMovementGoalPtr!=nullptr){
@@ -95,10 +96,10 @@ int main(int argc, char **argv) {
                     currentMovementGoalPtr->publish_feedback(feedback);
                 }else{
                     currentMovementGoalPtr->succeed(std::make_shared<ros_actor_action_server_msgs::action::Movement_Result>());
+                    currentMovementGoalPtr == nullptr;
                 }
             }
             currentFeedback = newFeedback;
-            std::cout << "Got feedback, State: " << currentFeedback.state << " | Progress: " << currentFeedback.progress << std::endl;
             mutex.unlock();
         }
     }).detach();