Add dock

README.md

@@ -96,10 +96,12 @@ dock1:
   type: "dockv3"
   frame: map
   pose: [0.3, 0.3, 0.0]
+  id: "kitchen_dock"
 dock2:
   type: "dockv1"
   frame: map
   pose: [0.0, 0.0, 0.4]
+  id: "42"
 ```
 
 If you'd prefer to specify the docks using an external file, you may use the `dock_database` parameter to specify the filepath to the yaml file. The file should be laid out like:
@@ -110,13 +112,15 @@ docks:
     type: "dockv3"
     frame: map
     pose: [0.3, 0.3, 0.0]
+    id: "kitchen_dock"
   dock2:
     type: "dockv1"
     frame: map
     pose: [0.0, 0.0, 0.4]
+    id: "42"
 ```
 
-Note that you may leave the `type` to an empty string **if** there is only one type of dock being used. The `frame` will also default to `map` if not otherwise specified. The `type` and `pose` fields are required. Note also that these can be in any frame, not just map (i.e. `odom`, `base_link`, etc) in both the database and action requests.
+Note that you may leave the `type` to an empty string **if** there is only one type of dock being used. The `frame` will also default to `map` if not otherwise specified. The `type` and `pose` fields are required. Note also that these can be in any frame, not just map (i.e. `odom`, `base_link`, etc) in both the database and action requests. You may also specify the `id` field, for example to select the associated AprilTag. If the dock plugin does not use it, you can leave it unspecified.
 
 ## Dock Plugin API
 

opennav_docking/include/opennav_docking/simple_charging_dock.hpp

@@ -81,7 +81,7 @@ class SimpleChargingDock : public opennav_docking_core::ChargingDock
    * @param pose The initial estimate of the dock pose.
    * @param frame The frame of the initial estimate.
    */
