add docking server to Nav2 package; migrate to msgs package

ros-navigation · Jun 8, 2024 · ee0a2c4 · ee0a2c4
1 parent fa77be7
commit ee0a2c4
Show file tree

Hide file tree

Showing 25 changed files with 112 additions and 166 deletions.
diff --git a/nav2_docking/README.md b/nav2_docking/README.md
@@ -7,7 +7,6 @@ This work is sponsored by [NVIDIA](https://www.nvidia.com/en-us/) and created by
 This is split into 4 packages
 
 - `opennav_docking`: Contains the main docking framework
-- `opennav_docking_msgs`: Contains the action interfaces for docking and undocking
 - `opennav_docking_core`: Contains the dock plugin header file to be implemented for each dock type
 - `opennav_docking_bt`: Contains behavior tree nodes and example XML files using the docking task server
 - `nova_carter_docking`: Contains an implementation using the Docking system with the Nvidia [Nova Carter](https://robotics.segway.com/nova-carter/) Robot platform and dock.

diff --git a/nav2_docking/opennav_docking/CMakeLists.txt b/nav2_docking/opennav_docking/CMakeLists.txt
@@ -54,7 +54,6 @@ set(dependencies
   tf2_geometry_msgs
   pluginlib
   yaml_cpp_vendor
-  opennav_docking_msgs
   opennav_docking_core
 )
 

diff --git a/nav2_docking/opennav_docking/include/opennav_docking/dock_database.hpp b/nav2_docking/opennav_docking/include/opennav_docking/dock_database.hpp
@@ -28,7 +28,7 @@
 #include "nav2_util/simple_action_server.hpp"
 #include "opennav_docking/utils.hpp"
 #include "opennav_docking/types.hpp"
-#include "opennav_docking_msgs/srv/reload_database.hpp"
+#include "nav2_msgs/srv/reload_dock_database.hpp"
 
 namespace opennav_docking
 {
@@ -123,14 +123,14 @@ class DockDatabase
    * @param response Service response
    */
   void reloadDbCb(
-    const std::shared_ptr<opennav_docking_msgs::srv::ReloadDatabase::Request> request,
-    std::shared_ptr<opennav_docking_msgs::srv::ReloadDatabase::Response> response);
+    const std::shared_ptr<nav2_msgs::srv::ReloadDockDatabase::Request> request,
+    std::shared_ptr<nav2_msgs::srv::ReloadDockDatabase::Response> response);
 
   rclcpp_lifecycle::LifecycleNode::WeakPtr node_;
   DockPluginMap dock_plugins_;
   DockMap dock_instances_;
   pluginlib::ClassLoader<opennav_docking_core::ChargingDock> dock_loader_;
-  rclcpp::Service<opennav_docking_msgs::srv::ReloadDatabase>::SharedPtr reload_db_service_;
+  rclcpp::Service<nav2_msgs::srv::ReloadDockDatabase>::SharedPtr reload_db_service_;
 };
 
 }  // namespace opennav_docking

diff --git a/nav2_docking/opennav_docking/include/opennav_docking/types.hpp b/nav2_docking/opennav_docking/include/opennav_docking/types.hpp
@@ -18,13 +18,13 @@
 #include <unordered_map>
 #include <string>
 
-#include "opennav_docking_msgs/action/dock_robot.hpp"
-#include "opennav_docking_msgs/action/undock_robot.hpp"
+#include "nav2_msgs/action/dock_robot.hpp"
+#include "nav2_msgs/action/undock_robot.hpp"
 #include "opennav_docking_core/charging_dock.hpp"
 #include "opennav_docking_core/docking_exceptions.hpp"
 
