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

Fixes to new actor server and tree nodes

Browse Source
This commit is contained in:
Bastian Hofmann
2023-03-09 04:42:15 +00:00
parent 67af90a280
commit 7293a2b797
10 changed files with 339 additions and 179 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -3,24 +3,6 @@
//
#include "ActorSystem.hpp"
#include <chrono>
#include <cstdio>
#include <fcntl.h>
#include <ignition/common/Console.hh>
#include <ignition/common/Mesh.hh>
#include <ignition/common/MeshManager.hh>
#include <ignition/common/SkeletonAnimation.hh>
#include <ignition/gazebo/Entity.hh>
#include <ignition/gazebo/EntityComponentManager.hh>
#include <ignition/gazebo/Types.hh>
#include <ignition/gazebo/components/Actor.hh>
#include <ignition/gazebo/components/Component.hh>
#include <ignition/gazebo/components/Pose.hh>
#include <rclcpp_action/create_server.hpp>
#include <rclcpp_action/server.hpp>
#include <sdf/Actor.hh>
#include <sys/mman.h>
#include <unistd.h>
IGNITION_ADD_PLUGIN(ignition::gazebo::ActorSystem, ignition::gazebo::System, ignition::gazebo::ActorSystem::ISystemPreUpdate,
ignition::gazebo::ActorSystem::ISystemConfigure)
@@ -66,11 +48,6 @@ void ActorSystem::switchAnimation(EntityComponentManager &_ecm, char *animationN
auto rootNode = currentSkeleton->RootNode()->Name();
currentRootNodeAnimation = currentSkeleton->Animation(0)->NodeAnimationByName(rootNode);
currentTranslationAlign = currentSkeleton->AlignTranslation(0, rootNode);
currentRotationAlign = currentSkeleton->AlignRotation(0, rootNode);
igndbg << "Translation: " << currentTranslationAlign << std::endl;
igndbg << "Rotation: " << currentRotationAlign << std::endl;
if (currentSkeleton != nullptr) {
duration = currentSkeleton->Animation(0)->Length();
@@ -91,6 +68,15 @@ void ActorSystem::switchAnimation(EntityComponentManager &_ecm, char *animationN
}
}
double Angle(ignition::math::Quaterniond a,ignition::math::Quaterniond b){
auto dot = fmin(abs(a.Dot(b)),1.0);
if(dot > 0.999999){
return 0.0;
}else{
return acos(dot) * 2;
}
}
void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ecm) {
threadMutex.lock();
@@ -127,6 +113,8 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
_ecm.CreateComponent(entity, components::TrajectoryPose(startPose));
}
addPoseToFeedback(startPose);
nextState = IDLE;
break;
}
@@ -152,44 +140,11 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
*animationTimeComponent = components::AnimationTime(newTime);
_ecm.SetChanged(entity, components::AnimationTime::typeId, ComponentState::OneTimeChange);
/*
if(currentRootNodeAnimation == nullptr){
break;
}
auto rootTransformation = currentRootNodeAnimation->FrameAt(newTimeSeconds,false);
math::Matrix4d totalTf = currentTranslationAlign * rootTransformation * currentRotationAlign;
auto translation = totalTf.Translation();
translation[0] = 0;
totalTf.SetTranslation(translation);
auto poseComp = _ecm.Component<components::Pose>(entity);
*poseComp = components::Pose (totalTf.Pose());
_ecm.SetChanged(entity, components::Pose::typeId, ComponentState::OneTimeChange);
*/
break;
}
case MOVEMENT: {
if (lastState == IDLE) {
igndbg << "Starting Movement..." << std::endl;
switchAnimation(_ecm, animation_name);
}
auto trajectoryPoseComp = _ecm.Component<components::TrajectoryPose>(this->entity);
auto actorPose = trajectoryPoseComp->Data();
auto targetPose = target_position;
auto targetDirection = math::Vector3d(targetPose[0], targetPose[1], targetPose[2]) - actorPose.Pos();
if (targetDirection.Length() < 0.05) {
igndbg << "Finished Movement..." << std::endl;
feedback.progress = 1.0f;
sendFeedback();
nextState = IDLE;
break;
}
auto animationTimeComponent = _ecm.Component<components::AnimationTime>(this->entity);
auto oldTimeSeconds = std::chrono::duration<double>(animationTimeComponent->Data()).count();
@@ -197,44 +152,95 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
auto newTimeSeconds = oldTimeSeconds + deltaTimeSeconds;
auto newTime = animationTimeComponent->Data() + _info.dt;
if (newTimeSeconds >= duration) {
newTimeSeconds -= duration;
newTime = std::chrono::duration_cast<std::chrono::steady_clock::duration>(std::chrono::duration<double>(newTimeSeconds));
}
auto turnSpeed = 1;
auto targetYaw = atan2(targetDirection.Y(), targetDirection.X());
auto currentYaw = fmod(actorPose.Rot().Yaw(), M_PI);
if (lastState == IDLE) {
igndbg << "Starting Movement..." << std::endl;
switchAnimation(_ecm, animation_name);
movementDetails.state = movementDetails.ROTATE_TO_TARGET;
movementDetails.time = 0.0;
movementDetails.stepTime = 0.0;
movementDetails.start = actorPose;
movementDetails.targetDiff.Pos() = movementDetails.target.Pos() - movementDetails.start.Pos();
auto angularDirection = fmod((targetYaw - currentYaw),
M_PI + 0.001); // additional 0.001 rad to prevent instant flip through 180 rotation
auto turnSpeed = 1.0;
if (angularDirection < 0) {
turnSpeed = -turnSpeed;
}
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 {
if (nullptr == currentRootNodeAnimation) {
ignerr << "Current animation doesn't move root node, this is unsupported for movement animations." << std::endl;
break;
if(movementDetails.targetDiff.Pos().Length() >= 0.001){
movementDetails.targetDiff.Rot() = ignition::math::Quaterniond::EulerToQuaternion(0.0, 0.0, atan2(movementDetails.targetDiff.Pos().Y(), movementDetails.targetDiff.Pos().X()));
movementDetails.rotateToTargetDuration = Angle(actorPose.Rot(),movementDetails.targetDiff.Rot()) / turnSpeed;
movementDetails.moveDuration = movementDetails.targetDiff.Pos().Length() / animation_distance * duration;
}else{
movementDetails.targetDiff.Rot() = movementDetails.start.Rot();
movementDetails.rotateToTargetDuration = 0.0;
movementDetails.moveDuration = 0.0;
}
double distance = (deltaTimeSeconds / duration) * animation_distance;
actorPose.Pos() += targetDirection.Normalize() * distance;
actorPose.Rot().Euler(0, 0, targetYaw);
*trajectoryPoseComp = components::TrajectoryPose(actorPose);
*animationTimeComponent = components::AnimationTime(newTime);
_ecm.SetChanged(entity, components::TrajectoryPose::typeId, ComponentState::OneTimeChange);
_ecm.SetChanged(entity, components::AnimationTime::typeId, ComponentState::OneTimeChange);
movementDetails.rotateToEndDuration = Angle(movementDetails.targetDiff.Rot(),movementDetails.target.Rot()) / turnSpeed;
}
movementDetails.time += deltaTimeSeconds;
movementDetails.stepTime += deltaTimeSeconds;
if (movementDetails.state == movementDetails.ROTATE_TO_TARGET){
if(movementDetails.stepTime<=movementDetails.rotateToTargetDuration){
actorPose.Rot() = ignition::math::Quaterniond::Slerp(movementDetails.stepTime/movementDetails.rotateToTargetDuration,movementDetails.start.Rot(),movementDetails.targetDiff.Rot(),true);
}else{
actorPose.Rot() = movementDetails.targetDiff.Rot();
movementDetails.state = movementDetails.MOVE;
movementDetails.stepTime -= movementDetails.rotateToTargetDuration;
}
}
if (movementDetails.state == movementDetails.MOVE){
if(movementDetails.stepTime<=movementDetails.moveDuration){
newTime = std::chrono::duration_cast<std::chrono::steady_clock::duration>(std::chrono::duration<double>(fmod(movementDetails.stepTime,duration)));
actorPose.Pos()=movementDetails.start.Pos()+(movementDetails.targetDiff.Pos()*(movementDetails.stepTime/movementDetails.moveDuration));
}else{
actorPose.Pos()=movementDetails.target.Pos();
movementDetails.state = movementDetails.ROTATE_TO_END;
movementDetails.stepTime -= movementDetails.moveDuration;
}
}
if (movementDetails.state == movementDetails.ROTATE_TO_END){
if(movementDetails.stepTime<=movementDetails.rotateToEndDuration){
actorPose.Rot() = ignition::math::Quaterniond::Slerp(movementDetails.stepTime/movementDetails.rotateToEndDuration,movementDetails.targetDiff.Rot(),movementDetails.target.Rot(),true);
}else{
actorPose.Rot() = movementDetails.target.Rot();
movementDetails.state = movementDetails.END;
movementDetails.stepTime -= movementDetails.rotateToEndDuration;
}
}
if (movementDetails.state == movementDetails.END || movementDetails.state == movementDetails.ROTATE_TO_END){
if (newTime.count() < duration / 2){
newTime = std::chrono::duration_cast<std::chrono::steady_clock::duration>(std::chrono::duration<double>(newTimeSeconds-(deltaTimeSeconds)));
if(newTime.count()<=0){
newTime = std::chrono::duration_cast<std::chrono::steady_clock::duration>(std::chrono::duration<double>(0.0));
if(movementDetails.state==movementDetails.END){
nextState = IDLE;
}
}
}else{
newTime = std::chrono::duration_cast<std::chrono::steady_clock::duration>(std::chrono::duration<double>(newTimeSeconds+(deltaTimeSeconds)));
if(newTime.count()>=duration){
newTime = std::chrono::duration_cast<std::chrono::steady_clock::duration>(std::chrono::duration<double>(duration));
if(movementDetails.state == movementDetails.END){
nextState = IDLE;
}
}
}
}
addPoseToFeedback(actorPose);
feedback.progress = movementDetails.time / (movementDetails.rotateToTargetDuration + movementDetails.moveDuration + movementDetails.rotateToEndDuration);
sendFeedback();
*trajectoryPoseComp = components::TrajectoryPose(actorPose);
_ecm.SetChanged(entity, components::TrajectoryPose::typeId, ComponentState::OneTimeChange);
*animationTimeComponent = components::AnimationTime(newTime);
_ecm.SetChanged(entity, components::AnimationTime::typeId, ComponentState::OneTimeChange);
break;
}
case IDLE:
@@ -294,26 +300,35 @@ void ActorSystem::messageQueueInterface(const char name[256]) {
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.target.position.x, receivedAction.target.position.y, receivedAction.target.position.z);
movementDetails.target.Pos().Set(receivedAction.target.position.x, receivedAction.target.position.y, receivedAction.target.position.z);
movementDetails.target.Rot().Set(receivedAction.target.orientation.w, receivedAction.target.orientation.x, receivedAction.target.orientation.y, receivedAction.target.orientation.z);
nextState = receivedAction.state;
igndbg << "Received Action; State: " << nextState << " Target: " << movementDetails.target.Pos() << " | " << movementDetails.target.Rot().Z() << std::endl;
threadMutex.unlock();
}
}).detach();
}
void ActorSystem::addPoseToFeedback(ignition::math::Pose3<double> pose){
feedback.current.position.x=pose.X();
feedback.current.position.y=pose.Y();
feedback.current.position.z=pose.Z();
feedback.current.orientation.w=pose.Rot().W();
feedback.current.orientation.x=pose.Rot().X();
feedback.current.orientation.y=pose.Rot().Y();
feedback.current.orientation.z=pose.Rot().Z();
}
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{
if(mq_send(feedbackQueue,(char *)&feedback,sizeof(FeedbackMessage),0)!=0){
ignerr << "Error " << errno << " sending feedback." << std::endl;
}
}

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

