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

Debugging and spacing.

Browse Source
This commit is contained in:
Bastian Hofmann 2023-02-02 10:58:13 +00:00
parent 355c8ff3b3
commit bf36c02d2a
3 changed files with 47 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -21,6 +21,7 @@
#include <sys/mman.h> #include <sys/mman.h>
#include <fcntl.h> #include <fcntl.h>
#include <cstdio> #include <cstdio>
#include <unistd.h>
IGNITION_ADD_PLUGIN( IGNITION_ADD_PLUGIN(
ignition::gazebo::ActorSystem, ignition::gazebo::ActorSystem,
@ -244,7 +245,7 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
} }
void ActorSystem::client(const std::string& topic){ void ActorSystem::rclcppServer(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);
@ -258,109 +259,122 @@ void ActorSystem::client(const std::string& topic){
node, node,
"animation", "animation",
[this](const rclcpp_action::GoalUUID goalUuid, const AnimationGoalPtr &animationGoal) { [this](const rclcpp_action::GoalUUID goalUuid, const AnimationGoalPtr &animationGoal) {
igndbg << "goal" << shared_mmap->currentState << std::endl;
if (shared_mmap->currentState == ActorSystemState::IDLE) { if (shared_mmap->currentState == ActorSystemState::IDLE) {
pthread_mutex_lock(&shared_mmap->mutex); pthread_mutex_lock(&shared_mmap->mutex);
//animationTarget = *animationGoal; igndbg << "goal l" << shared_mmap->currentState << std::endl;
strcpy(shared_mmap->animation_name,animationGoal->animation_name.data()); strcpy(shared_mmap->animation_name,animationGoal->animation_name.data());
pthread_mutex_unlock(&shared_mmap->mutex); pthread_mutex_unlock(&shared_mmap->mutex);
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; return rclcpp_action::GoalResponse::ACCEPT_AND_DEFER;
}else{ }else{
return rclcpp_action::GoalResponse::REJECT; return rclcpp_action::GoalResponse::REJECT;
} }
}, },
[this,&currentAnimationGoalPtr](const AnimationServerGoalPtr& animationGoalPtr) { [this](const AnimationServerGoalPtr& animationGoalPtr) {
currentAnimationGoalPtr = nullptr; igndbg << "cancel" << shared_mmap->currentState << std::endl;
if (shared_mmap->currentState == ActorSystemState::IDLE) { if (shared_mmap->currentState == ActorSystemState::IDLE) {
return rclcpp_action::CancelResponse::REJECT; return rclcpp_action::CancelResponse::REJECT;
}else{ }else{
pthread_mutex_lock(&shared_mmap->mutex); pthread_mutex_lock(&shared_mmap->mutex);
shared_mmap->currentState = ActorSystemState::IDLE; shared_mmap->currentState = ActorSystemState::IDLE;
pthread_mutex_unlock(&shared_mmap->mutex); pthread_mutex_unlock(&shared_mmap->mutex);
return rclcpp_action::CancelResponse::ACCEPT; return rclcpp_action::CancelResponse::ACCEPT;
} }
}, },
[this,&currentAnimationGoalPtr](const AnimationServerGoalPtr& animationGoalPtr) { [this,&currentAnimationGoalPtr](const AnimationServerGoalPtr& animationGoalPtr) {
igndbg << "accepted" << shared_mmap->currentState << std::endl;
pthread_mutex_lock(&shared_mmap->mutex); pthread_mutex_lock(&shared_mmap->mutex);
igndbg << "accepted l" << shared_mmap->currentState << std::endl;
igndbg << "got animation handle" << std::endl;
currentAnimationGoalPtr = animationGoalPtr; currentAnimationGoalPtr = animationGoalPtr;
shared_mmap->currentState = ActorSystemState::ANIMATION; shared_mmap->currentState = ActorSystemState::ANIMATION;
currentAnimationGoalPtr->execute();
pthread_mutex_unlock(&shared_mmap->mutex); pthread_mutex_unlock(&shared_mmap->mutex);
} }
); );
auto movementServer = rclcpp_action::create_server<action::Movement>( auto movementServer = rclcpp_action::create_server<action::Movement>(
node, node,
"movement", "movement",
[this](const rclcpp_action::GoalUUID goalUuid, const MovementGoalPtr &movementGoal ){ [this](const rclcpp_action::GoalUUID goalUuid, const MovementGoalPtr &movementGoal ){
if (shared_mmap->currentState == ActorSystemState::IDLE) { if (shared_mmap->currentState == ActorSystemState::IDLE) {
pthread_mutex_lock(&shared_mmap->mutex); pthread_mutex_lock(&shared_mmap->mutex);
//movementTarget = *movementGoal;
shared_mmap->target_position = math::Vector3d( shared_mmap->target_position = math::Vector3d(
movementGoal->target_position[0], movementGoal->target_position[0],
movementGoal->target_position[1], movementGoal->target_position[1],
movementGoal->target_position[2]); movementGoal->target_position[2]);
shared_mmap->animation_distance = movementGoal->animation_distance; shared_mmap->animation_distance = movementGoal->animation_distance;
strcpy(shared_mmap->animation_name,movementGoal->animation_name.data()); strcpy(shared_mmap->animation_name,movementGoal->animation_name.data());
pthread_mutex_unlock(&shared_mmap->mutex); 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;
} }
}, },
[this,&currentMovementGoalPtr](const MovementServerGoalPtr& movementGoalPtr ){ [this](const MovementServerGoalPtr& movementGoalPtr ){
currentMovementGoalPtr = nullptr;
if (shared_mmap->currentState == ActorSystemState::IDLE) { if (shared_mmap->currentState == ActorSystemState::IDLE) {
return rclcpp_action::CancelResponse::REJECT; return rclcpp_action::CancelResponse::REJECT;
}else{ }else{
pthread_mutex_lock(&shared_mmap->mutex); pthread_mutex_lock(&shared_mmap->mutex);
shared_mmap->currentState = ActorSystemState::IDLE; shared_mmap->currentState = ActorSystemState::IDLE;
pthread_mutex_unlock(&shared_mmap->mutex); pthread_mutex_unlock(&shared_mmap->mutex);
return rclcpp_action::CancelResponse::ACCEPT; return rclcpp_action::CancelResponse::ACCEPT;
} }
}, },
[this,&currentMovementGoalPtr](const MovementServerGoalPtr& movementGoalPtr ){ [this,&currentMovementGoalPtr](const MovementServerGoalPtr& movementGoalPtr ){
pthread_mutex_lock(&shared_mmap->mutex); pthread_mutex_lock(&shared_mmap->mutex);
igndbg << "got movement handle" << std::endl;
currentMovementGoalPtr = movementGoalPtr; currentMovementGoalPtr = movementGoalPtr;
shared_mmap->currentState = ActorSystemState::MOVEMENT; shared_mmap->currentState = ActorSystemState::MOVEMENT;
pthread_mutex_unlock(&shared_mmap->mutex); pthread_mutex_unlock(&shared_mmap->mutex);
} }
); );
#pragma clang diagnostic pop #pragma clang diagnostic pop
std::thread t([this,&currentAnimationGoalPtr,&currentMovementGoalPtr]() { std::thread t([this,&currentAnimationGoalPtr,&currentMovementGoalPtr]() {
float progress; float progress;
while(true) { while(true) {
read(feedback_pipe[0], &progress, sizeof(float)); read(feedback_pipe[0], &progress, sizeof(float));
pthread_mutex_lock(&shared_mmap->mutex); igndbg << progress << std::endl;
if(currentAnimationGoalPtr!=nullptr){ if(progress >= 1.0f){
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){ if(currentAnimationGoalPtr!=nullptr){
currentAnimationGoalPtr->succeed(std::make_shared<action::Animation::Result>()); currentAnimationGoalPtr->succeed(std::make_shared<action::Animation::Result>());
} }
if(currentMovementGoalPtr!=nullptr){ if(currentMovementGoalPtr!=nullptr){
currentMovementGoalPtr->succeed(std::make_shared<action::Movement::Result>()); currentMovementGoalPtr->succeed(std::make_shared<action::Movement::Result>());
} }
currentMovementGoalPtr = nullptr; }else{
currentAnimationGoalPtr = nullptr; 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);
}
} }
pthread_mutex_unlock(&shared_mmap->mutex); usleep(500);
} }
}); });
t.detach(); t.detach();
igndbg << "Spinning node..." << std::endl; igndbg << "Spinning node..." << std::endl;
@ -423,7 +437,7 @@ void ActorSystem::Configure(const Entity &_entity, const std::shared_ptr<const s
igndbg << "pipe setup" << std::endl; igndbg << "pipe setup" << std::endl;
if (pipe(feedback_pipe) < 0){ if (pipe(feedback_pipe)!=0){
igndbg << "pipe failed." << std::endl; igndbg << "pipe failed." << std::endl;
return; return;
} }
@ -434,7 +448,7 @@ 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); rclcppServer(topic);
break; break;
default: default:
ignmsg << "Parent fork." << std::endl; ignmsg << "Parent fork." << std::endl;

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

@ -88,7 +88,7 @@ namespace ignition {
private: private:
void switchAnimation(EntityComponentManager &_ecm, char *animationName); void switchAnimation(EntityComponentManager &_ecm, char *animationName);
void client(const std::string& topic); void rclcppServer(const std::string& topic);
}; };

4
test.sh Normal file → Executable file
View File

@ -1,2 +1,2 @@
ros2 action send_goal /ActorPlugin/movement ign_actor_plugin_msgs/action/Movement "{animation_name: 'walk', animation_speed: 1.0, target_position: [1,1,0], target_orientation: [0,0,0]}" ros2 action send_goal /ActorPlugin/animation ign_actor_plugin_msgs/action/Animation "{animation_name: 'walk', animation_speed: 1.0}"
ros2 action send_goal /ActorPlugin/animation ign_actor_plugin_msgs/action/Animation "{animation_name: 'go', animation_speed: 1.0}" ros2 action send_goal /ActorPlugin/movement ign_actor_plugin_msgs/action/Movement "{animation_name: 'walk', animation_speed: 1.0, animation_distance: 1.5, target_position: [1,1,0], target_orientation: [0,0,0]}"