-typedef opennav_docking_msgs::action::DockRobot DockRobot;
-typedef opennav_docking_msgs::action::UndockRobot UndockRobot;
+typedef nav2_msgs::action::DockRobot DockRobot;
+typedef nav2_msgs::action::UndockRobot UndockRobot;
 
 /**
 * @struct A dock instance struct for a database

diff --git a/nav2_docking/opennav_docking/package.xml b/nav2_docking/opennav_docking/package.xml
@@ -22,7 +22,6 @@
   <depend>sensor_msgs</depend>
   <depend>pluginlib</depend>
   <depend>yaml_cpp_vendor</depend>
-  <depend>opennav_docking_msgs</depend>
   <depend>opennav_docking_core</depend>
   <depend>tf2_ros</depend>
 

diff --git a/nav2_docking/opennav_docking/src/dock_database.cpp b/nav2_docking/opennav_docking/src/dock_database.cpp
@@ -42,7 +42,7 @@ bool DockDatabase::initialize(
     return true;
   }
 
-  reload_db_service_ = node->create_service<opennav_docking_msgs::srv::ReloadDatabase>(
+  reload_db_service_ = node->create_service<nav2_msgs::srv::ReloadDockDatabase>(
     "~/reload_database",
     std::bind(
       &DockDatabase::reloadDbCb, this,
@@ -68,8 +68,8 @@ void DockDatabase::deactivate()
 }
 
 void DockDatabase::reloadDbCb(
-  const std::shared_ptr<opennav_docking_msgs::srv::ReloadDatabase::Request> request,
-  std::shared_ptr<opennav_docking_msgs::srv::ReloadDatabase::Response> response)
+  const std::shared_ptr<nav2_msgs::srv::ReloadDockDatabase::Request> request,
+  std::shared_ptr<nav2_msgs::srv::ReloadDockDatabase::Response> response)
 {
   DockMap dock_instances;
   if (utils::parseDockFile(request->filepath, node_.lock(), dock_instances)) {

diff --git a/nav2_docking/opennav_docking/test/test_dock_database.cpp b/nav2_docking/opennav_docking/test/test_dock_database.cpp
@@ -109,9 +109,9 @@ TEST(DatabaseTests, reloadDbService)
 
   // Call service with a filepath
   auto client =
-    node->create_client<opennav_docking_msgs::srv::ReloadDatabase>("test/reload_database");
+    node->create_client<nav2_msgs::srv::ReloadDockDatabase>("test/reload_database");
 
-  auto request = std::make_shared<opennav_docking_msgs::srv::ReloadDatabase::Request>();
+  auto request = std::make_shared<nav2_msgs::srv::ReloadDockDatabase::Request>();
   request->filepath = ament_index_cpp::get_package_share_directory("opennav_docking") +
     "/test_dock_file.yaml";
   EXPECT_TRUE(client->wait_for_service(1s));
@@ -122,7 +122,7 @@ TEST(DatabaseTests, reloadDbService)
   EXPECT_TRUE(result.get()->success);
 
   // Try again with a bogus file
-  auto request2 = std::make_shared<opennav_docking_msgs::srv::ReloadDatabase::Request>();
+  auto request2 = std::make_shared<nav2_msgs::srv::ReloadDockDatabase::Request>();
   request2->filepath = ament_index_cpp::get_package_share_directory("opennav_docking") +
     "/file_does_not_exist.yaml";
   EXPECT_TRUE(client->wait_for_service(1s));

diff --git a/nav2_docking/opennav_docking/test/test_docking_server.py b/nav2_docking/opennav_docking/test/test_docking_server.py
@@ -22,7 +22,7 @@
 from launch_ros.actions import Node
 import launch_testing
 from nav2_msgs.action import NavigateToPose
-from opennav_docking_msgs.action import DockRobot, UndockRobot
+from nav2_msgs.action import DockRobot, UndockRobot
 import pytest
 import rclpy
 from rclpy.action import ActionClient, ActionServer

diff --git a/nav2_docking/opennav_docking_bt/CMakeLists.txt b/nav2_docking/opennav_docking_bt/CMakeLists.txt
@@ -11,7 +11,6 @@ find_package(nav2_core REQUIRED)
 find_package(nav2_behavior_tree REQUIRED)
 find_package(nav_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
-find_package(opennav_docking_msgs REQUIRED)
 find_package(behaviortree_cpp REQUIRED)
 
 # potentially replace with nav2_common, nav2_package()
@@ -32,7 +31,6 @@ set(dependencies
   nav2_behavior_tree
   nav_msgs
   geometry_msgs
-  opennav_docking_msgs
   behaviortree_cpp
 )
 

diff --git a/nav2_docking/opennav_docking_bt/include/opennav_docking_bt/dock_robot.hpp b/nav2_docking/opennav_docking_bt/include/opennav_docking_bt/dock_robot.hpp
@@ -19,21 +19,21 @@
 #include <memory>
 #include <vector>
 
-#include "opennav_docking_msgs/action/dock_robot.hpp"
+#include "nav2_msgs/action/dock_robot.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "nav2_behavior_tree/bt_action_node.hpp"
 
 namespace opennav_docking_bt
 {
 
 /**
- * @brief nav2_behavior_tree::BtActionNode class that wraps opennav_docking_msgs/DockRobot
+ * @brief nav2_behavior_tree::BtActionNode class that wraps opnav2_msgsennav_docking_msgs/DockRobot
  */
 class DockRobotAction
   : public nav2_behavior_tree::BtActionNode<
