ROS2 docking manager plugins (#394)

* Added base of BT docking plugins Signed-off-by: Jakub Delicat <[email protected]> * Teste dock and undock plugins Signed-off-by: Jakub Delicat <[email protected]> * Removed test dock manager Signed-off-by: Jakub Delicat <[email protected]> * run precommit Signed-off-by: Jakub Delicat <[email protected]> * moved plugins Signed-off-by: Jakub Delicat <[email protected]> * changed names of files and classes | changed DockRobotAction to DockRobot Signed-off-by: Jakub Delicat <[email protected]> * Fixed copyright Signed-off-by: Jakub Delicat <[email protected]> * Changed timeouts | changed using Signed-off-by: Jakub Delicat <[email protected]> --------- Signed-off-by: Jakub Delicat <[email protected]>
husarion · Aug 28, 2024 · 3bc8f34 · 3bc8f34
1 parent 8dcbd36
commit 3bc8f34
Show file tree

Hide file tree

Showing 8 changed files with 765 additions and 6 deletions.
diff --git a/panther_manager/CMakeLists.txt b/panther_manager/CMakeLists.txt
@@ -17,7 +17,8 @@ set(PACKAGE_DEPENDENCIES
     rclcpp_action
     sensor_msgs
     std_srvs
-    yaml-cpp)
+    yaml-cpp
+    opennav_docking_msgs)
 
 foreach(PACKAGE IN ITEMS ${PACKAGE_DEPENDENCIES})
   find_package(${PACKAGE} REQUIRED)
@@ -51,6 +52,13 @@ add_library(shutdown_hosts_from_file_bt_node SHARED
 target_link_libraries(shutdown_hosts_from_file_bt_node ssh yaml-cpp)
 list(APPEND plugin_libs shutdown_hosts_from_file_bt_node)
 
+add_library(dock_robot_bt_node SHARED src/plugins/action/dock_robot_node.cpp)
+list(APPEND plugin_libs dock_robot_bt_node)
+
+add_library(undock_robot_bt_node SHARED
+            src/plugins/action/undock_robot_node.cpp)
+list(APPEND plugin_libs undock_robot_bt_node)
+
 add_library(tick_after_timeout_bt_node SHARED
             src/plugins/decorator/tick_after_timeout_node.cpp)
 list(APPEND plugin_libs tick_after_timeout_bt_node)
@@ -136,6 +144,19 @@ if(BUILD_TESTING)
     src/plugins/action/shutdown_hosts_from_file_node.cpp)
   list(APPEND plugin_tests ${PROJECT_NAME}_test_shutdown_hosts_from_file_node)
 
