From 2e1255cf0b0a75c2b98d73ed7d1d339edd59783a Mon Sep 17 00:00:00 2001 From: redvinaa Date: Mon, 1 Jul 2024 15:28:06 +0200 Subject: [PATCH 1/4] Implement dock id --- .../include/opennav_docking/simple_charging_dock.hpp | 2 +- opennav_docking/include/opennav_docking/types.hpp | 1 + opennav_docking/include/opennav_docking/utils.hpp | 12 ++++++++++++ opennav_docking/src/docking_server.cpp | 4 ++-- opennav_docking/src/simple_charging_dock.cpp | 4 +++- 5 files changed, 19 insertions(+), 4 deletions(-) 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..d8b45ad 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,14 @@ 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", ""); + } + if (!node->get_parameter(dock_name + ".id", curr_dock.id)) { + RCLCPP_ERROR(node->get_logger(), "Dock %s has no valid 'id'.", dock_name.c_str()); + return false; + } + // 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..77ba3d7 100644 --- a/opennav_docking/src/simple_charging_dock.cpp +++ b/opennav_docking/src/simple_charging_dock.cpp @@ -166,8 +166,10 @@ 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) { + (void) id; + // If using not detection, set the dock pose to the static fixed-frame version if (!use_external_detection_pose_) { dock_pose_pub_->publish(pose); From 04a1187eaba171054edb9ec75c443dfb90357c63 Mon Sep 17 00:00:00 2001 From: redvinaa Date: Mon, 1 Jul 2024 15:28:39 +0200 Subject: [PATCH 2/4] Update tests --- opennav_docking/test/test_dock_file.yaml | 1 + opennav_docking/test/test_simple_charging_dock.cpp | 4 ++-- opennav_docking/test/test_utils.cpp | 7 +++++++ opennav_docking/test/testing_dock.cpp | 2 +- 4 files changed, 11 insertions(+), 3 deletions(-) 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; From 34d0912fd0d173e87cee18511dd35ef58f25f7c7 Mon Sep 17 00:00:00 2001 From: redvinaa Date: Mon, 1 Jul 2024 15:29:02 +0200 Subject: [PATCH 3/4] Update docs --- README.md | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index acfd1af..ed32bd9 100644 --- a/README.md +++ b/README.md @@ -96,10 +96,12 @@ dock1: type: "dockv3" frame: map pose: [0.3, 0.3, 0.0] + id: "1" dock2: type: "dockv1" frame: map pose: [0.0, 0.0, 0.4] + id: "2" ``` 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: "1" dock2: type: "dockv1" frame: map pose: [0.0, 0.0, 0.4] + id: "2" ``` -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 From 3b28febd1c035be7dc1a8c5f057c11b2f3d7eb4e Mon Sep 17 00:00:00 2001 From: redvinaa Date: Tue, 2 Jul 2024 13:02:25 +0000 Subject: [PATCH 4/4] Implement requested changes --- README.md | 8 ++++---- opennav_docking/include/opennav_docking/utils.hpp | 5 +---- opennav_docking/src/simple_charging_dock.cpp | 4 +--- 3 files changed, 6 insertions(+), 11 deletions(-) diff --git a/README.md b/README.md index ed32bd9..c3642ad 100644 --- a/README.md +++ b/README.md @@ -96,12 +96,12 @@ dock1: type: "dockv3" frame: map pose: [0.3, 0.3, 0.0] - id: "1" + id: "kitchen_dock" dock2: type: "dockv1" frame: map pose: [0.0, 0.0, 0.4] - id: "2" + 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: @@ -112,12 +112,12 @@ docks: type: "dockv3" frame: map pose: [0.3, 0.3, 0.0] - id: "1" + id: "kitchen_dock" dock2: type: "dockv1" frame: map pose: [0.0, 0.0, 0.4] - id: "2" + 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. 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. diff --git a/opennav_docking/include/opennav_docking/utils.hpp b/opennav_docking/include/opennav_docking/utils.hpp index d8b45ad..ce0b566 100644 --- a/opennav_docking/include/opennav_docking/utils.hpp +++ b/opennav_docking/include/opennav_docking/utils.hpp @@ -148,10 +148,7 @@ inline bool parseDockParams( if (!node->has_parameter(dock_name + ".id")) { node->declare_parameter(dock_name + ".id", ""); } - if (!node->get_parameter(dock_name + ".id", curr_dock.id)) { - RCLCPP_ERROR(node->get_logger(), "Dock %s has no valid 'id'.", dock_name.c_str()); - return false; - } + 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/simple_charging_dock.cpp b/opennav_docking/src/simple_charging_dock.cpp index 77ba3d7..3ff04db 100644 --- a/opennav_docking/src/simple_charging_dock.cpp +++ b/opennav_docking/src/simple_charging_dock.cpp @@ -166,10 +166,8 @@ geometry_msgs::msg::PoseStamped SimpleChargingDock::getStagingPose( return staging_pose; } -bool SimpleChargingDock::getRefinedPose(geometry_msgs::msg::PoseStamped & pose, std::string id) +bool SimpleChargingDock::getRefinedPose(geometry_msgs::msg::PoseStamped & pose, std::string /*id*/) { - (void) id; - // If using not detection, set the dock pose to the static fixed-frame version if (!use_external_detection_pose_) { dock_pose_pub_->publish(pose);