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

Unify blackboard and fix segfault

Browse Source
This commit is contained in:
Bastian Hofmann 2023-03-15 09:11:40 +00:00
parent 3be6dee6b8
commit b102cd405f
6 changed files with 125 additions and 93 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
src/btree_nodes/actorTree.xml
View File

@ -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>

11
src/btree_nodes/robotTree.xml
View File

@ -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>

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

@ -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;

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

@ -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;
}

6
src/btree_nodes/src/nodes/MoveConnection.cpp
View File

@ -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;

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

@ -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,[&currentFeedback,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, &currentAnimationGoalPtr, &currentMovementGoalPtr, &currentFeedback, &currentAction, &mutex] {
std::thread([feedbackQueue, &currentAnimationGoalPtr, &currentMovementGoalPtr, &currentFeedback, &currentAction] {
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",
[&currentAction, &mutex](__attribute__((unused)) const rclcpp_action::GoalUUID goalUuid, const AnimationGoalPtr &animationGoal) {
[&currentAction, &currentFeedback](__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, &currentAction, &mutex](__attribute__((unused)) const AnimationServerGoalPtr &animationGoalPtr) {
if (currentAction.state == IDLE) {
[actionQueue, &currentFeedback](__attribute__((unused)) const AnimationServerGoalPtr &animationGoalPtr) {
if (currentFeedback.state == IDLE) {
return rclcpp_action::CancelResponse::REJECT;
} else {
mutex.lock();
currentAction.state = IDLE;
sendAction(actionQueue, &currentAction);
mutex.unlock();
rosMutex.lock();
sendAction(actionQueue, &cancelAction);
waitForState(&currentFeedback, IDLE);
rosMutex.unlock();
return rclcpp_action::CancelResponse::ACCEPT;
}
},
[actionQueue, &currentAction, &currentAnimationGoalPtr, &mutex](const AnimationServerGoalPtr &animationGoalPtr) {
[actionQueue, &currentAction, &currentAnimationGoalPtr, &currentFeedback](const AnimationServerGoalPtr &animationGoalPtr) {
feedbackMutex.lock();
currentAnimationGoalPtr = animationGoalPtr;
feedbackMutex.unlock();
currentAction.state = ANIMATION;
sendAction(actionQueue, &currentAction);
mutex.unlock();
waitForState(&currentFeedback, ANIMATION);
rosMutex.unlock();
});
auto movementServer = rclcpp_action::create_server<action::Movement>(
node, "movement",
[&currentAction, &mutex](__attribute__((unused)) const rclcpp_action::GoalUUID goalUuid, const MovementGoalPtr &movementGoal) {
mutex.lock();
if (currentAction.state == IDLE) {
[&currentAction, &currentFeedback](__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, &currentAction, &mutex](__attribute__((unused)) const MovementServerGoalPtr &movementGoalPtr) {
if (currentAction.state == IDLE) {
[actionQueue, &currentFeedback](__attribute__((unused)) const MovementServerGoalPtr &movementGoalPtr) {
if (currentFeedback.state == IDLE) {
return rclcpp_action::CancelResponse::REJECT;
} else {
mutex.lock();
currentAction.state = IDLE;
sendAction(actionQueue, &currentAction);
mutex.unlock();
rosMutex.lock();
sendAction(actionQueue, &cancelAction);
waitForState(&currentFeedback, IDLE);
rosMutex.unlock();
return rclcpp_action::CancelResponse::ACCEPT;
}
},
[actionQueue, &currentAction, &currentMovementGoalPtr, &mutex](const MovementServerGoalPtr &movementGoalPtr) {
[actionQueue, &currentAction, &currentFeedback, &currentMovementGoalPtr](const MovementServerGoalPtr &movementGoalPtr) {
feedbackMutex.lock();
currentMovementGoalPtr = movementGoalPtr;
feedbackMutex.unlock();
currentAction.state = MOVEMENT;
sendAction(actionQueue, &currentAction);
mutex.unlock();
waitForState(&currentFeedback, MOVEMENT);
rosMutex.unlock();
});
while(rclcpp::ok()){
rclcpp::spin(node);
}
}