+  ament_add_gtest(
+    ${PROJECT_NAME}_test_dock_robot_node
+    test/plugins/action/test_dock_robot_node.cpp
+    src/plugins/action/dock_robot_node.cpp)
+  list(APPEND plugin_tests ${PROJECT_NAME}_test_dock_robot_node)
+
+  ament_add_gtest(
+    ${PROJECT_NAME}_test_undock_robot_node
+    test/plugins/action/test_undock_robot_node.cpp
+    src/plugins/action/undock_robot_node.cpp)
+
+  list(APPEND plugin_tests ${PROJECT_NAME}_test_undock_robot_node)
+
   ament_add_gtest(
     ${PROJECT_NAME}_test_tick_after_timeout_node
     test/plugins/decorator/test_tick_after_timeout_node.cpp

diff --git a/panther_manager/include/panther_manager/plugins/action/dock_robot_node.hpp b/panther_manager/include/panther_manager/plugins/action/dock_robot_node.hpp
@@ -0,0 +1,72 @@
+// Copyright 2024 Husarion sp. z o.o.
+//
+// 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 PANTHER_MANAGER_PLUGINS_ACTION_DOCK_ROBOT_NODE_HPP_
+#define PANTHER_MANAGER_PLUGINS_ACTION_DOCK_ROBOT_NODE_HPP_
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <behaviortree_ros2/bt_action_node.hpp>
+
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <opennav_docking_msgs/action/dock_robot.hpp>
+
+namespace panther_manager
+{
+
+class DockRobot : public BT::RosActionNode<opennav_docking_msgs::action::DockRobot>
+{
+  using DockRobotAction = opennav_docking_msgs::action::DockRobot;
+  using DockRobotActionResult = DockRobotAction::Result;
+
+public:
+  DockRobot(const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params)
+  : RosActionNode<DockRobotAction>(name, conf, params)
+  {
+  }
+
+  bool setGoal(Goal & goal) override;
+
+  void onHalt() override;
+
+  BT::NodeStatus onResultReceived(const WrappedResult & wr) override;
+
+  virtual BT::NodeStatus onFailure(BT::ActionNodeErrorCode error) override;
+
+  static BT::PortsList providedPorts()
+  {
+    return providedBasicPorts({
+      BT::InputPort<bool>("use_dock_id", true, "Whether to use the dock's ID or dock pose fields"),
+      BT::InputPort<std::string>("dock_id", "Dock ID or name to use"),
+      BT::InputPort<std::string>("dock_type", "The dock plugin type, if using dock pose"),
+      BT::InputPort<float>(
+        "max_staging_time", 1000.0, "Maximum time to navigate to the staging pose"),
+      BT::InputPort<bool>(
+        "navigate_to_staging_pose", true, "Whether to autonomously navigate to staging pose"),
+
+      BT::OutputPort<DockRobotActionResult::_success_type>(
+        "success", "If the action was successful"),
+      BT::OutputPort<DockRobotActionResult::_error_code_type>(
+        "error_code", "Contextual error code, if any"),
+      BT::OutputPort<DockRobotActionResult::_num_retries_type>(
+        "num_retries", "Number of retries attempted"),
+    });
+  }
+};
+
+}  // namespace panther_manager
+
+#endif  // PANTHER_MANAGER_PLUGINS_ACTION_DOCK_ROBOT_NODE_HPP_
diff --git a/panther_manager/include/panther_manager/plugins/action/undock_robot_node.hpp b/panther_manager/include/panther_manager/plugins/action/undock_robot_node.hpp
@@ -0,0 +1,67 @@
+// Copyright 2024 Husarion sp. z o.o.
+//
+// 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 PANTHER_MANAGER_PLUGINS_ACTION_UNDOCK_ROBOT_NODE_HPP_
+#define PANTHER_MANAGER_PLUGINS_ACTION_UNDOCK_ROBOT_NODE_HPP_
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <behaviortree_ros2/bt_action_node.hpp>
+
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <opennav_docking_msgs/action/undock_robot.hpp>
+
+namespace panther_manager
+{
+
+class UndockRobot : public BT::RosActionNode<opennav_docking_msgs::action::UndockRobot>
+{
+  using UndockRobotAction = opennav_docking_msgs::action::UndockRobot;
+  using UndockRobotActionResult = UndockRobotAction::Result;
+
+public:
+  UndockRobot(
+    const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params)
+  : RosActionNode<UndockRobotAction>(name, conf, params)
+  {
+  }
+
+  bool setGoal(Goal & goal) override;
+
+  void onHalt() override;
+
+  BT::NodeStatus onResultReceived(const WrappedResult & wr) override;
+
+  virtual BT::NodeStatus onFailure(BT::ActionNodeErrorCode error) override;
+
+  static BT::PortsList providedPorts()
+  {
+    return providedBasicPorts({
+      BT::InputPort<std::string>(
+        "dock_type", "The dock plugin type, if not previous instance used for docking"),
+      BT::InputPort<float>(
+        "max_undocking_time", 30.0, "Maximum time to get back to the staging pose"),
+
+      BT::OutputPort<UndockRobotActionResult::_success_type>(
+        "success", "If the action was successful"),
+      BT::OutputPort<UndockRobotActionResult::_error_code_type>("error_code", "Error code"),
+    });
+  }
+};
+
+}  // namespace panther_manager
+
+#endif  // PANTHER_MANAGER_PLUGINS_ACTION_UNDOCK_ROBOT_NODE_HPP_
diff --git a/panther_manager/src/plugins/action/dock_robot_node.cpp b/panther_manager/src/plugins/action/dock_robot_node.cpp
@@ -0,0 +1,92 @@
+// Copyright 2024 Husarion sp. z o.o.
+//
+// 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.
+
+#include "panther_manager/plugins/action/dock_robot_node.hpp"
+#include "panther_manager/behavior_tree_utils.hpp"
+
+namespace panther_manager
+{
+
+bool DockRobot::setGoal(Goal & goal)
+{
+  if (!this->getInput<std::string>("dock_type", goal.dock_type) || goal.dock_type.empty()) {
+    RCLCPP_ERROR_STREAM(
+      this->logger(), GetLoggerPrefix(name()) << "Failed to get input [dock_type]");
+    return false;
+  }
+
+  if (!this->getInput<bool>("use_dock_id", goal.use_dock_id)) {
+    goal.use_dock_id = false;
+    RCLCPP_WARN_STREAM(
+      this->logger(), GetLoggerPrefix(name())
+                        << "use_dock_id not set, using default value: " << goal.use_dock_id);
+  }
+
+  if (
+    (!this->getInput<std::string>("dock_id", goal.dock_id) || goal.dock_id.empty()) &&
+    goal.use_dock_id) {
+    RCLCPP_ERROR_STREAM(this->logger(), GetLoggerPrefix(name()) << "Failed to get input [dock_id]");
+    return false;
+  }
+
+  if (!this->getInput<bool>("navigate_to_staging_pose", goal.navigate_to_staging_pose)) {
+    goal.navigate_to_staging_pose = false;
+    RCLCPP_WARN_STREAM(
+      this->logger(), GetLoggerPrefix(name()) << "navigate_to_staging_pose not set, using default "
+                                                 "value: "
+                                              << goal.navigate_to_staging_pose);
+  }
+
+  if (
+    !this->getInput<float>("max_staging_time", goal.max_staging_time) &&
+    goal.navigate_to_staging_pose) {
+    RCLCPP_ERROR_STREAM(
+      this->logger(), GetLoggerPrefix(name()) << "Failed to get input [max_staging_time]");
+    return false;
+  }
+
+  return true;
+}
+
+BT::NodeStatus DockRobot::onResultReceived(const WrappedResult & wr)
+{
+  const auto & result = wr.result;
+
+  this->setOutput("success", result->success);
+  this->setOutput("error_code", result->error_code);
+  this->setOutput("num_retries", result->num_retries);
+
+  if (result->success) {
+    return BT::NodeStatus::SUCCESS;
+  }
+
+  return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus DockRobot::onFailure(BT::ActionNodeErrorCode error)
+{
+  RCLCPP_ERROR_STREAM(
+    this->logger(), GetLoggerPrefix(name()) << ": onFailure with error: " << toStr(error));
+  return BT::NodeStatus::FAILURE;
+}
+
+void DockRobot::onHalt()
+{
+  RCLCPP_INFO_STREAM(this->logger(), GetLoggerPrefix(name()) << ": onHalt");
+}
+
+}  // namespace panther_manager
+
+#include "behaviortree_ros2/plugins.hpp"
+CreateRosNodePlugin(panther_manager::DockRobot, "DockRobot");
diff --git a/panther_manager/src/plugins/action/undock_robot_node.cpp b/panther_manager/src/plugins/action/undock_robot_node.cpp
@@ -0,0 +1,67 @@
+// Copyright 2024 Husarion sp. z o.o.
+//
+// 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.
+
+#include "panther_manager/plugins/action/undock_robot_node.hpp"
+#include "panther_manager/behavior_tree_utils.hpp"
+
+namespace panther_manager
+{
+
+bool UndockRobot::setGoal(Goal & goal)
+{
+  if (!this->getInput<std::string>("dock_type", goal.dock_type) || goal.dock_type.empty()) {
+    RCLCPP_ERROR_STREAM(
+      this->logger(), GetLoggerPrefix(name()) << "Failed to get input [dock_type]");
+    return false;
+  }
+
+  if (!this->getInput<float>("max_undocking_time", goal.max_undocking_time)) {
+    RCLCPP_ERROR_STREAM(
+      this->logger(), GetLoggerPrefix(name()) << "Failed to get input [max_undocking_time]");
+    return false;
+  }
+
+  return true;
+}
+
+BT::NodeStatus UndockRobot::onResultReceived(const WrappedResult & wr)
+{
+  const auto & result = wr.result;
+
+  this->setOutput("success", result->success);
+  this->setOutput("error_code", result->error_code);
+
+  if (result->success) {
+    return BT::NodeStatus::SUCCESS;
+  }
+
+  return BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus UndockRobot::onFailure(BT::ActionNodeErrorCode error)
+{
+  RCLCPP_ERROR_STREAM(
+    this->logger(), GetLoggerPrefix(name()) << ": onFailure with error: " << toStr(error));
+  return BT::NodeStatus::FAILURE;
+}
+
+void UndockRobot::onHalt()
+{
+  RCLCPP_INFO_STREAM(this->logger(), GetLoggerPrefix(name()) << ": onHalt");
+}
+
+}  // namespace panther_manager
+
+#include "behaviortree_ros2/plugins.hpp"
+CreateRosNodePlugin(panther_manager::UndockRobot, "UndockRobot");