Final version of actor plugin

src/cpp_pubsub/CMakeLists.txt

@@ -1,38 +0,0 @@
-cmake_minimum_required(VERSION VERSION 3.5.1)
-project(cpp_pubsub)
-
-set(CMAKE_CXX_STANDARD 17)
-add_compile_options(-Wall -Wextra -Wpedantic)
-set(CMAKE_CXX_STANDARD_REQUIRED ON)
-
-# find dependencies
-
-find_package(ament_cmake REQUIRED)
-find_package(rclcpp REQUIRED)
-find_package(geometry_msgs REQUIRED)
-# uncomment the following section in order to fill in
-# further dependencies manually.
-# find_package(<dependency> REQUIRED)
-
-add_executable(talker src/publisher_member_function.cpp)
-ament_target_dependencies(talker geometry_msgs rclcpp)
-add_executable(listener src/subscriber_member_function.cpp)
-ament_target_dependencies(listener geometry_msgs rclcpp)
-
-install(TARGETS
-  talker
-  listener
-  DESTINATION lib/${PROJECT_NAME})
-
-if(BUILD_TESTING)
-  find_package(ament_lint_auto REQUIRED)
-  # the following line skips the linter which checks for copyrights
-  # uncomment the line when a copyright and license is not present in all source files
-  #set(ament_cmake_copyright_FOUND TRUE)
-  # the following line skips cpplint (only works in a git repo)
-  # uncomment the line when this package is not in a git repo
-  #set(ament_cmake_cpplint_FOUND TRUE)
-  ament_lint_auto_find_test_dependencies()
-endif()
-
-ament_package()

src/cpp_pubsub/src/publisher_member_function.cpp

@@ -1,52 +0,0 @@
-// Copyright 2016 Open Source Robotics Foundation, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#include <chrono>
-#include <memory>
-#include <geometry_msgs/msg/pose.hpp>
-
-#include "rclcpp/rclcpp.hpp"
-#include "std_msgs/msg/string.hpp"
-
-using namespace std::chrono_literals;
-
-/* This example creates a subclass of Node and uses std::bind() to register a
- * member function as a callback from the timer. */
-
-class MinimalPublisher : public rclcpp::Node{
-    public:
-        MinimalPublisher(): Node("minimal_publisher") {
-            publisher_ = this->create_publisher<geometry_msgs::msg::Pose>("targetPose", 10);
-            timer_ = this->create_wall_timer(
-                500ms, [this]{
-                    auto message = geometry_msgs::msg::Pose();
-                    message.position.x=i;
-                    RCLCPP_INFO(this->get_logger(), "Publishing: '%f %f %f'", message.position.x,message.position.y,message.position.z);
-                    i++;
-                    publisher_->publish(message);
-                });
-        }
-
-    private:
-        int i=0;
-        rclcpp::TimerBase::SharedPtr timer_;
-        rclcpp::Publisher<geometry_msgs::msg::Pose>::SharedPtr publisher_;
-};
-
-int main(int argc, char *argv[]) {
-    rclcpp::init(argc, argv);
-    rclcpp::spin(std::make_shared<MinimalPublisher>());
-    rclcpp::shutdown();
-    return 0;
-}

src/cpp_pubsub/src/subscriber_member_function.cpp

@@ -1,66 +0,0 @@
-// Copyright 2016 Open Source Robotics Foundation, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#include <memory>
-#include <geometry_msgs/msg/pose.hpp>
-
-#include "rclcpp/rclcpp.hpp"
-#include "std_msgs/msg/string.hpp"
-
-using std::placeholders::_1;
-/*
-class MinimalSubscriber : public rclcpp::Node
-{
-public:
-  MinimalSubscriber(): Node("minimal_subscriber"){}
-
-private:
-  rclcpp::Subscription<geometry_msgs::msg::Pose>::SharedPtr subscription_ = this->create_subscription<geometry_msgs::msg::Pose>(
-          "topic", 10, [this](const geometry_msgs::msg::Pose PH1) {
-              RCLCPP_INFO(this->get_logger(), "I heard: '%f %f %f'", PH1.position.x,PH1.position.y,PH1.position.z);
-
-              //RCUTILS_LOG_INFO_NAMED(this->get_logger().get_name(),"I heard: '%s'", PH1->data.c_str());
-          });
-};
-*/
-template <typename T>
-class MinimalSubscriber : public rclcpp::Node
-{
-public: MinimalSubscriber(const std::string& node_name, const std::string& topic_name, void(*fun)(T)): Node(node_name){
-        this->subscription_ = this->create_subscription<T>(
-                topic_name, 10, [fun](const T result) {
-                    fun(result);
-                });
-    }
-
-private:
-    typename rclcpp::Subscription<T>::SharedPtr subscription_;
-};
-
-template <typename T>
-std::thread spinMinimalSubscriber(const std::string& node_name, const std::string& topic_name, void(*callback)(T)){
-    return std::thread([node_name,topic_name,callback](){
-        rclcpp::spin(std::make_shared<MinimalSubscriber<T>>(node_name,topic_name,callback));
-        rclcpp::shutdown();
-    });
-}
-
-int main(int argc, char * argv[])
-{
-  rclcpp::init(0, nullptr);
-  spinMinimalSubscriber<geometry_msgs::msg::Pose>("talker","topic",[](geometry_msgs::msg::Pose pose){
-      printf("log: %f %f %f \n",pose.position.x,pose.position.y,pose.position.z);
-  }).join();
-  return 0;
-}

