Skip to content

Commit 44addba

Browse files
jocacacejocacace
committed
server output integration
1 parent a536944 commit 44addba

File tree

9 files changed

+181
-97
lines changed

9 files changed

+181
-97
lines changed

‎Docker/Dockerfile‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -4,6 +4,8 @@ FROM osrf/ros:jazzy-desktop-full
44
RUN apt-get update && apt-get upgrade -y
55
RUN DEBIAN_FRONTEND=noninteractive apt-get install software-properties-common ros-dev-tools python3-colcon-common-extensions -y
66
RUN DEBIAN_FRONTEND=noninteractive apt-get install ros-${ROS_DISTRO}-angles ros-${ROS_DISTRO}-tf2 ros-${ROS_DISTRO}-tf2-ros ros-${ROS_DISTRO}-tf2-geometry-msgs
7+
RUN DEBIAN_FRONTEND=noninteractive apt-get install ros-${ROS_DISTRO}-launch-testing -y
8+
RUN DEBIAN_FRONTEND=noninteractive apt-get install gedit -y
79
RUN DEBIAN_FRONTEND=noninteractive apt-get install libboost-filesystem1.83-dev
810
# BT.ROS2 related deps
911
RUN DEBIAN_FRONTEND=noninteractive apt-get install ros-${ROS_DISTRO}-generate-parameter-library -y

‎behaviortree_ros2/CMakeLists.txt‎

Lines changed: 18 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -23,7 +23,7 @@ set(THIS_PACKAGE_DEPS
2323
tf2
2424
tf2_geometry_msgs
2525
tf2_ros
26-
)
26+
)
2727

2828
find_package(ament_cmake REQUIRED)
2929

@@ -51,10 +51,11 @@ target_link_libraries(${PROJECT_NAME} bt_executor_parameters)
5151
# Executable to generate xml from .so plugin for "simple" and ros2 based bt plugins .so
5252
add_executable(bt_ros_plugins_to_xml src/tools/bt_ros_plugins_to_xml.cpp )
5353
include_directories(${behaviortree_cpp_INCLUDE_DIRS} PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include)
54+
5455
ament_target_dependencies(bt_ros_plugins_to_xml behaviortree_cpp rclcpp)
5556

5657

57-
add_executable(tree_execution_client src/tools/client/tree_execution_client.cpp)
58+
add_executable(tree_execution_client src/tools/client/main.cpp src/tools/client/tree_execution_client.cpp)
5859
ament_target_dependencies(tree_execution_client ${THIS_PACKAGE_DEPS})
5960
ament_target_dependencies(tree_execution_client behaviortree_cpp rclcpp)
6061

