Introduce new Actor functionality

src/gaz_simulation/world/gaz_ign.sdf

@@ -243,6 +243,18 @@
       </link>
     </model>
     
+    <actor name="actor_walking">
+      <pose>0 2 1.25 0 0 0</pose>
+      <skin>
+        <filename>https://fuel.gazebosim.org/1.0/Mingfei/models/actor/tip/files/meshes/walk.dae</filename>
+        <scale>1.0</scale>
+      </skin>
+      <animation name="walk">
+        <filename>https://fuel.gazebosim.org/1.0/Mingfei/models/actor/tip/files/meshes/walk.dae</filename>
+        <interpolate_x>true</interpolate_x>
+      </animation>
+    </actor>
+    
     <actor name="actor1">
       <pose>0 1 1.25 0 0 0</pose>
 
@@ -250,6 +262,11 @@
         <filename>/home/ros/go.dae</filename>
         <scale>1.0</scale>
       </skin>
+      <animation name="walking">
+        <filename>/home/ros/go.dae</filename>
+        <scale>1.000000</scale>
+        <interpolate_x>true</interpolate_x>
+      </animation>
       <plugin name="ActorSystem" filename="/home/ros/workspace/build/ign_actor_plugin/libActorPlugin.so">
         <target>2 2 1</target>
         <target_weight>1.15</target_weight>

src/gaz_simulation/world/gaz_new_test.sdf

@@ -0,0 +1,82 @@
+<sdf version="1.6">
+  <world name="empty">
+    <physics name="1ms" type="ignored">
+      <max_step_size>0.001</max_step_size>
+      <real_time_factor>1.0</real_time_factor>
+    </physics>
+    <plugin
+      filename="gz-sim-physics-system"
+      name="gz::sim::systems::Physics">
+    </plugin>
+    <plugin
+      filename="gz-sim-user-commands-system"
+      name="gz::sim::systems::UserCommands">
+    </plugin>
+    <plugin
+      filename="gz-sim-scene-broadcaster-system"
+      name="gz::sim::systems::SceneBroadcaster">
+    </plugin>
+    <plugin
+      filename="gz-sim-contact-system"
+      name="gz::sim::systems::Contact">
+    </plugin>
+
+    <light type="directional" name="sun">
+      <cast_shadows>true</cast_shadows>
+      <pose>0 0 10 0 0 0</pose>
+      <diffuse>0.8 0.8 0.8 1</diffuse>
+      <specular>0.2 0.2 0.2 1</specular>
+      <attenuation>
+        <range>1000</range>
+        <constant>0.9</constant>
+        <linear>0.01</linear>
+        <quadratic>0.001</quadratic>
+      </attenuation>
+      <direction>-0.5 0.1 -0.9</direction>
+    </light>
+    
+    <actor name="actor_walking">
+      <plugin name="ignition::gazebo::ActorSystem" filename="/home/ros/workspace/install/ign_actor_plugin/lib/ign_actor_plugin/libActorPlugin.so">
+        
+      </plugin>
+      <skin>
+        <filename>https://fuel.ignitionrobotics.org/1.0/Mingfei/models/actor/tip/files/meshes/walk.dae</filename>
+        <scale>1.0</scale>
+      </skin>
+      <animation name='walk'>
+        <filename>https://fuel.ignitionrobotics.org/1.0/Mingfei/models/actor/tip/files/meshes/walk.dae</filename>
+      </animation>
+      <animation name='go'>
+        <filename>/home/ros/go.dae</filename>
+      </animation>
+      
+    </actor>
+    
+    <model name="ground_plane">
+      <static>true</static>
+      <link name="link">
+        <collision name="collision">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+        </collision>
+        <visual name="visual">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+      </link>
+    </model>
+  </world>
+</sdf>

src/ign_actor_plugin/src/ActorSystem.cpp

@@ -2,43 +2,164 @@
 // Created by bastian on 31.08.22.
 //
 
+#include <cstdio>
+#include <ignition/common/Console.hh>
 #include <ignition/gazebo/Entity.hh>
 #include <ignition/gazebo/Types.hh>
+#include <ignition/gazebo/components/Component.hh>
+#include <ignition/gazebo/components/Pose.hh>
+#include <ignition/msgs/pose_trajectory.pb.h>
 #include <rclcpp_action/create_server.hpp>
 #include <rclcpp_action/server.hpp>
 #include "ActorSystem.hpp"
-
+#include <ignition/gazebo/components/Actor.hh>
+#include <ignition/gazebo/components/Name.hh>
+#include <ignition/gazebo/EntityComponentManager.hh>
+#include <ignition/gazebo/Util.hh>
+#include <sdf/Actor.hh>
 
 IGNITION_ADD_PLUGIN(
-        ActorSystem,
+        ignition::gazebo::ActorSystem,
         ignition::gazebo::System,
-        ActorSystem::ISystemPreUpdate,
+        ignition::gazebo::ActorSystem::ISystemPreUpdate,
-        ActorSystem::ISystemConfigure);
+        ignition::gazebo::ActorSystem::ISystemConfigure);
+
+using namespace ignition;
+using namespace gazebo;
+
+
 
 ActorSystem::ActorSystem() = default;
 
 ActorSystem::~ActorSystem() = default;
 