-    opennav_docking_msgs::action::DockRobot>
+    nav2_msgs::action::DockRobot>
 {
-  using Action = opennav_docking_msgs::action::DockRobot;
+  using Action = nav2_msgs::action::DockRobot;
   using ActionResult = Action::Result;
 
 public:

diff --git a/nav2_docking/opennav_docking_bt/include/opennav_docking_bt/undock_robot.hpp b/nav2_docking/opennav_docking_bt/include/opennav_docking_bt/undock_robot.hpp
@@ -19,21 +19,21 @@
 #include <memory>
 #include <vector>
 
-#include "opennav_docking_msgs/action/undock_robot.hpp"
+#include "nav2_msgs/action/undock_robot.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "nav2_behavior_tree/bt_action_node.hpp"
 
 namespace opennav_docking_bt
 {
 
 /**
- * @brief nav2_behavior_tree::BtActionNode class that wraps opennav_docking_msgs/UndockRobot
+ * @brief nav2_behavior_tree::BtActionNode class that wraps nav2_msgs/UndockRobot
  */
 class UndockRobotAction
   : public nav2_behavior_tree::BtActionNode<
-    opennav_docking_msgs::action::UndockRobot>
+    nav2_msgs::action::UndockRobot>
 {
-  using Action = opennav_docking_msgs::action::UndockRobot;
+  using Action = nav2_msgs::action::UndockRobot;
   using ActionResult = Action::Result;
 
 public:

diff --git a/nav2_docking/opennav_docking_bt/package.xml b/nav2_docking/opennav_docking_bt/package.xml
@@ -17,7 +17,6 @@
   <depend>nav2_msgs</depend>
   <depend>nav_msgs</depend>
   <depend>geometry_msgs</depend>
-  <depend>opennav_docking_msgs</depend>
   <depend>behaviortree_cpp</depend>
 
   <test_depend>ament_lint_common</test_depend>

diff --git a/nav2_docking/opennav_docking_bt/test/test_dock_robot.cpp b/nav2_docking/opennav_docking_bt/test/test_dock_robot.cpp
@@ -22,10 +22,10 @@
 #include "behaviortree_cpp/bt_factory.h"
 
 #include "nav2_behavior_tree/utils/test_action_server.hpp"
-#include "opennav_docking_bt/dock_robot.hpp"
+#include "nav2_msgs/dock_robot.hpp"
 
 class DockRobotActionServer
-  : public TestActionServer<opennav_docking_msgs::action::DockRobot>
+  : public TestActionServer<nav2_msgs::action::DockRobot>
 {
 public:
   DockRobotActionServer()
@@ -35,13 +35,13 @@ class DockRobotActionServer
 protected:
   void execute(
     const typename std::shared_ptr<
-      rclcpp_action::ServerGoalHandle<opennav_docking_msgs::action::DockRobot>>
+      rclcpp_action::ServerGoalHandle<nav2_msgs::action::DockRobot>>
     goal_handle)
   override
   {
     const auto goal = goal_handle->get_goal();
     auto result =
-      std::make_shared<opennav_docking_msgs::action::DockRobot::Result>();
+      std::make_shared<nav2_msgs::action::DockRobot::Result>();
     result->success = true;
     goal_handle->succeed(result);
   }

diff --git a/nav2_docking/opennav_docking_bt/test/test_undock_robot.cpp b/nav2_docking/opennav_docking_bt/test/test_undock_robot.cpp
@@ -25,7 +25,7 @@
 #include "opennav_docking_bt/undock_robot.hpp"
 
 class UndockRobotActionServer
-  : public TestActionServer<opennav_docking_msgs::action::UndockRobot>
+  : public TestActionServer<nav2_msgs::action::UndockRobot>
 {
 public:
   UndockRobotActionServer()
@@ -35,13 +35,13 @@ class UndockRobotActionServer
 protected:
   void execute(
     const typename std::shared_ptr<
-      rclcpp_action::ServerGoalHandle<opennav_docking_msgs::action::UndockRobot>>
+      rclcpp_action::ServerGoalHandle<nav2_msgs::action::UndockRobot>>
     goal_handle)
   override
   {
     const auto goal = goal_handle->get_goal();
     auto result =
-      std::make_shared<opennav_docking_msgs::action::UndockRobot::Result>();
+      std::make_shared<nav2_msgs::action::UndockRobot::Result>();
     result->success = true;
     goal_handle->succeed(result);
   }

diff --git a/nav2_docking/opennav_docking_core/CMakeLists.txt b/nav2_docking/opennav_docking_core/CMakeLists.txt
@@ -5,15 +5,15 @@ find_package(ament_cmake REQUIRED)
 find_package(nav2_common REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_lifecycle REQUIRED)
-find_package(opennav_docking_msgs REQUIRED)
 find_package(nav2_util REQUIRED)
+find_package(nav2_msgs REQUIRED)
 
 nav2_package()
 
 set(dependencies
   rclcpp
   rclcpp_lifecycle
-  opennav_docking_msgs
+  nav2_msgs
   nav2_util
 )
 

diff --git a/nav2_docking/opennav_docking_core/package.xml b/nav2_docking/opennav_docking_core/package.xml
@@ -12,7 +12,7 @@
   <depend>rclcpp</depend>
   <depend>rclcpp_lifecycle</depend>
   <depend>nav2_util</depend>
-  <depend>opennav_docking_msgs</depend>
+  <depend>nav2_msgs</depend>
 
   <test_depend>ament_lint_common</test_depend>
   <test_depend>ament_lint_auto</test_depend>

diff --git a/nav2_docking/opennav_docking_msgs/CMakeLists.txt b/nav2_docking/opennav_docking_msgs/CMakeLists.txt
diff --git a/nav2_docking/opennav_docking_msgs/README.md b/nav2_docking/opennav_docking_msgs/README.md
diff --git a/nav2_docking/opennav_docking_msgs/action/DockRobot.action b/nav2_docking/opennav_docking_msgs/action/DockRobot.action
diff --git a/nav2_docking/opennav_docking_msgs/action/UndockRobot.action b/nav2_docking/opennav_docking_msgs/action/UndockRobot.action