Skip to content

Commit d6e1b38

Browse files
Merge remote-tracking branch 'upstream/humble' into humble
2 parents 3a716e4 + 6c6aa07 commit d6e1b38

18 files changed

Lines changed: 2807 additions & 43 deletions

.github/workflows/test.yml

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -19,10 +19,10 @@ jobs:
1919
steps:
2020
- name: Build and run tests
2121
id: action-ros-ci
22-
uses: ros-tooling/action-ros-ci@v0.3
22+
uses: ros-tooling/action-ros-ci@v0.4
2323
with:
2424
target-ros2-distro: ${{ matrix.ros_distro }}
25-
- uses: actions/upload-artifact@v1
25+
- uses: actions/upload-artifact@v4
2626
with:
2727
name: colcon-logs
2828
path: ros_ws/log

Doxyfile

Lines changed: 2640 additions & 0 deletions
Large diffs are not rendered by default.

behaviortree_ros2/CMakeLists.txt

Lines changed: 10 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -27,15 +27,21 @@ generate_parameter_library(
2727
add_library(${PROJECT_NAME}
2828
src/bt_ros2.cpp
2929
src/bt_utils.cpp
30-
src/tree_execution_server.cpp )
31-
32-
ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_DEPS})
30+
src/tree_execution_server.cpp
31+
src/bt_ros_logger.cpp )
3332

3433
target_include_directories(${PROJECT_NAME} PUBLIC
3534
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
3635
$<INSTALL_INTERFACE:include>)
3736

38-
target_link_libraries(${PROJECT_NAME} bt_executor_parameters)
37+
target_link_libraries(${PROJECT_NAME} PUBLIC
38+
rclcpp::rclcpp
39+
rclcpp_action::rclcpp_action
40+
ament_index_cpp::ament_index_cpp
41+
behaviortree_cpp::behaviortree_cpp
42+
${btcpp_ros2_interfaces_TARGETS}
43+
bt_executor_parameters
44+
)
3945

4046

4147
######################################################

behaviortree_ros2/bt_executor_parameters.md

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -5,11 +5,11 @@ Default Config
55
bt_server:
66
ros__parameters:
77
action_name: bt_execution
8-
behavior_trees: '{}'
9-
groot2_port: 1667.0
10-
plugins: '{}'
11-
ros_plugins_timeout: 1000.0
12-
tick_frequency: 100.0
8+
behavior_trees: []
9+
groot2_port: 1667
10+
plugins: []
11+
ros_plugins_timeout: 1000
12+
tick_frequency: 100
1313