+void ActorSystem::PreUpdate(const UpdateInfo &_info, ::EntityComponentManager &_ecm) {
+    switch(this->actorState){
+        case ANIMATION: {
+            //if (animationTarget == nullptr) {
+            //    return;
+            //}
+            auto animationTimeComponent = _ecm.Component<components::AnimationTime>(this->entitiy);
             
+            auto newTime = animationTimeComponent->Data() + _info.dt;
             
-void ActorSystem::PreUpdate(const ignition::gazebo::UpdateInfo &_info, ignition::gazebo::EntityComponentManager &_ecm) {
+            auto test = std::chrono::duration_cast<std::chrono::milliseconds>(newTime);
             
+            ignwarn << "Animation state: " << test.count() << "\n";
+            *animationTimeComponent = components::AnimationTime(newTime);
+            
+            _ecm.SetChanged(entitiy, components::AnimationTime::typeId, ComponentState::OneTimeChange);
+            
+            return;
+        }
+        case MOVEMENT:{
+            if (movementTarget == nullptr) {
+                return;
+            }
+            auto trajPoseComp = _ecm.Component<components::TrajectoryPose>(this->entitiy);
+    
+            auto actorPose = trajPoseComp->Data();
+            auto initialPose = actorPose;
+    
+            auto targetPose = movementTarget.get()->target_position;
+    
+            double distanceTraveled = (actorPose.Pos() - initialPose.Pos()).Length();
+    
+            auto animTimeComp = _ecm.Component<components::AnimationTime>(
+                this->entitiy);
+
+            auto animTime = animTimeComp->Data() +
+                    std::chrono::duration_cast<std::chrono::steady_clock::duration>(
+                        std::chrono::duration<double>(
+                            distanceTraveled *
+                            1
+                        )
+                    );
+            *animTimeComp = components::AnimationTime(animTime);
+            return;
+        }
+        case IDLE:
+            return;
+    }
     
 }
 
 // Found too late: https://github.com/AlanSixth/gazebo_ros_action_tutorial/blob/master/src/gazebo_ros_action_tutorial.cc
+// https://github.com/gazebosim/gz-sim/blob/ign-gazebo6/src/systems/follow_actor/FollowActor.cc
+void ActorSystem::Configure(const Entity &_entity, const std::shared_ptr<const sdf::Element> &_sdf,
+                            EntityComponentManager &_ecm, EventManager &) {
+    igndbg << "Plugin configuring..." << std::endl;
     
-void ActorSystem::Configure(const ignition::gazebo::Entity &_entity, const std::shared_ptr<const sdf::Element> &_sdf,
+    this->entitiy = _entity;
-                            ignition::gazebo::EntityComponentManager &_ecm, ignition::gazebo::EventManager &) {
+    this->actorState = ActorSystemState::ANIMATION;
-    printf("Plugin configuring...\n");
     
-    
+    auto actorComp = _ecm.Component<components::Actor>(_entity);
-    /*auto actorComp = _ecm.Component<Actor>(_entity);
+    if (!actorComp) {
-  if (!actorComp)
+        ignerr << "No attached actor found, terminating." << std::endl;
-  {
-    gzerr << "Entity [" << _entity << "] is not an actor." << std::endl;
         return;
-  }*/
+    }
+    
+    
+    
+    igndbg << "Found this skin file: " << actorComp->Data().SkinFilename() << std::endl;
+    
+    std::string animationName = "go";
+    
+    if (!_sdf->HasElement("animation")) {
+        if (actorComp->Data().AnimationCount() < 1) {
+            ignerr << "Actor SDF doesn't have any animations." << std::endl;
+            return;
+        }
+        animationName = actorComp->Data().AnimationByIndex(0)->Name();
+    } else {
+        animationName = _sdf->Get<std::string>("animation");
+    }
+    igndbg << "Found this animation: " << animationName << std::endl;
+    
+    animationName = "walk";
+    
+    auto animationNameComp = _ecm.Component<components::AnimationName>(_entity);
+    if (nullptr == animationNameComp) {
+        _ecm.CreateComponent(_entity, components::AnimationName(animationName));
+    } else {
+        *animationNameComp = components::AnimationName(animationName);
+    }
+    
+    
+    for (const auto& elem: _ecm.ComponentTypes(_entity)) {
+        igndbg << "Component: " << elem << std::endl;
+    }
+    
+    _ecm.SetChanged(_entity, components::AnimationName::typeId, ComponentState::OneTimeChange);
+    
+    auto animTimeComp = _ecm.Component<components::AnimationTime>(_entity);
+    if (nullptr == animTimeComp) {
+        _ecm.CreateComponent(_entity, components::AnimationTime());
+    }
+    
+    ignition::math::Pose3d initialPose;
+    auto poseComp = _ecm.Component<components::Pose>(_entity);
+    if (nullptr == poseComp) {
+        _ecm.CreateComponent(_entity, components::Pose(
+            ignition::math::Pose3d::Zero));
+    } else {
+        initialPose = poseComp->Data();
+
+        // We'll be setting the actor's X/Y pose with respect to the world. So we
+        // zero the current values.
+        auto newPose = initialPose;
+        newPose.Pos().X(0);
+        newPose.Pos().Y(0);
+        *poseComp = components::Pose(newPose);
+    }
+    
+    
+    auto trajPoseComp = _ecm.Component<components::TrajectoryPose>(_entity);
+    if (nullptr == trajPoseComp) {
+        // Leave Z to the pose component, control only 2D with Trajectory
+        initialPose.Pos().Z(0);
+        _ecm.CreateComponent(_entity, components::TrajectoryPose(initialPose));
+    }
     
     rclcpp::init(0, {});
 
@@ -53,7 +174,6 @@ void ActorSystem::Configure(const ignition::gazebo::Entity &_entity, const std::
     
     node = rclcpp::Node::make_shared("moveService", topic);
     
-
     animationServer = rclcpp_action::create_server<ign_actor_plugin_msgs::action::Animation>(
             node,
             "animation",
@@ -64,6 +184,7 @@ void ActorSystem::Configure(const ignition::gazebo::Entity &_entity, const std::
             },
             [](AnimationServerGoalPtr PH1) {
                 return rclcpp_action::CancelResponse::ACCEPT;
+                return rclcpp_action::CancelResponse::REJECT;
             },
             [](AnimationServerGoalPtr PH1) {
                 
@@ -75,35 +196,19 @@ void ActorSystem::Configure(const ignition::gazebo::Entity &_entity, const std::
             "movement",
             [](rclcpp_action::GoalUUID goalUuid, MovementGoalPtr &movementGoal ){
                 return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
-                
             },
             [](MovementServerGoalPtr goalUuid ){
-                
                 return rclcpp_action::CancelResponse::ACCEPT;
             },
             [](MovementServerGoalPtr goalUuid ){}
     );
 
-    printf("Spinning node...\n");
+    igndbg << "Spinning node..." << std::endl;
     std::thread spinThread([this]() {
         while (true) {
-            printf("Wheee...\n");
+            igndbg << "Wheee..." << std::endl;
             rclcpp::spin(node);
         }
     });
     spinThread.detach();
 }
-
-/*
-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.hpp

@@ -5,6 +5,7 @@
 #ifndef BUILD_ACTORSYSTEM_H
 #define BUILD_ACTORSYSTEM_H
 
+#include <ignition/gazebo/Entity.hh>
 #include <ignition/gazebo/System.hh>
 #include <ignition/plugin/Register.hh>
 #include <queue>
@@ -20,15 +21,25 @@
 #define MovementGoalPtr const std::shared_ptr<const ign_actor_plugin_msgs::action::Movement::Goal>
 #define MovementServerGoalPtr const std::shared_ptr<rclcpp_action::ServerGoalHandle<ign_actor_plugin_msgs::action::Movement>>&
 
+namespace ignition {
+    namespace gazebo {
+        enum ActorSystemState{
+            IDLE,MOVEMENT,ANIMATION
+        };
+        
         class ActorSystem:
-        public ignition::gazebo::System,
+                public System,
-        public ignition::gazebo::ISystemPreUpdate,
+                public ISystemPreUpdate,
-        public ignition::gazebo::ISystemConfigure {
+                public ISystemConfigure {
             
             private:
                 std::shared_ptr<rclcpp::Node> node;
                 std::shared_ptr<rclcpp_action::Server<ign_actor_plugin_msgs::action::Animation>> animationServer;
                 std::shared_ptr<rclcpp_action::Server<ign_actor_plugin_msgs::action::Movement>> movementServer;
+                ActorSystemState actorState;
+                AnimationGoalPtr animationTarget;
+                MovementGoalPtr movementTarget;
+                Entity entitiy{kNullEntity};
             
             public:
                 ActorSystem();
@@ -37,16 +48,18 @@ public:
                 ~ActorSystem() override;
             
             public:
-    void PreUpdate(const ignition::gazebo::UpdateInfo &_info,
+                void PreUpdate(const UpdateInfo &_info,
-                   ignition::gazebo::EntityComponentManager &_ecm) override;
+                               EntityComponentManager &_ecm) override;
             
             public:
                 void Configure(const ignition::gazebo::Entity &_entity,
                                const std::shared_ptr<const sdf::Element> &_sdf,
-                   ignition::gazebo::EntityComponentManager &_ecm,
+                               EntityComponentManager &_ecm,
-                   ignition::gazebo::EventManager &/*_eventMgr*/) override;
+                               EventManager &/*_eventMgr*/) override;
             
 
         };
+    }
+}
 
 #endif //BUILD_ACTORSYSTEM_H