Merge pull request #1 from mjeronimo/pose-iterator

Add a decorator node that sequences its input poses

Author: Michael Jeronimo

ros2_behavior_tree/examples/minimal/main.cpp

@@ -66,13 +66,17 @@ static const char bt_xml[] =
               <ThrottleTickRate hz="1.0">
                 <Sequence>
                   <Message msg="GetPoseNearRobot"/>
-                  <GetPoseNearRobot robot_pose="{leader_pose}" nearby_pose="{nearby_pose}"/>
-                  <Message msg="Computing path to the goal"/>
-                  <ComputePathToPose action_name="compute_path_to_pose" server_timeout="1000" ros2_node="{ros_node_2}" goal="{nearby_pose}" planner_id="GridBased" path="{path}"/>
+                  <GetPosesNearRobot robot_pose="{leader_pose}" nearby_poses="{nearby_poses}"/>
+                  <ForEachPose poses="{nearby_poses}" pose="{goal_pose}">
+                    <Sequence>
+                      <Message msg="Computing path to the goal"/>
+                      <ComputePathToPose action_name="compute_path_to_pose" server_timeout="1000" ros2_node="{ros_node_2}" goal="{goal_pose}" planner_id="GridBased" path="{path_to_follow}"/>
+                    </Sequence>
+                  </ForEachPose>
                   <AsyncWait msec="1000"/>
                 </Sequence>
               </ThrottleTickRate>
-              <FollowPath action_name="follow_path" server_timeout="1000" ros2_node="{ros_node_2}" path="{path}" controller_id="FollowPath"/>
+              <FollowPath action_name="follow_path" server_timeout="1000" ros2_node="{ros_node_2}" path="{path_to_follow}" controller_id="FollowPath"/>
             </PipelineSequence>
           </DistanceConstraint>
         </ReactiveSequence>

ros2_behavior_tree/include/ros2_behavior_tree/action/async_wait_node.hpp

@@ -67,9 +67,9 @@ class AsyncWaitNode : public BT::AsyncActionNode
     // Wait for the specified duration
     std::mutex mtx;
     std::unique_lock<std::mutex> lck(mtx);
-    auto status = cv_.wait_for(lck, std::chrono::milliseconds(wait_duration_));
+    cv_.wait_for(lck, std::chrono::milliseconds(wait_duration_));
 
-    return (status == std::cv_status::timeout) ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE;
+    return BT::NodeStatus::SUCCESS;
   }
 
 private:

ros2_behavior_tree/include/ros2_behavior_tree/action/compute_path_to_pose_node.hpp

@@ -15,9 +15,7 @@
 #ifndef ROS2_BEHAVIOR_TREE__ACTION__COMPUTE_PATH_TO_POSE_NODE_HPP_
 #define ROS2_BEHAVIOR_TREE__ACTION__COMPUTE_PATH_TO_POSE_NODE_HPP_
 
-#include <memory>
 #include <string>
-#include <vector>
 
 #include "ros2_behavior_tree/ros2_action_client_node.hpp"
 #include "nav2_msgs/action/compute_path_to_pose.hpp"

ros2_behavior_tree/include/ros2_behavior_tree/action/follow_path_node.hpp

@@ -15,9 +15,7 @@
 #ifndef ROS2_BEHAVIOR_TREE__ACTION__FOLLOW_PATH_NODE_HPP_
 #define ROS2_BEHAVIOR_TREE__ACTION__FOLLOW_PATH_NODE_HPP_
 
-#include <memory>
 #include <string>
-#include <vector>
 
 #include "ros2_behavior_tree/ros2_action_client_node.hpp"
 #include "nav2_msgs/action/follow_path.hpp"

ros2_behavior_tree/include/ros2_behavior_tree/action/get_pose_near_robot_node.hpp

