MoveIt Pro Behavior Interface
5.2.0
Library for developing custom behaviors for use in MoveIt Pro
|
Namespaces | |
check_for_error_utils | |
Classes | |
class | ActionClientBehaviorBase |
A base class for behaviors which need to send a goal to a ROS action client and wait for a result. If the behavior is halted before the action result is received, the action goal will be canceled. More... | |
class | AddToVector |
Pushes an object into a vector and sets the updated vector to the blackboard. More... | |
class | AsyncBehaviorBase |
A base class for behaviors which need to asynchronously run a function that might take a long time to complete. More... | |
struct | BehaviorContext |
The BehaviorContext struct contains shared resources that are common between all instances of Behaviors that inherit from moveit_studio::behaviors::SharedResourcesNode. More... | |
class | ClockInterfaceBase |
A base class which provides an interface for retrieving timepoints from a monotonic clock. More... | |
class | SteadyClockInterface |
Implementation of ClockInterfaceBase for std::chrono::steady_clock. More... | |
class | ForEach |
A class template for creating a behavior tree decorator node to help iterate through a vector of items. More... | |
class | GetMessageFromTopicBehaviorBase |
Base class for Behaviors that get the latest message from a topic specified on an input data port and set that message to an output data port. More... | |
class | LoadMultipleFromYaml |
Loads types from a YAML file, and returns them as a vector in an output port. More... | |
class | LoadFromYaml |
Loads a type from a YAML file, and returns it in an output port. More... | |
class | PublisherInterfaceBase |
Defines an interface to a publisher that sends a message to a topic. More... | |
class | RclcppPublisherInterface |
Implementation of the publisher interface for a rclcpp publisher. More... | |
class | ResetVector |
Create an empty vector and set it to the blackboard. More... | |
class | SaveToYaml |
Save the contents of a ROS 2 message type to a YAML file in a specified namespace. Note: This Behavior template saves the pose into the ~/.config/moveit_pro/robot_config/objectives folder. More... | |
class | SendMessageToTopicBehaviorBase |
Base class for Behaviors that send a message to a topic. The message contents and topic name are specified as input ports. More... | |
class | ServiceClientBehaviorBase |
A base class for behaviors which need to send a request to a ROS service client and wait for a result. More... | |
class | ClientInterfaceBase |
Provides an interface to a service client that can send a single request at a time. WARNING - This class currently does not support calling syncSendRequest function asynchronously from multiple threads. More... | |
class | RclcppClientInterface |
Implements ClientInterfaceBase for the rclcpp service client. More... | |
class | SharedResourcesNode |
The SharedResourcesNode class provides a BehaviorContext object when constructing a BehaviorTree.Cpp node. More... | |
class | SharedResourcesNodeLoaderBase |
The SharedResourcesNodeLoaderBase class is a base class for Behavior loader plugins that register Behaviors inheriting from SharedResourcesNode. More... | |
Functions | |
bool | clientMustBeRecreated (const std::shared_ptr< rclcpp_action::ClientBase > &client, const std::string_view old_action_name, const std::string_view new_action_name) |
Compare an action client's current configuration to the desired new configuration to determine if the client needs to be recreated. More... | |
template<typename... Args> | |
std::optional< std::string > | maybe_error (BT::Expected< Args >... args) |
Check if any of the provided inputs represent error states and, if so, return their error messages. More... | |
template<typename E , typename... Args> | |
constexpr std::optional< E > | maybe_error (tl::expected< Args, E >... args) |
Tests if any of the expected args passed in has an error. More... | |
template<typename MessageT > | |
bool | shouldRecreateSubscriber (const std::shared_ptr< rclcpp::Subscription< MessageT >> &subscriber, const std::string &topic_name, const rclcpp::QoS &publisher_qos) |
Compare a subscriber's current configuration to the desired new configuration to determine if the subscriber needs to be recreated. More... | |
tl::expected< rclcpp::TopicEndpointInfo, std::string > | waitForPublisher (const std::shared_ptr< rclcpp::Node > &node, const std::string &topic_name) |
Waits for a publisher to be advertised on the provided topic, and gets the TopicEndpointInfo for the publisher once it appears. More... | |
bool | shouldRecreatePublisher (const std::shared_ptr< rclcpp::PublisherBase > &publisher, const std::string_view topic_name, const size_t queue_size, const rclcpp::ReliabilityPolicy reliability_policy) |
Compare a publisher's current configuration to the desired new configuration to determine if the publisher needs to be recreated. More... | |
bool | clientMustBeRecreated (const std::shared_ptr< rclcpp::ClientBase > &client, const std::string_view new_service_name) |
Compare a service client's current configuration to the desired new configuration to determine if the client needs to be recreated. More... | |
template<typename T > | |
BT::NodeBuilder | getDefaultNodeBuilder () |
Helper function to create a BT::NodeBuilder for a behavior tree node with the default constructor signature. More... | |
template<typename T > | |
BT::NodeBuilder | getSharedResourcesNodeBuilder (const std::shared_ptr< moveit_studio::behaviors::BehaviorContext > &shared_resources) |
Helper function to create a BT::NodeBuilder for a behavior tree node which takes shared_ptr<BehaviorContext> as an additional constructor parameter. More... | |
template<typename T > | |
void | registerBehavior (BT::BehaviorTreeFactory &factory, const std::string &name) |
Helper function to register a behavior with the default constructor signature with a BT::BehaviorTreeFactory. More... | |
template<typename T > | |
void | registerBehavior (BT::BehaviorTreeFactory &factory, const std::string &name, const std::shared_ptr< moveit_studio::behaviors::BehaviorContext > &shared_resources) |
Helper function to register a behavior derived from SharedResourcesNode with a BT::BehaviorTreeFactory. More... | |
Variables | |
constexpr auto | kDescriptionLoadFromYaml |
constexpr auto | kDescriptionLoadHeaderFromYaml = "<p>Loads a std::msg Header message from a YAML file.</p>" |
constexpr auto | kDescriptionLoadJointTrajectoryFromYaml |
constexpr auto | kDescriptionLoadObjectSubframeFromYaml |
constexpr auto | kDescriptionLoadPoseFromYaml = "<p>Loads a Pose message from a YAML file.</p>" |
constexpr auto | kDescriptionLoadPoseStampedFromYaml = "<p>Loads a PoseStamped message from a YAML file.</p>" |
constexpr auto | kDescriptionLoadPointStampedFromYaml = "<p>Loads a PointStamped message from a YAML file.</p>" |
constexpr auto | kDescriptionLoadQuaternionFromYaml = "<p>Loads a Quaternion message from a YAML file.</p>" |
constexpr auto | kDescriptionLoadVector3FromYaml = "<p>Loads a Vector3 message from a YAML file.</p>" |
constexpr auto | kDescriptionLoadTransformFromYaml = "<p>Loads a Transform message from a YAML file.</p>" |
constexpr auto | kDescriptionLoadTransformStampedFromYaml |
constexpr auto | kDescriptionLoadHeaderVectorFromYaml |
constexpr auto | kDescriptionLoadPoseVectorFromYaml = "<p>Loads a vector of Pose messages from a YAML file.</p>" |
constexpr auto | kDescriptionLoadPoseStampedVectorFromYaml |
constexpr auto | kDescriptionLoadPointStampedVectorFromYaml |
constexpr auto | kDescriptionLoadObjectSubframeVectorFromYaml |
constexpr auto | kDescriptionSaveToYaml |
constexpr auto | kDescriptionSaveHeaderToYaml |
constexpr auto | kDescriptionSaveJointTrajectoryToYaml |
constexpr auto | kDescriptionSavePoseToYaml |
constexpr auto | kDescriptionSavePoseStampedToYaml |
constexpr auto | kDescriptionSavePointStampedToYaml |
constexpr auto | kDescriptionSaveQuaternionToYaml |
constexpr auto | kDescriptionSaveVector3ToYaml |
constexpr auto | kDescriptionSaveTransformToYaml |
constexpr auto | kDescriptionSaveTransformStampedToYaml |
|
inline |
Compare a service client's current configuration to the desired new configuration to determine if the client needs to be recreated.
This checks two criteria:
This does NOT check if the client's QoS settings match the ones used by the server, because ServiceClientBehaviorBase always creates clients using the rmw_qos_profile_services_default
QoS profile. Also, it is unlikely that a ROS 2 service server would use customized QoS settings.
client | The service client to check |
new_service_name | The new service name |
|
inline |
Compare an action client's current configuration to the desired new configuration to determine if the client needs to be recreated.
This checks two criteria:
This does NOT check if the client's QoS settings match the ones used by the server, because ActionClientBehaviorBase always creates clients using the default ROS 2 action client QoS profile. It is also very complex to retrieve the QoS settings for an action server (since it's composed of a collection of services and topics). In any case action clients are almost always created with default QoS settings (or at the very least we've never seen one that used non-default settings).
client | The action client to check |
new_action_name | The new action name |
|
inline |
Helper function to create a BT::NodeBuilder for a behavior tree node with the default constructor signature.
T | Create a builder for this type of node. Must be derived from BT::TreeNode. |
|
inline |
Helper function to create a BT::NodeBuilder for a behavior tree node which takes shared_ptr<BehaviorContext> as an additional constructor parameter.
shared_resources | A shared_ptr to an instance of BehaviorContext, which will be provided when creating the behaviors registered by this function. |
T | Create a builder for this type of node. Must be derived from moveit_studio::behaviors::SharedResourcesNode. |
|
inline |
Check if any of the provided inputs represent error states and, if so, return their error messages.
This was inspired by fp's maybe_error function: https://github.com/tylerjw/fp/blob/b4bf17c2f7a99c07b6ab9b8706357572e960d638/include/fp/result.hpp#L216-L236
args | One or more BT::Expecteds (i.e., results from getInput) |
|
constexpr |
Tests if any of the expected args passed in has an error.
This was adapted from fp's maybe_error function with a small modification.
[in] | The | tl::expected<T, E> variables. All have to use the same error type. |
E | The error type |
Args | The value types for the tl::expected<T, E> args |
|
inline |
Helper function to register a behavior with the default constructor signature with a BT::BehaviorTreeFactory.
factory | Register behaviors with this factory. |
name | The name to use when registering this type of behavior. It is good practice to make this name match the name of the class (e.g., moveit_studio::behaviors::PlanMTCTask is registered as "PlanMTCTask"). |
T | Register a behavior of this type. |
|
inline |
Helper function to register a behavior derived from SharedResourcesNode with a BT::BehaviorTreeFactory.
factory | Register behaviors with this factory. |
name | The name to use when registering this type of behavior. It is good practice to make this name match the name of the class (e.g., moveit_studio::behaviors::PlanMTCTask is registered as "PlanMTCTask"). |
shared_resources | A shared_ptr to an instance of BehaviorContext, which will be provided when creating the behaviors registered by this function. |
T | Register a behavior of this type. |
|
inline |
Compare a publisher's current configuration to the desired new configuration to determine if the publisher needs to be recreated.
This checks two criteria:
publisher | The existing publisher to check. |
topic_name | The topic name for the new configuration. |
queue_size | The queue size for the new configuration. |
reliability_policy | The reliability policy for the new configuration |
bool moveit_studio::behaviors::shouldRecreateSubscriber | ( | const std::shared_ptr< rclcpp::Subscription< MessageT >> & | subscriber, |
const std::string & | topic_name, | ||
const rclcpp::QoS & | publisher_qos | ||
) |
Compare a subscriber's current configuration to the desired new configuration to determine if the subscriber needs to be recreated.
This checks three criteria:
subscriber | The existing subscriber to check. |
topic_name | The topic name for the new configuration. |
publisher_qos | The publisher's QoS for the new configuration. |
MessageT | The ROS message type used to specialize this function. |
tl::expected< rclcpp::TopicEndpointInfo, std::string > moveit_studio::behaviors::waitForPublisher | ( | const std::shared_ptr< rclcpp::Node > & | node, |
const std::string & | topic_name | ||
) |
Waits for a publisher to be advertised on the provided topic, and gets the TopicEndpointInfo for the publisher once it appears.
If multiple publishers are found on the same topic, this function just returns the TopicEndpointInfo for the first one in the order they were reported by the node. If multiple publishers advertise on the same topic with different reliability QoS settings the Behavior will only receive messages from the ones whose settings match the first publisher in the list. This situation will only happen if the publishers have been incorrectly configured, so this is not handled by this Behavior.
node | Node used to retrieve publisher info. |
topic_name | Name of the topic where we expect the publisher to advertise. |
|
inlineconstexpr |
|
inlineconstexpr |
|
inlineconstexpr |
|
inlineconstexpr |
|
inlineconstexpr |
|
inlineconstexpr |
|
inlineconstexpr |
|
inlineconstexpr |
|
inlineconstexpr |
|
inlineconstexpr |
|
inlineconstexpr |
|
inlineconstexpr |
|
inlineconstexpr |
|
inlineconstexpr |
|
inlineconstexpr |
|
inlineconstexpr |
|
inlineconstexpr |
|
inlineconstexpr |
|
inlineconstexpr |
|
inlineconstexpr |
|
inlineconstexpr |
|
inlineconstexpr |
|
inlineconstexpr |
|
inlineconstexpr |
|
inlineconstexpr |
|
inlineconstexpr |