ROS Index
|
ros2_bt_utils package from ros2_bt_utils reporos2_bt_utils |
Package Summary
| Tags | No category tags. |
| Version | 0.1.0 |
| License | BSD-3 |
| Build type | AMENT_CMAKE |
| Use | RECOMMENDED |
Repository Summary
| Checkout URI | https://github.com/miccol/ros2_bt_utils.git |
| VCS Type | git |
| VCS Version | main |
| Last Updated | 2023-01-31 |
| Dev Status | UNMAINTAINED |
| CI status | No Continuous Integration |
| Released | UNRELEASED |
| Tags | No category tags. |
| Contributing |
Help Wanted (0)
Good First Issues (0) Pull Requests to Review (0) |
Package Description
Additional Links
Maintainers
- Michele Colledanchise
Authors
ROS2 BT Utils
Package containing the base classes (virtual and templates) to ease the development Behavior Trees leaf nodes in ROS2, using BehaviorTree.CPP.
Dependencies
Installation
Clone this repo in your ROS2 workspace
cd <your-ws-src>
git clone https://github.com/miccol/ros2_bt_utils
Build
cd <your-ws>
colcon build
The classes contain the source code needed to set up the client side of ROS2’s Actions, Services, and subscribe to a Topic:
-
ActionROSActionClient(const std::string &name, const BT::NodeConfiguration &config, const std::string &ros_action_name)(The base class of a BT action with namenameand NodeConfigurationconfigthat implements the client side of the ROS Action namedros_action_name) -
ActionROSServiceClient(const std::string &name, const BT::NodeConfiguration &config, const std::string &service_name)(The base class of a BT action with namenameand NodeConfigurationconfigthat implements the client side of the ROS Service namedservice_name) -
ConditionTopicSubscriber(const std::string &name, const BT::NodeConfiguration &config, const std::string &topic_name)(The base class of a BT condition with namenameand NodeConfigurationconfigthat reads the most updated message of from the topictopic_name) -
ConditionROSServiceClient(const std::string &name, const BT::NodeConfiguration &config, const std::string &service_name)(The base class of a BT condition with namenameand NodeConfigurationconfigthat implements the client side of the ROS Service namedservice_name)
ActionROSActionClient
You just need to implement the following function:
YOURACTION::Goal computeGoal()
that computes the ROS Action Goal to be sent to the server.
BT::NodeStatus elaborateResultAndReturn(const ClientGoalHandle<YOURACTION>::WrappedResult result);
That elaborates the results and computes the BT::NodeStatus to be sent to the BT parent node. The return status should be SUCCESS or FAILURE.
void elaborateFeedback(const TOURACTIONGOALFeedbackConstPtr feedback);
that is called at each ROS Action feedback received.
Moreover, the BT Node returns FAILURE if the Result Code of the ROS Action is CANCELED or ABORTED. It returns RUNNING otherwise (It actually implementa a BT::CoroActionNode, see here the details.).
Below a simple example of an BT Action Node class that reads a geometry_msgs::msg::Pose from the BT blackboard and sends it as a goal to the action named navigate_to_pose from Nav2. It also writes the Estimated Time of Arrival (ETA):
header file:
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <nav2_msgs/action/navigate_to_pose.hpp>
#include <ros2_bt_utils/action_client.hpp>
using NavigateToPoseFeedback = typename rclcpp_action::ClientGoalHandle<
nav2_msgs::action::NavigateToPose>::Feedback::ConstSharedPtr;
class ActionGoto : public ros2_bt_utils::ActionROSActionClient <nav2_msgs::action::NavigateToPose>
{
public:
ActionGoto(const std::string &name, const BT::NodeConfiguration &config);
static BT::PortsList providedPorts()
{
return {BT::InputPort<geometry_msgs::msg::Pose>("pose")};
}
private:
nav2_msgs::action::NavigateToPose::Goal computeGoal() override;
void elaborateFeedback(const ROSActionGoalFeedbackConstPtr feedback) override;
BT::NodeStatus elaborateResultAndReturn(
const rclcpp_action::ClientGoalHandle<nav2_msgs::action::NavigateToPose>::WrappedResult result) override;
rclcpp::Node::SharedPtr ros_node_;
};
implementation file
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <nav2_msgs/action/navigate_to_pose.hpp>
#include <ros2_bt_utils/action_client.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
using NavigateToPoseFeedback = typename rclcpp_action::ClientGoalHandle<
nav2_msgs::action::NavigateToPose>::Feedback::ConstSharedPtr;
ActionGoto::ActionGoto(const std::string &name, const BT::NodeConfiguration &config)
: ActionROSActionClient(name, config, "navigate_to_pose")
{
RCLCPP_INFO(ros2_bt_utils::ROSNode()->get_logger(), "Creating %s", name.c_str());
}
nav2_msgs::action::NavigateToPose::Goal ActionGoto::computeGoal()
{
geometry_msgs::msg::Pose goal_pose;
getInput("pose", goal_pose); // get goa value from BT blackboard
nav2_msgs::action::NavigateToPose::Goal goal;
goal.pose.header.frame_id = "map";
goal.pose.pose = goal_pose;
return goal;
}
void ActionGoto::elaborateFeedback(const ROSActionGoalFeedbackConstPtr feedback)
{
RCLCPP_INFO(ros2_bt_utils::ROSNode()->get_logger(), "ETA: %d",
feedback.get()->estimated_time_remaining.sec);
}
BT::NodeStatus ActionGoto::elaborateResultAndReturn(
[[maybe_unused]] const rclcpp_action::ClientGoalHandle<
nav2_msgs::action::NavigateToPose>::WrappedResult result)
{
RCLCPP_INFO(ros2_bt_utils::ROSNode()->get_logger(), "Goal Reached. BT Node returning Success");
// NavigateToPose.action is std_msgs/Empty
return BT::NodeStatus::SUCCESS;
}
ActionROSServiceClient
You just need to implement the following functions:
YOURSERVICE::Request::SharedPtr computeRequest()
That computes the service request, and
BT::NodeStatus elaborateResponseAndReturn(const YOURSERVICE::Response response)
That takes the service response and returns the final return status of the BT node as BT::NodeStatus(i.e. BT::SUCCESS or BT::FAILURE).
ConditionTopicSubscriber
You just need to implement the following function:
BT::NodeStatus elaborateMessageAndReturn(const YOURTOPICMESSAGE msg)
that takes a topic message and returns the return status of the BT node as BT::NodeStatus(i.e. BT::SUCCESS or BT::FAILURE). However it may the the case that the Condition Node gets ticked before the message is published. This should not. For this reason you need to implement the function:
BT::NodeStatus boundaryConditionStatus()
whose return status will be used when the message msg is not published.
Below a simple example of a condition node class that reads the battery level from the topic /battery and compares it with reference_value from the blackboard;
header
#include <ros2_bt_utils/condition_topic_subscriber.hpp>
#include <sensor_msgs/msg/battery_state.hpp>
class ConditionBatteryLevelAbove
: public ros2_bt_utils::ConditionTopicSubscriber<sensor_msgs::msg::BatteryState>
{
public:
ConditionBatteryLevelAbove(const std::string &name, const BT::NodeConfiguration &config);
static BT::PortsList providedPorts() { return {BT::InputPort<float>("reference_value")}; }
private:
BT::NodeStatus elaborateMessageAndReturn(sensor_msgs::msg::BatteryState msg) override;
BT::NodeStatus boundaryConditionStatus() override;
double reference_value, current_value;
};
implementation file
#include <ros2_bt_utils/condition_topic_subscriber.hpp>
#include <sensor_msgs/msg/battery_state.hpp>
ConditionBatteryLevelAbove::ConditionBatteryLevelAbove(const std::string &name,
const BT::NodeConfiguration &config)
: ConditionTopicSubscriber(name, config, "/battery_state")
{
RCLCPP_INFO(ros2_bt_utils::ROSNode()->get_logger(), "Creating %s", name.c_str());
}
BT::NodeStatus ConditionBatteryLevelAbove::elaborateMessageAndReturn(
sensor_msgs::msg::BatteryState msg)
{
current_value = msg.percentage;
getInput("reference_value", reference_value);
return current_value > reference_value ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE;
}
BT::NodeStatus ConditionBatteryLevelAbove::boundaryConditionStatus()
{
// if no message has arrived, assumes that the battery level is high enough
RCLCPP_WARN(ros2_bt_utils::ROSNode()->get_logger(), "Assuming Battery level high ");
return BT::NodeStatus::SUCCESS;
}
ConditionROSServiceClient
You just need to implement the following functions:
YOURSERVICE::Request::SharedPtr computeRequest()
That computes the service request, and
BT::NodeStatus elaborateResponseAndReturn(const YOURSERVICE::Response response)
That takes the service response and returns the return status of the BT node as BT::NodeStatus(i.e. SUCCESS or FAILURE).
Wiki Tutorials
Package Dependencies
| Deps | Name |
|---|---|
| ament_cmake | |
| ament_lint_auto | |
| ament_lint_common | |
| std_msgs | |
| example_interfaces | |
| rclcpp | |
| rclcpp_action | |
| behaviortree_cpp_v3 |
System Dependencies
Dependant Packages
| Name | Deps |
|---|---|
| simple_bts | |
| tour_bts |