@@ -0,0 +1,92 @@
+// Copyright (c) 2018 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef ROS2_BEHAVIOR_TREE__ACTION__GET_POSES_NEAR_ROBOT_NODE_HPP_
+#define ROS2_BEHAVIOR_TREE__ACTION__GET_POSES_NEAR_ROBOT_NODE_HPP_
+
+#include <string>
+#include <memory>
+#include <vector>
+
+#include "behaviortree_cpp_v3/action_node.h"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
+
+namespace ros2_behavior_tree
+{
+
+class GetPosesNearRobotNode : public BT::SyncActionNode
+{
+public:
+  GetPosesNearRobotNode(const std::string & name, const BT::NodeConfiguration & config)
+  : BT::SyncActionNode(name, config)
+  {
+  }
+
+  static BT::PortsList providedPorts()
+  {
+    return {
+      BT::InputPort<std::shared_ptr<geometry_msgs::msg::PoseStamped>>("robot_pose",
+        "The current pose of the robot"),
+      BT::OutputPort<std::vector<geometry_msgs::msg::PoseStamped>>("nearby_poses",
+        "Poses near the robot")
+    };
+  }
+
+  BT::NodeStatus tick() override
+  {
+    std::shared_ptr<geometry_msgs::msg::PoseStamped> robot_pose;
+    if (!getInput<std::shared_ptr<geometry_msgs::msg::PoseStamped>>("robot_pose", robot_pose)) {
+      throw BT::RuntimeError("Missing parameter [robot_pose] in GetPosesNearRobot node");
+    }
+
+    std::vector<geometry_msgs::msg::PoseStamped> nearby_poses;
+
+    // TODO(mjeronimo): Hard-coded values until we implement this operation
+
+    geometry_msgs::msg::PoseStamped pose1;
+    pose1.pose.position.x = robot_pose->pose.position.x - 1.2;
+    pose1.pose.position.y = robot_pose->pose.position.y;
+    pose1.pose.position.z = robot_pose->pose.position.z;
+    pose1.pose.orientation.x = robot_pose->pose.orientation.x;
+    pose1.pose.orientation.y = robot_pose->pose.orientation.y;
+    pose1.pose.orientation.z = robot_pose->pose.orientation.z;
+    pose1.pose.orientation.w = robot_pose->pose.orientation.w;
+
+    nearby_poses.push_back(pose1);
+
+    geometry_msgs::msg::PoseStamped pose2;
+    pose2.pose.position.x = robot_pose->pose.position.x;
+    pose2.pose.position.y = robot_pose->pose.position.y - 1.2;
+    pose2.pose.position.z = robot_pose->pose.position.z;
+    pose2.pose.orientation.x = robot_pose->pose.orientation.x;
+    pose2.pose.orientation.y = robot_pose->pose.orientation.y;
+    pose2.pose.orientation.z = robot_pose->pose.orientation.z;
+    pose2.pose.orientation.w = robot_pose->pose.orientation.w;
+
+    nearby_poses.push_back(pose2);
+
+    if (!setOutput<std::vector<geometry_msgs::msg::PoseStamped>>("nearby_poses", nearby_poses)) {
+      throw BT::RuntimeError(
+              "Failed to set output port value [nearby_poses] for GetPosesNearRobot");
+    }
+
+    return BT::NodeStatus::SUCCESS;
+  }
+};
+
+}  // namespace ros2_behavior_tree
+
+#endif  // ROS2_BEHAVIOR_TREE__ACTION__GET_POSES_NEAR_ROBOT_NODE_HPP_

ros2_behavior_tree/include/ros2_behavior_tree/action/get_poses_near_robot_node.hpp

@@ -0,0 +1,95 @@
+// Copyright (c) 2018 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef ROS2_BEHAVIOR_TREE__DECORATOR__FOR_EACH_POSE_NODE_HPP_
+#define ROS2_BEHAVIOR_TREE__DECORATOR__FOR_EACH_POSE_NODE_HPP_
+
+#include <string>
+#include <memory>
+#include <vector>
+
+#include "behaviortree_cpp_v3/action_node.h"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
+
+namespace ros2_behavior_tree
+{
+
+class ForEachPoseNode : public BT::DecoratorNode
+{
+public:
+  ForEachPoseNode(const std::string & name, const BT::NodeConfiguration & config)
+  : BT::DecoratorNode(name, config)
+  {
+  }
+
+  static BT::PortsList providedPorts()
+  {
+    return {
+      BT::InputPort<std::vector<geometry_msgs::msg::PoseStamped>>("poses", "Poses to iterate over"),
+      BT::OutputPort<geometry_msgs::msg::PoseStamped>("pose", "The next pose in the sequence")
+    };
+  }
+
+  BT::NodeStatus tick() override
+  {
+    std::vector<geometry_msgs::msg::PoseStamped> poses;
+    if (!getInput<std::vector<geometry_msgs::msg::PoseStamped>>("poses", poses)) {
+      throw BT::RuntimeError("Missing parameter [poses] in GetNextPose node");
+    }
+
+    if (poses.size() == 0) {
+      return BT::NodeStatus::FAILURE;
+    }
+
+    setStatus(BT::NodeStatus::RUNNING);
+
+    if (!setOutput<geometry_msgs::msg::PoseStamped>("pose", poses[0])) {
+      throw BT::RuntimeError("Failed to set output port value [next_pose] for GetNextPose");
+    }
+
+    for (int i = 0; i < poses.size(); ) {
+      if (!setOutput<geometry_msgs::msg::PoseStamped>("pose", poses[i])) {
+        throw BT::RuntimeError("Failed to set output port value [next_pose] for GetNextPose");
+      }
+
+      auto child_state = child_node_->executeTick();
+
+      switch (child_state) {
+        case BT::NodeStatus::SUCCESS:
+          return BT::NodeStatus::SUCCESS;
+
+        case BT::NodeStatus::RUNNING:
+          continue;
+
+        case BT::NodeStatus::FAILURE:
+          // Try the next one
+          i++;
+          continue;
+
+        default:
+          throw BT::LogicError("Invalid status return from BT node");
+      }
+    }
+
+    // None of the poses worked, so fail
+    child()->halt();
+    return BT::NodeStatus::FAILURE;
+  }
+};
+
+}  // namespace ros2_behavior_tree
+
+#endif  // ROS2_BEHAVIOR_TREE__DECORATOR__FOR_EACH_POSE_NODE_HPP_

