diff --git a/README.md b/README.md index acfd1af..c3642ad 100644 --- a/README.md +++ b/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 diff --git a/opennav_docking/include/opennav_docking/simple_charging_dock.hpp b/opennav_docking/include/opennav_docking/simple_charging_dock.hpp index 93c3166..2b87744 100644 --- a/opennav_docking/include/opennav_docking/simple_charging_dock.hpp +++ b/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 diff --git a/opennav_docking/include/opennav_docking/types.hpp b/opennav_docking/include/opennav_docking/types.hpp index a7f038d..430181e 100644 --- a/opennav_docking/include/opennav_docking/types.hpp +++ b/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}; }; diff --git a/opennav_docking/include/opennav_docking/utils.hpp b/opennav_docking/include/opennav_docking/utils.hpp index 8b0182b..ce0b566 100644 --- a/opennav_docking/include/opennav_docking/utils.hpp +++ b/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(); + } + // 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); } diff --git a/opennav_docking/src/docking_server.cpp b/opennav_docking/src/docking_server.cpp index c70e4b2..75d428e 100644 --- a/opennav_docking/src/docking_server.cpp +++ b/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"); } diff --git a/opennav_docking/src/simple_charging_dock.cpp b/opennav_docking/src/simple_charging_dock.cpp index bccc121..3ff04db 100644 --- a/opennav_docking/src/simple_charging_dock.cpp +++ b/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_) { diff --git a/opennav_docking/test/test_dock_file.yaml b/opennav_docking/test/test_dock_file.yaml index 2b0ba54..2e92bf5 100644 --- a/opennav_docking/test/test_dock_file.yaml +++ b/opennav_docking/test/test_dock_file.yaml @@ -6,3 +6,4 @@ docks: dock2: type: "dockv1" pose: [0.0, 0.0, 0.4] + id: "2" diff --git a/opennav_docking/test/test_simple_charging_dock.cpp b/opennav_docking/test/test_simple_charging_dock.cpp index c97f443..e778d72 100644 --- a/opennav_docking/test/test_simple_charging_dock.cpp +++ b/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); diff --git a/opennav_docking/test/test_utils.cpp b/opennav_docking/test/test_utils.cpp index fe98d40..f8474e9 100644 --- a/opennav_docking/test/test_utils.cpp +++ b/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 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) diff --git a/opennav_docking/test/testing_dock.cpp b/opennav_docking/test/testing_dock.cpp index a519245..bb4b622 100644 --- a/opennav_docking/test/testing_dock.cpp +++ b/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;