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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/btree_nodes/CMakeLists.txt
View File

@ -50,6 +50,7 @@ set(CPP_FILES
src/nodes/InAreaTest.cpp
src/nodes/InterruptableSequence.cpp
src/nodes/SetRobotVelocity.cpp
src/nodes/ActorAnimation.cpp
src/nodes/ActorMovement.cpp
)

9
src/btree_nodes/nodes.xml
View File

@ -18,6 +18,15 @@
<input_port name="area">Area to generate pose in</input_port>
<output_port name="pose">Generated pose in area</output_port>
</Action>
<Action ID="ActorMovement">
<input_port name="target">Target to move actor to</input_port>
<input_port name="animation">Animation name to use</input_port>
</Action>
<Action ID="ActorAnimation">
<input_port name="animation">Animation name to use</input_port>
</Action>
<Action ID="MoveActorToTarget">
<input_port name="current">Current actor position</input_port>

51
src/btree_nodes/src/Tree.cpp
View File

@ -14,6 +14,7 @@
#include "nodes/InterruptableSequence.h"
#include "nodes/SetRobotVelocity.h"
#include "nodes/ActorMovement.h"
#include "nodes/ActorAnimation.h"
#include <chrono>
#include <thread>
@ -25,33 +26,31 @@ int main(int argc, char **argv) {
node_options.automatically_declare_parameters_from_overrides(true);
auto mainNode = rclcpp::Node::make_shared("tree_general_node", node_options);
auto moveNode = rclcpp::Node::make_shared("tree_moveit_node", node_options);
rclcpp::executors::SingleThreadedExecutor executor;
executor.add_node(mainNode);
std::thread([&executor]() { executor.spin(); }).detach();
BehaviorTreeFactory factory;
const std::shared_ptr<Area> safeArea = std::make_shared<Area>();
safeArea->triangles = {std::vector<Position2D>({
safeArea->triangles = std::vector<Position2D>({
{ 1, 3.5 },
{ 1, 7 },
{ 3, 3.5 },
{ 7, 7 },
{ 3, -1 },
{ 7, -1 }
})};
});
const std::shared_ptr<Area> warningArea = std::make_shared<Area>();
warningArea->triangles = {std::vector<Position2D>({
warningArea->triangles = std::vector<Position2D>({
{ -1, 2.5 },
{ -1, 3.5 },
{ 2, 2.5 },
{ 3, 3.5 },
{ 2, -1 },
{ 3, -1 }
})};
});
const std::shared_ptr<Area> unsafeArea = std::make_shared<Area>();
unsafeArea->triangles = std::vector<Position2D>({
@ -82,7 +81,6 @@ int main(int argc, char **argv) {
factory.registerNodeType<GenerateXYPose>("GenerateXYPose");
factory.registerNodeType<AmICalled>("AmICalled");
//factory.registerNodeType<ActorMovement>("ActorMovement");
factory.registerNodeType<MoveActorToTarget>("MoveActorToTarget");
factory.registerNodeType<WeightedRandomNode>("WeightedRandom");
factory.registerNodeType<OffsetPose>("OffsetPose");
@ -91,17 +89,25 @@ int main(int argc, char **argv) {
factory.registerNodeType<InterruptableSequence>("InterruptableSequence");
auto moveNode = rclcpp::Node::make_shared("tree_moveit_node", node_options);
auto connection = std::make_shared<MoveConnection>(moveNode, "arm");
executor.add_node(moveNode);
NodeBuilder builderIisyMove = [&connection](const std::string &name, const NodeConfiguration &config) {
factory.registerBuilder<RobotMove>("RobotMove", [&connection](const std::string &name, const NodeConfiguration &config) {
return std::make_unique<RobotMove>(name, config, &connection);
};
factory.registerBuilder<RobotMove>("RobotMove", builderIisyMove);
});
NodeBuilder builderIisyVelocity = [&connection](const std::string &name, const NodeConfiguration &config) {
factory.registerBuilder<SetRobotVelocity>("SetRobotVelocity", [&connection](const std::string &name, const NodeConfiguration &config) {
return std::make_unique<SetRobotVelocity>(name, config, &connection);
});
auto actorAnimationNode = rclcpp::Node::make_shared("actorAnimationNode",node_options);
NodeBuilder builderActorAnimation = [&actorAnimationNode](const std::string &name, const NodeConfiguration &config) {
return std::make_unique<ActorAnimation>(name, config, actorAnimationNode, "/ActorPlugin/animation");
};
factory.registerBuilder<SetRobotVelocity>("SetRobotVelocity", builderIisyVelocity);
executor.add_node(actorAnimationNode);
bool called;
auto IsCalled = [&called](__attribute__((unused)) TreeNode& parent_node) -> NodeStatus{
@ -142,6 +148,25 @@ int main(int argc, char **argv) {
std::cout << "Starting subscriber." << std::endl;
std::thread([&executor]() {
while(rclcpp::ok()){
executor.spin();
}
}).detach();
auto actorMovementNode = rclcpp::Node::make_shared("actorMovementNode",node_options);
NodeBuilder builderActorMovement = [&actorMovementNode, &trees](const std::string &name, const NodeConfiguration &config) {
return std::make_unique<ActorMovement>(name, config, actorMovementNode, "/ActorPlugin/movement",[&trees](std::shared_ptr<const Movement::Feedback> feedback){
for (const auto &item : trees){
item->set("actorPos", feedback);
}
});
};
executor.add_node(actorMovementNode);
MinimalSubscriber<geometry_msgs::msg::Pose>::createAsThread("tree_get_currentPose", "currentPose",
[&trees](geometry_msgs::msg::Pose pose) {
auto sharedPose = std::make_shared<geometry_msgs::msg::Pose>(pose);

60
src/btree_nodes/src/nodes/ActorAnimation.cpp Normal file
View File

@ -0,0 +1,60 @@
//
// Created by bastian on 26.03.22.
//
#include "ActorAnimation.h"
ActorAnimation::ActorAnimation(const std::string &name, const BT::NodeConfiguration &config, std::shared_ptr<rclcpp::Node> node, const std::string &actionName)
: BT::StatefulActionNode(name, config) {
client = rclcpp_action::create_client<Animation>(node,actionName);
}
BT::PortsList ActorAnimation::providedPorts() {
return {
BT::InputPort<std::string>("animation"),
};
}
BT::NodeStatus ActorAnimation::onStart() {
std::cout << "started animation" << std::endl;
ros_actor_action_server_msgs::action::Animation::Goal goal;
auto res = getInput<std::string>("animation", goal.animation_name);
if (!res) {
std::cerr << "[ no animation name specified ]" << std::endl;
std::cout << res.error() << std::endl;
return BT::NodeStatus::FAILURE;
}
goal.animation_speed=1.0;
auto send_goal_options = Client<Animation>::SendGoalOptions();
send_goal_options.result_callback = [=](const ClientGoalHandle<Animation>::WrappedResult & parameter) {
mutex.lock();
if(parameter.code == ResultCode::SUCCEEDED){
result = BT::NodeStatus::SUCCESS;
}else{
result = BT::NodeStatus::FAILURE;
}
mutex.unlock();
};
client->async_send_goal(goal,send_goal_options);
return BT::NodeStatus::RUNNING;
}
BT::NodeStatus ActorAnimation::onRunning() {
mutex.lock();
auto status = result;
mutex.unlock();
return status;
}
void ActorAnimation::onHalted() {
std::cout << "halted animation" << std::endl;
client->async_cancel_all_goals();
}

40
src/btree_nodes/src/nodes/ActorAnimation.h Normal file
View File

@ -0,0 +1,40 @@
//
// Created by bastian on 26.03.22.
//
#ifndef BUILD_ACTORANIMATION_H
#define BUILD_ACTORANIMATION_H
#include <behaviortree_cpp_v3/action_node.h>
#include <behaviortree_cpp_v3/basic_types.h>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include <rclcpp_action/client_goal_handle.hpp>
#include <geometry_msgs/msg/pose.hpp>
#include <ros_actor_action_server_msgs/action/animation.hpp>
#include <memory>
#include <cstdlib>
using Animation = ros_actor_action_server_msgs::action::Animation;
using namespace rclcpp_action;
class ActorAnimation : public BT::StatefulActionNode {
public:
ActorAnimation(const std::string &name, const BT::NodeConfiguration &config, std::shared_ptr<rclcpp::Node> node, const std::string &actionName);
static BT::PortsList providedPorts();
BT::NodeStatus onStart() override;
BT::NodeStatus onRunning() override;
void onHalted() override;
private:
BT::NodeStatus result;
std::mutex mutex;
std::shared_ptr<Client<Animation>> client;
};
#endif //BUILD_ACTORANIMATION_H

18
src/btree_nodes/src/nodes/ActorMovement.cpp
View File

@ -4,18 +4,17 @@
#include "ActorMovement.h"
#include <memory>
ActorMovement::ActorMovement(const std::string &name, const BT::NodeConfiguration &config)
ActorMovement::ActorMovement(const std::string &name, const BT::NodeConfiguration &config, std::shared_ptr<rclcpp::Node> node, const std::string &actionName, std::function<void(std::shared_ptr<const Movement::Feedback>)> positionCallback)
: BT::StatefulActionNode(name, config) {
//this->client=client;
client = rclcpp_action::create_client<Movement>(node,actionName);
this->positionCallback = positionCallback;
}
BT::PortsList ActorMovement::providedPorts() {
return {
BT::OutputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("current"),
BT::InputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("target")
BT::InputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("target"),
BT::InputPort<std::string>("animation"),
};
}
@ -24,6 +23,7 @@ BT::NodeStatus ActorMovement::onStart() {
ros_actor_action_server_msgs::action::Movement::Goal goal;
std::shared_ptr<geometry_msgs::msg::Pose> target;
auto res = getInput<std::shared_ptr<geometry_msgs::msg::Pose>>("target", target);
if (!res) {
@ -37,9 +37,11 @@ BT::NodeStatus ActorMovement::onStart() {
goal.animation_speed=1.0;
goal.target = *target;
auto send_goal_options = Client<Movement>::SendGoalOptions();
send_goal_options.feedback_callback = [=](__attribute__((unused)) std::shared_ptr<ClientGoalHandle<Movement>> goal_handle, const std::shared_ptr<const Movement::Feedback> feedback) {
setOutput("current",std::make_shared<geometry_msgs::msg::Pose>(feedback->current));
positionCallback(feedback);
};
send_goal_options.result_callback = [=](const ClientGoalHandle<Movement>::WrappedResult & parameter) {
mutex.lock();
@ -51,8 +53,6 @@ BT::NodeStatus ActorMovement::onStart() {
mutex.unlock();
};
auto node = rclcpp::Node::make_shared("actorMovementPublisher");
client = rclcpp_action::create_client<Movement>(node,"/actorPlugin/movement");
client->async_send_goal(goal,send_goal_options);
return BT::NodeStatus::RUNNING;

5
src/btree_nodes/src/nodes/ActorMovement.h
View File

@ -20,12 +20,10 @@ using namespace rclcpp_action;
class ActorMovement : public BT::StatefulActionNode {
public:
ActorMovement(const std::string &name, const BT::NodeConfiguration &config);
ActorMovement(const std::string &name, const BT::NodeConfiguration &config, std::shared_ptr<rclcpp::Node> node, const std::string &actionName, std::function<void(std::shared_ptr<const Movement::Feedback>)> positionCallback);
static BT::PortsList providedPorts();
std::shared_ptr<geometry_msgs::msg::Pose> target;
BT::NodeStatus onStart() override;
BT::NodeStatus onRunning() override;
@ -33,6 +31,7 @@ class ActorMovement : public BT::StatefulActionNode {
void onHalted() override;
private:
std::function<void(std::shared_ptr<const Movement::Feedback>)> positionCallback;
BT::NodeStatus result;
std::mutex mutex;
std::shared_ptr<Client<Movement>> client;

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

3
src/ros_actor_action_server/src/Server.cpp
View File

@ -86,6 +86,7 @@ int main(int argc, char **argv) {
currentAnimationGoalPtr->publish_feedback(feedback);
}else{
currentAnimationGoalPtr->succeed(std::make_shared<ros_actor_action_server_msgs::action::Animation_Result>());
currentAnimationGoalPtr == nullptr;
}
}
if(currentAction.state==MOVEMENT && currentMovementGoalPtr!=nullptr){
@ -95,10 +96,10 @@ int main(int argc, char **argv) {
currentMovementGoalPtr->publish_feedback(feedback);
}else{
currentMovementGoalPtr->succeed(std::make_shared<ros_actor_action_server_msgs::action::Movement_Result>());
currentMovementGoalPtr == nullptr;
}
}
currentFeedback = newFeedback;
std::cout << "Got feedback, State: " << currentFeedback.state << " | Progress: " << currentFeedback.progress << std::endl;
mutex.unlock();
}
}).detach();