Add unit tests for follow path, compute path to pose, and navigate to pose BT action nodes (#1844)

* Add compute path to pose unit tests

Signed-off-by: Sarthak Mittal <[email protected]>

* Add follow path action unit tests

Signed-off-by: Sarthak Mittal <[email protected]>

* Add navigate to pose action unit tests

Signed-off-by: Sarthak Mittal <[email protected]>

Author: naiveHobo

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp

@@ -0,0 +1,55 @@
+// 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 NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__COMPUTE_PATH_TO_POSE_ACTION_HPP_
+#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__COMPUTE_PATH_TO_POSE_ACTION_HPP_
+
+#include <string>
+
+#include "nav2_msgs/action/compute_path_to_pose.hpp"
+#include "nav_msgs/msg/path.h"
+#include "nav2_behavior_tree/bt_action_node.hpp"
+
+namespace nav2_behavior_tree
+{
+
+class ComputePathToPoseAction : public BtActionNode<nav2_msgs::action::ComputePathToPose>
+{
+public:
+  ComputePathToPoseAction(
+    const std::string & xml_tag_name,
+    const std::string & action_name,
+    const BT::NodeConfiguration & conf);
+
+  void on_tick() override;
+
+  BT::NodeStatus on_success() override;
+
+  static BT::PortsList providedPorts()
+  {
+    return providedBasicPorts(
+      {
+        BT::OutputPort<nav_msgs::msg::Path>("path", "Path created by ComputePathToPose node"),
+        BT::InputPort<geometry_msgs::msg::PoseStamped>("goal", "Destination to plan to"),
+        BT::InputPort<std::string>("planner_id", ""),
+      });
+  }
+
+private:
+  bool first_time_{true};
+};
+
+}  // namespace nav2_behavior_tree
+
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__COMPUTE_PATH_TO_POSE_ACTION_HPP_

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp

@@ -0,0 +1,50 @@
+// 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 NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__FOLLOW_PATH_ACTION_HPP_
+#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__FOLLOW_PATH_ACTION_HPP_
+
+#include <string>
+
+#include "nav2_msgs/action/follow_path.hpp"
+#include "nav2_behavior_tree/bt_action_node.hpp"
+
+namespace nav2_behavior_tree
+{
+
+class FollowPathAction : public BtActionNode<nav2_msgs::action::FollowPath>
+{
+public:
+  FollowPathAction(
+    const std::string & xml_tag_name,
+    const std::string & action_name,
+    const BT::NodeConfiguration & conf);
+
+  void on_tick() override;
+
+  void on_wait_for_result() override;
+
+  static BT::PortsList providedPorts()
+  {
+    return providedBasicPorts(
+      {
+        BT::InputPort<nav_msgs::msg::Path>("path", "Path to follow"),
+        BT::InputPort<std::string>("controller_id", ""),
+      });
+  }
+};
+
+}  // namespace nav2_behavior_tree
+
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__FOLLOW_PATH_ACTION_HPP_

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_to_pose_action.hpp

@@ -0,0 +1,51 @@
+// 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 NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__NAVIGATE_TO_POSE_ACTION_HPP_
+#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__NAVIGATE_TO_POSE_ACTION_HPP_
+
+#include <string>
+
+#include "geometry_msgs/msg/point.hpp"
+#include "geometry_msgs/msg/quaternion.hpp"
+#include "nav2_msgs/action/navigate_to_pose.hpp"
+#include "nav2_behavior_tree/bt_action_node.hpp"
+
+namespace nav2_behavior_tree
+{
+
+class NavigateToPoseAction : public BtActionNode<nav2_msgs::action::NavigateToPose>
+{
+public:
+  NavigateToPoseAction(
+    const std::string & xml_tag_name,
+    const std::string & action_name,
+    const BT::NodeConfiguration & conf);
+
+  void on_tick() override;
+
+  // Any BT node that accepts parameters must provide a requiredNodeParameters method
+  static BT::PortsList providedPorts()
+  {
+    return providedBasicPorts(
+      {
+        BT::InputPort<geometry_msgs::msg::Point>("position", "Position"),
+        BT::InputPort<geometry_msgs::msg::Quaternion>("orientation", "Orientation")
+      });
+  }
+};
+
+}  // namespace nav2_behavior_tree
+
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__NAVIGATE_TO_POSE_ACTION_HPP_

nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp

@@ -12,61 +12,39 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef NAV2_BEHAVIOR_TREE__COMPUTE_PATH_TO_POSE_ACTION_HPP_
-#define NAV2_BEHAVIOR_TREE__COMPUTE_PATH_TO_POSE_ACTION_HPP_
-
 #include <memory>
 #include <string>
 
-#include "nav2_msgs/action/compute_path_to_pose.hpp"
-#include "nav_msgs/msg/path.h"
-#include "nav2_behavior_tree/bt_action_node.hpp"
+#include "nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp"
 
 namespace nav2_behavior_tree
 {
 
-class ComputePathToPoseAction : public BtActionNode<nav2_msgs::action::ComputePathToPose>
+ComputePathToPoseAction::ComputePathToPoseAction(
+  const std::string & xml_tag_name,
+  const std::string & action_name,
+  const BT::NodeConfiguration & conf)
+: BtActionNode<nav2_msgs::action::ComputePathToPose>(xml_tag_name, action_name, conf)
 {
-public:
-  ComputePathToPoseAction(
-    const std::string & xml_tag_name,
-    const std::string & action_name,
-    const BT::NodeConfiguration & conf)
-  : BtActionNode<nav2_msgs::action::ComputePathToPose>(xml_tag_name, action_name, conf)
-  {
-  }
-
-  void on_tick() override
-  {
-    getInput("goal", goal_.pose);
-    getInput("planner_id", goal_.planner_id);
-  }
+}
 
-  BT::NodeStatus on_success() override
-  {
-    setOutput("path", result_.result->path);
+void ComputePathToPoseAction::on_tick()
+{
+  getInput("goal", goal_.pose);
+  getInput("planner_id", goal_.planner_id);
+}
 
-    if (first_time_) {
-      first_time_ = false;
-    } else {
-      config().blackboard->set("path_updated", true);
-    }
-    return BT::NodeStatus::SUCCESS;
-  }
+BT::NodeStatus ComputePathToPoseAction::on_success()
+{
+  setOutput("path", result_.result->path);
 
-  static BT::PortsList providedPorts()
-  {
-    return providedBasicPorts(
-      {
-        BT::OutputPort<nav_msgs::msg::Path>("path", "Path created by ComputePathToPose node"),
-        BT::InputPort<geometry_msgs::msg::PoseStamped>("goal", "Destination to plan to"),
-        BT::InputPort<std::string>("planner_id", ""),
-      });
+  if (first_time_) {
+    first_time_ = false;
+  } else {
+    config().blackboard->set("path_updated", true);
   }
-
-private:
-  bool first_time_{true};
-};
+  return BT::NodeStatus::SUCCESS;
+}
 
 }  // namespace nav2_behavior_tree
 
@@ -83,5 +61,3 @@ BT_REGISTER_NODES(factory)
   factory.registerBuilder<nav2_behavior_tree::ComputePathToPoseAction>(
     "ComputePathToPose", builder);
 }
