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

Many changes to actor system.

Browse Source 
Changed Actions to fit their integrated functionality.
This commit is contained in:
Bastian Hofmann
2022-12-20 15:34:08 +00:00
parent 9ecac8db13
commit 38263dd13f
4 changed files with 211 additions and 132 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -2,146 +2,102 @@
// Created by bastian on 31.08.22.
//
#include <chrono>
#include <cstdio>
#include <ign_actor_plugin_msgs/action/detail/animation__struct.hpp>
#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>
#include <ignition/common/MeshManager.hh>
#include <ignition/common/Skeleton.hh>
#include <ignition/common/SkeletonAnimation.hh>
#include <ignition/common/Mesh.hh>
IGNITION_ADD_PLUGIN(
ignition::gazebo::ActorSystem,
ignition::gazebo::System,
ignition::gazebo::ActorSystem::ISystemPreUpdate,
ignition::gazebo::ActorSystem::ISystemConfigure);
using namespace ignition;
using namespace gazebo;
ignition::gazebo::ActorSystem::ISystemConfigure)
using namespace ignition::gazebo;
using namespace ign_actor_plugin_msgs;
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);
void ActorSystem::switchAnimation(EntityComponentManager &_ecm, std::string& animationName){
igndbg << "Initial setup for new animation." << std::endl;
auto newTime = animationTimeComponent->Data() + _info.dt;
auto actorComp = _ecm.Component<components::Actor>(entity);
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;
this->entitiy = _entity;
this->actorState = ActorSystemState::ANIMATION;
auto actorComp = _ecm.Component<components::Actor>(_entity);
if (!actorComp) {
ignerr << "No attached actor found, terminating." << 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 {
auto animationNameComp = _ecm.Component<components::AnimationName>(entity);
*animationNameComp = components::AnimationName(animationName);
_ecm.SetChanged(entity, components::AnimationName::typeId, ComponentState::OneTimeChange);
auto animationTimeComponent = _ecm.Component<components::AnimationTime>(entity);
*animationTimeComponent = components::AnimationTime(std::chrono::milliseconds::zero());
_ecm.SetChanged(entity, components::AnimationTime::typeId, ComponentState::OneTimeChange);
int foundIndex = -1;
for (int animationIndex = 0 ; animationIndex < actorComp->Data().AnimationCount() ; animationIndex++) {
auto animation = actorComp->Data().AnimationByIndex(animationIndex);
if (animation->Name() == animationName){
foundIndex = animationIndex;
break;
}
}
if(foundIndex!=-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;
for (const auto& elem: _ecm.ComponentTypes(_entity)) {
igndbg << "Component: " << elem << std::endl;
auto skeleton = common::MeshManager::Instance()->Load(fileName)->MeshSkeleton();
if (skeleton != nullptr){
duration = skeleton->Animation(0)->Length();
igndbg << "length: " << duration << " seconds." << std::endl;
} else {
ignerr << "No skeleton found!" << std::endl;
}
} else {
ignerr << "No animation found!" << std::endl;
}
}
_ecm.SetChanged(_entity, components::AnimationName::typeId, ComponentState::OneTimeChange);
void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ecm) {
threadMutex.lock();
switch(this->currentState){
case SETUP: {
igndbg << "Setting up actor plugin" << std::endl;
auto animTimeComp = _ecm.Component<components::AnimationTime>(_entity);
auto actorComp = _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 animTimeComp = _ecm.Component<components::AnimationTime>(entity);
if (nullptr == animTimeComp) {
_ecm.CreateComponent(_entity, components::AnimationTime());
_ecm.CreateComponent(entity, components::AnimationTime());
}
ignition::math::Pose3d initialPose;
auto poseComp = _ecm.Component<components::Pose>(_entity);
auto poseComp = _ecm.Component<components::Pose>(entity);
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();
@@ -154,59 +110,167 @@ void ActorSystem::Configure(const Entity &_entity, const std::shared_ptr<const s
}
auto trajPoseComp = _ecm.Component<components::TrajectoryPose>(_entity);
if (nullptr == trajPoseComp) {
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));
//initialPose.Pos().Z(0);
_ecm.CreateComponent(entity, components::TrajectoryPose(initialPose));
}
currentState = ActorSystemState::IDLE;
break;
}
case ANIMATION: {
if (lastState == ActorSystemState::IDLE){
switchAnimation(_ecm, animationTarget.animation_name);
}
auto animationTimeComponent = _ecm.Component<components::AnimationTime>(this->entity);
auto newTime = animationTimeComponent->Data() + _info.dt;
auto newTimeSeconds = std::chrono::duration<double>(newTime).count();
auto feedback = std::make_shared<action::Animation::Feedback>();
feedback->set__progress((float) (newTimeSeconds/duration));
currentAnimationGoalPtr->publish_feedback(feedback);
if (newTimeSeconds >= duration){
currentAnimationGoalPtr->succeed(std::make_shared<action::Animation::Result>());
igndbg << "Animation " << animationTarget.animation_name << " finished." << std::endl;
currentState = ActorSystemState::IDLE;
break;
}
*animationTimeComponent = components::AnimationTime(newTime);
_ecm.SetChanged(entity, components::AnimationTime::typeId, ComponentState::OneTimeChange);
break;
}
case MOVEMENT:{
if (lastState == ActorSystemState::IDLE){
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 animTimeComp = _ecm.Component<components::AnimationTime>(
this->entity);
auto animTime = animTimeComp->Data() +
std::chrono::duration_cast<std::chrono::steady_clock::duration>(
std::chrono::duration<double>(
distanceTraveled *
1
)
);
*animTimeComp = components::AnimationTime(animTime);
break;
}
case IDLE:
break;
}
lastState = currentState;
threadMutex.unlock();
}
// 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 << "actor plugin configuring..." << std::endl;
auto actorComp = _ecm.Component<components::Actor>(_entity);
if (nullptr == actorComp) {
ignerr << "Plugin not attached to actor, terminating." << std::endl;
return;
}
this->entity = _entity;
this->currentState = ActorSystemState::SETUP;
this->lastState = ActorSystemState::IDLE;
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);
animationServer = rclcpp_action::create_server<ign_actor_plugin_msgs::action::Animation>(
animationServer = rclcpp_action::create_server<action::Animation>(
node,
"animation",
[](rclcpp_action::GoalUUID goalUuid,
AnimationGoalPtr &animationGoal) {
[this](const rclcpp_action::GoalUUID goalUuid, const AnimationGoalPtr &animationGoal) {
if (this->currentState == ActorSystemState::IDLE) {
threadMutex.lock();
animationTarget = *animationGoal;
threadMutex.unlock();
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}else{
return rclcpp_action::GoalResponse::REJECT;
},
[](AnimationServerGoalPtr PH1) {
return rclcpp_action::CancelResponse::ACCEPT;
return rclcpp_action::CancelResponse::REJECT;
},
[](AnimationServerGoalPtr PH1) {
}
},
[this](const AnimationServerGoalPtr PH1) {
if (this->currentState == ActorSystemState::IDLE) {
return rclcpp_action::CancelResponse::REJECT;
}else{
threadMutex.lock();
this->currentState = ActorSystemState::IDLE;
threadMutex.unlock();
return rclcpp_action::CancelResponse::ACCEPT;
}
},
[this](const AnimationServerGoalPtr PH1) {
threadMutex.lock();
currentAnimationGoalPtr = PH1;
currentState = ActorSystemState::ANIMATION;
threadMutex.unlock();
}
);
movementServer = rclcpp_action::create_server<ign_actor_plugin_msgs::action::Movement>(
movementServer = rclcpp_action::create_server<action::Movement>(
node,
"movement",
[](rclcpp_action::GoalUUID goalUuid, MovementGoalPtr &movementGoal ){
[this](const rclcpp_action::GoalUUID goalUuid, const MovementGoalPtr &movementGoal ){
if (this->currentState == ActorSystemState::IDLE) {
threadMutex.lock();
movementTarget = *movementGoal;
threadMutex.unlock();
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}else{
return rclcpp_action::GoalResponse::REJECT;
}
},
[](MovementServerGoalPtr goalUuid ){
[this](const MovementServerGoalPtr goalUuid ){
if (this->currentState == ActorSystemState::IDLE) {
return rclcpp_action::CancelResponse::REJECT;
}else{
threadMutex.lock();
this->currentState = ActorSystemState::IDLE;
threadMutex.unlock();
return rclcpp_action::CancelResponse::ACCEPT;
}
},
[](MovementServerGoalPtr goalUuid ){}
[this](const MovementServerGoalPtr goalUuid ){
threadMutex.lock();
currentMovementGoalPtr = goalUuid;
currentState = ActorSystemState::MOVEMENT;
threadMutex.unlock();
}
);
igndbg << "Spinning node..." << std::endl;
std::thread spinThread([this]() {
while (true) {
igndbg << "Wheee..." << std::endl;
printf("Spinning...\n");
rclcpp::spin(node);
}
});

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

@@ -5,26 +5,36 @@
#ifndef BUILD_ACTORSYSTEM_H
#define BUILD_ACTORSYSTEM_H
#include <cstddef>
#include <ignition/gazebo/Entity.hh>
#include <ignition/gazebo/System.hh>
#include <ignition/plugin/Register.hh>
#include <optional>
#include <queue>
#include <rclcpp/node.hpp>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/server.hpp>
#include <ign_actor_plugin_msgs/action/animation.hpp>
#include <ign_actor_plugin_msgs/action/movement.hpp>
#include <rclcpp_action/server_goal_handle.hpp>
#include <string>
#define AnimationGoalPtr const std::shared_ptr<const ign_actor_plugin_msgs::action::Animation::Goal>
#define AnimationServerGoalPtr const std::shared_ptr<rclcpp_action::ServerGoalHandle<ign_actor_plugin_msgs::action::Animation>>&
using namespace ign_actor_plugin_msgs;
using rclcpp_action::ServerGoalHandle;
#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>>&
#define AnimationActionServer rclcpp_action::Server<action::Animation>
#define MovementActionServer rclcpp_action::Server<action::Movement>
#define AnimationGoalPtr std::shared_ptr<const action::Animation::Goal>
#define AnimationServerGoalPtr std::shared_ptr<ServerGoalHandle<action::Animation>>
#define MovementGoalPtr std::shared_ptr<const action::Movement::Goal>
#define MovementServerGoalPtr std::shared_ptr<ServerGoalHandle<action::Movement>>
namespace ignition {
namespace gazebo {
enum ActorSystemState{
IDLE,MOVEMENT,ANIMATION
SETUP,IDLE,MOVEMENT,ANIMATION
};
class ActorSystem:
@@ -34,12 +44,16 @@ namespace ignition {
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};
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;
public:
ActorSystem();
@@ -57,6 +71,9 @@ namespace ignition {
EntityComponentManager &_ecm,
EventManager &/*_eventMgr*/) override;
private:
void switchAnimation(EntityComponentManager &_ecm, std::string& animationName);
};
}

1
src/ign_actor_plugin_msgs/action/Animation.action
View File

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

1
src/ign_actor_plugin_msgs/action/Movement.action
View File

@@ -3,6 +3,5 @@ float32 animation_speed
float32[3] target_position
float32[3] target_orientation
---
bool success
---
float32 progress