@@ -142,4 +143,19 @@ ament_index_register_resource(
142143
ament_export_include_directories(include)
143144
ament_export_libraries(${PROJECT_NAME})
144145

146+
147+
if(BUILD_TESTING)
148+
149+
find_package(ament_cmake_gtest REQUIRED)
150+
ament_add_gtest(bt_ros2_tests test/bt_ros2_tests.cpp src/tools/client/tree_execution_client.cpp)
151+
target_include_directories(bt_ros2_tests PUBLIC
152+
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
153+
$<INSTALL_INTERFACE:include>
154+
)
155+
ament_target_dependencies(bt_ros2_tests ${THIS_PACKAGE_DEPS})
156+
ament_target_dependencies(bt_ros2_tests behaviortree_cpp rclcpp)
157+
158+
159+
endif()
160+
145161
ament_package()

‎behaviortree_ros2/include/behaviortree_ros2/tools/client/tree_execution_client.hpp‎

Lines changed: 22 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,22 @@
1+
#ifndef TREE_EXECUTION_CLIENT_HPP
2+
#define TREE_EXECUTION_CLIENT_HPP
3+
4+
#include <rclcpp/rclcpp.hpp>
5+
#include <rclcpp_action/rclcpp_action.hpp>
6+
#include <btcpp_ros2_interfaces/action/execute_tree.hpp>
7+
8+
class TreeExecutionClient : public rclcpp::Node
9+
{
10+
public:
11+
using ExecuteTree = btcpp_ros2_interfaces::action::ExecuteTree;
12+
using GoalHandleExecuteTree = rclcpp_action::ClientGoalHandle<ExecuteTree>;
13+
14+
explicit TreeExecutionClient(const std::string &target_tree, const rclcpp::NodeOptions &options = rclcpp::NodeOptions());
15+
16+
private:
17+
rclcpp_action::Client<ExecuteTree>::SharedPtr action_client_;
18+
19+
void send_goal(const std::string &target_tree);
20+
};
21+
22+
#endif // TREE_EXECUTION_CLIENT_HPP

‎behaviortree_ros2/package.xml‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -28,6 +28,7 @@
2828
<depend>tf2</depend>
2929
<depend>tf2_geometry_msgs</depend>
3030
<depend>tf2_ros</depend>
31+
3132
<export>
3233
<build_type>ament_cmake</build_type>
3334
</export>

‎behaviortree_ros2/src/tools/client/main.cpp‎

Lines changed: 17 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,17 @@
1+
#include "behaviortree_ros2/tools/client/tree_execution_client.hpp"
2+
3+
int main(int argc, char **argv)
4+
{
5+
rclcpp::init(argc, argv);
6+
7+
if (argc < 2) {
8+
RCLCPP_ERROR(rclcpp::get_logger("tree_execution_client"), "Usage: ros2 run package_name node_name <target_tree_name>");
9+
return 1;
10+
}
11+
12+
std::string target_tree = argv[1];
13+
auto node = std::make_shared<TreeExecutionClient>(target_tree);
14+
rclcpp::spin(node);
15+
rclcpp::shutdown();
16+
return 0;
17+
}

‎behaviortree_ros2/src/tools/client/tree_execution_client.cpp‎

Lines changed: 65 additions & 89 deletions
Original file line numberDiff line numberDiff line change
@@ -1,102 +1,78 @@
1-
#include <rclcpp/rclcpp.hpp>
2-
#include <rclcpp_action/rclcpp_action.hpp>
3-
#include <btcpp_ros2_interfaces/action/execute_tree.hpp>
1+
#include "behaviortree_ros2/tools/client/tree_execution_client.hpp"
42

5-
class TreeExecutionClient : public rclcpp::Node
3+
TreeExecutionClient::TreeExecutionClient(const std::string &target_tree, const rclcpp::NodeOptions &options)
4+
: Node("tree_execution_client", options)
65
{
7-
public:
8-
using ExecuteTree = btcpp_ros2_interfaces::action::ExecuteTree;
9-
using GoalHandleExecuteTree = rclcpp_action::ClientGoalHandle<ExecuteTree>;
6+
action_client_ = rclcpp_action::create_client<ExecuteTree>(this, "/bt_action_server_example");
107

11-
explicit TreeExecutionClient(const std::string& target_tree, const rclcpp::NodeOptions &options = rclcpp::NodeOptions())
12-
: Node("tree_execution_client", options)
8+
if (!action_client_->wait_for_action_server(std::chrono::seconds(10)))
139
{
14-
action_client_ = rclcpp_action::create_client<ExecuteTree>(this, "/bt_action_server_example");
15-
16-
if (!action_client_->wait_for_action_server(std::chrono::seconds(10)))
17-
{
18-
RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting");
19-
rclcpp::shutdown();
20-
return;
21-
}
22-
23-
if (target_tree.empty())
24-
{
25-
RCLCPP_ERROR(this->get_logger(), "Target tree name must be specified");
26-
rclcpp::shutdown();
27-
return;
28-
}
10+
RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting");
11+
rclcpp::shutdown();
12+
return;
13+
}
2914

30-
send_goal(target_tree);
15+
if (target_tree.empty())
16+
{
17+
RCLCPP_ERROR(this->get_logger(), "Target tree name must be specified");
18+
rclcpp::shutdown();
19+
return;
3120
}
3221

33-
private:
34-
rclcpp_action::Client<ExecuteTree>::SharedPtr action_client_;
22+
send_goal(target_tree);
23+
}
3524

36-
void send_goal(const std::string &target_tree)
37-
{
38-
auto goal_msg = ExecuteTree::Goal();
39-
goal_msg.target_tree = target_tree;
40-
goal_msg.payload = ""; // Optional payload can be set here
25+
void TreeExecutionClient::send_goal(const std::string &target_tree)
26+
{
27+
auto goal_msg = ExecuteTree::Goal();
28+
goal_msg.target_tree = target_tree;
29+
goal_msg.payload = ""; // Optional payload
4130

42-
RCLCPP_INFO(this->get_logger(), "Sending goal to execute tree: %s", target_tree.c_str());
31+
RCLCPP_INFO(this->get_logger(), "Sending goal to execute tree: %s", target_tree.c_str());
4332

44-
auto send_goal_options = rclcpp_action::Client<ExecuteTree>::SendGoalOptions();
45-
46-
send_goal_options.goal_response_callback =
47-
[this](GoalHandleExecuteTree::SharedPtr goal_handle) {
48-
if (!goal_handle)
49-
{
50-
RCLCPP_ERROR(this->get_logger(), "Goal was rejected by the server");
51-
}
52-
else
53-
{
54-
RCLCPP_INFO(this->get_logger(), "Goal accepted by the server, waiting for result");
55-
}
56-
};
33+
auto send_goal_options = rclcpp_action::Client<ExecuteTree>::SendGoalOptions();
5734

58-
send_goal_options.feedback_callback =
59-
[this](GoalHandleExecuteTree::SharedPtr, const std::shared_ptr<const ExecuteTree::Feedback> feedback) {
60-
RCLCPP_INFO(this->get_logger(), "Feedback received: %s", feedback->message.c_str());
61-
};
35+
send_goal_options.goal_response_callback =
36+
[this](GoalHandleExecuteTree::SharedPtr goal_handle)
37+
{
38+
if (!goal_handle)
39+
{
40+
RCLCPP_ERROR(this->get_logger(), "Goal was rejected by the server");
41+
}
42+
else
43+
{
44+
RCLCPP_INFO(this->get_logger(), "Goal accepted by the server, waiting for result");
45+
}
46+
};
6247

63-
send_goal_options.result_callback =
64-
[this](const GoalHandleExecuteTree::WrappedResult &result) {
65-
switch (result.code)
66-
{
67-
case rclcpp_action::ResultCode::SUCCEEDED:
68-
RCLCPP_INFO(this->get_logger(), "Result received: Status=%d, Message=%s",
69-
result.result->node_status, result.result->return_message.c_str());
70-
break;
71-
case rclcpp_action::ResultCode::ABORTED:
72-
RCLCPP_ERROR(this->get_logger(), "Goal was aborted");
73-
break;
74-
case rclcpp_action::ResultCode::CANCELED:
75-
RCLCPP_WARN(this->get_logger(), "Goal was canceled");
76-
break;
77-
default:
78-
RCLCPP_ERROR(this->get_logger(), "Unknown result code");
79-
break;
80-
}
81-
rclcpp::shutdown();
82-
};
83-
84-
action_client_->async_send_goal(goal_msg, send_goal_options);
85-
}
86-
};
48+
send_goal_options.feedback_callback =
49+
[this](GoalHandleExecuteTree::SharedPtr,
50+
const std::shared_ptr<const ExecuteTree::Feedback> feedback)
51+
{
52+
RCLCPP_INFO(this->get_logger(), "Feedback received: %s", feedback->message.c_str());
53+
};
8754

88-
int main(int argc, char **argv)
89-
{
90-
rclcpp::init(argc, argv);
91-
92-
if (argc < 2) {
93-
RCLCPP_ERROR(rclcpp::get_logger("tree_execution_client"), "Usage: ros2 run package_name node_name <target_tree_name>");
94-
return 1;
95-
}
96-
97-
std::string target_tree = argv[1];
98-
auto node = std::make_shared<TreeExecutionClient>(target_tree);
99-
rclcpp::spin(node);
100-
rclcpp::shutdown();
101-
return 0;
102-
}
55+
send_goal_options.result_callback =
56+
[this](const GoalHandleExecuteTree::WrappedResult &result)
57+
{
58+
switch (result.code)
59+
{
60+
case rclcpp_action::ResultCode::SUCCEEDED:
61+
RCLCPP_INFO(this->get_logger(), "Result received: Status=%d, Message=%s",
62+
result.result->node_status, result.result->return_message.c_str());
63+
break;
64+
case rclcpp_action::ResultCode::ABORTED:
65+
RCLCPP_ERROR(this->get_logger(), "Goal was aborted");
66+
break;
67+
case rclcpp_action::ResultCode::CANCELED:
68+
RCLCPP_WARN(this->get_logger(), "Goal was canceled");
69+
break;
70+
default:
71+
RCLCPP_ERROR(this->get_logger(), "Unknown result code");
72+
break;
73+
}
74+
rclcpp::shutdown();
75+
};
76+
77+
action_client_->async_send_goal(goal_msg, send_goal_options);
78+
}