-
-#endif  // NAV2_BEHAVIOR_TREE__COMPUTE_PATH_TO_POSE_ACTION_HPP_

nav2_behavior_tree/plugins/action/follow_path_action.cpp

@@ -12,59 +12,42 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef NAV2_BEHAVIOR_TREE__FOLLOW_PATH_ACTION_HPP_
-#define NAV2_BEHAVIOR_TREE__FOLLOW_PATH_ACTION_HPP_
-
 #include <memory>
 #include <string>
 
-#include "nav2_msgs/action/follow_path.hpp"
-#include "nav2_behavior_tree/bt_action_node.hpp"
+#include "nav2_behavior_tree/plugins/action/follow_path_action.hpp"
 
 namespace nav2_behavior_tree
 {
 
-class FollowPathAction : public BtActionNode<nav2_msgs::action::FollowPath>
+FollowPathAction::FollowPathAction(
+  const std::string & xml_tag_name,
+  const std::string & action_name,
+  const BT::NodeConfiguration & conf)
+: BtActionNode<nav2_msgs::action::FollowPath>(xml_tag_name, action_name, conf)
 {
-public:
-  FollowPathAction(
-    const std::string & xml_tag_name,
-    const std::string & action_name,
-    const BT::NodeConfiguration & conf)
-  : BtActionNode<nav2_msgs::action::FollowPath>(xml_tag_name, action_name, conf)
-  {
-    config().blackboard->set("path_updated", false);
-  }
-
-  void on_tick() override
-  {
-    getInput("path", goal_.path);
-    getInput("controller_id", goal_.controller_id);
-  }
+  config().blackboard->set("path_updated", false);
+}
 
-  void on_wait_for_result() override
-  {
-    // Check if the goal has been updated
-    if (config().blackboard->get<bool>("path_updated")) {
-      // Reset the flag in the blackboard
-      config().blackboard->set("path_updated", false);
+void FollowPathAction::on_tick()
+{
+  getInput("path", goal_.path);
+  getInput("controller_id", goal_.controller_id);
+}
 
-      // Grab the new goal and set the flag so that we send the new goal to
-      // the action server on the next loop iteration
-      getInput("path", goal_.path);
-      goal_updated_ = true;
-    }
-  }
+void FollowPathAction::on_wait_for_result()
+{
+  // Check if the goal has been updated
+  if (config().blackboard->get<bool>("path_updated")) {
+    // Reset the flag in the blackboard
+    config().blackboard->set("path_updated", false);
 
-  static BT::PortsList providedPorts()
-  {
-    return providedBasicPorts(
-      {
-        BT::InputPort<nav_msgs::msg::Path>("path", "Path to follow"),
-        BT::InputPort<std::string>("controller_id", ""),
-      });
+    // Grab the new goal and set the flag so that we send the new goal to
+    // the action server on the next loop iteration
+    getInput("path", goal_.path);
+    goal_updated_ = true;
   }
-};
+}
 
 }  // namespace nav2_behavior_tree
 
@@ -81,5 +64,3 @@ BT_REGISTER_NODES(factory)
   factory.registerBuilder<nav2_behavior_tree::FollowPathAction>(
     "FollowPath", builder);
 }
-
-#endif  // NAV2_BEHAVIOR_TREE__FOLLOW_PATH_ACTION_HPP_