ros2_behavior_tree/include/ros2_behavior_tree/decorator/for_each_pose_node.hpp

@@ -19,6 +19,7 @@
 #include <vector>
 
 #include "behaviortree_cpp_v3/xml_parsing.h"
+#include "behaviortree_cpp_v3/loggers/bt_cout_logger.h"
 #include "rclcpp/rclcpp.hpp"
 
 namespace ros2_behavior_tree
@@ -50,6 +51,8 @@ BehaviorTree::execute(
   // Create the corresponding Behavior Tree
   BT::Tree tree = xml_parser_.instantiateTree(blackboard_);
 
+  BT::StdCoutLogger logger(tree);
+
   // Set up a loop rate controller based on the desired tick period
   rclcpp::WallRate loop_rate(tick_period);
 

ros2_behavior_tree/src/behavior_tree.cpp

@@ -21,14 +21,15 @@
 #include "ros2_behavior_tree/action/create_ros2_node.hpp"
 #include "ros2_behavior_tree/action/create_transform_buffer_node.hpp"
 #include "ros2_behavior_tree/action/follow_path_node.hpp"
-#include "ros2_behavior_tree/action/get_pose_near_robot_node.hpp"
+#include "ros2_behavior_tree/action/get_poses_near_robot_node.hpp"
 #include "ros2_behavior_tree/action/transform_pose_node.hpp"
 #include "ros2_behavior_tree/condition/can_transform_node.hpp"
 #include "ros2_behavior_tree/control/first_result_node.hpp"
 #include "ros2_behavior_tree/control/pipeline_sequence_node.hpp"
 #include "ros2_behavior_tree/control/recovery_node.hpp"
 #include "ros2_behavior_tree/control/round_robin_node.hpp"
 #include "ros2_behavior_tree/decorator/distance_constraint_node.hpp"
+#include "ros2_behavior_tree/decorator/for_each_pose_node.hpp"
 #include "ros2_behavior_tree/decorator/forever_node.hpp"
 #include "ros2_behavior_tree/decorator/repeat_until_node.hpp"
 #include "ros2_behavior_tree/decorator/throttle_tick_rate_node.hpp"
@@ -57,7 +58,8 @@ NodeRegistrar::RegisterNodes(BT::BehaviorTreeFactory & factory)
   factory.registerNodeType<ros2_behavior_tree::FirstResultNode>("FirstResult");
   factory.registerNodeType<ros2_behavior_tree::FollowPathNode>("FollowPath");
   factory.registerNodeType<ros2_behavior_tree::ForeverNode>("Forever");
-  factory.registerNodeType<ros2_behavior_tree::GetPoseNearRobotNode>("GetPoseNearRobot");
+  factory.registerNodeType<ros2_behavior_tree::ForEachPoseNode>("ForEachPose");
+  factory.registerNodeType<ros2_behavior_tree::GetPosesNearRobotNode>("GetPosesNearRobot");
   factory.registerNodeType<ros2_behavior_tree::PipelineSequenceNode>("PipelineSequence");
   factory.registerNodeType<ros2_behavior_tree::RecoveryNode>("Recovery");
   factory.registerNodeType<ros2_behavior_tree::RepeatUntilNode>("RepeatUntil");