1414
```
1515

behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp

Lines changed: 21 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,7 @@
1717

1818
#include <memory>
1919
#include <string>
20+
#include <optional>
2021
#include <rclcpp/executors.hpp>
2122
#include <rclcpp/allocator/allocator_common.hpp>
2223
#include "behaviortree_cpp/action_node.h"
@@ -103,6 +104,10 @@ class RosActionNode : public BT::ActionNodeBase
103104
* @brief Any subclass of RosActionNode that has ports must implement a
104105
* providedPorts method and call providedBasicPorts in it.
105106
*
107+
* The basic ports:
108+
*
109+
* - `action_name` Action server name
110+
*
106111
* @param addition Additional ports to add to BT port list
107112
* @return PortsList containing basic ports along with node-specific ports
108113
*/
@@ -152,8 +157,15 @@ class RosActionNode : public BT::ActionNodeBase
152157
}
153158

154159
/** Callback invoked when something goes wrong.
160+
* The result is provided if it is available.
155161
* It must return either SUCCESS or FAILURE.
156162
*/
163+
virtual BT::NodeStatus onFailure(ActionNodeErrorCode error,
164+
const std::optional<WrappedResult>&)
165+
{
166+
return onFailure(error);
167+
}
168+
157169
virtual BT::NodeStatus onFailure(ActionNodeErrorCode /*error*/)
158170
{
159171
return NodeStatus::FAILURE;
@@ -244,7 +256,7 @@ RosActionNode<T>::ActionClientInstance::ActionClientInstance(
244256
std::shared_ptr<rclcpp::Node> node, const std::string& action_name)
245257
{
246258
callback_group =
247-
node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
259+
node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false);
248260
callback_executor.add_callback_group(callback_group, node->get_node_base_interface());
249261
action_client = rclcpp_action::create_client<T>(node, action_name, callback_group);
250262
}
@@ -309,7 +321,7 @@ inline bool RosActionNode<T>::createClient(const std::string& action_name)
309321
if(it == registry.end() || it->second.expired())
310322
{
311323
client_instance_ = std::make_shared<ActionClientInstance>(node, action_name);
312-
registry.insert({ action_client_key_, client_instance_ });
324+
registry.insert_or_assign(action_client_key_, client_instance_);
313325
}
314326
else
315327
{
@@ -360,8 +372,8 @@ inline NodeStatus RosActionNode<T>::tick()
360372

361373
if(!client_instance_)
362374
{
363-
throw BT::RuntimeError("RosActionNode: no client was specified neither as default or "
364-
"in the ports");
375+
throw BT::RuntimeError("RosActionNode: no client was specified, neither as default "
376+
"nor in the ports");
365377
}
366378

367379
auto& action_client = client_instance_->action_client;
@@ -390,7 +402,7 @@ inline NodeStatus RosActionNode<T>::tick()
390402

391403
if(!setGoal(goal))
392404
{
393-
return CheckStatus(onFailure(INVALID_GOAL));
405+
return CheckStatus(onFailure(INVALID_GOAL, {}));
394406
}
395407

396408
typename ActionClient::SendGoalOptions goal_options;
@@ -432,7 +444,7 @@ inline NodeStatus RosActionNode<T>::tick()
432444
// Check if server is ready
433445
if(!action_client->action_server_is_ready())
434446
{
435-
return onFailure(SERVER_UNREACHABLE);
447+
return onFailure(SERVER_UNREACHABLE, {});
436448
}
437449

438450
future_goal_handle_ = action_client->async_send_goal(goal, goal_options);
@@ -459,7 +471,7 @@ inline NodeStatus RosActionNode<T>::tick()
459471
{
460472
if((now() - time_goal_sent_) > timeout)
461473
{
462-
return CheckStatus(onFailure(SEND_GOAL_TIMEOUT));
474+
return CheckStatus(onFailure(SEND_GOAL_TIMEOUT, {}));
463475
}
464476
else
465477
{
@@ -490,11 +502,11 @@ inline NodeStatus RosActionNode<T>::tick()
490502
{
491503
if(result_.code == rclcpp_action::ResultCode::ABORTED)
492504
{
493-
return CheckStatus(onFailure(ACTION_ABORTED));
505+
return CheckStatus(onFailure(ACTION_ABORTED, result_));
494506
}
495507
else if(result_.code == rclcpp_action::ResultCode::CANCELED)
496508
{
497-
return CheckStatus(onFailure(ACTION_CANCELLED));
509+
return CheckStatus(onFailure(ACTION_CANCELLED, result_));
498510
}
499511
else
500512
{
Lines changed: 32 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,32 @@
1+
#ifndef BT_ROS_LOGGER_H
2+
#define BT_ROS_LOGGER_H
3+
4+
#include <cstring>
5+
#include <rclcpp/executors.hpp>
6+
#include <rclcpp/allocator/allocator_common.hpp>
7+
#include <rclcpp/logging.hpp>
8+
#include "behaviortree_cpp/loggers/abstract_logger.h"
9+
10+
namespace BT
11+
{
12+
/**
13+
* @brief RosLogger is a very simple logger that
14+
* displays all the transitions on the console.
15+
*/
16+
17+
class RosLogger : public StatusChangeLogger
18+
{
19+
public:
20+
RosLogger(const BT::Tree& tree, std::shared_ptr<rclcpp::Node> node);
21+
~RosLogger() override;
22+
23+
virtual void flush() override;
24+
25+
private:
26+
virtual void callback(Duration timestamp, const TreeNode& node, NodeStatus prev_status,
27+
NodeStatus status) override;
28+
std::weak_ptr<rclcpp::Node> node_;
29+
};
30+
} // namespace BT
31+
32+
#endif // BT_ROS_LOGGER_H

behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp

Lines changed: 9 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,8 @@
1919
#include <string>
2020
#include <rclcpp/executors.hpp>
2121
#include <rclcpp/allocator/allocator_common.hpp>
22+
#include <rclcpp/version.h>
23+
#include <rclcpp/qos.hpp>
2224
#include "behaviortree_cpp/bt_factory.h"
2325

2426
#include "behaviortree_ros2/ros_node_params.hpp"
@@ -216,8 +218,14 @@ inline RosServiceNode<T>::ServiceClientInstance::ServiceClientInstance(
216218
node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false);
217219
callback_executor.add_callback_group(callback_group, node->get_node_base_interface());
218220

221+
// For Jazzy and Later Support
222+
#if RCLCPP_VERSION_GTE(28, 0, 0)
223+
service_client =
224+
node->create_client<T>(service_name, rclcpp::ServicesQoS(), callback_group);
225+
#else
219226
service_client = node->create_client<T>(service_name, rmw_qos_profile_services_default,
220227
callback_group);
228+
#endif
221229
}
222230

223231
template <class T>
@@ -275,7 +283,7 @@ inline bool RosServiceNode<T>::createClient(const std::string& service_name)
275283
if(it == registry.end() || it->second.expired())
276284
{
277285
srv_instance_ = std::make_shared<ServiceClientInstance>(node, service_name);
278-
registry.insert({ client_key, srv_instance_ });
286+
registry.insert_or_assign(client_key, srv_instance_);
279287

280288
RCLCPP_INFO(logger(), "Node [%s] created service client [%s]", name().c_str(),
281289
service_name.c_str());

behaviortree_ros2/include/behaviortree_ros2/bt_topic_sub_node.hpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -264,7 +264,7 @@ inline bool RosTopicSubNode<T>::createSubscriber(const std::string& topic_name,
264264
if(it == registry.end() || it->second.expired())
265265
{
266266
sub_instance_ = std::make_shared<SubscriberInstance>(node, topic_name, qos_profile);
267-
registry.insert({ subscriber_key_, sub_instance_ });
267+
registry.insert_or_assign(subscriber_key_, sub_instance_);
268268

269269
RCLCPP_INFO(logger(), "Node [%s] created Subscriber to topic [%s]", name().c_str(),
270270
topic_name.c_str());

behaviortree_ros2/include/behaviortree_ros2/bt_utils.hpp

Lines changed: 4 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -11,17 +11,18 @@
1111
// COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR
1212
// OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
1313

14+
#pragma once
15+
1416
#include <functional>
1517
#include <memory>
1618
#include <thread>
1719

18-
// auto-generated header, created by generate_parameter_library
19-
#include "bt_executor_parameters.hpp"
20-
2120
#include "btcpp_ros2_interfaces/msg/node_status.hpp"
2221

2322
#include "behaviortree_cpp/bt_factory.h"
2423

24+
// auto-generated header, created by generate_parameter_library
25+
#include "behaviortree_ros2/bt_executor_parameters.hpp"
2526
#include "behaviortree_ros2/plugins.hpp"
2627
#include "behaviortree_ros2/ros_node_params.hpp"
2728

behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp

Lines changed: 8 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -11,6 +11,8 @@
1111
// COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR
1212
// OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
1313

14+
#pragma once
15+
1416
#include <memory>
1517
#include <optional>
1618

@@ -28,6 +30,11 @@ namespace BT
2830
* @brief TreeExecutionServer class hosts a ROS Action Server that is able
2931
* to load Behavior plugins, BehaviorTree.xml files and execute them.
3032
*
33+
* These are named in the ROS2 node's parameters, see
34+
* @link bt_executor_parameters.md @endlink
35+
*
36+
* A `BT::Groot2Publisher` is included inside (see BehaviorTree.CPP for more information).
37+
*
3138
* It can be customized by overriding its virtual functions.
3239
*/
3340
class TreeExecutionServer
@@ -72,7 +79,7 @@ class TreeExecutionServer
7279
/// @brief Pointer to the global blackboard
7380
BT::Blackboard::Ptr globalBlackboard();
7481

75-
/// @brief Pointer to the global blackboard
82+
/// @brief The behavior tree factory
7683
BT::BehaviorTreeFactory& factory();
7784

7885
protected:

0 commit comments

Comments
 (0)