src/gz_ros2_control

@@ -1 +0,0 @@
-Subproject commit b0c29c2f3245eca25753fbe2527eeacc7cb9dac0

src/ign_actor_plugin/CMakeLists.txt

@@ -10,25 +10,27 @@ find_package(rclcpp_action REQUIRED)
 find_package(rclcpp_components REQUIRED)
 find_package(ignition-cmake2 REQUIRED)
 find_package(ignition-gazebo6 REQUIRED)
-find_package(ign_actor_plugin_msgs REQUIRED)
+find_package(ros_actor_message_queue_msgs REQUIRED)
+find_package(ros_actor_action_server_msgs REQUIRED)
 
 find_package(ignition-plugin1 REQUIRED COMPONENTS register)
 set(IGN_PLUGIN_VER ${ignition-plugin1_VERSION_MAJOR})
 
-ament_export_dependencies(ActorPlugin 
+ament_export_dependencies(ign_actor_plugin
     "rosidl_default_runtime"
-        "ign_actor_plugin_msgs"
+    "ros_actor_action_server_msgs"
     "rclcpp"
     "rclcpp_action"
-        "rclcpp_components")
+    "rclcpp_components"
+)
 
 # Add sources for each plugin to be registered.
-add_library(ActorPlugin SHARED src/ActorSystem.cpp)
+add_library(ign_actor_plugin SHARED src/ActorSystem.cpp)
-ament_target_dependencies(ActorPlugin rclcpp rclcpp_action ign_actor_plugin_msgs)
+ament_target_dependencies(ign_actor_plugin rclcpp rclcpp_action ros_actor_message_queue_msgs ros_actor_action_server_msgs)
-set_property(TARGET ActorPlugin PROPERTY CXX_STANDARD 17)
+set_property(TARGET ign_actor_plugin PROPERTY CXX_STANDARD 17)
-target_compile_options(ActorPlugin PRIVATE -std=c++17)
+target_compile_options(ign_actor_plugin PRIVATE -std=c++17)
 