‎behaviortree_ros2/src/tree_execution_server.cpp‎

Lines changed: 16 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -134,7 +134,7 @@ TreeExecutionServer::handle_goal(const rclcpp_action::GoalUUID& /* uuid */,
134134
if(!onGoalReceived(goal->target_tree, goal->payload))
135135
{
136136
return rclcpp_action::GoalResponse::REJECT;
137-
}
137+
} // is this function useful?
138138
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
139139
}
140140

@@ -178,13 +178,23 @@ void TreeExecutionServer::execute(
178178

179179
// This blackboard will be owned by "MainTree". It parent is p_->global_blackboard
180180
auto root_blackboard = BT::Blackboard::create(p_->global_blackboard);
181+
try {
182+
RCLCPP_INFO(kLogger, "Creating tree: %s", goal->target_tree.c_str());
183+
p_->tree = p_->factory.createTree(goal->target_tree, root_blackboard);
184+
} catch (const std::runtime_error& e) {
185+
RCLCPP_ERROR(rclcpp::get_logger("bt_executor"), "Failed to create tree: %s", e.what());
186+
status = BT::NodeStatus::FAILURE;
187+
action_result->node_status = ConvertNodeStatus(status);
188+
action_result->return_message = std::string("Failed to create tree: ") + e.what();
189+
goal_handle->abort(action_result);
190+
return;
191+
} // Abort the execution if the tree creation fails since the tree is not loaded
192+
193+
p_->tree_name = goal->target_tree;
194+
p_->payload = goal->payload;
195+
onTreeCreated(p_->tree);
181196

182-
p_->tree = p_->factory.createTree(goal->target_tree, root_blackboard);
183-
p_->tree_name = goal->target_tree;
184-
p_->payload = goal->payload;
185197

186-
// call user defined function after the tree has been created
187-
onTreeCreated(p_->tree);
188198
p_->groot_publisher.reset();
189199
BT::DebuggableTree debugTree{std::shared_ptr<BT::Tree>(&p_->tree, [](BT::Tree*) {}), true, false};
190200
BT::PublisherZMQ publisher(debugTree, p_->params.groot2_port);

‎behaviortree_ros2/test/bt_ros2_tests.cpp‎

Lines changed: 22 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,22 @@
1+
#include <algorithm>
2+
#include <gtest/gtest.h>
3+
#include <rclcpp/rclcpp.hpp>
4+
#include "behaviortree_ros2/tools/client/tree_execution_client.hpp"
5+
6+
7+
TEST(package_name, BTROS2_TEST) {
8+
9+
auto node = std::make_shared<TreeExecutionClient>("ExampleTree1");
10+
rclcpp::executors::SingleThreadedExecutor executor;
11+
executor.add_node(node);
12+
13+
ASSERT_EQ("test", "value");
14+
}
15+
16+
17+
18+
int main(int argc, char** argv) {
19+
testing::InitGoogleTest(&argc, argv);
20+
rclcpp::init(argc, argv);
21+
return RUN_ALL_TESTS();
22+
}

‎bt_examples/behavior_trees/example_2.xml‎

Lines changed: 18 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,18 @@
1+
<?xml version="1.0"?>
2+
<root BTCPP_format="4" main_tree_to_execute="ExampleTree1">
3+
<!-- ////////// -->
4+
<BehaviorTree ID="ExampleTree1">
5+
<Repeat num_cycles="10">
6+
<Sequence>
7+
<Delay delay_msec="1000">
8+
<AlwaysSuccess/>
9+
</Delay>
10+
</Sequence>
11+
</Repeat>
12+
</BehaviorTree>
13+
<!-- ////////// -->
14+
<TreeNodesModel>
15+
<SubTree ID="ExampleTree1" description=""/>
16+
</TreeNodesModel>
17+
<!-- ////////// -->
18+
</root>

0 commit comments

Comments
 (0)