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

Added full shmem implementation, working without feedback.

Browse Source
This commit is contained in:
Bastian Hofmann 2023-02-02 06:18:24 +00:00
parent fc7a74a8e8
commit 16e33e647d
2 changed files with 140 additions and 82 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -36,7 +36,7 @@ ActorSystem::ActorSystem() = default;
ActorSystem::~ActorSystem() = default; ActorSystem::~ActorSystem() = default;
void ActorSystem::switchAnimation(EntityComponentManager &_ecm, std::string& animationName){ void ActorSystem::switchAnimation(EntityComponentManager &_ecm, char* animationName){
igndbg << "Initial setup for new animation." << std::endl; igndbg << "Initial setup for new animation." << std::endl;
auto actorComp = _ecm.Component<components::Actor>(entity); auto actorComp = _ecm.Component<components::Actor>(entity);
@ -83,8 +83,6 @@ void ActorSystem::switchAnimation(EntityComponentManager &_ecm, std::string& ani
ignerr << "No skeleton found!" << std::endl; ignerr << "No skeleton found!" << std::endl;
} }
auto initialPose = currentRootNodeAnimation -> FrameAt(0).Pose(); auto initialPose = currentRootNodeAnimation -> FrameAt(0).Pose();
auto poseComponent = _ecm.Component<components::Pose>(entity); auto poseComponent = _ecm.Component<components::Pose>(entity);
@ -99,8 +97,8 @@ void ActorSystem::switchAnimation(EntityComponentManager &_ecm, std::string& ani
} }
void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ecm) { void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ecm) {
threadMutex.lock(); pthread_mutex_lock(&shared_mmap->mutex);
switch(this->currentState){ switch(shared_mmap->currentState){
case SETUP: { case SETUP: {
igndbg << "Setting up actor plugin" << std::endl; igndbg << "Setting up actor plugin" << std::endl;
@ -126,25 +124,25 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
_ecm.CreateComponent(entity, components::TrajectoryPose(ignition::math::Pose3d::Zero)); _ecm.CreateComponent(entity, components::TrajectoryPose(ignition::math::Pose3d::Zero));
} }
currentState = ActorSystemState::IDLE; shared_mmap->currentState = ActorSystemState::IDLE;
break; break;
} }
case ANIMATION: { case ANIMATION: {
if (lastState == ActorSystemState::IDLE){ if (shared_mmap->lastState == ActorSystemState::IDLE){
switchAnimation(_ecm, animationTarget.animation_name); switchAnimation(_ecm, shared_mmap->animation_name);
} }
auto animationTimeComponent = _ecm.Component<components::AnimationTime>(this->entity); auto animationTimeComponent = _ecm.Component<components::AnimationTime>(this->entity);
auto newTime = animationTimeComponent->Data() + _info.dt; auto newTime = animationTimeComponent->Data() + _info.dt;
auto newTimeSeconds = std::chrono::duration<double>(newTime).count(); auto newTimeSeconds = std::chrono::duration<double>(newTime).count();
auto feedback = std::make_shared<action::Animation::Feedback>();
auto feedback = (float) (newTimeSeconds/duration);
feedback->set__progress((float) (newTimeSeconds/duration)); write(feedback_pipe[1],&feedback,sizeof(float));
currentAnimationGoalPtr->publish_feedback(feedback); //currentAnimationGoalPtr->publish_feedback(feedback);
if (newTimeSeconds >= duration){ if (newTimeSeconds >= duration){
currentAnimationGoalPtr->succeed(std::make_shared<action::Animation::Result>()); //currentAnimationGoalPtr->succeed(std::make_shared<action::Animation::Result>());
igndbg << "Animation " << animationTarget.animation_name << " finished." << std::endl; igndbg << "Animation " << shared_mmap->animation_name << " finished." << std::endl;
currentState = ActorSystemState::IDLE; shared_mmap->currentState = ActorSystemState::IDLE;
break; break;
} }
@ -172,20 +170,22 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
break; break;
} }
case MOVEMENT:{ case MOVEMENT:{
if (lastState == ActorSystemState::IDLE){ if (shared_mmap->lastState == ActorSystemState::IDLE){
igndbg << "Starting Movement..." << std::endl; igndbg << "Starting Movement..." << std::endl;
switchAnimation(_ecm, movementTarget.animation_name); switchAnimation(_ecm, shared_mmap->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 targetPose = movementTarget.target_position; auto targetPose = shared_mmap->target_position;
auto targetDirection = math::Vector3d(targetPose[0],targetPose[1],targetPose[2]) - actorPose.Pos(); auto targetDirection = math::Vector3d(targetPose[0],targetPose[1],targetPose[2]) - actorPose.Pos();
if (targetDirection.Length()<0.05){ if (targetDirection.Length()<0.05){
currentMovementGoalPtr->succeed(std::make_shared<action::Movement::Result>()); float feedback = 1.0f;
currentState = ActorSystemState::IDLE; write(feedback_pipe[1],&feedback,sizeof(float));
//currentMovementGoalPtr->succeed(std::make_shared<action::Movement::Result>());
shared_mmap->currentState = ActorSystemState::IDLE;
break; break;
} }
@ -220,7 +220,7 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
break; break;
} }
double distance = (deltaTimeSeconds/duration)*movementTarget.animation_distance; double distance = (deltaTimeSeconds/duration)*shared_mmap->animation_distance;
actorPose.Pos() += targetDirection.Normalize() * distance; actorPose.Pos() += targetDirection.Normalize() * distance;
@ -234,58 +234,56 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
break; break;
} }
case IDLE: case IDLE:
if (lastState != currentState){ if (shared_mmap->lastState != shared_mmap->currentState){
igndbg << "Now idling..." << std::endl; igndbg << "Now idling..." << std::endl;
} }
break; break;
} }
lastState = currentState; shared_mmap->lastState = shared_mmap->currentState;
threadMutex.unlock(); pthread_mutex_unlock(&shared_mmap->mutex);
} }
struct shared_mmap
{
pthread_mutex_t mutex;
ActorSystemState currentState,lastState;
};
void client(std::string topic,shared_mmap *sharedMmap){ void ActorSystem::client(const std::string& topic){
rclcpp::init(0, {}); rclcpp::init(0, {});
auto node = rclcpp::Node::make_shared("moveService", topic); auto node = rclcpp::Node::make_shared("moveService", topic);
AnimationServerGoalPtr currentAnimationGoalPtr;
MovementServerGoalPtr currentMovementGoalPtr;
#pragma clang diagnostic push #pragma clang diagnostic push
#pragma ide diagnostic ignored "UnusedValue" #pragma ide diagnostic ignored "UnusedValue"
auto animationServer = rclcpp_action::create_server<action::Animation>( auto animationServer = rclcpp_action::create_server<action::Animation>(
node, node,
"animation", "animation",
[sharedMmap](const rclcpp_action::GoalUUID goalUuid, const AnimationGoalPtr &animationGoal) { [this](const rclcpp_action::GoalUUID goalUuid, const AnimationGoalPtr &animationGoal) {
if (sharedMmap->currentState == ActorSystemState::IDLE) { if (shared_mmap->currentState == ActorSystemState::IDLE) {
pthread_mutex_lock(&sharedMmap->mutex); pthread_mutex_lock(&shared_mmap->mutex);
//animationTarget = *animationGoal; //animationTarget = *animationGoal;
pthread_mutex_unlock(&sharedMmap->mutex); strcpy(shared_mmap->animation_name,animationGoal->animation_name.data());
pthread_mutex_unlock(&shared_mmap->mutex);
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}else{ }else{
return rclcpp_action::GoalResponse::REJECT; return rclcpp_action::GoalResponse::REJECT;
} }
}, },
[sharedMmap](const AnimationServerGoalPtr& animationGoalPtr) { [this,&currentAnimationGoalPtr](const AnimationServerGoalPtr& animationGoalPtr) {
if (sharedMmap->currentState == ActorSystemState::IDLE) { currentAnimationGoalPtr = nullptr;
if (shared_mmap->currentState == ActorSystemState::IDLE) {
return rclcpp_action::CancelResponse::REJECT; return rclcpp_action::CancelResponse::REJECT;
}else{ }else{
pthread_mutex_lock(&sharedMmap->mutex); pthread_mutex_lock(&shared_mmap->mutex);
sharedMmap->currentState = ActorSystemState::IDLE; shared_mmap->currentState = ActorSystemState::IDLE;
pthread_mutex_unlock(&sharedMmap->mutex); pthread_mutex_unlock(&shared_mmap->mutex);
return rclcpp_action::CancelResponse::ACCEPT; return rclcpp_action::CancelResponse::ACCEPT;
} }
}, },
[sharedMmap](const AnimationServerGoalPtr& animationGoalPtr) { [this,&currentAnimationGoalPtr](const AnimationServerGoalPtr& animationGoalPtr) {
pthread_mutex_lock(&sharedMmap->mutex); pthread_mutex_lock(&shared_mmap->mutex);
//currentAnimationGoalPtr = animationGoalPtr; currentAnimationGoalPtr = animationGoalPtr;
sharedMmap->currentState = ActorSystemState::ANIMATION; shared_mmap->currentState = ActorSystemState::ANIMATION;
pthread_mutex_unlock(&sharedMmap->mutex); pthread_mutex_unlock(&shared_mmap->mutex);
} }
); );
@ -293,36 +291,78 @@ void client(std::string topic,shared_mmap *sharedMmap){
auto movementServer = rclcpp_action::create_server<action::Movement>( auto movementServer = rclcpp_action::create_server<action::Movement>(
node, node,
"movement", "movement",
[sharedMmap](const rclcpp_action::GoalUUID goalUuid, const MovementGoalPtr &movementGoal ){ [this](const rclcpp_action::GoalUUID goalUuid, const MovementGoalPtr &movementGoal ){
if (sharedMmap->currentState == ActorSystemState::IDLE) { if (shared_mmap->currentState == ActorSystemState::IDLE) {
pthread_mutex_lock(&sharedMmap->mutex); pthread_mutex_lock(&shared_mmap->mutex);
//movementTarget = *movementGoal; //movementTarget = *movementGoal;
pthread_mutex_unlock(&sharedMmap->mutex); shared_mmap->target_position = math::Vector3d(
movementGoal->target_position[0],
movementGoal->target_position[1],
movementGoal->target_position[2]);
shared_mmap->animation_distance = movementGoal->animation_distance;
strcpy(shared_mmap->animation_name,movementGoal->animation_name.data());
pthread_mutex_unlock(&shared_mmap->mutex);
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}else{ }else{
return rclcpp_action::GoalResponse::REJECT; return rclcpp_action::GoalResponse::REJECT;
} }
}, },
[sharedMmap](const MovementServerGoalPtr& movementGoalPtr ){ [this,&currentMovementGoalPtr](const MovementServerGoalPtr& movementGoalPtr ){
if (sharedMmap->currentState == ActorSystemState::IDLE) { currentMovementGoalPtr = nullptr;
if (shared_mmap->currentState == ActorSystemState::IDLE) {
return rclcpp_action::CancelResponse::REJECT; return rclcpp_action::CancelResponse::REJECT;
}else{ }else{
pthread_mutex_lock(&sharedMmap->mutex); pthread_mutex_lock(&shared_mmap->mutex);
sharedMmap->currentState = ActorSystemState::IDLE; shared_mmap->currentState = ActorSystemState::IDLE;
pthread_mutex_unlock(&sharedMmap->mutex); pthread_mutex_unlock(&shared_mmap->mutex);
return rclcpp_action::CancelResponse::ACCEPT; return rclcpp_action::CancelResponse::ACCEPT;
} }
}, },
[sharedMmap](const MovementServerGoalPtr& movementGoalPtr ){ [this,&currentMovementGoalPtr](const MovementServerGoalPtr& movementGoalPtr ){
pthread_mutex_lock(&sharedMmap->mutex); pthread_mutex_lock(&shared_mmap->mutex);
//currentMovementGoalPtr = movementGoalPtr; currentMovementGoalPtr = movementGoalPtr;
sharedMmap->currentState = ActorSystemState::MOVEMENT; shared_mmap->currentState = ActorSystemState::MOVEMENT;
pthread_mutex_unlock(&sharedMmap->mutex); pthread_mutex_unlock(&shared_mmap->mutex);
} }
); );
#pragma clang diagnostic pop #pragma clang diagnostic pop
std::thread t([this,&currentAnimationGoalPtr,&currentMovementGoalPtr]() {
float progress;
while(true) {
read(feedback_pipe[0], &progress, sizeof(float));
pthread_mutex_lock(&shared_mmap->mutex);
if(currentAnimationGoalPtr!=nullptr){
auto feedback = std::make_shared<action::Animation::Feedback>();
feedback->set__progress(progress);
currentAnimationGoalPtr->publish_feedback(feedback);
}
if(currentMovementGoalPtr!=nullptr){
auto feedback = std::make_shared<action::Movement::Feedback>();
feedback->set__progress(progress);
currentMovementGoalPtr->publish_feedback(feedback);
}
if(progress == 1.0){
if(currentAnimationGoalPtr!=nullptr){
currentAnimationGoalPtr->succeed(std::make_shared<action::Animation::Result>());
}
if(currentMovementGoalPtr!=nullptr){
currentMovementGoalPtr->succeed(std::make_shared<action::Movement::Result>());
}
currentMovementGoalPtr = nullptr;
currentAnimationGoalPtr = nullptr;
}
pthread_mutex_unlock(&shared_mmap->mutex);
}
});
t.detach();
igndbg << "Spinning node..." << std::endl; igndbg << "Spinning node..." << std::endl;
while (true) { while (true) {
printf("Spinning...\n"); printf("Spinning...\n");
@ -341,21 +381,25 @@ void ActorSystem::Configure(const Entity &_entity, const std::shared_ptr<const s
ignerr << "Plugin not attached to actor, terminating." << std::endl; ignerr << "Plugin not attached to actor, terminating." << std::endl;
return; return;
} }
std::string topic = "ActorPlugin";
if (_sdf->HasElement("topic")) {
topic = _sdf->Get<std::string>("topic");
}
this->entity = _entity; this->entity = _entity;
this->currentState = ActorSystemState::SETUP;
this->lastState = ActorSystemState::IDLE;
igndbg << "shmem setup" << std::endl;
int fd = open(".", O_TMPFILE|O_RDWR, 00600); int fd = open(".", O_TMPFILE|O_RDWR, 00600);
auto structSize = sizeof(struct shared_mmap); auto structSize = sizeof(struct sharedMmap);
if(ftruncate(fd, structSize)!=0){ if(ftruncate(fd, structSize)!=0){
ignerr << "ftruncate" << std::endl; ignerr << "ftruncate" << std::endl;
return; return;
} }
auto *sharedMmap = (struct shared_mmap *) mmap(nullptr,structSize,PROT_READ|PROT_WRITE,MAP_SHARED,fd,0); shared_mmap = (struct sharedMmap *) mmap(nullptr,structSize,PROT_READ|PROT_WRITE,MAP_SHARED,fd,0);
//close(fd); //close(fd);
if (sharedMmap == MAP_FAILED){ if (shared_mmap == MAP_FAILED){
ignerr << "MAP_FAILED" << std::endl; ignerr << "MAP_FAILED" << std::endl;
return; return;
} }
@ -369,14 +413,19 @@ void ActorSystem::Configure(const Entity &_entity, const std::shared_ptr<const s
ignerr << "pthread_mutexattr_setpshared" << std::endl; ignerr << "pthread_mutexattr_setpshared" << std::endl;
return; return;
} }
if(pthread_mutex_init(&sharedMmap->mutex, &mutexattr)!=0){ if(pthread_mutex_init(&shared_mmap->mutex, &mutexattr)!=0){
ignerr << "pthread_mutex_init" << std::endl; ignerr << "pthread_mutex_init" << std::endl;
return; return;
} }
std::string topic = "ActorPlugin"; shared_mmap->currentState = ActorSystemState::SETUP;
if (_sdf->HasElement("topic")) { shared_mmap->lastState = ActorSystemState::IDLE;
topic = _sdf->Get<std::string>("topic");
igndbg << "pipe setup" << std::endl;
if (pipe(feedback_pipe) < 0){
igndbg << "pipe failed." << std::endl;
return;
} }
switch(fork()){ switch(fork()){
@ -385,12 +434,10 @@ void ActorSystem::Configure(const Entity &_entity, const std::shared_ptr<const s
break; break;
case 0: case 0:
ignmsg << "Child fork." << std::endl; ignmsg << "Child fork." << std::endl;
client(topic,sharedMmap); client(topic);
break; break;
default: default:
ignmsg << "Parent fork." << std::endl; ignmsg << "Parent fork." << std::endl;
break; break;
} }
} }

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

@ -39,26 +39,36 @@ namespace ignition {
SETUP, IDLE, MOVEMENT, ANIMATION SETUP, IDLE, MOVEMENT, ANIMATION
}; };
struct sharedMmap{
pthread_mutex_t mutex;
ActorSystemState currentState,lastState;
char animation_name[256];
double animation_distance;
math::Vector3d target_position;
};
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;
MovementServerGoalPtr currentMovementGoalPtr; //MovementServerGoalPtr currentMovementGoalPtr;
double duration{}; double duration{};
Entity entity{kNullEntity}; Entity entity{kNullEntity};
std::mutex threadMutex; //std::mutex threadMutex;
std::shared_ptr<common::Skeleton> currentSkeleton; std::shared_ptr<common::Skeleton> currentSkeleton;
common::NodeAnimation* currentRootNodeAnimation; common::NodeAnimation* currentRootNodeAnimation;
math::Matrix4<double> currentRotationAlign, currentTranslationAlign; math::Matrix4<double> currentRotationAlign, currentTranslationAlign;
struct sharedMmap* shared_mmap;
int feedback_pipe[2];
public: public:
ActorSystem(); ActorSystem();
@ -77,7 +87,8 @@ namespace ignition {
EventManager &/*_eventMgr*/) override; EventManager &/*_eventMgr*/) override;
private: private:
void switchAnimation(EntityComponentManager &_ecm, std::string &animationName); void switchAnimation(EntityComponentManager &_ecm, char *animationName);
void client(const std::string& topic);
}; };