-target_link_libraries(ActorPlugin
+target_link_libraries(ign_actor_plugin
     ignition-gazebo6::ignition-gazebo6
     ignition-plugin${IGN_PLUGIN_VER}::ignition-plugin${IGN_PLUGIN_VER}
     ${rclcpp_LIBRARIES}
@@ -36,7 +38,8 @@ target_link_libraries(ActorPlugin
 )
 
 install(TARGETS
-  ActorPlugin
+    ign_actor_plugin
-  DESTINATION lib/${PROJECT_NAME})
+    DESTINATION lib/${PROJECT_NAME}
+)
 
 ament_package()

src/ign_actor_plugin/package.xml

@@ -11,7 +11,8 @@
   <depend>rclcpp</depend>
   <depend>rclcpp_action</depend>
   <depend>rclcpp_components</depend>
-  <depend>ign_actor_plugin_msgs</depend>
+  <depend>ros_actor_message_queue_msgs</depend>
+  <depend>ros_actor_action_server_msgs</depend>
   <depend>ignition-cmake2</depend>
   <depend>ignition-gazebo6</depend>
   <test_depend>ament_lint_auto</test_depend>

src/ign_actor_plugin/src/ActorSystem.cpp

@@ -2,36 +2,31 @@
 // Created by bastian on 31.08.22.
 //
 
+#include "ActorSystem.hpp"
 #include <chrono>
-#include <ign_actor_plugin_msgs/action/detail/animation__struct.hpp>
+#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 "ActorSystem.hpp"
-#include <ignition/gazebo/components/Actor.hh>
-#include <ignition/gazebo/EntityComponentManager.hh>
 #include <sdf/Actor.hh>
-#include <ignition/common/MeshManager.hh>
-#include <ignition/common/SkeletonAnimation.hh>
-#include <ignition/common/Mesh.hh>
 #include <sys/mman.h>
-#include <fcntl.h>
-#include <cstdio>
 #include <unistd.h>
 
-IGNITION_ADD_PLUGIN(
+IGNITION_ADD_PLUGIN(ignition::gazebo::ActorSystem, ignition::gazebo::System, ignition::gazebo::ActorSystem::ISystemPreUpdate,
-        ignition::gazebo::ActorSystem,
-        ignition::gazebo::System,
-        ignition::gazebo::ActorSystem::ISystemPreUpdate,
                     ignition::gazebo::ActorSystem::ISystemConfigure)
 
-
 using namespace ignition::gazebo;
-using namespace ign_actor_plugin_msgs;
+using namespace ros_actor_action_server_msgs;
 
 ActorSystem::ActorSystem() = default;
 
@@ -91,15 +86,17 @@ void ActorSystem::switchAnimation(EntityComponentManager &_ecm, char* animationN
         newPose.Pos().X(0);
         newPose.Pos().Y(0);
         *poseComponent = components::Pose(newPose);
-
     } else {
         ignerr << "No animation found!" << std::endl;
     }
 }
 
 void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ecm) {
-    pthread_mutex_lock(&shared_mmap->mutex);
+    threadMutex.lock();
-    switch(shared_mmap->currentState){
+
+    
+
+    switch (currentState) {
     case SETUP: {
         igndbg << "Setting up actor plugin" << std::endl;
 
@@ -125,32 +122,29 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
             _ecm.CreateComponent(entity, components::TrajectoryPose(ignition::math::Pose3d::Zero));
         }
 
-            shared_mmap->currentState = ActorSystemState::IDLE;
+        currentState = IDLE;
         break;
     }
     case ANIMATION: {
-            if (shared_mmap->lastState == ActorSystemState::IDLE){
+        if (lastState == IDLE) {
-                switchAnimation(_ecm, shared_mmap->animation_name);
+            switchAnimation(_ecm, animation_name);
         }
         auto animationTimeComponent = _ecm.Component<components::AnimationTime>(this->entity);
         auto newTime = animationTimeComponent->Data() + _info.dt;
         auto newTimeSeconds = std::chrono::duration<double>(newTime).count();
 
-            auto feedback = (float) (newTimeSeconds/duration);
+        auto feedback = newTimeSeconds / duration;
-            write(feedback_pipe[1],&feedback,sizeof(float));
+        sendFeedback(feedback);
-            //currentAnimationGoalPtr->publish_feedback(feedback);
 
         if (newTimeSeconds >= duration) {
-                //currentAnimationGoalPtr->succeed(std::make_shared<action::Animation::Result>());
+            igndbg << "Animation " << animation_name << " finished." << std::endl;
-                igndbg << "Animation " << shared_mmap->animation_name << " finished." << std::endl;
+            currentState = IDLE;
-                shared_mmap->currentState = ActorSystemState::IDLE;
             break;
         }
 
         *animationTimeComponent = components::AnimationTime(newTime);
         _ecm.SetChanged(entity, components::AnimationTime::typeId, ComponentState::OneTimeChange);
 
-
         /*
         if(currentRootNodeAnimation == nullptr){
             break;
@@ -171,22 +165,20 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
         break;
     }
     case MOVEMENT: {
-            if (shared_mmap->lastState == ActorSystemState::IDLE){
+        if (lastState == IDLE) {
             igndbg << "Starting Movement..." << std::endl;
-                switchAnimation(_ecm, shared_mmap->animation_name);
+            switchAnimation(_ecm, animation_name);
         }
 
         auto trajectoryPoseComp = _ecm.Component<components::TrajectoryPose>(this->entity);
         auto actorPose = trajectoryPoseComp->Data();
-            auto targetPose = shared_mmap->target_position;
+        auto targetPose = target_position;
 
         auto targetDirection = math::Vector3d(targetPose[0], targetPose[1], targetPose[2]) - actorPose.Pos();
 
         if (targetDirection.Length() < 0.05) {
-                float feedback = 1.0f;
+            sendFeedback(1.0);
-                write(feedback_pipe[1],&feedback,sizeof(float));
+            currentState = IDLE;
-                //currentMovementGoalPtr->succeed(std::make_shared<action::Movement::Result>());
-                shared_mmap->currentState = ActorSystemState::IDLE;
             break;
         }
 
@@ -204,7 +196,8 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
         auto targetYaw = atan2(targetDirection.Y(), targetDirection.X());
         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 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) {
@@ -221,7 +214,7 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
                 break;
             }
 
-                double distance = (deltaTimeSeconds/duration)*shared_mmap->animation_distance;
+            double distance = (deltaTimeSeconds / duration) * animation_distance;
 
             actorPose.Pos() += targetDirection.Normalize() * distance;
 
@@ -235,159 +228,74 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
         break;
     }
     case IDLE:
-            if (shared_mmap->lastState != shared_mmap->currentState){
-                igndbg << "Now idling..." << std::endl;
-            }
         break;
     }
-    shared_mmap->lastState = shared_mmap->currentState;
+
-    pthread_mutex_unlock(&shared_mmap->mutex);
+    if (lastState != currentState) {
+        igndbg << "State change: " << lastState << " -> " << currentState << std::endl;
+        sendFeedback(0.0);
     }
     
-
+    lastState = currentState;
-void ActorSystem::rclcppServer(const std::string& topic){
+    threadMutex.unlock();
-    rclcpp::init(0, {});
-
-    auto node = rclcpp::Node::make_shared("moveService", topic);
-
-    AnimationServerGoalPtr currentAnimationGoalPtr;
-    MovementServerGoalPtr currentMovementGoalPtr;
-
-#pragma clang diagnostic push
-#pragma ide diagnostic ignored "UnusedValue"
-    auto animationServer = rclcpp_action::create_server<action::Animation>(
-            node,
-            "animation",
-            [this](const rclcpp_action::GoalUUID goalUuid, const AnimationGoalPtr &animationGoal) {
-                igndbg << "goal" << shared_mmap->currentState << std::endl;
-                if (shared_mmap->currentState == ActorSystemState::IDLE) {
-                    pthread_mutex_lock(&shared_mmap->mutex);
-                    igndbg << "goal l" << shared_mmap->currentState << std::endl;
-
-                    strcpy(shared_mmap->animation_name,animationGoal->animation_name.data());
-
-                    pthread_mutex_unlock(&shared_mmap->mutex);
-                    return rclcpp_action::GoalResponse::ACCEPT_AND_DEFER;
-                }else{
-                    return rclcpp_action::GoalResponse::REJECT;
 }
-            },
-            [this](const AnimationServerGoalPtr& animationGoalPtr) {
-                igndbg << "cancel" << shared_mmap->currentState << std::endl;
-                if (shared_mmap->currentState == ActorSystemState::IDLE) {
-                    return rclcpp_action::CancelResponse::REJECT;
-                }else{
-                    pthread_mutex_lock(&shared_mmap->mutex);
 
-                    shared_mmap->currentState = ActorSystemState::IDLE;
+void ActorSystem::messageQueueInterface(const char name[256]) {
-                    pthread_mutex_unlock(&shared_mmap->mutex);
+    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);
 
-                    return rclcpp_action::CancelResponse::ACCEPT;
+    if (feedbackQueue == (mqd_t)-1) {
+        ignerr << "Could not create queue. (" << errno << ")" << std::endl;
+        return;
     }
-            },
-            [this,&currentAnimationGoalPtr](const AnimationServerGoalPtr& animationGoalPtr) {
-                igndbg << "accepted" << shared_mmap->currentState << std::endl;
-                pthread_mutex_lock(&shared_mmap->mutex);
-                igndbg << "accepted l" << shared_mmap->currentState << std::endl;
 
-                igndbg << "got animation handle" << std::endl;
+    strcpy(generatedName,name);
-                currentAnimationGoalPtr = animationGoalPtr;
+    strcat(generatedName,"Action");
-
+    queueAttributes.mq_msgsize = sizeof(ros_actor_message_queue_msgs::ActionMessage);
-                shared_mmap->currentState = ActorSystemState::ANIMATION;
+    actionQueue = mq_open(generatedName, O_CREAT | O_RDWR, S_IRWXU | S_IRWXG, &queueAttributes);
-                currentAnimationGoalPtr->execute();
-
-                pthread_mutex_unlock(&shared_mmap->mutex);
 
+    if (actionQueue == (mqd_t)-1) {
+        ignerr << "Could not create queue. (" << errno << ")" << std::endl;
+        return;
     }
-    );
 
-    auto movementServer = rclcpp_action::create_server<action::Movement>(
+    std::thread([this] {
-            node,
+        ActionMessage receivedAction;
-            "movement",
-            [this](const rclcpp_action::GoalUUID goalUuid, const MovementGoalPtr &movementGoal ){
-                if (shared_mmap->currentState == ActorSystemState::IDLE) {
-                    pthread_mutex_lock(&shared_mmap->mutex);
-
-                    shared_mmap->target_position = math::Vector3d(
-                            movementGoal->target_position[0],
-                            movementGoal->target_position[1],
-                            movementGoal->target_position[2]);
-                    shared_mmap->animation_distance = movementGoal->animation_distance;
-                    strcpy(shared_mmap->animation_name,movementGoal->animation_name.data());
-
-                    pthread_mutex_unlock(&shared_mmap->mutex);
-                    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
-                }else{
-                    return rclcpp_action::GoalResponse::REJECT;
-                }
-            },
-            [this](const MovementServerGoalPtr& movementGoalPtr ){
-                if (shared_mmap->currentState == ActorSystemState::IDLE) {
-                    return rclcpp_action::CancelResponse::REJECT;
-                }else{
-                    pthread_mutex_lock(&shared_mmap->mutex);
-
-                    shared_mmap->currentState = ActorSystemState::IDLE;
-
-                    pthread_mutex_unlock(&shared_mmap->mutex);
-                    return rclcpp_action::CancelResponse::ACCEPT;
-                }
-            },
-            [this,&currentMovementGoalPtr](const MovementServerGoalPtr& movementGoalPtr ){
-                pthread_mutex_lock(&shared_mmap->mutex);
-
-                igndbg << "got movement handle" << std::endl;
-                currentMovementGoalPtr = movementGoalPtr;
-                shared_mmap->currentState = ActorSystemState::MOVEMENT;
-
-                pthread_mutex_unlock(&shared_mmap->mutex);
-            }
-    );
-#pragma clang diagnostic pop
-
-    std::thread t([this,&currentAnimationGoalPtr,&currentMovementGoalPtr]() {
-        float progress;
         while (true) {
-            read(feedback_pipe[0], &progress, sizeof(float));
+            mq_receive(actionQueue, (char *)&receivedAction, sizeof(ActionMessage), nullptr);
+            threadMutex.lock();
 
-            igndbg << progress << std::endl;
+            strcpy(animation_name, receivedAction.animationName);
+            // animation_speed = receivedAction.animationSpeed;
+            animation_distance = receivedAction.animationDistance;
+            target_position = math::Vector3d(receivedAction.positionX, receivedAction.positionY, receivedAction.positionZ);
+            currentState = receivedAction.state;
 
-            if(progress >= 1.0f){
+            std::cout << "Got Action" << std::endl;
-                if(currentAnimationGoalPtr!=nullptr){
+            threadMutex.unlock();
+        }
+    }).detach();
+}
 
-                    currentAnimationGoalPtr->succeed(std::make_shared<action::Animation::Result>());
+void ActorSystem::sendFeedback(double progress){
-                }
+    FeedbackMessage message;
-                if(currentMovementGoalPtr!=nullptr){
+    message.progress = progress;
-                    currentMovementGoalPtr->succeed(std::make_shared<action::Movement::Result>());
+    message.state = currentState;
-                }
+    mq_send(feedbackQueue,(char *)&message,sizeof(FeedbackMessage),0);
-            }else{
-                if(currentAnimationGoalPtr!=nullptr){
-                    auto feedback = std::make_shared<action::Animation::Feedback>();
-                    feedback->set__progress(progress);
-                    currentAnimationGoalPtr->publish_feedback(feedback);
-                }
-                if(currentMovementGoalPtr!=nullptr){
-                    auto feedback = std::make_shared<action::Movement::Feedback>();
-                    feedback->set__progress(progress);
-                    currentMovementGoalPtr->publish_feedback(feedback);
-                }
-            }
-            usleep(500);
-        }
-    });
-    t.detach();
-
-    igndbg << "Spinning node..." << std::endl;
-    while (true) {
-        printf("Spinning...\n");
-        rclcpp::spin(node);
-    }
 }
 
 // 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,
+void ActorSystem::Configure(const Entity &_entity, const std::shared_ptr<const sdf::Element> &_sdf, EntityComponentManager &_ecm,
-                            EntityComponentManager &_ecm, EventManager &) {
+                            EventManager &) {
+    
     igndbg << "actor plugin configuring..." << std::endl;
 
     auto actorComp = _ecm.Component<components::Actor>(_entity);
@@ -396,62 +304,20 @@ void ActorSystem::Configure(const Entity &_entity, const std::shared_ptr<const s
         return;
     }
 
-    std::string topic = "ActorPlugin";
+    char topic[256] = "/ActorPlugin";
     if (_sdf->HasElement("topic")) {
-        topic = _sdf->Get<std::string>("topic");
+        auto topicString = _sdf->Get<std::string>("topic");
+        if (topicString.size() >= 256) {
+            ignerr << "queue name too long, not starting plugin!";
+            return;
+        }
+        strcpy(topic, topicString.c_str());
     }
 
     this->entity = _entity;
 
-    igndbg << "shmem setup" << std::endl;
+    messageQueueInterface(topic);
-    int fd = open(".", O_TMPFILE|O_RDWR, 00600);
-    auto structSize = sizeof(struct sharedMmap);
-    if(ftruncate(fd, structSize)!=0){
-        ignerr << "ftruncate" << std::endl;
-        return;
-    }
-    shared_mmap = (struct sharedMmap *) mmap(nullptr,structSize,PROT_READ|PROT_WRITE,MAP_SHARED,fd,0);
-    //close(fd);
 
-    if (shared_mmap == MAP_FAILED){
+    currentState = SETUP;
-        ignerr << "MAP_FAILED" << std::endl;
+    lastState = SETUP;
-        return;
-    }
-
-    pthread_mutexattr_t mutexattr;
-    if(pthread_mutexattr_init(&mutexattr)!=0){
-        ignerr << "pthread_mutexattr_init" << std::endl;
-        return;
-    }
-    if(pthread_mutexattr_setpshared(&mutexattr, PTHREAD_PROCESS_SHARED)!=0){
-        ignerr << "pthread_mutexattr_setpshared" << std::endl;
-        return;
-    }
-    if(pthread_mutex_init(&shared_mmap->mutex, &mutexattr)!=0){
-        ignerr << "pthread_mutex_init" << std::endl;
-        return;
-    }
-
-    shared_mmap->currentState = ActorSystemState::SETUP;
-    shared_mmap->lastState = ActorSystemState::IDLE;
-
-    igndbg << "pipe setup" << std::endl;
-
-    if (pipe(feedback_pipe)!=0){
-        igndbg << "pipe failed." << std::endl;
-        return;
-    }
-
-    switch(fork()){
-        case -1:
-            ignerr << "Could not fork." << std::endl;
-            break;
-        case 0:
-            ignmsg << "Child fork." << std::endl;
-            rclcppServer(topic);
-            break;
-        default:
-            ignmsg << "Parent fork." << std::endl;
-            break;
-    }
 }

src/ign_actor_plugin/src/ActorSystem.hpp

@@ -14,14 +14,19 @@
 #include <rclcpp/node.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp_action/server.hpp>
-#include <ign_actor_plugin_msgs/action/animation.hpp>
+#include <ros_actor_action_server_msgs/action/animation.hpp>
-#include <ign_actor_plugin_msgs/action/movement.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 ign_actor_plugin_msgs;
+using namespace ros_actor_message_queue_msgs;
+using namespace ros_actor_action_server_msgs;
 using rclcpp_action::ServerGoalHandle;
 
 #define AnimationActionServer rclcpp_action::Server<action::Animation>
@@ -35,17 +40,6 @@ using rclcpp_action::ServerGoalHandle;
 
 namespace ignition {
     namespace gazebo {
-        enum ActorSystemState {
-            SETUP, IDLE, MOVEMENT, ANIMATION
-        };
-
-        struct sharedMmap{
-            pthread_mutex_t  mutex;
-            ActorSystemState currentState,lastState;
-            char animation_name[256];
-            double animation_distance;
-            math::Vector3d target_position;
-        };
 
         class ActorSystem :
                 public System,
@@ -61,14 +55,18 @@ namespace ignition {
             //action::Movement::Goal movementTarget;
             //AnimationServerGoalPtr currentAnimationGoalPtr;
             //MovementServerGoalPtr currentMovementGoalPtr;
+            math::Vector3d target_position;
+            double animation_distance;
+            char animation_name[256];
+            ActorPluginState currentState,lastState;
             double duration{};
             Entity entity{kNullEntity};
-            //std::mutex threadMutex;
+            std::mutex threadMutex;
             std::shared_ptr<common::Skeleton> currentSkeleton;
             common::NodeAnimation* currentRootNodeAnimation;
             math::Matrix4<double> currentRotationAlign, currentTranslationAlign;
-            struct sharedMmap* shared_mmap;
+            mqd_t feedbackQueue;
-            int feedback_pipe[2];
+            mqd_t actionQueue;
 
         public:
             ActorSystem();
@@ -88,7 +86,8 @@ namespace ignition {
 
         private:
             void switchAnimation(EntityComponentManager &_ecm, char *animationName);
-            void rclcppServer(const std::string& topic);
+            void messageQueueInterface(const char name[256]);
+            void sendFeedback(double feedback);
 
 
         };

src/ros_actor_action_server/CMakeLists.txt

@@ -0,0 +1,57 @@
+cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR)
+project(ros_actor_action_server)
+
+add_compile_options(-Wall -Wextra -Wpedantic)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_POSITION_INDEPENDENT_CODE ON)
+
+
+
+# find dependencies
+
+set(DEPENDENCIES
+        ament_cmake
+        std_msgs
+        rclcpp
+        rclcpp_action
+        ros_actor_action_server_msgs
+        ros_actor_message_queue_msgs
+)
+
+foreach(dep IN LISTS DEPENDENCIES)
+    find_package(${dep} REQUIRED)
+endforeach()
+
+include_directories(
+  ${ros_actor_message_queue_msgs_INCLUDE_DIRS}
+)
+
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+add_executable(ros_actor_action_server src/Server.cpp)
+set_property(TARGET ros_actor_action_server PROPERTY CXX_STANDARD 17)
+ament_target_dependencies(ros_actor_action_server ${DEPENDENCIES})
+
+#add_executable(talker src/publisher_member_function.cpp)
+#ament_target_dependencies(talker geometry_msgs rclcpp)
+#add_executable(listener src/subscriber_member_function.cpp)
+#ament_target_dependencies(listener geometry_msgs rclcpp)
+
+install(TARGETS
+        ros_actor_action_server
+        DESTINATION lib/${PROJECT_NAME})
+
+if (BUILD_TESTING)
+    find_package(ament_lint_auto REQUIRED)
+    # the following line skips the linter which checks for copyrights
+    # uncomment the line when a copyright and license is not present in all source files
+    #set(ament_cmake_copyright_FOUND TRUE)
+    # the following line skips cpplint (only works in a git repo)
+    # uncomment the line when this package is not in a git repo
+    #set(ament_cmake_cpplint_FOUND TRUE)
+    ament_lint_auto_find_test_dependencies()
+endif ()
+
+ament_package()

src/ros_actor_action_server/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
-  <name>cpp_pubsub</name>
+  <name>ros_actor_action_server</name>
   <version>0.0.0</version>
   <description>TODO: Package description</description>
   <maintainer email="bastian@todo.todo">bastian</maintainer>
@@ -13,6 +13,9 @@
   <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>
+
   <export>
     <build_type>ament_cmake</build_type>
   </export>

src/ros_actor_action_server/src/Server.cpp

@@ -0,0 +1,169 @@
+//
+// Created by ros on 2/6/23.
+//
+
+#include "Server.hpp"
+
+#include <mqueue.h>
+
+#include <cstdio>
+
+using namespace ros_actor_message_queue_msgs;
+
+void sendAction(mqd_t queue, ActionMessage *message) { mq_send(queue, (const char *)message, sizeof(ActionMessage), 0); }
+
+int main(int argc, char **argv) {
+    rclcpp::init(argc, argv);
+
+    auto node = rclcpp::Node::make_shared("ActorPlugin", "ActorPlugin");
+
+    node->get_node_options().arguments();
+
+    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) {
+        perror(nullptr);
+        return -1;
+    }
+
+    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);
+
+    if (actionQueue == (mqd_t) -1) {
+        perror(nullptr);
+        return -1;
+    }
+
+    AnimationServerGoalPtr currentAnimationGoalPtr;
+    MovementServerGoalPtr currentMovementGoalPtr;
+
+    double progress;
+
+    FeedbackMessage currentFeedback;
+    ActionMessage currentAction;
+    std::mutex mutex;
+
+    currentAction.state = IDLE;
+
+    std::thread([feedbackQueue, &currentAnimationGoalPtr, &currentMovementGoalPtr, &currentFeedback, &currentAction, &mutex] {
+        while (true) {
+            FeedbackMessage newFeedback;
+            if(mq_receive(feedbackQueue, (char *)&newFeedback, sizeof(ros_actor_message_queue_msgs::FeedbackMessage), nullptr)==-1){
+                perror(nullptr);
+            }
+            mutex.lock();
+            if (newFeedback.state != currentFeedback.state) {
+                currentAction.state = newFeedback.state;
+            }
+            if(currentAction.state==ANIMATION && currentAnimationGoalPtr!=nullptr){
+                if(newFeedback.progress<1){
+                    auto feedback = std::make_shared<ros_actor_action_server_msgs::action::Animation_Feedback>();
+                    feedback->progress = newFeedback.progress;
+                    currentAnimationGoalPtr->publish_feedback(feedback);
+                }else{
+                    currentAnimationGoalPtr->succeed(std::make_shared<ros_actor_action_server_msgs::action::Animation_Result>());
+                }
+            }
+            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;
+                    currentMovementGoalPtr->publish_feedback(feedback);
+                }else{
+                    currentMovementGoalPtr->succeed(std::make_shared<ros_actor_action_server_msgs::action::Movement_Result>());
+                }
+            }
+            currentFeedback = newFeedback;
+            std::cout << "Got feedback, State: " << currentFeedback.state << " | Progress: " << currentFeedback.progress << std::endl;
+            mutex.unlock();
+        }
+    }).detach();
+
+    auto animationServer = rclcpp_action::create_server<action::Animation>(
+        node, "animation",
+        [&currentAction, &mutex](const rclcpp_action::GoalUUID goalUuid, const AnimationGoalPtr &animationGoal) {
+            // igndbg << "goal" << shared_mmap->currentState << std::endl;
+            mutex.lock();
+
+            if (currentAction.state == IDLE) {
+                std::cout << "Accepting..." << std::endl;
+                strcpy(currentAction.animationName, animationGoal->animation_name.data());
+
+                return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+            } else {
+                std::cout << "Rejecting..." << std::endl;
+                mutex.unlock();
+                return rclcpp_action::GoalResponse::REJECT;
+            }
+        },
+        [actionQueue, &currentAction, &mutex](const AnimationServerGoalPtr &animationGoalPtr) {
+            if (currentAction.state == IDLE) {
+                return rclcpp_action::CancelResponse::REJECT;
+            } else {
+                mutex.lock();
+
+                currentAction.state = IDLE;
+                sendAction(actionQueue, &currentAction);
+
+                mutex.unlock();
+
+                return rclcpp_action::CancelResponse::ACCEPT;
+            }
+        },
+        [actionQueue, &currentAction, &currentAnimationGoalPtr, &mutex](const AnimationServerGoalPtr &animationGoalPtr) {
+            currentAnimationGoalPtr = animationGoalPtr;
+            currentAction.state = ANIMATION;
+            sendAction(actionQueue, &currentAction);
+
+            mutex.unlock();
+        });
+
+    auto movementServer = rclcpp_action::create_server<action::Movement>(
+        node, "movement",
+        [&currentAction, &mutex](const rclcpp_action::GoalUUID goalUuid, const MovementGoalPtr &movementGoal) {
+            mutex.lock();
+            if (currentAction.state == IDLE) {
+                std::cout << "Accepting..." << std::endl;
+                currentAction.positionX = movementGoal->target_position[0];
+                currentAction.positionY = movementGoal->target_position[1];
+                currentAction.positionZ = movementGoal->target_position[2];
+                currentAction.animationDistance = movementGoal->animation_distance;
+                strcpy(currentAction.animationName, movementGoal->animation_name.data());
+
+                return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+            } else {
+                std::cout << "Rejecting..." << std::endl;
+                mutex.unlock();
+                return rclcpp_action::GoalResponse::REJECT;
+            }
+        },
+        [actionQueue, &currentAction, &mutex](const MovementServerGoalPtr &movementGoalPtr) {
+            if (currentAction.state == IDLE) {
+                return rclcpp_action::CancelResponse::REJECT;
+            } else {
+                mutex.lock();
+
+                currentAction.state = IDLE;
+                sendAction(actionQueue, &currentAction);
+
+                mutex.unlock();
+                return rclcpp_action::CancelResponse::ACCEPT;
+            }
+        },
+        [actionQueue, &currentAction, &currentMovementGoalPtr, &mutex](const MovementServerGoalPtr &movementGoalPtr) {
+            currentMovementGoalPtr = movementGoalPtr;
+            currentAction.state = MOVEMENT;
+            sendAction(actionQueue, &currentAction);
+
+            mutex.unlock();
+        });
+    
+    while(true){
+        rclcpp::spin(node);
+    }
+}

src/ros_actor_action_server/src/Server.hpp

@@ -0,0 +1,29 @@
+//
+// Created by ros on 2/6/23.
+//
+
+#ifndef BUILD_SERVER_H
+#define BUILD_SERVER_H
+
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp_action/rclcpp_action.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 <fcntl.h>
+#include <sys/stat.h>
+#include <mqueue.h>
+
+using namespace ros_actor_action_server_msgs;
+using rclcpp_action::ServerGoalHandle;
+
+#define AnimationActionServer rclcpp_action::Server<action::Animation>
+#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>>
+
+#endif //BUILD_SERVER_H

src/ros_actor_action_server_msgs/CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR)
-project(ign_actor_plugin_msgs)
+project(ros_actor_action_server_msgs)
 
 find_package(rosidl_default_generators REQUIRED)
 
@@ -7,5 +7,6 @@ rosidl_generate_interfaces(${PROJECT_NAME}
         "action/Animation.action"
         "action/Movement.action"
         )
+ament_export_dependencies(rosidl_default_runtime)
 
 ament_package()

src/ros_actor_action_server_msgs/package.xml

@@ -1,9 +1,9 @@
 <?xml version="1.0"?>
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
-  <name>ign_actor_plugin_msgs</name>
+  <name>ros_actor_action_server_msgs</name>
   <version>0.0.0</version>
-  <description>Plugin for Gazebo Ignition to remote control actors</description>
+  <description>ROS2 message definitions for ros_actor_action_server</description>
   <maintainer email="bastian@todo.todo">Bastian Hofmann</maintainer>
   <license>TODO: License declaration</license>
 

src/ros_actor_message_queue_msgs/CMakeLists.txt

@@ -0,0 +1,28 @@
+cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR)
+project(ros_actor_message_queue_msgs)
+
+find_package(ament_cmake REQUIRED)
+
+add_library(${PROJECT_NAME} INTERFACE)
+
+target_include_directories(${PROJECT_NAME} INTERFACE
+  "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+  "$<INSTALL_INTERFACE:include>"
+)
+target_link_libraries(${PROJECT_NAME} INTERFACE)
+
+install(TARGETS ${PROJECT_NAME}
+  EXPORT "export_${PROJECT_NAME}"
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION lib
+  INCLUDES DESTINATION include
+)
+
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION include/${PROJECT_NAME}
+)
+
+ament_export_targets("export_${PROJECT_NAME}")
+
+ament_package()

src/ros_actor_message_queue_msgs/include/ros_actor_message_queue_msgs/MessageTypes.hpp

@@ -0,0 +1,17 @@
+namespace ros_actor_message_queue_msgs{
+enum ActorPluginState{
+  SETUP,IDLE,ANIMATION,MOVEMENT
+  };
+
+struct FeedbackMessage{
+  ActorPluginState state;
+  float progress;
+};
+
+struct ActionMessage{
+  ActorPluginState state;
+  float positionX = 0.0f, positionY = 0.0f, positionZ = 0.0f;
+  float animationSpeed = 1.0f,animationDistance = 1.0f;
+  char animationName[256];
+};
+}

src/ros_actor_message_queue_msgs/package.xml

@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>ros_actor_message_queue_msgs</name>
+  <version>0.0.0</version>
+  <description>POSIX message queue struct definitions for ros_actor_action_server</description>
+  <maintainer email="bastian@todo.todo">Bastian Hofmann</maintainer>
+  <license>TODO: License declaration</license>
+
+  <depend>ament_cmake</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>