@@ -6,24 +6,44 @@
#define BUILD_ACTORSYSTEM_H
#include <cstddef>
#include <ignition/gazebo/Entity.hh>
#include <ignition/gazebo/System.hh>
#include <ignition/plugin/Register.hh>
#include <fcntl.h>
#include <mqueue.h>
#include <chrono>
#include <cstdio>
#include <fcntl.h>
#include <unistd.h>
#include <optional>
#include <queue>
#include <string>
#include <cmath>
#include <sys/stat.h>
#include <sys/mman.h>
#include <ignition/gazebo/System.hh>
#include <ignition/plugin/Register.hh>
#include <ignition/common/Console.hh>
#include <ignition/common/Mesh.hh>
#include <ignition/common/MeshManager.hh>
#include <ignition/common/SkeletonAnimation.hh>
#include <ignition/common/Skeleton.hh>
#include <ignition/common/NodeAnimation.hh>
#include <ignition/gazebo/Entity.hh>
#include <ignition/gazebo/EntityComponentManager.hh>
#include <ignition/gazebo/Types.hh>
#include <ignition/gazebo/components/Actor.hh>
#include <ignition/gazebo/components/Component.hh>
#include <ignition/gazebo/components/Pose.hh>
#include <sdf/Actor.hh>
#include <rclcpp/node.hpp>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/create_server.hpp>
#include <rclcpp_action/server.hpp>
#include <rclcpp_action/server_goal_handle.hpp>
#include <ros_actor_action_server_msgs/action/animation.hpp>
#include <ros_actor_action_server_msgs/action/movement.hpp>
#include <ros_actor_message_queue_msgs/MessageTypes.hpp>
#include <rclcpp_action/server_goal_handle.hpp>
#include <string>
#include <ignition/common/Skeleton.hh>
#include <ignition/common/NodeAnimation.hh>
#include <fcntl.h>
#include <sys/stat.h>
#include <mqueue.h>
using namespace ros_actor_message_queue_msgs;
using namespace ros_actor_action_server_msgs;
@@ -41,60 +61,50 @@ using rclcpp_action::ServerGoalHandle;
namespace ignition {
namespace gazebo {
class ActorSystem :
public System,
class ActorSystem : public System,
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;
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;
private:
ignition::math::Pose3d startPose = ignition::math::Pose3d::Zero;
ignition::math::Pose3d target_pose;
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;
mqd_t feedbackQueue;
mqd_t actionQueue;
FeedbackMessage feedback;
public:
ActorSystem();
struct {
enum {ROTATE_TO_TARGET,MOVE,ROTATE_TO_END,END} state;
ignition::math::Pose3d start;
ignition::math::Pose3d targetDiff;
ignition::math::Pose3d target;
double rotateToTargetDuration,moveDuration,rotateToEndDuration,time,stepTime;
} movementDetails;
public:
~ActorSystem() override;
public:
ActorSystem();
~ActorSystem() override;
void PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ecm) override;
void Configure(const ignition::gazebo::Entity &animationGoalPtr,
const std::shared_ptr<const sdf::Element> &_sdf,
EntityComponentManager &_ecm,
EventManager &/*_eventMgr*/) override;
public:
void PreUpdate(const UpdateInfo &_info,
EntityComponentManager &_ecm) override;
private:
void switchAnimation(EntityComponentManager &_ecm, char *animationName);
void messageQueueInterface(const char name[256]);
void sendFeedback();
void addPoseToFeedback(ignition::math::Pose3<double> pose);
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();
void addPoseToFeedback(ignition::math::Pose3<double> pose);
};
}
};
}
}
#endif //BUILD_ACTORSYSTEM_H