From e042ec1b13a38a66632d655e90a5e0c8c291129e Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 2 Jul 2024 08:31:41 -0700 Subject: [PATCH] Revert "Add dock id (#46)" This reverts commit 3fb0491f5a356ef79f9cf131654b0d8cdf3326bb. --- README.md | 6 +----- .../include/opennav_docking/simple_charging_dock.hpp | 2 +- opennav_docking/include/opennav_docking/types.hpp | 1 - opennav_docking/include/opennav_docking/utils.hpp | 9 --------- opennav_docking/src/docking_server.cpp | 4 ++-- opennav_docking/src/simple_charging_dock.cpp | 2 +- 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 +- 10 files changed, 8 insertions(+), 30 deletions(-) diff --git a/README.md b/README.md index c3642ad..acfd1af 100644 --- a/README.md +++ b/README.md @@ -96,12 +96,10 @@ 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: @@ -112,15 +110,13 @@ 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. 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. +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. ## 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 2b87744..93c3166 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, std::string id); + virtual bool getRefinedPose(geometry_msgs::msg::PoseStamped & pose); /** * @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 430181e..a7f038d 100644 --- a/opennav_docking/include/opennav_docking/types.hpp +++ b/opennav_docking/include/opennav_docking/types.hpp @@ -39,7 +39,6 @@ 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 ce0b566..8b0182b 100644 --- a/opennav_docking/include/opennav_docking/utils.hpp +++ b/opennav_docking/include/opennav_docking/utils.hpp @@ -95,10 +95,6 @@ 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); } @@ -145,11 +141,6 @@ 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 75d428e..c70e4b2 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, dock->id)) { + while (!dock->plugin->getRefinedPose(dock_pose)) { 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, dock->id)) { + if (!dock->plugin->getRefinedPose(dock_pose)) { 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 3ff04db..bccc121 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, std::string /*id*/) +bool SimpleChargingDock::getRefinedPose(geometry_msgs::msg::PoseStamped & pose) { // 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 2e92bf5..2b0ba54 100644 --- a/opennav_docking/test/test_dock_file.yaml +++ b/opennav_docking/test/test_dock_file.yaml @@ -6,4 +6,3 @@ 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 e778d72..c97f443 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 f8474e9..fe98d40 100644 --- a/opennav_docking/test/test_utils.cpp +++ b/opennav_docking/test/test_utils.cpp @@ -60,9 +60,6 @@ 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); @@ -72,8 +69,6 @@ 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) @@ -113,8 +108,6 @@ 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 bb4b622..a519245 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 &, std::string) + virtual bool getRefinedPose(geometry_msgs::msg::PoseStamped &) { // Always return false to trigger a timeout, when no exceptions are thrown return false;