Unify blackboard and fix segfault
This commit is contained in:
Bastian Hofmann
parent
3be6dee6b8
commit
b102cd405f
@ -14,18 +14,25 @@
|
||||
<Action ID="GenerateXYPose" area="{warningArea}" pose="{actorTarget}"/>
|
||||
<Action ID="GenerateXYPose" area="{unsafeArea}" pose="{actorTarget}"/>
|
||||
</Control>
|
||||
<Action ID="MoveActorToTarget" current="{actorPos}" target="{actorTarget}"/>
|
||||
<Action ID="ActorMovement" animation="walk" target="{actorTarget}"/>
|
||||
</Sequence>
|
||||
</ReactiveSequence>
|
||||
<Sequence>
|
||||
<Action ID="GenerateXYPose" area="{unsafeArea}" pose="{actorTarget}"/>
|
||||
<Action ID="MoveActorToTarget" current="{actorPos}" target="{actorTarget}"/>
|
||||
<Action ID="ActorMovement" animation="walk" target="{actorTarget}"/>
|
||||
<Action ID="SetCalledTo" state="false"/>
|
||||
</Sequence>
|
||||
</Fallback>
|
||||
</BehaviorTree>
|
||||
<!-- ////////// -->
|
||||
<TreeNodesModel>
|
||||
<Action ID="ActorAnimation">
|
||||
<input_port name="animation">Name of animation to play</input_port>
|
||||
</Action>
|
||||
<Action ID="ActorMovement">
|
||||
<input_port name="animation">Name of animation to play</input_port>
|
||||
<input_port name="target">Pose to move to</input_port>
|
||||
</Action>
|
||||
<Condition ID="IsCalled"/>
|
||||
<Action ID="SetCalledTo">
|
||||
<input_port name="state">state to set called to</input_port>
|
||||
|
||||
@ -28,6 +28,13 @@
|
||||
</BehaviorTree>
|
||||
<!-- ////////// -->
|
||||
<TreeNodesModel>
|
||||
<Action ID="ActorAnimation">
|
||||
<input_port name="animation">Name of animation to play</input_port>
|
||||
</Action>
|
||||
<Action ID="ActorMovement">
|
||||
<input_port name="animation">Name of animation to play</input_port>
|
||||
<input_port name="target">Pose to move to</input_port>
|
||||
</Action>
|
||||
<Condition ID="IsCalled"/>
|
||||
<Action ID="SetCalledTo">
|
||||
<input_port name="state">state to set called to</input_port>
|
||||
@ -40,10 +47,6 @@
|
||||
<input_port name="area">Bounds to check in</input_port>
|
||||
<input_port name="pose">Position of object</input_port>
|
||||
</Condition>
|
||||
<Action ID="MoveActorToTarget">
|
||||
<input_port name="current">Current actor position</input_port>
|
||||
<input_port name="target">Target to move actor to</input_port>
|
||||
</Action>
|
||||
<Action ID="OffsetPose">
|
||||
<input_port name="input">initial position as Pose</input_port>
|
||||
<input_port name="offset">offset as a Point</input_port>
|
||||
|
||||
@ -1,4 +1,5 @@
|
||||
#include "Area.h"
|
||||
#include <behaviortree_cpp_v3/blackboard.h>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <random>
|
||||
#include <rclcpp/utilities.hpp>
|
||||
@ -19,16 +20,14 @@
|
||||
#include <chrono>
|
||||
#include <thread>
|
||||
|
||||
|
||||
|
||||
int main(int argc, char **argv) {
|
||||
rclcpp::init(argc, argv);
|
||||
rclcpp::NodeOptions node_options;
|
||||
node_options.automatically_declare_parameters_from_overrides(true);
|
||||
|
||||
auto mainNode = rclcpp::Node::make_shared("tree_general_node", node_options);
|
||||
auto blackboardVector = std::vector<BT::Blackboard::Ptr>();
|
||||
|
||||
auto blackboard = Blackboard::create();
|
||||
auto blackboardMutex = std::mutex();
|
||||
|
||||
rclcpp::executors::SingleThreadedExecutor executor;
|
||||
|
||||
@ -108,16 +107,19 @@ int main(int argc, char **argv) {
|
||||
NodeBuilder builderActorAnimation = [&actorAnimationNode](const std::string &name, const NodeConfiguration &config) {
|
||||
return std::make_unique<ActorAnimation>(name, config, actorAnimationNode, "/ActorPlugin/animation");
|
||||
};
|
||||
factory.registerBuilder<ActorAnimation>("ActorAnimation",builderActorAnimation);
|
||||
executor.add_node(actorAnimationNode);
|
||||
|
||||
auto actorMovementNode = rclcpp::Node::make_shared("actorMovementNode",node_options);
|
||||
NodeBuilder builderActorMovement = [&actorMovementNode, &blackboardVector](const std::string &name, const NodeConfiguration &config) {
|
||||
return std::make_unique<ActorMovement>(name, config, actorMovementNode, "/ActorPlugin/movement",[&blackboardVector](std::shared_ptr<const Movement::Feedback> feedback){
|
||||
for (auto blackboard : blackboardVector){
|
||||
blackboard->set("actorPos", feedback);
|
||||
}
|
||||
NodeBuilder builderActorMovement = [&actorMovementNode, &blackboard, &blackboardMutex](const std::string &name, const NodeConfiguration &config) {
|
||||
return std::make_unique<ActorMovement>(name, config, actorMovementNode, "/ActorPlugin/movement",[&blackboard,&blackboardMutex](std::shared_ptr<const Movement::Feedback> feedback){
|
||||
blackboardMutex.lock();
|
||||
blackboard->set("actorPos", std::make_shared<geometry_msgs::msg::Pose>(feedback->current));
|
||||
blackboardMutex.unlock();
|
||||
});
|
||||
};
|
||||
factory.registerBuilder<ActorMovement>("ActorMovement",builderActorMovement);
|
||||
executor.add_node(actorMovementNode);
|
||||
|
||||
bool called = false;
|
||||
auto IsCalled = [&called](__attribute__((unused)) TreeNode& parent_node) -> NodeStatus{
|
||||
@ -138,24 +140,19 @@ int main(int argc, char **argv) {
|
||||
|
||||
std::cout << "Creating tree." << std::endl;
|
||||
|
||||
Tree actorTree = factory.createTreeFromFile("/home/ros/workspace/src/btree_nodes/actorTree.xml");
|
||||
Tree robotTree = factory.createTreeFromFile("/home/ros/workspace/src/btree_nodes/robotTree.xml");
|
||||
|
||||
blackboardVector.push_back(actorTree.rootBlackboard());
|
||||
blackboardVector.push_back(robotTree.rootBlackboard());
|
||||
Tree actorTree = factory.createTreeFromFile("/home/ros/workspace/src/btree_nodes/actorTree.xml",blackboard);
|
||||
Tree robotTree = factory.createTreeFromFile("/home/ros/workspace/src/btree_nodes/robotTree.xml",blackboard);
|
||||
|
||||
auto init = std::make_shared<geometry_msgs::msg::Pose>();
|
||||
init->position.x = 6.0;
|
||||
init->position.y = 6.0;
|
||||
|
||||
for (auto blackboard : blackboardVector){
|
||||
blackboard->set("safeArea",safeArea);
|
||||
blackboard->set("warningArea",warningArea);
|
||||
blackboard->set("unsafeArea",unsafeArea);
|
||||
blackboard->set("negativeYTable",negativeYTable);
|
||||
blackboard->set("positiveYTable",positiveYTable);
|
||||
blackboard->set("actorPos", init);
|
||||
}
|
||||
blackboard->set("safeArea",safeArea);
|
||||
blackboard->set("warningArea",warningArea);
|
||||
blackboard->set("unsafeArea",unsafeArea);
|
||||
blackboard->set("negativeYTable",negativeYTable);
|
||||
blackboard->set("positiveYTable",positiveYTable);
|
||||
blackboard->set("actorPos", init);
|
||||
|
||||
std::cout << "Starting subscriber." << std::endl;
|
||||
|
||||
@ -166,13 +163,13 @@ int main(int argc, char **argv) {
|
||||
std::cout << "Executor stopped." << std::endl;
|
||||
}).detach();
|
||||
|
||||
executor.add_node(actorMovementNode);
|
||||
|
||||
std::cout << "Looping." << std::endl;
|
||||
|
||||
while (rclcpp::ok()) {
|
||||
blackboardMutex.lock();
|
||||
actorTree.tickRoot();
|
||||
robotTree.tickRoot();
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(20));
|
||||
blackboardMutex.unlock();
|
||||
}
|
||||
|
||||
std::cout << "End." << std::endl;
|
||||
|
||||
@ -3,6 +3,7 @@
|
||||
//
|
||||
|
||||
#include "ActorMovement.h"
|
||||
#include <behaviortree_cpp_v3/basic_types.h>
|
||||
|
||||
|
||||
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)
|
||||
@ -37,30 +38,40 @@ 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) {
|
||||
send_goal_options.feedback_callback = [&](__attribute__((unused)) std::shared_ptr<ClientGoalHandle<Movement>> goal_handle, std::shared_ptr<const Movement::Feedback> feedback) {
|
||||
positionCallback(feedback);
|
||||
};
|
||||
send_goal_options.result_callback = [=](const ClientGoalHandle<Movement>::WrappedResult & parameter) {
|
||||
send_goal_options.result_callback = [&](ClientGoalHandle<Movement>::WrappedResult parameter) {
|
||||
mutex.lock();
|
||||
if(parameter.code == ResultCode::SUCCEEDED){
|
||||
std::cout << "Success?" << std::endl;
|
||||
result = BT::NodeStatus::SUCCESS;
|
||||
}else{
|
||||
std::cout << "Failure?" << std::endl;
|
||||
result = BT::NodeStatus::FAILURE;
|
||||
}
|
||||
mutex.unlock();
|
||||
};
|
||||
send_goal_options.goal_response_callback = [&](ClientGoalHandle<Movement>::SharedPtr parameter){
|
||||
if(!parameter){
|
||||
std::cout << "ActorMovement rejected by server." << std::endl;
|
||||
result = BT::NodeStatus::FAILURE;
|
||||
}
|
||||
};
|
||||
|
||||
client->async_send_goal(goal,send_goal_options);
|
||||
|
||||
result = BT::NodeStatus::RUNNING;
|
||||
return BT::NodeStatus::RUNNING;
|
||||
}
|
||||
|
||||
BT::NodeStatus ActorMovement::onRunning() {
|
||||
mutex.lock();
|
||||
auto status = result;
|
||||
if(result != BT::NodeStatus::RUNNING){
|
||||
result = BT::NodeStatus::IDLE;
|
||||
}
|
||||
mutex.unlock();
|
||||
return status;
|
||||
}
|
||||
@ -68,4 +79,5 @@ BT::NodeStatus ActorMovement::onRunning() {
|
||||
void ActorMovement::onHalted() {
|
||||
std::cout << "halted move" << std::endl;
|
||||
client->async_cancel_all_goals();
|
||||
result = BT::NodeStatus::IDLE;
|
||||
}
|
||||
|
||||
@ -4,6 +4,8 @@
|
||||
|
||||
#include "MoveConnection.h"
|
||||
|
||||
MoveConnection::MoveConnection(const std::shared_ptr<rclcpp::Node> &node, const std::string &planningGroup) : moveGroup(node,planningGroup) {}
|
||||
|
||||
void MoveConnection::planAndExecute(const std::shared_ptr<geometry_msgs::msg::Pose> &pose,
|
||||
const std::function<void(bool)> &callback) {
|
||||
std::cout<<"planAndExecute."<<std::endl;
|
||||
@ -56,10 +58,6 @@ void MoveConnection::setSpeedMultiplier(const double speed){
|
||||
}
|
||||
}
|
||||
|
||||
MoveConnection::MoveConnection(const std::shared_ptr<rclcpp::Node> &node, const std::string &planningGroup) : moveGroup(node,planningGroup) {
|
||||
|
||||
}
|
||||
|
||||
void MoveConnection::stop() {
|
||||
std::cout<<"Stopping MoveConnection"<<std::endl;
|
||||
cancelled = true;
|
||||
|
||||
@ -3,20 +3,35 @@
|
||||
//
|
||||
|
||||
#include "Server.hpp"
|
||||
|
||||
#include <condition_variable>
|
||||
|
||||
using namespace ros_actor_message_queue_msgs;
|
||||
|
||||
static const char* feedbackName;
|
||||
static const char* actionName;
|
||||
|
||||
static std::mutex feedbackMutex;
|
||||
static std::mutex rosMutex;
|
||||
|
||||
static std::condition_variable stateCondition;
|
||||
|
||||
static ActionMessage cancelAction;
|
||||
|
||||
void sendAction(mqd_t queue, ActionMessage *message) {
|
||||
std::cout << "Sending action..." << std::endl;
|
||||
mq_send(queue, (const char *)message, sizeof(ActionMessage), 0);
|
||||
std::cout << "Sent action" << std::endl;
|
||||
}
|
||||
|
||||
void waitForState(FeedbackMessage *currentFeedback,ActorPluginState state){
|
||||
std::unique_lock lock(feedbackMutex);
|
||||
stateCondition.wait(lock,[¤tFeedback,state]{return currentFeedback->state==state;});
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
int main(int argc, char **argv) {
|
||||
cancelAction.state = IDLE;
|
||||
rclcpp::init(argc, argv);
|
||||
|
||||
auto node = rclcpp::Node::make_shared("ActorPlugin", "ActorPlugin");
|
||||
@ -64,90 +79,89 @@ int main(int argc, char **argv) {
|
||||
|
||||
FeedbackMessage currentFeedback;
|
||||
ActionMessage currentAction;
|
||||
std::mutex mutex;
|
||||
|
||||
currentAction.state = IDLE;
|
||||
|
||||
std::thread([feedbackQueue, ¤tAnimationGoalPtr, ¤tMovementGoalPtr, ¤tFeedback, ¤tAction, &mutex] {
|
||||
std::thread([feedbackQueue, ¤tAnimationGoalPtr, ¤tMovementGoalPtr, ¤tFeedback, ¤tAction] {
|
||||
while (true) {
|
||||
FeedbackMessage newFeedback;
|
||||
if(mq_receive(feedbackQueue, (char *)&newFeedback, sizeof(ros_actor_message_queue_msgs::FeedbackMessage), nullptr)==-1){
|
||||
perror(nullptr);
|
||||
}
|
||||
mutex.lock();
|
||||
if (newFeedback.state != currentFeedback.state) {
|
||||
currentAction.state = newFeedback.state;
|
||||
feedbackMutex.lock();
|
||||
if(newFeedback.state==ANIMATION && currentAnimationGoalPtr!=nullptr){
|
||||
auto feedback = std::make_shared<ros_actor_action_server_msgs::action::Animation_Feedback>();
|
||||
feedback->progress = newFeedback.progress;
|
||||
currentAnimationGoalPtr->publish_feedback(feedback);
|
||||
}
|
||||
if(currentAction.state==ANIMATION && currentAnimationGoalPtr!=nullptr){
|
||||
if(newFeedback.progress<1){
|
||||
auto feedback = std::make_shared<ros_actor_action_server_msgs::action::Animation_Feedback>();
|
||||
feedback->progress = newFeedback.progress;
|
||||
currentAnimationGoalPtr->publish_feedback(feedback);
|
||||
}else{
|
||||
if(newFeedback.state==MOVEMENT && currentMovementGoalPtr!=nullptr){
|
||||
auto feedback = std::make_shared<ros_actor_action_server_msgs::action::Movement_Feedback>();
|
||||
feedback->current = newFeedback.current;
|
||||
currentMovementGoalPtr->publish_feedback(feedback);
|
||||
}
|
||||
if (newFeedback.state==IDLE) {
|
||||
if(currentFeedback.state==ANIMATION && currentAnimationGoalPtr!=nullptr){
|
||||
currentAnimationGoalPtr->succeed(std::make_shared<ros_actor_action_server_msgs::action::Animation_Result>());
|
||||
currentAnimationGoalPtr = nullptr;
|
||||
}
|
||||
}
|
||||
if(currentAction.state==MOVEMENT && currentMovementGoalPtr!=nullptr){
|
||||
if(newFeedback.progress<1){
|
||||
auto feedback = std::make_shared<ros_actor_action_server_msgs::action::Movement_Feedback>();
|
||||
feedback->current = newFeedback.current;
|
||||
currentMovementGoalPtr->publish_feedback(feedback);
|
||||
}else{
|
||||
if(currentFeedback.state==MOVEMENT && currentMovementGoalPtr!=nullptr){
|
||||
currentMovementGoalPtr->succeed(std::make_shared<ros_actor_action_server_msgs::action::Movement_Result>());
|
||||
currentMovementGoalPtr = nullptr;
|
||||
}
|
||||
}
|
||||
currentFeedback = newFeedback;
|
||||
mutex.unlock();
|
||||
feedbackMutex.unlock();
|
||||
stateCondition.notify_all();
|
||||
}
|
||||
}).detach();
|
||||
|
||||
auto animationServer = rclcpp_action::create_server<action::Animation>(
|
||||
node, "animation",
|
||||
[¤tAction, &mutex](__attribute__((unused)) const rclcpp_action::GoalUUID goalUuid, const AnimationGoalPtr &animationGoal) {
|
||||
[¤tAction, ¤tFeedback](__attribute__((unused)) const rclcpp_action::GoalUUID goalUuid, const AnimationGoalPtr &animationGoal) {
|
||||
// igndbg << "goal" << shared_mmap->currentState << std::endl;
|
||||
mutex.lock();
|
||||
rosMutex.lock();
|
||||
|
||||
if (currentAction.state == IDLE) {
|
||||
if (currentFeedback.state == IDLE) {
|
||||
std::cout << "Accepting..." << std::endl;
|
||||
|
||||
strcpy(currentAction.animationName, animationGoal->animation_name.data());
|
||||
|
||||
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
|
||||
} else {
|
||||
std::cout << "Rejecting..." << std::endl;
|
||||
mutex.unlock();
|
||||
rosMutex.unlock();
|
||||
return rclcpp_action::GoalResponse::REJECT;
|
||||
}
|
||||
},
|
||||
[actionQueue, ¤tAction, &mutex](__attribute__((unused)) const AnimationServerGoalPtr &animationGoalPtr) {
|
||||
if (currentAction.state == IDLE) {
|
||||
[actionQueue, ¤tFeedback](__attribute__((unused)) const AnimationServerGoalPtr &animationGoalPtr) {
|
||||
if (currentFeedback.state == IDLE) {
|
||||
return rclcpp_action::CancelResponse::REJECT;
|
||||
} else {
|
||||
mutex.lock();
|
||||
rosMutex.lock();
|
||||
sendAction(actionQueue, &cancelAction);
|
||||
|
||||
currentAction.state = IDLE;
|
||||
sendAction(actionQueue, ¤tAction);
|
||||
|
||||
mutex.unlock();
|
||||
waitForState(¤tFeedback, IDLE);
|
||||
rosMutex.unlock();
|
||||
|
||||
return rclcpp_action::CancelResponse::ACCEPT;
|
||||
}
|
||||
},
|
||||
[actionQueue, ¤tAction, ¤tAnimationGoalPtr, &mutex](const AnimationServerGoalPtr &animationGoalPtr) {
|
||||
[actionQueue, ¤tAction, ¤tAnimationGoalPtr, ¤tFeedback](const AnimationServerGoalPtr &animationGoalPtr) {
|
||||
feedbackMutex.lock();
|
||||
currentAnimationGoalPtr = animationGoalPtr;
|
||||
feedbackMutex.unlock();
|
||||
currentAction.state = ANIMATION;
|
||||
sendAction(actionQueue, ¤tAction);
|
||||
|
||||
mutex.unlock();
|
||||
waitForState(¤tFeedback, ANIMATION);
|
||||
rosMutex.unlock();
|
||||
});
|
||||
|
||||
auto movementServer = rclcpp_action::create_server<action::Movement>(
|
||||
node, "movement",
|
||||
[¤tAction, &mutex](__attribute__((unused)) const rclcpp_action::GoalUUID goalUuid, const MovementGoalPtr &movementGoal) {
|
||||
mutex.lock();
|
||||
if (currentAction.state == IDLE) {
|
||||
[¤tAction, ¤tFeedback](__attribute__((unused)) const rclcpp_action::GoalUUID goalUuid, const MovementGoalPtr &movementGoal) {
|
||||
rosMutex.lock();
|
||||
if (currentFeedback.state == IDLE) {
|
||||
std::cout << "Accepting..." << std::endl;
|
||||
|
||||
currentAction.target = movementGoal->target;
|
||||
currentAction.animationDistance = movementGoal->animation_distance;
|
||||
strcpy(currentAction.animationName, movementGoal->animation_name.data());
|
||||
@ -155,34 +169,35 @@ int main(int argc, char **argv) {
|
||||
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
|
||||
} else {
|
||||
std::cout << "Rejecting..." << std::endl;
|
||||
mutex.unlock();
|
||||
rosMutex.unlock();
|
||||
return rclcpp_action::GoalResponse::REJECT;
|
||||
}
|
||||
},
|
||||
[actionQueue, ¤tAction, &mutex](__attribute__((unused)) const MovementServerGoalPtr &movementGoalPtr) {
|
||||
if (currentAction.state == IDLE) {
|
||||
[actionQueue, ¤tFeedback](__attribute__((unused)) const MovementServerGoalPtr &movementGoalPtr) {
|
||||
if (currentFeedback.state == IDLE) {
|
||||
return rclcpp_action::CancelResponse::REJECT;
|
||||
} else {
|
||||
mutex.lock();
|
||||
rosMutex.lock();
|
||||
sendAction(actionQueue, &cancelAction);
|
||||
|
||||
currentAction.state = IDLE;
|
||||
sendAction(actionQueue, ¤tAction);
|
||||
waitForState(¤tFeedback, IDLE);
|
||||
rosMutex.unlock();
|
||||
|
||||
mutex.unlock();
|
||||
return rclcpp_action::CancelResponse::ACCEPT;
|
||||
}
|
||||
},
|
||||
[actionQueue, ¤tAction, ¤tMovementGoalPtr, &mutex](const MovementServerGoalPtr &movementGoalPtr) {
|
||||
[actionQueue, ¤tAction, ¤tFeedback, ¤tMovementGoalPtr](const MovementServerGoalPtr &movementGoalPtr) {
|
||||
feedbackMutex.lock();
|
||||
currentMovementGoalPtr = movementGoalPtr;
|
||||
feedbackMutex.unlock();
|
||||
currentAction.state = MOVEMENT;
|
||||
sendAction(actionQueue, ¤tAction);
|
||||
|
||||
mutex.unlock();
|
||||
waitForState(¤tFeedback, MOVEMENT);
|
||||
rosMutex.unlock();
|
||||
});
|
||||
|
||||
while(rclcpp::ok()){
|
||||
rclcpp::spin(node);
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
Loading…
x
Reference in New Issue
Block a user