Connected and added movement part of Plugin.
This commit is contained in:
Bastian Hofmann
parent
38263dd13f
commit
f9f1766004
@ -48,29 +48,50 @@ void ActorSystem::switchAnimation(EntityComponentManager &_ecm, std::string& ani
|
||||
*animationTimeComponent = components::AnimationTime(std::chrono::milliseconds::zero());
|
||||
_ecm.SetChanged(entity, components::AnimationTime::typeId, ComponentState::OneTimeChange);
|
||||
|
||||
int foundIndex = -1;
|
||||
int foundAnimationIndex = -1;
|
||||
for (int animationIndex = 0 ; animationIndex < actorComp->Data().AnimationCount() ; animationIndex++) {
|
||||
auto animation = actorComp->Data().AnimationByIndex(animationIndex);
|
||||
if (animation->Name() == animationName){
|
||||
foundIndex = animationIndex;
|
||||
foundAnimationIndex = animationIndex;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if(foundIndex!=-1){
|
||||
if(foundAnimationIndex != -1){
|
||||
igndbg << "Found animation to play" << std::endl;
|
||||
auto fileName = actorComp->Data().AnimationByIndex(foundIndex)->Filename();
|
||||
igndbg << "Path: " << fileName << std::endl;
|
||||
igndbg << "Name: " << common::MeshManager::Instance()->Load(fileName)->Name() << std::endl;
|
||||
auto fileName = actorComp->Data().AnimationByIndex(foundAnimationIndex)->Filename();
|
||||
|
||||
auto skeleton = common::MeshManager::Instance()->Load(fileName)->MeshSkeleton();
|
||||
currentSkeleton = common::MeshManager::Instance()->Load(fileName)->MeshSkeleton();
|
||||
|
||||
if (skeleton != nullptr){
|
||||
duration = skeleton->Animation(0)->Length();
|
||||
igndbg << "FileName: " << fileName << std::endl;
|
||||
igndbg << "AnimCnt: " << currentSkeleton->AnimationCount() << std::endl;
|
||||
|
||||
auto rootNode = currentSkeleton->RootNode()->Name();
|
||||
|
||||
currentRootNodeAnimation = currentSkeleton->Animation(0)->NodeAnimationByName(rootNode);
|
||||
currentTranslationAlign = currentSkeleton->AlignTranslation(0, rootNode);
|
||||
currentRotationAlign = currentSkeleton->AlignRotation(0, rootNode);
|
||||
|
||||
igndbg << "Translation: " << currentTranslationAlign << std::endl;
|
||||
igndbg << "Rotation: " << currentRotationAlign << std::endl;
|
||||
|
||||
if (currentSkeleton != nullptr){
|
||||
duration = currentSkeleton->Animation(0)->Length();
|
||||
igndbg << "length: " << duration << " seconds." << std::endl;
|
||||
} else {
|
||||
ignerr << "No skeleton found!" << std::endl;
|
||||
}
|
||||
|
||||
|
||||
|
||||
auto initialPose = currentRootNodeAnimation -> FrameAt(0).Pose();
|
||||
auto poseComponent = _ecm.Component<components::Pose>(entity);
|
||||
|
||||
auto newPose = initialPose;
|
||||
newPose.Pos().X(0);
|
||||
newPose.Pos().Y(0);
|
||||
*poseComponent = components::Pose(newPose);
|
||||
|
||||
} else {
|
||||
ignerr << "No animation found!" << std::endl;
|
||||
}
|
||||
@ -82,40 +103,26 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
|
||||
case SETUP: {
|
||||
igndbg << "Setting up actor plugin" << std::endl;
|
||||
|
||||
auto actorComp = _ecm.Component<components::Actor>(entity);
|
||||
auto actorComponent = _ecm.Component<components::Actor>(entity);
|
||||
|
||||
auto animationNameComp = _ecm.Component<components::AnimationName>(entity);
|
||||
if (nullptr == animationNameComp) {
|
||||
_ecm.CreateComponent(entity, components::AnimationName(actorComp->Data().AnimationByIndex(0)->Name()));
|
||||
auto animationNameComponent = _ecm.Component<components::AnimationName>(entity);
|
||||
if (nullptr == animationNameComponent) {
|
||||
_ecm.CreateComponent(entity, components::AnimationName(actorComponent->Data().AnimationByIndex(0)->Name()));
|
||||
}
|
||||
|
||||
auto animTimeComp = _ecm.Component<components::AnimationTime>(entity);
|
||||
if (nullptr == animTimeComp) {
|
||||
auto animTimeComponent = _ecm.Component<components::AnimationTime>(entity);
|
||||
if (nullptr == animTimeComponent) {
|
||||
_ecm.CreateComponent(entity, components::AnimationTime());
|
||||
}
|
||||
|
||||
ignition::math::Pose3d initialPose;
|
||||
auto poseComp = _ecm.Component<components::Pose>(entity);
|
||||
if (nullptr == poseComp) {
|
||||
auto poseComponent = _ecm.Component<components::Pose>(entity);
|
||||
if (nullptr == poseComponent) {
|
||||
_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 trajectoryPoseComp = _ecm.Component<components::TrajectoryPose>(entity);
|
||||
if (nullptr == trajectoryPoseComp) {
|
||||
// Leave Z to the pose component, control only 2D with Trajectory
|
||||
//initialPose.Pos().Z(0);
|
||||
_ecm.CreateComponent(entity, components::TrajectoryPose(initialPose));
|
||||
auto trajectoryPoseComponent = _ecm.Component<components::TrajectoryPose>(entity);
|
||||
if (nullptr == trajectoryPoseComponent) {
|
||||
_ecm.CreateComponent(entity, components::TrajectoryPose(ignition::math::Pose3d::Zero));
|
||||
}
|
||||
|
||||
currentState = ActorSystemState::IDLE;
|
||||
@ -143,34 +150,113 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
|
||||
*animationTimeComponent = components::AnimationTime(newTime);
|
||||
_ecm.SetChanged(entity, components::AnimationTime::typeId, ComponentState::OneTimeChange);
|
||||
|
||||
|
||||
/*
|
||||
if(currentRootNodeAnimation == nullptr){
|
||||
break;
|
||||
}
|
||||
|
||||
auto rootTransformation = currentRootNodeAnimation->FrameAt(newTimeSeconds,false);
|
||||
math::Matrix4d totalTf = currentTranslationAlign * rootTransformation * currentRotationAlign;
|
||||
|
||||
auto translation = totalTf.Translation();
|
||||
translation[0] = 0;
|
||||
totalTf.SetTranslation(translation);
|
||||
|
||||
auto poseComp = _ecm.Component<components::Pose>(entity);
|
||||
*poseComp = components::Pose (totalTf.Pose());
|
||||
_ecm.SetChanged(entity, components::Pose::typeId, ComponentState::OneTimeChange);
|
||||
*/
|
||||
|
||||
break;
|
||||
}
|
||||
case MOVEMENT:{
|
||||
if (lastState == ActorSystemState::IDLE){
|
||||
igndbg << "Starting Movement..." << std::endl;
|
||||
switchAnimation(_ecm, movementTarget.animation_name);
|
||||
}
|
||||
|
||||
auto trajectoryPoseComp = _ecm.Component<components::TrajectoryPose>(this->entity);
|
||||
auto actorPose = trajectoryPoseComp->Data();
|
||||
auto initialPose = actorPose;
|
||||
auto targetPose = movementTarget.target_position;
|
||||
|
||||
double distanceTraveled = (actorPose.Pos() - initialPose.Pos()).Length();
|
||||
auto targetDirection = actorPose.Pos() - math::Vector3d(targetPose[0],targetPose[1],targetPose[2]);
|
||||
|
||||
auto animTimeComp = _ecm.Component<components::AnimationTime>(
|
||||
this->entity);
|
||||
if (targetDirection.Length()<0.05){
|
||||
currentMovementGoalPtr->succeed(std::make_shared<action::Movement::Result>());
|
||||
currentState = ActorSystemState::IDLE;
|
||||
break;
|
||||
}
|
||||
|
||||
auto animTime = animTimeComp->Data() +
|
||||
std::chrono::duration_cast<std::chrono::steady_clock::duration>(
|
||||
std::chrono::duration<double>(
|
||||
distanceTraveled *
|
||||
1
|
||||
)
|
||||
);
|
||||
*animTimeComp = components::AnimationTime(animTime);
|
||||
auto animationTimeComponent = _ecm.Component<components::AnimationTime>(this->entity);
|
||||
auto oldTimeSeconds = std::chrono::duration<double>(animationTimeComponent->Data()).count();
|
||||
auto deltaTimeSeconds = std::chrono::duration<double>(_info.dt).count();
|
||||
auto newTimeSeconds = oldTimeSeconds + deltaTimeSeconds;
|
||||
auto newTime = animationTimeComponent->Data() + _info.dt;
|
||||
|
||||
if(newTimeSeconds >= duration){
|
||||
newTimeSeconds -= duration;
|
||||
newTime = std::chrono::duration_cast<std::chrono::steady_clock::duration>(std::chrono::duration<double>(newTimeSeconds));
|
||||
}
|
||||
|
||||
auto targetYaw = atan2(targetDirection.Y(),targetDirection.X());
|
||||
auto currentYaw = actorPose.Rot().Yaw();
|
||||
|
||||
auto angularDirection = fmod((targetYaw - currentYaw), 2*M_PI);
|
||||
if (angularDirection>M_PI){
|
||||
angularDirection-=M_PI;
|
||||
}
|
||||
if (angularDirection<=-M_PI){
|
||||
angularDirection+=M_PI;
|
||||
}
|
||||
auto turnSpeed = 1.0;
|
||||
|
||||
if (angularDirection < 0){
|
||||
turnSpeed = -turnSpeed;
|
||||
}
|
||||
|
||||
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{
|
||||
if (nullptr == currentRootNodeAnimation){
|
||||
ignerr << "Current animation doesn't move root node, this is unsupported for movement animations." << std::endl;
|
||||
break;
|
||||
}
|
||||
|
||||
double distance;
|
||||
if (oldTimeSeconds<=newTimeSeconds){
|
||||
auto oldPos = currentRootNodeAnimation->FrameAt(oldTimeSeconds).Pose().X();
|
||||
auto newPos = currentRootNodeAnimation->FrameAt(newTimeSeconds).Pose().X();
|
||||
distance = oldPos - newPos;
|
||||
}else{
|
||||
auto oldPos = currentRootNodeAnimation->FrameAt(oldTimeSeconds).Pose().X();
|
||||
// \/
|
||||
auto endPos = currentRootNodeAnimation->KeyFrame(currentRootNodeAnimation->FrameCount()-1).second.Pose().X();
|
||||
// Big Distance
|
||||
auto startPos = currentRootNodeAnimation->KeyFrame(0).second.Pose().X();
|
||||
// \/
|
||||
auto newPos = currentRootNodeAnimation->FrameAt(newTimeSeconds).Pose().X();
|
||||
|
||||
distance = (oldPos - endPos) + (startPos - newPos);
|
||||
igndbg << (oldPos - endPos) << "|" << (startPos - newPos) << std::endl;
|
||||
}
|
||||
|
||||
actorPose.Pos() += targetDirection.Normalize() * distance;
|
||||
actorPose.Rot().Euler(0,0,targetYaw);
|
||||
*trajectoryPoseComp = components::TrajectoryPose(actorPose);
|
||||
*animationTimeComponent = components::AnimationTime(newTime);
|
||||
|
||||
_ecm.SetChanged(entity, components::TrajectoryPose::typeId, ComponentState::OneTimeChange);
|
||||
_ecm.SetChanged(entity, components::AnimationTime::typeId, ComponentState::OneTimeChange);
|
||||
}
|
||||
break;
|
||||
}
|
||||
case IDLE:
|
||||
if (lastState != currentState){
|
||||
igndbg << "Now idling..." << std::endl;
|
||||
}
|
||||
break;
|
||||
}
|
||||
lastState = currentState;
|
||||
@ -202,6 +288,8 @@ void ActorSystem::Configure(const Entity &_entity, const std::shared_ptr<const s
|
||||
|
||||
node = rclcpp::Node::make_shared("moveService", topic);
|
||||
|
||||
#pragma clang diagnostic push
|
||||
#pragma ide diagnostic ignored "UnusedValue"
|
||||
animationServer = rclcpp_action::create_server<action::Animation>(
|
||||
node,
|
||||
"animation",
|
||||
@ -216,7 +304,7 @@ void ActorSystem::Configure(const Entity &_entity, const std::shared_ptr<const s
|
||||
}
|
||||
|
||||
},
|
||||
[this](const AnimationServerGoalPtr PH1) {
|
||||
[this](const AnimationServerGoalPtr& animationGoalPtr) {
|
||||
if (this->currentState == ActorSystemState::IDLE) {
|
||||
return rclcpp_action::CancelResponse::REJECT;
|
||||
}else{
|
||||
@ -227,14 +315,15 @@ void ActorSystem::Configure(const Entity &_entity, const std::shared_ptr<const s
|
||||
}
|
||||
|
||||
},
|
||||
[this](const AnimationServerGoalPtr PH1) {
|
||||
[this](const AnimationServerGoalPtr& animationGoalPtr) {
|
||||
threadMutex.lock();
|
||||
currentAnimationGoalPtr = PH1;
|
||||
currentAnimationGoalPtr = animationGoalPtr;
|
||||
currentState = ActorSystemState::ANIMATION;
|
||||
threadMutex.unlock();
|
||||
}
|
||||
);
|
||||
|
||||
|
||||
movementServer = rclcpp_action::create_server<action::Movement>(
|
||||
node,
|
||||
"movement",
|
||||
@ -249,7 +338,7 @@ void ActorSystem::Configure(const Entity &_entity, const std::shared_ptr<const s
|
||||
return rclcpp_action::GoalResponse::REJECT;
|
||||
}
|
||||
},
|
||||
[this](const MovementServerGoalPtr goalUuid ){
|
||||
[this](const MovementServerGoalPtr& movementGoalPtr ){
|
||||
if (this->currentState == ActorSystemState::IDLE) {
|
||||
return rclcpp_action::CancelResponse::REJECT;
|
||||
}else{
|
||||
@ -259,13 +348,14 @@ void ActorSystem::Configure(const Entity &_entity, const std::shared_ptr<const s
|
||||
return rclcpp_action::CancelResponse::ACCEPT;
|
||||
}
|
||||
},
|
||||
[this](const MovementServerGoalPtr goalUuid ){
|
||||
[this](const MovementServerGoalPtr& movementGoalPtr ){
|
||||
threadMutex.lock();
|
||||
currentMovementGoalPtr = goalUuid;
|
||||
currentMovementGoalPtr = movementGoalPtr;
|
||||
currentState = ActorSystemState::MOVEMENT;
|
||||
threadMutex.unlock();
|
||||
}
|
||||
);
|
||||
#pragma clang diagnostic pop
|
||||
|
||||
igndbg << "Spinning node..." << std::endl;
|
||||
std::thread spinThread([this]() {
|
||||
|
||||
@ -18,6 +18,8 @@
|
||||
#include <ign_actor_plugin_msgs/action/movement.hpp>
|
||||
#include <rclcpp_action/server_goal_handle.hpp>
|
||||
#include <string>
|
||||
#include <ignition/common/Skeleton.hh>
|
||||
#include <ignition/common/NodeAnimation.hh>
|
||||
|
||||
using namespace ign_actor_plugin_msgs;
|
||||
using rclcpp_action::ServerGoalHandle;
|
||||
@ -33,46 +35,49 @@ using rclcpp_action::ServerGoalHandle;
|
||||
|
||||
namespace ignition {
|
||||
namespace gazebo {
|
||||
enum ActorSystemState{
|
||||
SETUP,IDLE,MOVEMENT,ANIMATION
|
||||
enum ActorSystemState {
|
||||
SETUP, IDLE, MOVEMENT, ANIMATION
|
||||
};
|
||||
|
||||
class ActorSystem:
|
||||
class ActorSystem :
|
||||
public System,
|
||||
public ISystemPreUpdate,
|
||||
public ISystemConfigure {
|
||||
|
||||
private:
|
||||
std::shared_ptr<rclcpp::Node> node;
|
||||
std::shared_ptr<AnimationActionServer> animationServer;
|
||||
std::shared_ptr<MovementActionServer> movementServer;
|
||||
ActorSystemState currentState,lastState;
|
||||
action::Animation::Goal animationTarget;
|
||||
action::Movement::Goal movementTarget;
|
||||
AnimationServerGoalPtr currentAnimationGoalPtr;
|
||||
MovementServerGoalPtr currentMovementGoalPtr;
|
||||
double duration{};
|
||||
Entity entity{kNullEntity};
|
||||
std::mutex threadMutex;
|
||||
private:
|
||||
std::shared_ptr<rclcpp::Node> node;
|
||||
std::shared_ptr<AnimationActionServer > animationServer;
|
||||
std::shared_ptr<MovementActionServer > movementServer;
|
||||
ActorSystemState currentState, lastState;
|
||||
action::Animation::Goal animationTarget;
|
||||
action::Movement::Goal movementTarget;
|
||||
AnimationServerGoalPtr currentAnimationGoalPtr;
|
||||
MovementServerGoalPtr currentMovementGoalPtr;
|
||||
double duration{};
|
||||
Entity entity{kNullEntity};
|
||||
std::mutex threadMutex;
|
||||
std::shared_ptr<common::Skeleton> currentSkeleton;
|
||||
common::NodeAnimation* currentRootNodeAnimation;
|
||||
math::Matrix4<double> currentRotationAlign, currentTranslationAlign;
|
||||
|
||||
public:
|
||||
ActorSystem();
|
||||
public:
|
||||
ActorSystem();
|
||||
|
||||
public:
|
||||
~ActorSystem() override;
|
||||
public:
|
||||
~ActorSystem() override;
|
||||
|
||||
public:
|
||||
void PreUpdate(const UpdateInfo &_info,
|
||||
EntityComponentManager &_ecm) override;
|
||||
public:
|
||||
void PreUpdate(const UpdateInfo &_info,
|
||||
EntityComponentManager &_ecm) override;
|
||||
|
||||
public:
|
||||
void Configure(const ignition::gazebo::Entity &_entity,
|
||||
const std::shared_ptr<const sdf::Element> &_sdf,
|
||||
EntityComponentManager &_ecm,
|
||||
EventManager &/*_eventMgr*/) override;
|
||||
public:
|
||||
void Configure(const ignition::gazebo::Entity &animationGoalPtr,
|
||||
const std::shared_ptr<const sdf::Element> &_sdf,
|
||||
EntityComponentManager &_ecm,
|
||||
EventManager &/*_eventMgr*/) override;
|
||||
|
||||
private:
|
||||
void switchAnimation(EntityComponentManager &_ecm, std::string& animationName);
|
||||
private:
|
||||
void switchAnimation(EntityComponentManager &_ecm, std::string &animationName);
|
||||
|
||||
|
||||
};
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user