diff --git a/src/gaz_simulation/world/gaz_ign.sdf b/src/gaz_simulation/world/gaz_ign.sdf
index a9ebc9e..dcd4b46 100644
--- a/src/gaz_simulation/world/gaz_ign.sdf
+++ b/src/gaz_simulation/world/gaz_ign.sdf
@@ -243,6 +243,18 @@
+
+ 0 2 1.25 0 0 0
+
+ https://fuel.gazebosim.org/1.0/Mingfei/models/actor/tip/files/meshes/walk.dae
+ 1.0
+
+
+ https://fuel.gazebosim.org/1.0/Mingfei/models/actor/tip/files/meshes/walk.dae
+ true
+
+
+
0 1 1.25 0 0 0
@@ -250,6 +262,11 @@
/home/ros/go.dae
1.0
+
+ /home/ros/go.dae
+ 1.000000
+ true
+
2 2 1
1.15
diff --git a/src/gaz_simulation/world/gaz_new_test.sdf b/src/gaz_simulation/world/gaz_new_test.sdf
new file mode 100644
index 0000000..22fe398
--- /dev/null
+++ b/src/gaz_simulation/world/gaz_new_test.sdf
@@ -0,0 +1,82 @@
+
+
+
+ 0.001
+ 1.0
+
+
+
+
+
+
+
+
+
+
+
+ true
+ 0 0 10 0 0 0
+ 0.8 0.8 0.8 1
+ 0.2 0.2 0.2 1
+
+ 1000
+ 0.9
+ 0.01
+ 0.001
+
+ -0.5 0.1 -0.9
+
+
+
+
+
+
+
+ https://fuel.ignitionrobotics.org/1.0/Mingfei/models/actor/tip/files/meshes/walk.dae
+ 1.0
+
+
+ https://fuel.ignitionrobotics.org/1.0/Mingfei/models/actor/tip/files/meshes/walk.dae
+
+
+ /home/ros/go.dae
+
+
+
+
+
+ true
+
+
+
+
+ 0 0 1
+ 100 100
+
+
+
+
+
+
+ 0 0 1
+ 100 100
+
+
+
+ 0.8 0.8 0.8 1
+ 0.8 0.8 0.8 1
+ 0.8 0.8 0.8 1
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/ign_actor_plugin/src/ActorSystem.cpp b/src/ign_actor_plugin/src/ActorSystem.cpp
index 08f6d91..043abaa 100644
--- a/src/ign_actor_plugin/src/ActorSystem.cpp
+++ b/src/ign_actor_plugin/src/ActorSystem.cpp
@@ -2,44 +2,165 @@
// Created by bastian on 31.08.22.
//
+#include
+#include
#include
#include
+#include
+#include
+#include
#include
#include
#include "ActorSystem.hpp"
-
+#include
+#include
+#include
+#include
+#include
IGNITION_ADD_PLUGIN(
- ActorSystem,
+ ignition::gazebo::ActorSystem,
ignition::gazebo::System,
- ActorSystem::ISystemPreUpdate,
- ActorSystem::ISystemConfigure);
+ ignition::gazebo::ActorSystem::ISystemPreUpdate,
+ 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(this->entitiy);
+
+ auto newTime = animationTimeComponent->Data() + _info.dt;
+
+ auto test = std::chrono::duration_cast(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(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(
+ this->entitiy);
-
-void ActorSystem::PreUpdate(const ignition::gazebo::UpdateInfo &_info, ignition::gazebo::EntityComponentManager &_ecm) {
-
+ auto animTime = animTimeComp->Data() +
+ std::chrono::duration_cast(
+ std::chrono::duration(
+ 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
-
-void ActorSystem::Configure(const ignition::gazebo::Entity &_entity, const std::shared_ptr &_sdf,
- ignition::gazebo::EntityComponentManager &_ecm, ignition::gazebo::EventManager &) {
- printf("Plugin configuring...\n");
+// 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 &_sdf,
+ EntityComponentManager &_ecm, EventManager &) {
+ igndbg << "Plugin configuring..." << std::endl;
+
+ this->entitiy = _entity;
+ this->actorState = ActorSystemState::ANIMATION;
+
+ auto actorComp = _ecm.Component(_entity);
+ if (!actorComp) {
+ ignerr << "No attached actor found, terminating." << std::endl;
+ return;
+ }
- /*auto actorComp = _ecm.Component(_entity);
- if (!actorComp)
- {
- 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("animation");
+ }
+ igndbg << "Found this animation: " << animationName << std::endl;
+
+ animationName = "walk";
+
+ auto animationNameComp = _ecm.Component(_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(_entity);
+ if (nullptr == animTimeComp) {
+ _ecm.CreateComponent(_entity, components::AnimationTime());
+ }
+
+ ignition::math::Pose3d initialPose;
+ auto poseComp = _ecm.Component(_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(_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, {});
std::string topic = "ActorPlugin";
@@ -50,10 +171,9 @@ void ActorSystem::Configure(const ignition::gazebo::Entity &_entity, const std::
if (_sdf->HasElement("actor_name")) {
name = _sdf->Get("actor_name");
}
-
+
node = rclcpp::Node::make_shared("moveService", topic);
-
-
+
animationServer = rclcpp_action::create_server(
node,
"animation",
@@ -64,9 +184,10 @@ 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(_entity);
- if (nullptr == animationNameComp)
- {
- _ecm.CreateComponent(_entity, components::AnimationName(animationName));
- }
- else
- {
- *animationNameComp = components::AnimationName(animationName);
- }
-}
-*/
\ No newline at end of file
+}
\ No newline at end of file
diff --git a/src/ign_actor_plugin/src/ActorSystem.hpp b/src/ign_actor_plugin/src/ActorSystem.hpp
index cfd659a..6fb7ca0 100644
--- a/src/ign_actor_plugin/src/ActorSystem.hpp
+++ b/src/ign_actor_plugin/src/ActorSystem.hpp
@@ -5,6 +5,7 @@
#ifndef BUILD_ACTORSYSTEM_H
#define BUILD_ACTORSYSTEM_H
+#include
#include
#include
#include
@@ -20,33 +21,45 @@
#define MovementGoalPtr const std::shared_ptr
#define MovementServerGoalPtr const std::shared_ptr>&
-class ActorSystem:
- public ignition::gazebo::System,
- public ignition::gazebo::ISystemPreUpdate,
- public ignition::gazebo::ISystemConfigure {
+namespace ignition {
+ namespace gazebo {
+ enum ActorSystemState{
+ IDLE,MOVEMENT,ANIMATION
+ };
+
+ class ActorSystem:
+ public System,
+ public ISystemPreUpdate,
+ public ISystemConfigure {
+
+ private:
+ std::shared_ptr node;
+ std::shared_ptr> animationServer;
+ std::shared_ptr> movementServer;
+ ActorSystemState actorState;
+ AnimationGoalPtr animationTarget;
+ MovementGoalPtr movementTarget;
+ Entity entitiy{kNullEntity};
+
+ public:
+ ActorSystem();
+
+ public:
+ ~ActorSystem() override;
+
+ public:
+ void PreUpdate(const UpdateInfo &_info,
+ EntityComponentManager &_ecm) override;
+
+ public:
+ void Configure(const ignition::gazebo::Entity &_entity,
+ const std::shared_ptr &_sdf,
+ EntityComponentManager &_ecm,
+ EventManager &/*_eventMgr*/) override;
+
-private:
- std::shared_ptr node;
- std::shared_ptr> animationServer;
- std::shared_ptr> movementServer;
-
-public:
- ActorSystem();
-
-public:
- ~ActorSystem() override;
-
-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 &_sdf,
- ignition::gazebo::EntityComponentManager &_ecm,
- ignition::gazebo::EventManager &/*_eventMgr*/) override;
-
-
-};
+ };
+ }
+}
#endif //BUILD_ACTORSYSTEM_H