-  virtual bool getRefinedPose(geometry_msgs::msg::PoseStamped & pose);
+  virtual bool getRefinedPose(geometry_msgs::msg::PoseStamped & pose, std::string id);
 
   /**
    * @copydoc opennav_docking_core::ChargingDock::isDocked

opennav_docking/include/opennav_docking/types.hpp

@@ -39,6 +39,7 @@ struct Dock
   geometry_msgs::msg::Pose pose;
   std::string frame;
   std::string type;
+  std::string id;
   opennav_docking_core::ChargingDock::Ptr plugin{nullptr};
 };
 

opennav_docking/include/opennav_docking/utils.hpp

@@ -95,6 +95,10 @@ inline bool parseDockFile(
     curr_dock.pose.position.y = pose_arr[1];
     curr_dock.pose.orientation = orientationAroundZAxis(pose_arr[2]);
 
+    if (dock_attribs["id"]) {
+      curr_dock.id = dock_attribs["id"].as<std::string>();
+    }
+
     // Insert into dock instance database
     dock_db.emplace(dock_name, curr_dock);
   }
@@ -141,6 +145,11 @@ inline bool parseDockParams(
     curr_dock.pose.position.y = pose_arr[1];
     curr_dock.pose.orientation = orientationAroundZAxis(pose_arr[2]);
 
+    if (!node->has_parameter(dock_name + ".id")) {
+      node->declare_parameter(dock_name + ".id", "");
+    }
+    node->get_parameter(dock_name + ".id", curr_dock.id);
+
     // Insert into dock instance database
     dock_db.emplace(dock_name, curr_dock);
   }

opennav_docking/src/docking_server.cpp

@@ -364,7 +364,7 @@ void DockingServer::doInitialPerception(Dock * dock, geometry_msgs::msg::PoseSta
   rclcpp::Rate loop_rate(controller_frequency_);
   auto start = this->now();
   auto timeout = rclcpp::Duration::from_seconds(initial_perception_timeout_);
-  while (!dock->plugin->getRefinedPose(dock_pose)) {
+  while (!dock->plugin->getRefinedPose(dock_pose, dock->id)) {
     if (this->now() - start > timeout) {
       throw opennav_docking_core::FailedToDetectDock("Failed initial dock detection");
     }
@@ -400,7 +400,7 @@ bool DockingServer::approachDock(Dock * dock, geometry_msgs::msg::PoseStamped &
     }
 
     // Update perception
-    if (!dock->plugin->getRefinedPose(dock_pose)) {
+    if (!dock->plugin->getRefinedPose(dock_pose, dock->id)) {
       throw opennav_docking_core::FailedToDetectDock("Failed dock detection");
     }
 

opennav_docking/src/simple_charging_dock.cpp

@@ -166,7 +166,7 @@ geometry_msgs::msg::PoseStamped SimpleChargingDock::getStagingPose(
   return staging_pose;
 }
 
-bool SimpleChargingDock::getRefinedPose(geometry_msgs::msg::PoseStamped & pose)
+bool SimpleChargingDock::getRefinedPose(geometry_msgs::msg::PoseStamped & pose, std::string /*id*/)
 {
   // If using not detection, set the dock pose to the static fixed-frame version
   if (!use_external_detection_pose_) {

opennav_docking/test/test_dock_file.yaml

@@ -6,3 +6,4 @@ docks:
   dock2:
     type: "dockv1"
     pose: [0.0, 0.0, 0.4]
+    id: "2"

opennav_docking/test/test_simple_charging_dock.cpp

@@ -219,7 +219,7 @@ TEST(SimpleChargingDockTests, RefinedPoseTest)
 
   // Timestamps are outdated; this is after timeout
   EXPECT_FALSE(dock->isDocked());
-  EXPECT_FALSE(dock->getRefinedPose(pose));
+  EXPECT_FALSE(dock->getRefinedPose(pose, ""));
 
   geometry_msgs::msg::PoseStamped detected_pose;
   detected_pose.header.stamp = node->now();
@@ -230,7 +230,7 @@ TEST(SimpleChargingDockTests, RefinedPoseTest)
   rclcpp::spin_some(node->get_node_base_interface());
 
   pose.header.frame_id = "my_frame";
-  EXPECT_TRUE(dock->getRefinedPose(pose));
+  EXPECT_TRUE(dock->getRefinedPose(pose, ""));
   EXPECT_NEAR(pose.pose.position.x, 0.1, 0.01);
   EXPECT_NEAR(pose.pose.position.y, -0.1, 0.01);
 

opennav_docking/test/test_utils.cpp

@@ -60,6 +60,9 @@ TEST(UtilsTests, parseDockParams2)
   node->declare_parameter("dockC.pose", rclcpp::ParameterValue(dock_pose));
   node->declare_parameter("dockD.pose", rclcpp::ParameterValue(dock_pose));
 
+  // Don't declare C, check if empty string default
+  node->declare_parameter("dockD.id", rclcpp::ParameterValue("D"));
+
   std::vector<std::string> docks_param;
   node->get_parameter("docks", docks_param);
 
@@ -69,6 +72,8 @@ TEST(UtilsTests, parseDockParams2)
   EXPECT_EQ(db["dockC"].type, std::string("typeA"));
   EXPECT_EQ(db["dockC"].pose.position.x, 0.3);
   EXPECT_EQ(db["dockC"].pose.position.y, 0.3);
+  EXPECT_EQ(db["dockC"].id, std::string(""));
+  EXPECT_EQ(db["dockD"].id, std::string("D"));
 }
 
 TEST(UtilsTests, parseDockParams3)
@@ -108,6 +113,8 @@ TEST(UtilsTests, parseDockFile)
   EXPECT_EQ(db["dock2"].pose.position.x, 0.0);
   EXPECT_EQ(db["dock2"].pose.position.y, 0.0);
   EXPECT_NE(db["dock2"].pose.orientation.w, 1.0);
+  EXPECT_EQ(db["dock1"].id, std::string(""));
+  EXPECT_EQ(db["dock2"].id, std::string("2"));
 }
 
 TEST(UtilsTests, testgetDockPoseStamped)

opennav_docking/test/testing_dock.cpp

@@ -72,7 +72,7 @@ class TestFailureDock : public opennav_docking_core::ChargingDock
     return geometry_msgs::msg::PoseStamped();
   }
 
-  virtual bool getRefinedPose(geometry_msgs::msg::PoseStamped &)
+  virtual bool getRefinedPose(geometry_msgs::msg::PoseStamped &, std::string)
   {
     // Always return false to trigger a timeout, when no exceptions are thrown
     return false;

opennav_docking_core/include/opennav_docking_core/charging_dock.hpp

@@ -81,7 +81,7 @@ class ChargingDock
    * @param pose The initial estimate of the dock pose.
    * @param frame The frame of the initial estimate.
    */
-  virtual bool getRefinedPose(geometry_msgs::msg::PoseStamped & pose) = 0;
+  virtual bool getRefinedPose(geometry_msgs::msg::PoseStamped & pose, std::string id) = 0;
 
   /**
    * @brief Have we made contact with dock? This can be implemented in a variety