yenon/ROS_Workspace
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Connected and added movement part of Plugin.

Browse Source
This commit is contained in:
Bastian Hofmann 2022-12-21 04:58:51 +00:00
parent 38263dd13f
commit f9f1766004
3 changed files with 189 additions and 94 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

194
src/ign_actor_plugin/src/ActorSystem.cpp
View File

@ -48,29 +48,50 @@ void ActorSystem::switchAnimation(EntityComponentManager &_ecm, std::string& ani
*animationTimeComponent = components::AnimationTime(std::chrono::milliseconds::zero()); *animationTimeComponent = components::AnimationTime(std::chrono::milliseconds::zero());
_ecm.SetChanged(entity, components::AnimationTime::typeId, ComponentState::OneTimeChange); _ecm.SetChanged(entity, components::AnimationTime::typeId, ComponentState::OneTimeChange);
int foundIndex = -1; int foundAnimationIndex = -1;
for (int animationIndex = 0 ; animationIndex < actorComp->Data().AnimationCount() ; animationIndex++) { for (int animationIndex = 0 ; animationIndex < actorComp->Data().AnimationCount() ; animationIndex++) {
auto animation = actorComp->Data().AnimationByIndex(animationIndex); auto animation = actorComp->Data().AnimationByIndex(animationIndex);
if (animation->Name() == animationName){ if (animation->Name() == animationName){
foundIndex = animationIndex; foundAnimationIndex = animationIndex;
break; break;
} }
} }
if(foundIndex!=-1){ if(foundAnimationIndex != -1){
igndbg << "Found animation to play" << std::endl; igndbg << "Found animation to play" << std::endl;
auto fileName = actorComp->Data().AnimationByIndex(foundIndex)->Filename(); auto fileName = actorComp->Data().AnimationByIndex(foundAnimationIndex)->Filename();
igndbg << "Path: " << fileName << std::endl;
igndbg << "Name: " << common::MeshManager::Instance()->Load(fileName)->Name() << std::endl;
auto skeleton = common::MeshManager::Instance()->Load(fileName)->MeshSkeleton(); currentSkeleton = common::MeshManager::Instance()->Load(fileName)->MeshSkeleton();
if (skeleton != nullptr){ igndbg << "FileName: " << fileName << std::endl;
duration = skeleton->Animation(0)->Length(); 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; igndbg << "length: " << duration << " seconds." << std::endl;
} else { } else {
ignerr << "No skeleton found!" << std::endl; 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 { } else {
ignerr << "No animation found!" << std::endl; ignerr << "No animation found!" << std::endl;
} }
@ -82,40 +103,26 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
case SETUP: { case SETUP: {
igndbg << "Setting up actor plugin" << std::endl; 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); auto animationNameComponent = _ecm.Component<components::AnimationName>(entity);
if (nullptr == animationNameComp) { if (nullptr == animationNameComponent) {
_ecm.CreateComponent(entity, components::AnimationName(actorComp->Data().AnimationByIndex(0)->Name())); _ecm.CreateComponent(entity, components::AnimationName(actorComponent->Data().AnimationByIndex(0)->Name()));
} }
auto animTimeComp = _ecm.Component<components::AnimationTime>(entity); auto animTimeComponent = _ecm.Component<components::AnimationTime>(entity);
if (nullptr == animTimeComp) { if (nullptr == animTimeComponent) {
_ecm.CreateComponent(entity, components::AnimationTime()); _ecm.CreateComponent(entity, components::AnimationTime());
} }
ignition::math::Pose3d initialPose; auto poseComponent = _ecm.Component<components::Pose>(entity);
auto poseComp = _ecm.Component<components::Pose>(entity); if (nullptr == poseComponent) {
if (nullptr == poseComp) {
_ecm.CreateComponent(entity, components::Pose(ignition::math::Pose3d::Zero)); _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 trajectoryPoseComponent = _ecm.Component<components::TrajectoryPose>(entity);
if (nullptr == trajectoryPoseComponent) {
auto trajectoryPoseComp = _ecm.Component<components::TrajectoryPose>(entity); _ecm.CreateComponent(entity, components::TrajectoryPose(ignition::math::Pose3d::Zero));
if (nullptr == trajectoryPoseComp) {
// Leave Z to the pose component, control only 2D with Trajectory
//initialPose.Pos().Z(0);
_ecm.CreateComponent(entity, components::TrajectoryPose(initialPose));
} }
currentState = ActorSystemState::IDLE; currentState = ActorSystemState::IDLE;
@ -143,34 +150,113 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
*animationTimeComponent = components::AnimationTime(newTime); *animationTimeComponent = components::AnimationTime(newTime);
_ecm.SetChanged(entity, components::AnimationTime::typeId, ComponentState::OneTimeChange); _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; break;
} }
case MOVEMENT:{ case MOVEMENT:{
if (lastState == ActorSystemState::IDLE){ if (lastState == ActorSystemState::IDLE){
igndbg << "Starting Movement..." << std::endl;
switchAnimation(_ecm, movementTarget.animation_name); switchAnimation(_ecm, movementTarget.animation_name);
} }
auto trajectoryPoseComp = _ecm.Component<components::TrajectoryPose>(this->entity); auto trajectoryPoseComp = _ecm.Component<components::TrajectoryPose>(this->entity);
auto actorPose = trajectoryPoseComp->Data(); auto actorPose = trajectoryPoseComp->Data();
auto initialPose = actorPose;
auto targetPose = movementTarget.target_position; 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>( if (targetDirection.Length()<0.05){
this->entity); currentMovementGoalPtr->succeed(std::make_shared<action::Movement::Result>());
currentState = ActorSystemState::IDLE;
break;
}
auto animTime = animTimeComp->Data() + auto animationTimeComponent = _ecm.Component<components::AnimationTime>(this->entity);
std::chrono::duration_cast<std::chrono::steady_clock::duration>( auto oldTimeSeconds = std::chrono::duration<double>(animationTimeComponent->Data()).count();
std::chrono::duration<double>( auto deltaTimeSeconds = std::chrono::duration<double>(_info.dt).count();
distanceTraveled * auto newTimeSeconds = oldTimeSeconds + deltaTimeSeconds;
1 auto newTime = animationTimeComponent->Data() + _info.dt;
)
); if(newTimeSeconds >= duration){
*animTimeComp = components::AnimationTime(animTime); 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; break;
} }
case IDLE: case IDLE:
if (lastState != currentState){
igndbg << "Now idling..." << std::endl;
}
break; break;
} }
lastState = currentState; 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); node = rclcpp::Node::make_shared("moveService", topic);
#pragma clang diagnostic push
#pragma ide diagnostic ignored "UnusedValue"
animationServer = rclcpp_action::create_server<action::Animation>( animationServer = rclcpp_action::create_server<action::Animation>(
node, node,
"animation", "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) { if (this->currentState == ActorSystemState::IDLE) {
return rclcpp_action::CancelResponse::REJECT; return rclcpp_action::CancelResponse::REJECT;
}else{ }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(); threadMutex.lock();
currentAnimationGoalPtr = PH1; currentAnimationGoalPtr = animationGoalPtr;
currentState = ActorSystemState::ANIMATION; currentState = ActorSystemState::ANIMATION;
threadMutex.unlock(); threadMutex.unlock();
} }
); );
movementServer = rclcpp_action::create_server<action::Movement>( movementServer = rclcpp_action::create_server<action::Movement>(
node, node,
"movement", "movement",
@ -249,7 +338,7 @@ void ActorSystem::Configure(const Entity &_entity, const std::shared_ptr<const s
return rclcpp_action::GoalResponse::REJECT; return rclcpp_action::GoalResponse::REJECT;
} }
}, },
[this](const MovementServerGoalPtr goalUuid ){ [this](const MovementServerGoalPtr& movementGoalPtr ){
if (this->currentState == ActorSystemState::IDLE) { if (this->currentState == ActorSystemState::IDLE) {
return rclcpp_action::CancelResponse::REJECT; return rclcpp_action::CancelResponse::REJECT;
}else{ }else{
@ -259,13 +348,14 @@ void ActorSystem::Configure(const Entity &_entity, const std::shared_ptr<const s
return rclcpp_action::CancelResponse::ACCEPT; return rclcpp_action::CancelResponse::ACCEPT;
} }
}, },
[this](const MovementServerGoalPtr goalUuid ){ [this](const MovementServerGoalPtr& movementGoalPtr ){
threadMutex.lock(); threadMutex.lock();
currentMovementGoalPtr = goalUuid; currentMovementGoalPtr = movementGoalPtr;
currentState = ActorSystemState::MOVEMENT; currentState = ActorSystemState::MOVEMENT;
threadMutex.unlock(); threadMutex.unlock();
} }
); );
#pragma clang diagnostic pop
igndbg << "Spinning node..." << std::endl; igndbg << "Spinning node..." << std::endl;
std::thread spinThread([this]() { std::thread spinThread([this]() {

21
src/ign_actor_plugin/src/ActorSystem.hpp
View File

@ -18,6 +18,8 @@
#include <ign_actor_plugin_msgs/action/movement.hpp> #include <ign_actor_plugin_msgs/action/movement.hpp>
#include <rclcpp_action/server_goal_handle.hpp> #include <rclcpp_action/server_goal_handle.hpp>
#include <string> #include <string>
#include <ignition/common/Skeleton.hh>
#include <ignition/common/NodeAnimation.hh>
using namespace ign_actor_plugin_msgs; using namespace ign_actor_plugin_msgs;
using rclcpp_action::ServerGoalHandle; using rclcpp_action::ServerGoalHandle;
@ -33,20 +35,20 @@ using rclcpp_action::ServerGoalHandle;
namespace ignition { namespace ignition {
namespace gazebo { namespace gazebo {
enum ActorSystemState{ enum ActorSystemState {
SETUP,IDLE,MOVEMENT,ANIMATION SETUP, IDLE, MOVEMENT, ANIMATION
}; };
class ActorSystem: class ActorSystem :
public System, public System,
public ISystemPreUpdate, public ISystemPreUpdate,
public ISystemConfigure { public ISystemConfigure {
private: private:
std::shared_ptr<rclcpp::Node> node; std::shared_ptr<rclcpp::Node> node;
std::shared_ptr<AnimationActionServer> animationServer; std::shared_ptr<AnimationActionServer > animationServer;
std::shared_ptr<MovementActionServer> movementServer; std::shared_ptr<MovementActionServer > movementServer;
ActorSystemState currentState,lastState; ActorSystemState currentState, lastState;
action::Animation::Goal animationTarget; action::Animation::Goal animationTarget;
action::Movement::Goal movementTarget; action::Movement::Goal movementTarget;
AnimationServerGoalPtr currentAnimationGoalPtr; AnimationServerGoalPtr currentAnimationGoalPtr;
@ -54,6 +56,9 @@ namespace ignition {
double duration{}; double duration{};
Entity entity{kNullEntity}; Entity entity{kNullEntity};
std::mutex threadMutex; std::mutex threadMutex;
std::shared_ptr<common::Skeleton> currentSkeleton;
common::NodeAnimation* currentRootNodeAnimation;
math::Matrix4<double> currentRotationAlign, currentTranslationAlign;
public: public:
ActorSystem(); ActorSystem();
@ -66,13 +71,13 @@ namespace ignition {
EntityComponentManager &_ecm) override; EntityComponentManager &_ecm) override;
public: public:
void Configure(const ignition::gazebo::Entity &_entity, void Configure(const ignition::gazebo::Entity &animationGoalPtr,
const std::shared_ptr<const sdf::Element> &_sdf, const std::shared_ptr<const sdf::Element> &_sdf,
EntityComponentManager &_ecm, EntityComponentManager &_ecm,
EventManager &/*_eventMgr*/) override; EventManager &/*_eventMgr*/) override;
private: private:
void switchAnimation(EntityComponentManager &_ecm, std::string& animationName); void switchAnimation(EntityComponentManager &_ecm, std::string &animationName);
}; };