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

Changed message types and error handling to better suit behavior tree.

Browse Source
This commit is contained in:
Bastian Hofmann
2023-03-06 14:50:30 +00:00
parent d9074b1103
commit 67af90a280
21 changed files with 246 additions and 151 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -97,7 +97,8 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
if(nextState != currentState){
igndbg << "State change before: " << currentState << " -> " << nextState << std::endl;
currentState = nextState;
sendFeedback(0.0);
feedback.progress = 0.0f;
sendFeedback();
}
switch (currentState) {
@@ -123,7 +124,7 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
auto trajectoryPoseComponent = _ecm.Component<components::TrajectoryPose>(entity);
if (nullptr == trajectoryPoseComponent) {
_ecm.CreateComponent(entity, components::TrajectoryPose(ignition::math::Pose3d::Zero));
_ecm.CreateComponent(entity, components::TrajectoryPose(startPose));
}
nextState = IDLE;
@@ -137,13 +138,14 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
auto newTime = animationTimeComponent->Data() + _info.dt;
auto newTimeSeconds = std::chrono::duration<double>(newTime).count();
auto feedback = newTimeSeconds / duration;
sendFeedback(feedback);
feedback.progress = newTimeSeconds / duration;
sendFeedback();
if (newTimeSeconds >= duration) {
igndbg << "Animation " << animation_name << " finished." << std::endl;
nextState = IDLE;
sendFeedback(0.0);
feedback.progress = 0.0f;
sendFeedback();
break;
}
@@ -183,7 +185,8 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
if (targetDirection.Length() < 0.05) {
igndbg << "Finished Movement..." << std::endl;
sendFeedback(1.0);
feedback.progress = 1.0f;
sendFeedback();
nextState = IDLE;
break;
}
@@ -212,6 +215,7 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
if (abs(angularDirection) > 0.01) {
actorPose.Rot().Euler(0, 0, actorPose.Rot().Yaw() + (turnSpeed * deltaTimeSeconds));
*trajectoryPoseComp = components::TrajectoryPose(actorPose);
_ecm.SetChanged(entity, components::TrajectoryPose::typeId, ComponentState::OneTimeChange);
} else {
@@ -243,55 +247,72 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
}
void ActorSystem::messageQueueInterface(const char name[256]) {
mq_attr queueAttributes;
queueAttributes.mq_flags = 0;
queueAttributes.mq_curmsgs = 0;
queueAttributes.mq_msgsize = sizeof(ros_actor_message_queue_msgs::FeedbackMessage);
queueAttributes.mq_maxmsg = 1;
char generatedName[256];
strcpy(generatedName,name);
strcat(generatedName,"Feedback");
feedbackQueue = mq_open(generatedName, O_CREAT | O_RDWR, S_IRWXU | S_IRWXG, &queueAttributes);
if (feedbackQueue == (mqd_t)-1) {
ignerr << "Could not create queue. (" << errno << ")" << std::endl;
return;
igndbg << "Waiting for " << generatedName << std::endl;
feedbackQueue = (mqd_t) -1;
while(feedbackQueue == (mqd_t) -1){
feedbackQueue = mq_open(generatedName, O_RDWR);
if (feedbackQueue == (mqd_t) -1) {
if(errno!=ENOENT){
perror(nullptr);
return;
}
mq_close(feedbackQueue);
sleep(100);
}
}
strcpy(generatedName,name);
strcat(generatedName,"Action");
queueAttributes.mq_msgsize = sizeof(ros_actor_message_queue_msgs::ActionMessage);
actionQueue = mq_open(generatedName, O_CREAT | O_RDWR, S_IRWXU | S_IRWXG, &queueAttributes);
if (actionQueue == (mqd_t)-1) {
ignerr << "Could not create queue. (" << errno << ")" << std::endl;
return;
igndbg << "Waiting for " << generatedName << std::endl;
actionQueue = (mqd_t) -1;
while(actionQueue == (mqd_t) -1){
actionQueue = mq_open(generatedName, O_RDWR);
if (actionQueue == (mqd_t) -1) {
if(errno!=ENOENT){
perror(nullptr);
return;
}
mq_close(actionQueue);
sleep(100);
}
}
igndbg << "Queues online, starting listener" << std::endl;
std::thread([this] {
ActionMessage receivedAction;
while (true) {
igndbg << "Waiting for action" << std::endl;
mq_receive(actionQueue, (char *)&receivedAction, sizeof(ActionMessage), nullptr);
igndbg << "Got Action" << std::endl;
threadMutex.lock();
strcpy(animation_name, receivedAction.animationName);
// animation_speed = receivedAction.animationSpeed;
animation_distance = receivedAction.animationDistance;
target_position = math::Vector3d(receivedAction.positionX, receivedAction.positionY, receivedAction.positionZ);
target_position = math::Vector3d(receivedAction.target.position.x, receivedAction.target.position.y, receivedAction.target.position.z);
nextState = receivedAction.state;
std::cout << "Got Action" << std::endl;
threadMutex.unlock();
}
}).detach();
}
void ActorSystem::sendFeedback(double progress){
FeedbackMessage message;
message.progress = progress;
message.state = currentState;
if(mq_send(feedbackQueue,(char *)&message,sizeof(FeedbackMessage),0)==0){
igndbg << "Sent feedback. (State: " << lastState << "->" << currentState << " | Progress: "<< progress <<")" << std::endl;
void ActorSystem::sendFeedback(){
feedback.state = currentState;
if(mq_send(feedbackQueue,(char *)&feedback,sizeof(FeedbackMessage),0)==0){
igndbg << "Sent feedback. (State: " << lastState << "->" << currentState << " | Progress: "<< feedback.progress <<")" << std::endl;
}else{
ignerr << "Error " << errno << " sending feedback." << std::endl;
}
@@ -320,6 +341,21 @@ void ActorSystem::Configure(const Entity &_entity, const std::shared_ptr<const s
strcpy(topic, topicString.c_str());
}
if (_sdf->HasElement("x")) {
auto x = _sdf->Get<float>("x");
startPose.SetX(x);
}
if (_sdf->HasElement("y")) {
auto y = _sdf->Get<float>("y");
startPose.SetY(y);
}
if (_sdf->HasElement("rot")) {
auto rot = _sdf->Get<float>("rot");
startPose.Rot().Euler(0,0,rot);
}
this->entity = _entity;
messageQueueInterface(topic);

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

@@ -39,55 +39,58 @@ using rclcpp_action::ServerGoalHandle;
#define MovementServerGoalPtr std::shared_ptr<ServerGoalHandle<action::Movement>>
namespace ignition {
namespace gazebo {
namespace gazebo {
class ActorSystem :
class ActorSystem :
public System,
public ISystemPreUpdate,
public ISystemConfigure {
public ISystemPreUpdate,
public ISystemConfigure {
private:
//std::shared_ptr<rclcpp::Node> node;
//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;
math::Vector3d target_position;
double animation_distance;
char animation_name[256];
ActorPluginState nextState,currentState,lastState;
double duration{};
Entity entity{kNullEntity};
std::mutex threadMutex;
std::shared_ptr<common::Skeleton> currentSkeleton;
common::NodeAnimation* currentRootNodeAnimation;
math::Matrix4<double> currentRotationAlign, currentTranslationAlign;
mqd_t feedbackQueue;
mqd_t actionQueue;
private:
//std::shared_ptr<rclcpp::Node> node;
//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;
ignition::math::v6::Pose3d startPose = ignition::math::Pose3d::Zero;
math::Vector3d target_position;
double animation_distance;
char animation_name[256];
ActorPluginState nextState,currentState,lastState;
double duration{};
Entity entity{kNullEntity};
std::mutex threadMutex;
std::shared_ptr<common::Skeleton> currentSkeleton;
common::NodeAnimation* currentRootNodeAnimation;
math::Matrix4<double> currentRotationAlign, currentTranslationAlign;
mqd_t feedbackQueue;
mqd_t actionQueue;
FeedbackMessage feedback;
public:
ActorSystem();
public:
ActorSystem();
public:
~ActorSystem() override;
public:
~ActorSystem() override;
public:
void PreUpdate(const UpdateInfo &_info,
EntityComponentManager &_ecm) override;
public:
void PreUpdate(const UpdateInfo &_info,
EntityComponentManager &_ecm) override;
public:
void Configure(const ignition::gazebo::Entity &animationGoalPtr,
const std::shared_ptr<const sdf::Element> &_sdf,
EntityComponentManager &_ecm,
EventManager &/*_eventMgr*/) override;
public:
void Configure(const ignition::gazebo::Entity &animationGoalPtr,
const std::shared_ptr<const sdf::Element> &_sdf,
EntityComponentManager &_ecm,
EventManager &/*_eventMgr*/) override;
private:
void switchAnimation(EntityComponentManager &_ecm, char *animationName);
void messageQueueInterface(const char name[256]);
void sendFeedback(double feedback);
private:
void switchAnimation(EntityComponentManager &_ecm, char *animationName);
void messageQueueInterface(const char name[256]);
void sendFeedback();
void addPoseToFeedback(ignition::math::Pose3<double> pose);
};