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

First changes to fix multiple rclcpp instances in same process

Browse Source
This commit is contained in:
Bastian Hofmann 2023-01-31 19:37:45 +00:00
parent e0e44b1ffa
commit fc7a74a8e8
2 changed files with 131 additions and 81 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -3,7 +3,6 @@
//
#include <chrono>
#include <cstdio>
#include <ign_actor_plugin_msgs/action/detail/animation__struct.hpp>
#include <ignition/common/Console.hh>
#include <ignition/gazebo/Entity.hh>
@ -19,6 +18,9 @@
#include <ignition/common/MeshManager.hh>
#include <ignition/common/SkeletonAnimation.hh>
#include <ignition/common/Mesh.hh>
#include <sys/mman.h>
#include <fcntl.h>
#include <cstdio>
IGNITION_ADD_PLUGIN(
ignition::gazebo::ActorSystem,
@ -241,6 +243,93 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
threadMutex.unlock();
}
struct shared_mmap
{
pthread_mutex_t mutex;
ActorSystemState currentState,lastState;
};
void client(std::string topic,shared_mmap *sharedMmap){
rclcpp::init(0, {});
auto node = rclcpp::Node::make_shared("moveService", topic);
#pragma clang diagnostic push
#pragma ide diagnostic ignored "UnusedValue"
auto animationServer = rclcpp_action::create_server<action::Animation>(
node,
"animation",
[sharedMmap](const rclcpp_action::GoalUUID goalUuid, const AnimationGoalPtr &animationGoal) {
if (sharedMmap->currentState == ActorSystemState::IDLE) {
pthread_mutex_lock(&sharedMmap->mutex);
//animationTarget = *animationGoal;
pthread_mutex_unlock(&sharedMmap->mutex);
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}else{
return rclcpp_action::GoalResponse::REJECT;
}
},
[sharedMmap](const AnimationServerGoalPtr& animationGoalPtr) {
if (sharedMmap->currentState == ActorSystemState::IDLE) {
return rclcpp_action::CancelResponse::REJECT;
}else{
pthread_mutex_lock(&sharedMmap->mutex);
sharedMmap->currentState = ActorSystemState::IDLE;
pthread_mutex_unlock(&sharedMmap->mutex);
return rclcpp_action::CancelResponse::ACCEPT;
}
},
[sharedMmap](const AnimationServerGoalPtr& animationGoalPtr) {
pthread_mutex_lock(&sharedMmap->mutex);
//currentAnimationGoalPtr = animationGoalPtr;
sharedMmap->currentState = ActorSystemState::ANIMATION;
pthread_mutex_unlock(&sharedMmap->mutex);
}
);
auto movementServer = rclcpp_action::create_server<action::Movement>(
node,
"movement",
[sharedMmap](const rclcpp_action::GoalUUID goalUuid, const MovementGoalPtr &movementGoal ){
if (sharedMmap->currentState == ActorSystemState::IDLE) {
pthread_mutex_lock(&sharedMmap->mutex);
//movementTarget = *movementGoal;
pthread_mutex_unlock(&sharedMmap->mutex);
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}else{
return rclcpp_action::GoalResponse::REJECT;
}
},
[sharedMmap](const MovementServerGoalPtr& movementGoalPtr ){
if (sharedMmap->currentState == ActorSystemState::IDLE) {
return rclcpp_action::CancelResponse::REJECT;
}else{
pthread_mutex_lock(&sharedMmap->mutex);
sharedMmap->currentState = ActorSystemState::IDLE;
pthread_mutex_unlock(&sharedMmap->mutex);
return rclcpp_action::CancelResponse::ACCEPT;
}
},
[sharedMmap](const MovementServerGoalPtr& movementGoalPtr ){
pthread_mutex_lock(&sharedMmap->mutex);
//currentMovementGoalPtr = movementGoalPtr;
sharedMmap->currentState = ActorSystemState::MOVEMENT;
pthread_mutex_unlock(&sharedMmap->mutex);
}
);
#pragma clang diagnostic pop
igndbg << "Spinning node..." << std::endl;
while (true) {
printf("Spinning...\n");
rclcpp::spin(node);
}
}
// 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,
@ -257,90 +346,51 @@ void ActorSystem::Configure(const Entity &_entity, const std::shared_ptr<const s
this->currentState = ActorSystemState::SETUP;
this->lastState = ActorSystemState::IDLE;
rclcpp::init(0, {});
int fd = open(".", O_TMPFILE|O_RDWR, 00600);
auto structSize = sizeof(struct shared_mmap);
if(ftruncate(fd, structSize)!=0){
ignerr << "ftruncate" << std::endl;
return;
}
auto *sharedMmap = (struct shared_mmap *) mmap(nullptr,structSize,PROT_READ|PROT_WRITE,MAP_SHARED,fd,0);
//close(fd);
if (sharedMmap == MAP_FAILED){
ignerr << "MAP_FAILED" << std::endl;
return;
}
pthread_mutexattr_t mutexattr;
if(pthread_mutexattr_init(&mutexattr)!=0){
ignerr << "pthread_mutexattr_init" << std::endl;
return;
}
if(pthread_mutexattr_setpshared(&mutexattr, PTHREAD_PROCESS_SHARED)!=0){
ignerr << "pthread_mutexattr_setpshared" << std::endl;
return;
}
if(pthread_mutex_init(&sharedMmap->mutex, &mutexattr)!=0){
ignerr << "pthread_mutex_init" << std::endl;
return;
}
std::string topic = "ActorPlugin";
if (_sdf->HasElement("topic")) {
topic = _sdf->Get<std::string>("topic");
}
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",
[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;
switch(fork()){
case -1:
ignerr << "Could not fork." << std::endl;
break;
case 0:
ignmsg << "Child fork." << std::endl;
client(topic,sharedMmap);
break;
default:
ignmsg << "Parent fork." << std::endl;
break;
}
},
[this](const AnimationServerGoalPtr& animationGoalPtr) {
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& animationGoalPtr) {
threadMutex.lock();
currentAnimationGoalPtr = animationGoalPtr;
currentState = ActorSystemState::ANIMATION;
threadMutex.unlock();
}
);
movementServer = rclcpp_action::create_server<action::Movement>(
node,
"movement",
[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;
}
},
[this](const MovementServerGoalPtr& movementGoalPtr ){
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 MovementServerGoalPtr& movementGoalPtr ){
threadMutex.lock();
currentMovementGoalPtr = movementGoalPtr;
currentState = ActorSystemState::MOVEMENT;
threadMutex.unlock();
}
);
#pragma clang diagnostic pop
igndbg << "Spinning node..." << std::endl;
std::thread spinThread([this]() {
while (true) {
printf("Spinning...\n");
rclcpp::spin(node);
}
});
spinThread.detach();
}

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

@ -46,8 +46,8 @@ namespace ignition {
private:
std::shared_ptr<rclcpp::Node> node;
std::shared_ptr<AnimationActionServer > animationServer;
std::shared_ptr<MovementActionServer > movementServer;
std::shared_ptr<AnimationActionServer> animationServer;
std::shared_ptr<MovementActionServer> movementServer;
ActorSystemState currentState, lastState;
action::Animation::Goal animationTarget;
action::Movement::Goal movementTarget;