|
| 1 | +// Copyright (c) 2018 Intel Corporation |
| 2 | +// |
| 3 | +// Licensed under the Apache License, Version 2.0 (the "License"); |
| 4 | +// you may not use this file except in compliance with the License. |
| 5 | +// You may obtain a copy of the License at |
| 6 | +// |
| 7 | +// http://www.apache.org/licenses/LICENSE-2.0 |
| 8 | +// |
| 9 | +// Unless required by applicable law or agreed to in writing, software |
| 10 | +// distributed under the License is distributed on an "AS IS" BASIS, |
| 11 | +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
| 12 | +// See the License for the specific language governing permissions and |
| 13 | +// limitations under the License. |
| 14 | + |
| 15 | +#ifndef ROS2_BEHAVIOR_TREE__CAN_TRANSFORM_NODE_HPP_ |
| 16 | +#define ROS2_BEHAVIOR_TREE__CAN_TRANSFORM_NODE_HPP_ |
| 17 | + |
| 18 | +#include <string> |
| 19 | +#include <memory> |
| 20 | + |
| 21 | +#include "behaviortree_cpp_v3/action_node.h" |
| 22 | +#include "geometry_msgs/msg/pose_stamped.hpp" |
| 23 | +#include "rclcpp/rclcpp.hpp" |
| 24 | +#include "tf2_geometry_msgs/tf2_geometry_msgs.h" |
| 25 | +#include "tf2_ros/buffer.h" |
| 26 | + |
| 27 | +namespace ros2_behavior_tree |
| 28 | +{ |
| 29 | + |
| 30 | +class CanTransformNode : public BT::SyncActionNode |
| 31 | +{ |
| 32 | +public: |
| 33 | + CanTransformNode(const std::string & name, const BT::NodeConfiguration & config) |
| 34 | + : BT::SyncActionNode(name, config) |
| 35 | + { |
| 36 | + } |
| 37 | + |
| 38 | + static BT::PortsList providedPorts() |
| 39 | + { |
| 40 | + return { |
| 41 | + BT::InputPort<std::shared_ptr<rclcpp::Node>>("node_handle", "The ROS2 node to use"), |
| 42 | + BT::InputPort<std::shared_ptr<tf2_ros::Buffer>>("transform_buffer", "The transform buffer to use"), |
| 43 | + BT::InputPort<std::string>("source_frame", "The source frame for the transform"), |
| 44 | + BT::InputPort<std::string>("target_frame", "The target frame for the transform") |
| 45 | + }; |
| 46 | + } |
| 47 | + |
| 48 | + BT::NodeStatus tick() override |
| 49 | + { |
| 50 | + std::shared_ptr<rclcpp::Node> node; |
| 51 | + if (!getInput<std::shared_ptr<rclcpp::Node>>("node_handle", node)) { |
| 52 | + throw BT::RuntimeError("Missing parameter [node_handle] in CanTransform node"); |
| 53 | + } |
| 54 | + |
| 55 | + std::shared_ptr<tf2_ros::Buffer> tf_buffer; |
| 56 | + if (!getInput<std::shared_ptr<tf2_ros::Buffer>>("transform_buffer", tf_buffer)) { |
| 57 | + throw BT::RuntimeError("Missing parameter [transform_buffer] in CanTransform node"); |
| 58 | + } |
| 59 | + |
| 60 | + std::string source_frame; |
| 61 | + if (!getInput<std::string>("source_frame", source_frame)) { |
| 62 | + throw BT::RuntimeError("Missing parameter [source_frame] in CanTransform node"); |
| 63 | + } |
| 64 | + |
| 65 | + std::string target_frame; |
| 66 | + if (!getInput<std::string>("target_frame", target_frame)) { |
| 67 | + throw BT::RuntimeError("Missing parameter [target_frame] in CanTransform node"); |
| 68 | + } |
| 69 | + |
| 70 | + rclcpp::Time transform_time = node->now(); |
| 71 | + std::string tf_error; |
| 72 | + double tf_tolerance = 1.0; |
| 73 | + |
| 74 | + if (tf_buffer->canTransform(target_frame, source_frame, |
| 75 | + tf2_ros::fromMsg(transform_time), tf2::durationFromSec(tf_tolerance), &tf_error)) |
| 76 | + { |
| 77 | + return BT::NodeStatus::SUCCESS; |
| 78 | + } else { |
| 79 | + RCLCPP_ERROR(node->get_logger(), "Transform from %s to %s with tolerance %.2f failed: %s", |
| 80 | + source_frame.c_str(), target_frame.c_str(), tf_tolerance, tf_error.c_str()); |
| 81 | + return BT::NodeStatus::FAILURE; |
| 82 | + } |
| 83 | + } |
| 84 | +}; |
| 85 | + |
| 86 | +} // namespace ros2_behavior_tree |
| 87 | + |
| 88 | +#endif // ROS2_BEHAVIOR_TREE__CAN_TRANSFORM_NODE_HPP_ |
0 commit comments