Added ROS action server to new ignition actor plugin

src/ign_actor_plugin/CMakeLists.txt

@@ -1,15 +1,36 @@
 cmake_minimum_required(VERSION 3.24)
-project(ActorPlugin)
+project(ign_actor_plugin)
 
 set(IGN_PLUGIN_VER 0)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_action REQUIRED)
+find_package(rclcpp_components REQUIRED)
 find_package(ignition-cmake2 REQUIRED)
 find_package(ignition-gazebo5 REQUIRED)
+
+find_package(rosidl_default_generators REQUIRED)
+
 ign_find_package(ignition-plugin1 REQUIRED COMPONENTS register)
 set(IGN_PLUGIN_VER ${ignition-plugin1_VERSION_MAJOR})
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+        "action/Animation.action"
+        "action/Movement.action"
+        )
+
+ament_export_dependencies(rosidl_default_runtime)
+
 # Add sources for each plugin to be registered.
-add_library(SampleSystem src/ActorSystem.cpp)
+add_library(ActorPlugin src/ActorSystem.cpp)
-set_property(TARGET SampleSystem PROPERTY CXX_STANDARD 17)
+ament_target_dependencies(ActorPlugin rclcpp)
-target_link_libraries(SampleSystem
+set_property(TARGET ActorPlugin PROPERTY CXX_STANDARD 17)
+
+rosidl_target_interfaces(ActorPlugin
+        ${PROJECT_NAME} "rosidl_typesupport_cpp")
+
+target_link_libraries(ActorPlugin
         ignition-plugin${IGN_PLUGIN_VER}::ignition-plugin${IGN_PLUGIN_VER}
         ignition-gazebo5::ignition-gazebo5
-        )
+        )
+
+ament_package()

src/ign_actor_plugin/action/Animation.action

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

src/ign_actor_plugin/action/Movement.action

@@ -0,0 +1,8 @@
+string animation_name
+float32 animation_speed
+float32[3] target_position
+float32[3] target_orientation
+---
+bool success
+---
+float32 progress

src/ign_actor_plugin/package.xml

@@ -0,0 +1,28 @@
+<?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</name>
+  <version>0.0.0</version>
+  <description>Plugin for Gazebo Ignition to remote control actors</description>
+  <maintainer email="bastian@todo.todo">Bastian Hofmann</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>rosidl_default_generators</buildtool_depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_action</depend>
+  <depend>rclcpp_components</depend>
+  <depend>ign_actor_plugin_messages</depend>
+  <depend>action_msgs</depend>
+  <depend>ignition-cmake2</depend>
+  <depend>ignition-gazebo5</depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/ign_actor_plugin/src/ActorSystem.cpp

@@ -2,6 +2,7 @@
 // Created by bastian on 31.08.22.
 //
 
+#include <rclcpp_action/create_server.hpp>
 #include "ActorSystem.h"
 
 IGNITION_ADD_PLUGIN(
@@ -15,4 +16,42 @@ ActorSystem::~ActorSystem() = default;
 
 void ActorSystem::PreUpdate(const ignition::gazebo::UpdateInfo &_info, ignition::gazebo::EntityComponentManager &_ecm) {
 
+    //_ecm.EachNew<>()
 }
+
+// Found too late: https://github.com/AlanSixth/gazebo_ros_action_tutorial/blob/master/src/gazebo_ros_action_tutorial.cc
+
+void ActorSystem::Configure(const ignition::gazebo::Entity &_entity, const std::shared_ptr<const sdf::Element> &_sdf,
+        ignition::gazebo::EntityComponentManager &_ecm, ignition::gazebo::EventManager &) {
+    rclcpp::init(0, {});
+    std::string topic = "ActorPlugin";
+    if(_sdf->HasElement("topic")){
+        topic = _sdf->Get<std::string>("topic");
+    }
+    std::string name = "Actor";
+    if(_sdf->HasElement("actor_name")){
+        name = _sdf->Get<std::string>("actor_name");
+    }
+
+    node = rclcpp::Node::make_shared("moveService",topic);
+
+    rclcpp_action::create_server<ign_actor_plugin::action::Animation>(node,"animation",
+            std::bind(&ActorSystem::handle_animation_goal,this,std::placeholders::_1,std::placeholders::_2),
+            std::bind(&ActorSystem::handle_animation_cancel,this,std::placeholders::_1),
+            std::bind(&ActorSystem::handle_animation_accepted,this,std::placeholders::_1)
+            );
+}
+
+/*
+void loadAnimation(std::string name){
+    auto animationNameComp = _ecm.Component<components::AnimationName>(_entity);
+    if (nullptr == animationNameComp)
+    {
+        _ecm.CreateComponent(_entity, components::AnimationName(animationName));
+    }
+    else
+    {
+        *animationNameComp = components::AnimationName(animationName);
+    }
+}
+*/

src/ign_actor_plugin/src/ActorSystem.h

@@ -7,10 +7,18 @@
 
 #include <ignition/gazebo/System.hh>
 #include <ignition/plugin/Register.hh>
+#include <rclcpp/node.hpp>
+#include "rclcpp/rclcpp.hpp"
+#include "ign_actor_plugin/action/animation.hpp"
+
 
 class ActorSystem:
         public ignition::gazebo::System,
-        public ignition::gazebo::ISystemPreUpdate {
+        public ignition::gazebo::ISystemPreUpdate,
+        public ignition::gazebo::ISystemConfigure{
+
+private:
+    std::shared_ptr<rclcpp::Node> node;
 
 public:
     ActorSystem();
@@ -20,7 +28,24 @@ public:
 
 public: void PreUpdate(const ignition::gazebo::UpdateInfo &_info,
             ignition::gazebo::EntityComponentManager &_ecm) override;
+
+public: void Configure(const ignition::gazebo::Entity &_entity,
+            const std::shared_ptr<const sdf::Element> &_sdf,
+            ignition::gazebo::EntityComponentManager &_ecm,
+            ignition::gazebo::EventManager &/*_eventMgr*/) override;
+
+private: rclcpp_action::GoalResponse handle_animation_goal(const rclcpp_action::GoalUUID & uuid,
+            std::shared_ptr<const ign_actor_plugin::action::Animation::Goal> goal);
+
+private: rclcpp_action::CancelResponse handle_animation_cancel(
+            const std::shared_ptr<rclcpp_action::ServerGoalHandle<ign_actor_plugin::action::Animation>> goal_handle);
+
+private: void handle_animation_accepted(
+            const std::shared_ptr<rclcpp_action::ServerGoalHandle<ign_actor_plugin::action::Animation>> goal_handle);
+
 };
 
 
+
+
 #endif //BUILD_ACTORSYSTEM_H