Changed message types and error handling to better suit behavior tree.

src/btree_nodes/CMakeLists.txt

@@ -12,10 +12,11 @@ set(CMAKE_POSITION_INDEPENDENT_CODE ON)
 set(DEPENDENCIES
         ament_cmake
         rclcpp
+        rclcpp_action
         #geometry_msgs
         std_msgs
         behaviortree_cpp_v3
-        rclcpp_action
+
         tf2_geometry_msgs
         moveit_core
         moveit_ros_planning_interface

src/btree_nodes/package.xml

@@ -9,6 +9,7 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
   <depend>rclcpp</depend>
+  <depend>rclcpp_action</depend>
   <depend>geometry_msgs</depend>
   <depend>ros_actor_action_server_msgs</depend>
   <test_depend>ament_lint_auto</test_depend>

src/btree_nodes/src/Tree.cpp

@@ -82,7 +82,7 @@ int main(int argc, char **argv) {
     factory.registerNodeType<GenerateXYPose>("GenerateXYPose");
     factory.registerNodeType<AmICalled>("AmICalled");
 
-    factory.registerNodeType<ActorMovement>("ActorMovement");
+    //factory.registerNodeType<ActorMovement>("ActorMovement");
     factory.registerNodeType<MoveActorToTarget>("MoveActorToTarget");
     factory.registerNodeType<WeightedRandomNode>("WeightedRandom");
     factory.registerNodeType<OffsetPose>("OffsetPose");
@@ -104,7 +104,7 @@ int main(int argc, char **argv) {
     factory.registerBuilder<SetRobotVelocity>("SetRobotVelocity", builderIisyVelocity);
 
     bool called;
-    auto IsCalled = [&called](TreeNode& parent_node) -> NodeStatus{
+    auto IsCalled = [&called](__attribute__((unused)) TreeNode& parent_node) -> NodeStatus{
         return called ? NodeStatus::SUCCESS : NodeStatus::FAILURE;
     };
 

src/btree_nodes/src/nodes/ActorMovement.cpp

@@ -4,61 +4,68 @@
 
 #include "ActorMovement.h"
 
-BT::ActorMovement::ActorMovement(const std::string &name, const BT::NodeConfiguration &config)
+#include <memory>
-        : StatefulActionNode(name, config) {}
 
-BT::PortsList BT::ActorMovement::providedPorts() {
+
+ActorMovement::ActorMovement(const std::string &name, const BT::NodeConfiguration &config)
+        : BT::StatefulActionNode(name, config) {
+            //this->client=client;
+        }
+
+BT::PortsList ActorMovement::providedPorts() {
     return {
-            InputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("current"),
+        BT::OutputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("current"),
-            InputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("target")
+        BT::InputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("target")
     };
 }
 
-BT::NodeStatus BT::ActorMovement::onStart() {
+BT::NodeStatus ActorMovement::onStart() {
     std::cout << "started moving" << std::endl;
 
-    rclcpp::Node node("targetPublisherNode");
+    ros_actor_action_server_msgs::action::Movement::Goal goal;
-    auto publisher = node.create_publisher<geometry_msgs::msg::Pose>("targetPose", 10);
+
     auto res = getInput<std::shared_ptr<geometry_msgs::msg::Pose>>("target", target);
 
     if (!res) {
         std::cout << "[ no target available ]" << std::endl;
         std::cout << res.error() << std::endl;
-        return NodeStatus::FAILURE;
+        return BT::NodeStatus::FAILURE;
     }
 
-    publisher->publish(*target);
+    goal.animation_distance=1.5;
-    return NodeStatus::RUNNING;
+    goal.animation_name="walk";
+    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));
+    };
+    send_goal_options.result_callback = [=](const ClientGoalHandle<Movement>::WrappedResult & parameter) {
+        mutex.lock();
+        if(parameter.code == ResultCode::SUCCEEDED){
+            result = BT::NodeStatus::SUCCESS;
+        }else{
+            result = BT::NodeStatus::FAILURE;
+        }
+        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;
 }
 
-BT::NodeStatus BT::ActorMovement::onRunning() {
+BT::NodeStatus ActorMovement::onRunning() {
-    std::shared_ptr<geometry_msgs::msg::Pose> current;
+    mutex.lock();
-
+    auto status = result;
-    auto res = getInput<std::shared_ptr<geometry_msgs::msg::Pose>>("current", current);
+    mutex.unlock();
-    if (!res) {
+    return status;
-        std::cout << "[ no current position available ]" << std::endl;
-        std::cout << res.error() << std::endl;
-        return NodeStatus::FAILURE;
-    }
-
-    double dX = target->position.x - current->position.x;
-    double dY = target->position.y - current->position.y;
-
-    auto distance = sqrt(dX * dX + dY * dY);
-
-    if (distance < 0.3) {
-        return NodeStatus::SUCCESS;
-    } else {
-        return NodeStatus::RUNNING;
-    }
 }
 
-void BT::ActorMovement::onHalted() {
+void ActorMovement::onHalted() {
     std::cout << "halted move" << std::endl;
-    rclcpp::Node node("targetPublisherNode");
+    client->async_cancel_all_goals();
-    auto publisher = node.create_publisher<geometry_msgs::msg::Pose>("targetPose", 10);
-    geometry_msgs::msg::Pose inf;
-    inf.position.x = HUGE_VAL;
-    inf.position.y = HUGE_VAL;
-    publisher->publish(inf);
 }

src/btree_nodes/src/nodes/ActorMovement.h

@@ -6,25 +6,38 @@
 #define BUILD_ACTORMOVEMENT_H
 
 #include <behaviortree_cpp_v3/action_node.h>
-#include <geometry_msgs/msg/pose.hpp>
+#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/movement.hpp>
+#include <memory>
+#include <cstdlib>
 
-namespace BT {
+using Movement = ros_actor_action_server_msgs::action::Movement;
-    class ActorMovement : public StatefulActionNode {
+using namespace rclcpp_action;
+
+class ActorMovement : public BT::StatefulActionNode {
     public:
-        ActorMovement(const std::string &name, const NodeConfiguration &config);
+        ActorMovement(const std::string &name, const BT::NodeConfiguration &config);
 
-        static PortsList providedPorts();
+        static BT::PortsList providedPorts();
 
         std::shared_ptr<geometry_msgs::msg::Pose> target;
 
-        NodeStatus onStart() override;
+        BT::NodeStatus onStart() override;
 
-        NodeStatus onRunning() override;
+        BT::NodeStatus onRunning() override;
 
         void onHalted() override;
-    };
+
-}
+    private:
+        BT::NodeStatus result;
+        std::mutex mutex;
+        std::shared_ptr<Client<Movement>> client;
+        //rclcpp::Client<ros_actor_action_server_msgs::action::Movement>::FutureAndRequestId currentRequest;
+};
 
 
 #endif //BUILD_ACTORMOVEMENT_H

src/btree_nodes/src/nodes/MoveConnection.cpp

@@ -26,7 +26,7 @@ void MoveConnection::planAndExecute(const std::shared_ptr<geometry_msgs::msg::Po
         std::cout<<"Parameters set."<<std::endl;
 
         moveit::planning_interface::MoveGroupInterface::Plan plan;
-        if(moveGroup.plan(plan)==moveit::planning_interface::MoveItErrorCode::SUCCESS){
+        if(moveGroup.plan(plan)==moveit::core::MoveItErrorCode::SUCCESS){
             std::cout<<"Planned a path."<<std::endl;
         }else{
             std::cout<<"Error during planning."<<std::endl;
@@ -38,7 +38,7 @@ void MoveConnection::planAndExecute(const std::shared_ptr<geometry_msgs::msg::Po
             lock.unlock();
             return;
         }
-        bool success = moveGroup.execute(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS;
+        bool success = moveGroup.execute(plan) == moveit::core::MoveItErrorCode::SUCCESS;
         if(!cancelled) {
             callback(success);
         }

src/ign_actor_plugin/src/ActorSystem.cpp

@@ -97,7 +97,8 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
     if(nextState != currentState){
         igndbg << "State change before: " << currentState << " -> " << nextState << std::endl;
         currentState = nextState;
-        sendFeedback(0.0);
+        feedback.progress = 0.0f;
+        sendFeedback();
     }
 
     switch (currentState) {
@@ -123,7 +124,7 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
 
         auto trajectoryPoseComponent = _ecm.Component<components::TrajectoryPose>(entity);
         if (nullptr == trajectoryPoseComponent) {
-            _ecm.CreateComponent(entity, components::TrajectoryPose(ignition::math::Pose3d::Zero));
+            _ecm.CreateComponent(entity, components::TrajectoryPose(startPose));
         }
 
         nextState = IDLE;
@@ -137,13 +138,14 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
         auto newTime = animationTimeComponent->Data() + _info.dt;
         auto newTimeSeconds = std::chrono::duration<double>(newTime).count();
 
-        auto feedback = newTimeSeconds / duration;
+        feedback.progress = newTimeSeconds / duration;
-        sendFeedback(feedback);
+        sendFeedback();
 
         if (newTimeSeconds >= duration) {
             igndbg << "Animation " << animation_name << " finished." << std::endl;
             nextState = IDLE;
-            sendFeedback(0.0);
+            feedback.progress = 0.0f;
+            sendFeedback();
             break;
         }
 
@@ -183,7 +185,8 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
 
         if (targetDirection.Length() < 0.05) {
             igndbg << "Finished Movement..." << std::endl;
-            sendFeedback(1.0);
+            feedback.progress = 1.0f;
+            sendFeedback();
             nextState = IDLE;
             break;
         }
@@ -212,6 +215,7 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
 
         if (abs(angularDirection) > 0.01) {
             actorPose.Rot().Euler(0, 0, actorPose.Rot().Yaw() + (turnSpeed * deltaTimeSeconds));
+
             *trajectoryPoseComp = components::TrajectoryPose(actorPose);
             _ecm.SetChanged(entity, components::TrajectoryPose::typeId, ComponentState::OneTimeChange);
         } else {
@@ -243,55 +247,72 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
 }
 
 void ActorSystem::messageQueueInterface(const char name[256]) {
-    mq_attr queueAttributes;
-    queueAttributes.mq_flags = 0;
-    queueAttributes.mq_curmsgs = 0;
-    queueAttributes.mq_msgsize = sizeof(ros_actor_message_queue_msgs::FeedbackMessage);
-    queueAttributes.mq_maxmsg = 1;
     char generatedName[256];
+
     strcpy(generatedName,name);
     strcat(generatedName,"Feedback");
-    feedbackQueue = mq_open(generatedName, O_CREAT | O_RDWR, S_IRWXU | S_IRWXG, &queueAttributes);
 
-    if (feedbackQueue == (mqd_t)-1) {
+    igndbg << "Waiting for " << generatedName << std::endl;
-        ignerr << "Could not create queue. (" << errno << ")" << std::endl;
+    feedbackQueue = (mqd_t) -1;
-        return;
+    while(feedbackQueue == (mqd_t) -1){
+        feedbackQueue = mq_open(generatedName, O_RDWR);
+
+        if (feedbackQueue == (mqd_t) -1) {
+            if(errno!=ENOENT){
+                perror(nullptr);
+                return;
+            }
+            mq_close(feedbackQueue);
+            sleep(100);
+        }
     }
 
     strcpy(generatedName,name);
     strcat(generatedName,"Action");
-    queueAttributes.mq_msgsize = sizeof(ros_actor_message_queue_msgs::ActionMessage);
-    actionQueue = mq_open(generatedName, O_CREAT | O_RDWR, S_IRWXU | S_IRWXG, &queueAttributes);
 
-    if (actionQueue == (mqd_t)-1) {
+    igndbg << "Waiting for " << generatedName << std::endl;
-        ignerr << "Could not create queue. (" << errno << ")" << std::endl;
+    actionQueue = (mqd_t) -1;
-        return;
+    while(actionQueue == (mqd_t) -1){
+        actionQueue = mq_open(generatedName, O_RDWR);
+
+        if (actionQueue == (mqd_t) -1) {
+            if(errno!=ENOENT){
+                perror(nullptr);
+                return;
+            }
+            mq_close(actionQueue);
+            sleep(100);
+        }
     }
 
+    igndbg << "Queues online, starting listener" << std::endl;
+
     std::thread([this] {
         ActionMessage receivedAction;
         while (true) {
+            igndbg << "Waiting for action" << std::endl;
+
             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.positionX, receivedAction.positionY, receivedAction.positionZ);
+            target_position = math::Vector3d(receivedAction.target.position.x, receivedAction.target.position.y, receivedAction.target.position.z);
             nextState = receivedAction.state;
 
-            std::cout << "Got Action" << std::endl;
             threadMutex.unlock();
         }
     }).detach();
 }
 
-void ActorSystem::sendFeedback(double progress){
+void ActorSystem::sendFeedback(){
-    FeedbackMessage message;
+    feedback.state = currentState;
-    message.progress = progress;
+    if(mq_send(feedbackQueue,(char *)&feedback,sizeof(FeedbackMessage),0)==0){
-    message.state = currentState;
+        igndbg << "Sent feedback. (State: " << lastState << "->" << currentState << " | Progress: "<< feedback.progress <<")" << std::endl;
-    if(mq_send(feedbackQueue,(char *)&message,sizeof(FeedbackMessage),0)==0){
-        igndbg << "Sent feedback. (State: " << lastState << "->" << currentState << " | Progress: "<< progress <<")" << std::endl;
     }else{
         ignerr << "Error " << errno << " sending feedback." << std::endl;
     }
@@ -320,6 +341,21 @@ void ActorSystem::Configure(const Entity &_entity, const std::shared_ptr<const s
         strcpy(topic, topicString.c_str());
     }
 
+    if (_sdf->HasElement("x")) {
+        auto x = _sdf->Get<float>("x");
+        startPose.SetX(x);
+    }
+
+    if (_sdf->HasElement("y")) {
+        auto y = _sdf->Get<float>("y");
+        startPose.SetY(y);
+    }
+
+    if (_sdf->HasElement("rot")) {
+        auto rot = _sdf->Get<float>("rot");
+        startPose.Rot().Euler(0,0,rot);
+    }
+
     this->entity = _entity;
 
     messageQueueInterface(topic);

src/ign_actor_plugin/src/ActorSystem.hpp

@@ -39,55 +39,58 @@ using rclcpp_action::ServerGoalHandle;
 #define MovementServerGoalPtr std::shared_ptr<ServerGoalHandle<action::Movement>>
 
 namespace ignition {
-    namespace gazebo {
+namespace gazebo {
 
-        class ActorSystem :
+class ActorSystem :
                 public System,
-                public ISystemPreUpdate,
+                    public ISystemPreUpdate,
-                public ISystemConfigure {
+                    public ISystemConfigure {
 
-        private:
+  private:
-            //std::shared_ptr<rclcpp::Node> node;
+    //std::shared_ptr<rclcpp::Node> node;
-            //std::shared_ptr<AnimationActionServer> animationServer;
+    //std::shared_ptr<AnimationActionServer> animationServer;
-            //std::shared_ptr<MovementActionServer> movementServer;
+    //std::shared_ptr<MovementActionServer> movementServer;
-            //ActorSystemState currentState, lastState;
+    //ActorSystemState currentState, lastState;
-            //action::Animation::Goal animationTarget;
+    //action::Animation::Goal animationTarget;
-            //action::Movement::Goal movementTarget;
+    //action::Movement::Goal movementTarget;
-            //AnimationServerGoalPtr currentAnimationGoalPtr;
+    //AnimationServerGoalPtr currentAnimationGoalPtr;
-            //MovementServerGoalPtr currentMovementGoalPtr;
+    //MovementServerGoalPtr currentMovementGoalPtr;
-            math::Vector3d target_position;
+    ignition::math::v6::Pose3d startPose = ignition::math::Pose3d::Zero;
-            double animation_distance;
+    math::Vector3d target_position;
-            char animation_name[256];
+    double animation_distance;
-            ActorPluginState nextState,currentState,lastState;
+    char animation_name[256];
-            double duration{};
+    ActorPluginState nextState,currentState,lastState;
-            Entity entity{kNullEntity};
+    double duration{};
-            std::mutex threadMutex;
+    Entity entity{kNullEntity};
-            std::shared_ptr<common::Skeleton> currentSkeleton;
+    std::mutex threadMutex;
-            common::NodeAnimation* currentRootNodeAnimation;
+    std::shared_ptr<common::Skeleton> currentSkeleton;
-            math::Matrix4<double> currentRotationAlign, currentTranslationAlign;
+    common::NodeAnimation* currentRootNodeAnimation;
-            mqd_t feedbackQueue;
+    math::Matrix4<double> currentRotationAlign, currentTranslationAlign;
-            mqd_t actionQueue;
+    mqd_t feedbackQueue;
+    mqd_t actionQueue;
+    FeedbackMessage feedback;
 
-        public:
+  public:
-            ActorSystem();
+    ActorSystem();
 
-        public:
+  public:
-            ~ActorSystem() override;
+    ~ActorSystem() override;
 
-        public:
+  public:
-            void PreUpdate(const UpdateInfo &_info,
+    void PreUpdate(const UpdateInfo &_info,
-                           EntityComponentManager &_ecm) override;
+                   EntityComponentManager &_ecm) override;
 
-        public:
+  public:
-            void Configure(const ignition::gazebo::Entity &animationGoalPtr,
+    void Configure(const ignition::gazebo::Entity &animationGoalPtr,
-                           const std::shared_ptr<const sdf::Element> &_sdf,
+                   const std::shared_ptr<const sdf::Element> &_sdf,
-                           EntityComponentManager &_ecm,
+                   EntityComponentManager &_ecm,
-                           EventManager &/*_eventMgr*/) override;
+                   EventManager &/*_eventMgr*/) override;
 
-        private:
+  private:
-            void switchAnimation(EntityComponentManager &_ecm, char *animationName);
+    void switchAnimation(EntityComponentManager &_ecm, char *animationName);
-            void messageQueueInterface(const char name[256]);
+    void messageQueueInterface(const char name[256]);
-            void sendFeedback(double feedback);
+    void sendFeedback();
+    void addPoseToFeedback(ignition::math::Pose3<double> pose);
 
 
         };

src/ign_world/world/gaz_new_test.sdf

@@ -25,6 +25,9 @@
     
     <actor name="actor_walking">
       <plugin name="ignition::gazebo::ActorSystem" filename="/home/ros/workspace/install/ign_actor_plugin/lib/ign_actor_plugin/libign_actor_plugin.so">
+        <x>2</x>
+        <y>2</y>
+        <rot>180</rot>
       </plugin>
       <skin>
         <filename>/home/ros/walk.dae</filename>

src/iisy_config_2/config/iisy.urdf

@@ -31,7 +31,7 @@
     </visual>
     <inertial>
       <origin rpy="0 0 0" xyz="-0.043517 0.000000 0.054914"/>
-      <mass value="6"/>
+      <mass value="0.1"/>
       <inertia ixx="1.277905" ixy="0.0" ixz="0.138279" iyy="3.070705" iyz="0.0" izz="3.284356"/>
     </inertial>
   </link>
@@ -49,7 +49,7 @@
     </visual>
     <inertial>
       <origin rpy="0 0 0" xyz="0 -0.005 0.08"/>
-      <mass value="2"/>
+      <mass value="0.1"/>
       <inertia ixx="0.32666666666667" ixy="0.0" ixz="0.0" iyy="0.32666666666667" iyz="0.0" izz="0.25"/>
     </inertial>
   </link>
@@ -66,7 +66,7 @@
     </visual>
     <inertial>
       <origin rpy="0 0 0" xyz="0 -0.050000 0.150000"/>
-      <mass value="4"/>
+      <mass value="0.1"/>
       <inertia ixx="8.266347" ixy="0.0" ixz="0.0" iyy="8.647519" iyz="0.0" izz="8.647519"/>
     </inertial>
   </link>
@@ -84,7 +84,7 @@
     </visual>
     <inertial>
       <origin rpy="0 0 0" xyz="0 0.07 0.025"/>
-      <mass value="2"/>
+      <mass value="0.1"/>
       <inertia ixx="0.5" ixy="0.0" ixz="0.0" iyy="0.5" iyz="0.0" izz="0.25"/>
     </inertial>
   </link>
@@ -101,7 +101,7 @@
     </visual>
     <inertial>
       <origin rpy="0 0 0" xyz="0 0.037363 0.086674"/>
-      <mass value="2"/>
+      <mass value="0.1"/>
       <inertia ixx="1.431738" ixy="0.0" ixz="0.0" iyy="1.131426" iyz="-0.342192" izz="0.807202"/>
     </inertial>
   </link>
@@ -118,7 +118,7 @@
     </visual>
     <inertial>
       <origin rpy="0 0 0" xyz="0 -0.055000 0.021413"/>
-      <mass value="2"/>
+      <mass value="0.1"/>
       <inertia ixx="0.362124" ixy="0.0" ixz="0.0" iyy="0.343531" iyz="0.0" izz="0.283368"/>
     </inertial>
   </link>
@@ -135,7 +135,7 @@
     </visual>
     <inertial>
       <origin rpy="0 0 0" xyz="0 -0.015462 0.040000"/>
-      <mass value="0.8"/>
+      <mass value="0.1"/>
       <inertia ixx="0.228470" ixy="0.0" ixz="0.0" iyy="0.137641" iyz="0.0" izz="0.252003"/>
     </inertial>
   </link>

src/iisy_config_2/config/moveit_controllers.yaml

@@ -1,6 +1,8 @@
 # MoveIt uses this configuration for controller management
 trajectory_execution:
-  allowed_execution_duration_scaling: 1.2 # Wait for moves to finish without timeout
+  trajectory_duration_monitoring: false
+  allowed_execution_duration_scaling: 2.0 # Wait for moves to finish without timeout
+  allowed_goal_duration_margin: 2.0
 
 moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager
 

src/iisy_config_2/config/ros2_controllers.yaml

@@ -1,7 +1,7 @@
 # This config file is used by ros2_control
 controller_manager:
   ros__parameters:
-    update_rate: 100  # Hz
+    update_rate: 1000  # Hz
 
     arm_controller:
       type: joint_trajectory_controller/JointTrajectoryController
@@ -23,4 +23,8 @@ arm_controller:
       - position
     state_interfaces:
       - position
-      - velocity
+      - velocity
+    open_loop_control: true
+    constraints:
+      stopped_velocity_tolerance: 0.01
+      goal_time: 0.0

src/ros_actor_action_server/package.xml

@@ -12,7 +12,7 @@
   <depend>geometry_msgs</depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
-
+  
   <depend>ros_actor_action_server_msgs</depend>
   <depend>ros_actor_message_queue_msgs</depend>
 

src/ros_actor_action_server/src/Server.cpp

@@ -7,10 +7,15 @@
 #include <mqueue.h>
 
 #include <cstdio>
+#include <rclcpp/logger.hpp>
 
 using namespace ros_actor_message_queue_msgs;
 
-void sendAction(mqd_t queue, ActionMessage *message) { mq_send(queue, (const char *)message, sizeof(ActionMessage), 0); }
+void sendAction(mqd_t queue, ActionMessage *message) { 
+    std::cout << "Sending action..." << std::endl;
+    mq_send(queue, (const char *)message, sizeof(ActionMessage), 0); 
+    std::cout << "Sent action" << std::endl;
+}
 
 int main(int argc, char **argv) {
     rclcpp::init(argc, argv);
@@ -19,31 +24,45 @@ int main(int argc, char **argv) {
 
     node->get_node_options().arguments();
 
+    feedbackName = "/ActorPluginFeedback";
+    actionName = "/ActorPluginAction";
+
     mq_attr queueAttributes;
     queueAttributes.mq_flags = 0;
     queueAttributes.mq_curmsgs = 0;
-    queueAttributes.mq_msgsize = sizeof(ros_actor_message_queue_msgs::FeedbackMessage);
     queueAttributes.mq_maxmsg = 1;
-    mqd_t feedbackQueue = mq_open("/ActorPluginFeedback", O_CREAT | O_RDWR, S_IRWXU | S_IRWXG, &queueAttributes);
 
-    if (feedbackQueue == (mqd_t) -1) {
+    mq_unlink(feedbackName);
+    queueAttributes.mq_msgsize = sizeof(ros_actor_message_queue_msgs::FeedbackMessage);
+    mqd_t feedbackQueue = mq_open(feedbackName, O_CREAT | O_EXCL | O_RDWR, S_IRWXU | S_IRWXG, &queueAttributes);
+    if(feedbackQueue == (mqd_t) -1){
         perror(nullptr);
         return -1;
     }
 
+    std::atexit([]{
+        std::cout << "Shutting down " << feedbackName << std::endl;
+        mq_unlink(feedbackName);
+    });
+    
+    mq_unlink(actionName);
     queueAttributes.mq_msgsize = sizeof(ros_actor_message_queue_msgs::ActionMessage);
-    mqd_t actionQueue = mq_open("/ActorPluginAction", O_CREAT | O_RDWR, S_IRWXU | S_IRWXG, &queueAttributes);
+    mqd_t actionQueue = mq_open(actionName, O_CREAT | O_EXCL | O_RDWR, S_IRWXU | S_IRWXG, &queueAttributes);
-
+    if(actionQueue == (mqd_t) -1){
-    if (actionQueue == (mqd_t) -1) {
         perror(nullptr);
         return -1;
     }
 
+    std::atexit([]{
+        std::cout << "Shutting down " << actionName << std::endl;
+        mq_unlink(actionName);
+    });
+
+    std::cout << "All message queues online." << std::endl;
+
     AnimationServerGoalPtr currentAnimationGoalPtr;
     MovementServerGoalPtr currentMovementGoalPtr;
 
-    double progress;
-
     FeedbackMessage currentFeedback;
     ActionMessage currentAction;
     std::mutex mutex;
@@ -72,7 +91,7 @@ int main(int argc, char **argv) {
             if(currentAction.state==MOVEMENT && currentMovementGoalPtr!=nullptr){
                 if(newFeedback.progress<1){
                     auto feedback = std::make_shared<ros_actor_action_server_msgs::action::Movement_Feedback>();
-                    feedback->progress = newFeedback.progress;
+                    feedback->current = newFeedback.current;
                     currentMovementGoalPtr->publish_feedback(feedback);
                 }else{
                     currentMovementGoalPtr->succeed(std::make_shared<ros_actor_action_server_msgs::action::Movement_Result>());
@@ -129,9 +148,7 @@ int main(int argc, char **argv) {
             mutex.lock();
             if (currentAction.state == IDLE) {
                 std::cout << "Accepting..." << std::endl;
-                currentAction.positionX = movementGoal->target_position[0];
+                currentAction.target = movementGoal->target;
-                currentAction.positionY = movementGoal->target_position[1];
-                currentAction.positionZ = movementGoal->target_position[2];
                 currentAction.animationDistance = movementGoal->animation_distance;
                 strcpy(currentAction.animationName, movementGoal->animation_name.data());
 
@@ -163,7 +180,7 @@ int main(int argc, char **argv) {
             mutex.unlock();
         });
     
-    while(true){
+    while(rclcpp::ok()){
         rclcpp::spin(node);
     }
 }

src/ros_actor_action_server/src/Server.hpp

@@ -17,6 +17,9 @@
 using namespace ros_actor_action_server_msgs;
 using rclcpp_action::ServerGoalHandle;
 
+static char *feedbackName;
+static char *actionName;
+
 #define AnimationActionServer rclcpp_action::Server<action::Animation>
 #define MovementActionServer rclcpp_action::Server<action::Movement>
 

src/ros_actor_action_server_msgs/CMakeLists.txt

@@ -2,10 +2,12 @@ cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR)
 project(ros_actor_action_server_msgs)
 
 find_package(rosidl_default_generators REQUIRED)
+find_package(geometry_msgs REQUIRED)
 
 rosidl_generate_interfaces(${PROJECT_NAME}
         "action/Animation.action"
         "action/Movement.action"
+        DEPENDENCIES geometry_msgs
         )
 ament_export_dependencies(rosidl_default_runtime)
 

src/ros_actor_action_server_msgs/action/Movement.action

@@ -1,8 +1,7 @@
 string animation_name
 float32 animation_speed
 float32 animation_distance
-float32[3] target_position
+geometry_msgs/Pose target
-float32[3] target_orientation
 ---
 ---
-float32 progress
+geometry_msgs/Pose current

src/ros_actor_action_server_msgs/package.xml

@@ -9,6 +9,7 @@
 
   <buildtool_depend>rosidl_default_generators</buildtool_depend>
   <depend>action_msgs</depend>
+  <depend>geometry_msgs</depend>
   <member_of_group>rosidl_interface_packages</member_of_group>
 
   <export>

src/ros_actor_message_queue_msgs/CMakeLists.txt

@@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR)
 project(ros_actor_message_queue_msgs)
 
 find_package(ament_cmake REQUIRED)
+find_package(geometry_msgs REQUIRED)
 
 add_library(${PROJECT_NAME} INTERFACE)
 

src/ros_actor_message_queue_msgs/include/ros_actor_message_queue_msgs/MessageTypes.hpp

@@ -1,3 +1,4 @@
+#include <geometry_msgs/msg/detail/pose__struct.hpp>
 namespace ros_actor_message_queue_msgs{
 enum ActorPluginState{
   SETUP,IDLE,ANIMATION,MOVEMENT
@@ -6,11 +7,12 @@ enum ActorPluginState{
 struct FeedbackMessage{
   ActorPluginState state;
   float progress;
+  geometry_msgs::msg::Pose current;
 };
 
 struct ActionMessage{
   ActorPluginState state;
-  float positionX = 0.0f, positionY = 0.0f, positionZ = 0.0f;
+  geometry_msgs::msg::Pose target;
   float animationSpeed = 1.0f,animationDistance = 1.0f;
   char animationName[256];
 };

test.sh

@@ -1,2 +1,2 @@
 ros2 action send_goal /ActorPlugin/animation ign_actor_plugin_msgs/action/Animation "{animation_name: 'walk', animation_speed: 1.0}"
-ros2 action send_goal /ActorPlugin/movement ign_actor_plugin_msgs/action/Movement "{animation_name: 'walk', animation_speed: 1.0, animation_distance: 1.5, target_position: [1,1,0], target_orientation: [0,0,0]}"
+ros2 action send_goal /ActorPlugin/movement ros_actor_action_server_msgs/action/Movement '{animation_name: "walk", animation_speed: 1.0, animation_distance: 1.5, target: {orientation: {x: 0.0, y: 0.0, z: 0.0, w: 0.0}, position: {x: 2.0, y: 0.0, z: 0.0}}}'