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

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

@ -21,6 +21,7 @@
#include <sys/mman.h>
#include <fcntl.h>
#include <cstdio>
#include <unistd.h>
IGNITION_ADD_PLUGIN(
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, {});
auto node = rclcpp::Node::make_shared("moveService", topic);
@ -258,84 +259,108 @@ void ActorSystem::client(const std::string& topic){
node,
"animation",
[this](const rclcpp_action::GoalUUID goalUuid, const AnimationGoalPtr &animationGoal) {
igndbg << "goal" << shared_mmap->currentState << std::endl;
if (shared_mmap->currentState == ActorSystemState::IDLE) {
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());
pthread_mutex_unlock(&shared_mmap->mutex);
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
return rclcpp_action::GoalResponse::ACCEPT_AND_DEFER;
}else{
return rclcpp_action::GoalResponse::REJECT;
}
},
[this,&currentAnimationGoalPtr](const AnimationServerGoalPtr& animationGoalPtr) {
currentAnimationGoalPtr = nullptr;
[this](const AnimationServerGoalPtr& animationGoalPtr) {
igndbg << "cancel" << shared_mmap->currentState << std::endl;
if (shared_mmap->currentState == ActorSystemState::IDLE) {
return rclcpp_action::CancelResponse::REJECT;
}else{
pthread_mutex_lock(&shared_mmap->mutex);
shared_mmap->currentState = ActorSystemState::IDLE;
pthread_mutex_unlock(&shared_mmap->mutex);
return rclcpp_action::CancelResponse::ACCEPT;
}
},
[this,&currentAnimationGoalPtr](const AnimationServerGoalPtr& animationGoalPtr) {
igndbg << "accepted" << shared_mmap->currentState << std::endl;
pthread_mutex_lock(&shared_mmap->mutex);
igndbg << "accepted l" << shared_mmap->currentState << std::endl;
igndbg << "got animation handle" << std::endl;
currentAnimationGoalPtr = animationGoalPtr;
shared_mmap->currentState = ActorSystemState::ANIMATION;
currentAnimationGoalPtr->execute();
pthread_mutex_unlock(&shared_mmap->mutex);
}
);
auto movementServer = rclcpp_action::create_server<action::Movement>(
node,
"movement",
[this](const rclcpp_action::GoalUUID goalUuid, const MovementGoalPtr &movementGoal ){
if (shared_mmap->currentState == ActorSystemState::IDLE) {
pthread_mutex_lock(&shared_mmap->mutex);
//movementTarget = *movementGoal;
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;
}else{
return rclcpp_action::GoalResponse::REJECT;
}
},
[this,&currentMovementGoalPtr](const MovementServerGoalPtr& movementGoalPtr ){
currentMovementGoalPtr = nullptr;
[this](const MovementServerGoalPtr& movementGoalPtr ){
if (shared_mmap->currentState == ActorSystemState::IDLE) {
return rclcpp_action::CancelResponse::REJECT;
}else{
pthread_mutex_lock(&shared_mmap->mutex);
shared_mmap->currentState = ActorSystemState::IDLE;
pthread_mutex_unlock(&shared_mmap->mutex);
return rclcpp_action::CancelResponse::ACCEPT;
}
},
[this,&currentMovementGoalPtr](const MovementServerGoalPtr& movementGoalPtr ){
pthread_mutex_lock(&shared_mmap->mutex);
igndbg << "got movement handle" << std::endl;
currentMovementGoalPtr = movementGoalPtr;
shared_mmap->currentState = ActorSystemState::MOVEMENT;
pthread_mutex_unlock(&shared_mmap->mutex);
}
);
#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);
igndbg << progress << std::endl;
if(progress >= 1.0f){
if(currentAnimationGoalPtr!=nullptr){
currentAnimationGoalPtr->succeed(std::make_shared<action::Animation::Result>());
}
if(currentMovementGoalPtr!=nullptr){
currentMovementGoalPtr->succeed(std::make_shared<action::Movement::Result>());
}
}else{
if(currentAnimationGoalPtr!=nullptr){
auto feedback = std::make_shared<action::Animation::Feedback>();
feedback->set__progress(progress);
@ -346,21 +371,10 @@ void ActorSystem::client(const std::string& topic){
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);
usleep(500);
}
});
t.detach();
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;
if (pipe(feedback_pipe) < 0){
if (pipe(feedback_pipe)!=0){
igndbg << "pipe failed." << std::endl;
return;
}
@ -434,7 +448,7 @@ void ActorSystem::Configure(const Entity &_entity, const std::shared_ptr<const s
break;
case 0:
ignmsg << "Child fork." << std::endl;
client(topic);
rclcppServer(topic);
break;
default:
ignmsg << "Parent fork." << std::endl;

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

@ -88,7 +88,7 @@ namespace ignition {
private:
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: 'go', animation_speed: 1.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/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]}"