diff --git a/nav2_docking/opennav_docking/test/CMakeLists.txt b/nav2_docking/opennav_docking/test/CMakeLists.txt index a49f245c92..3400c1d8db 100644 --- a/nav2_docking/opennav_docking/test/CMakeLists.txt +++ b/nav2_docking/opennav_docking/test/CMakeLists.txt @@ -52,6 +52,15 @@ ament_target_dependencies(test_simple_charging_dock target_link_libraries(test_simple_charging_dock ${library_name} simple_charging_dock ) +ament_add_gtest(test_simple_non_charging_dock + test_simple_non_charging_dock.cpp +) +ament_target_dependencies(test_simple_non_charging_dock + ${dependencies} +) +target_link_libraries(test_simple_non_charging_dock + ${library_name} simple_non_charging_dock +) # Test Pose Filter (unit) ament_add_gtest(test_pose_filter diff --git a/nav2_docking/opennav_docking/test/test_simple_charging_dock.cpp b/nav2_docking/opennav_docking/test/test_simple_charging_dock.cpp index 6bc7439129..7193bb188b 100644 --- a/nav2_docking/opennav_docking/test/test_simple_charging_dock.cpp +++ b/nav2_docking/opennav_docking/test/test_simple_charging_dock.cpp @@ -48,6 +48,7 @@ TEST(SimpleChargingDockTests, ObjectLifecycle) EXPECT_FALSE(dock->isCharging()); EXPECT_TRUE(dock->disableCharging()); EXPECT_TRUE(dock->hasStoppedCharging()); + EXPECT_TRUE(dock->isCharger()); dock->deactivate(); dock->cleanup(); @@ -66,6 +67,8 @@ TEST(SimpleChargingDockTests, BatteryState) dock->configure(node, "my_dock", nullptr); dock->activate(); + geometry_msgs::msg::PoseStamped pose; + EXPECT_TRUE(dock->getRefinedPose(pose, "")); // Below threshold sensor_msgs::msg::BatteryState msg; diff --git a/nav2_docking/opennav_docking/test/test_simple_non_charging_dock.cpp b/nav2_docking/opennav_docking/test/test_simple_non_charging_dock.cpp new file mode 100644 index 0000000000..89c423f5ff --- /dev/null +++ b/nav2_docking/opennav_docking/test/test_simple_non_charging_dock.cpp @@ -0,0 +1,205 @@ +// Copyright (c) 2024 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "sensor_msgs/msg/battery_state.hpp" +#include "sensor_msgs/msg/joint_state.hpp" +#include "opennav_docking/simple_non_charging_dock.hpp" +#include "ament_index_cpp/get_package_share_directory.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "tf2/utils.h" + +// Testing the simple non-charging dock plugin + +class RosLockGuard +{ +public: + RosLockGuard() {rclcpp::init(0, nullptr);} + ~RosLockGuard() {rclcpp::shutdown();} +}; +RosLockGuard g_rclcpp; + +namespace opennav_docking +{ + +TEST(SimpleNonChargingDockTests, ObjectLifecycle) +{ + auto node = std::make_shared("test"); + node->declare_parameter("my_dock.use_external_detection_pose", rclcpp::ParameterValue(true)); + + auto dock = std::make_unique(); + dock->configure(node, "my_dock", nullptr); + dock->activate(); + + // Check initial states + EXPECT_THROW(dock->isCharging(), std::runtime_error); + EXPECT_THROW(dock->disableCharging(), std::runtime_error); + EXPECT_THROW(dock->hasStoppedCharging(), std::runtime_error); + EXPECT_FALSE(dock->isCharger()); + + dock->deactivate(); + dock->cleanup(); + dock.reset(); +} + +TEST(SimpleNonChargingDockTests, StallDetection) +{ + auto node = std::make_shared("test"); + auto pub = node->create_publisher( + "joint_states", rclcpp::QoS(1)); + pub->on_activate(); + node->declare_parameter("my_dock.use_stall_detection", rclcpp::ParameterValue(true)); + std::vector names = {"left_motor", "right_motor"}; + node->declare_parameter("my_dock.stall_joint_names", rclcpp::ParameterValue(names)); + node->declare_parameter("my_dock.stall_velocity_threshold", rclcpp::ParameterValue(0.1)); + node->declare_parameter("my_dock.stall_effort_threshold", rclcpp::ParameterValue(5.0)); + + auto dock = std::make_unique(); + + dock->configure(node, "my_dock", nullptr); + dock->activate(); + geometry_msgs::msg::PoseStamped pose; + EXPECT_TRUE(dock->getRefinedPose(pose, "")); + + // Stopped, but below effort threshold + sensor_msgs::msg::JointState msg; + msg.name = {"left_motor", "right_motor", "another_motor"}; + msg.velocity = {0.0, 0.0, 0.0}; + msg.effort = {0.0, 0.0, 0.0}; + pub->publish(msg); + rclcpp::Rate r(2); + r.sleep(); + rclcpp::spin_some(node->get_node_base_interface()); + + EXPECT_FALSE(dock->isDocked()); + + // Moving, with effort + sensor_msgs::msg::JointState msg2; + msg2.name = {"left_motor", "right_motor", "another_motor"}; + msg2.velocity = {1.0, 1.0, 0.0}; + msg2.effort = {5.1, -5.1, 0.0}; + pub->publish(msg2); + rclcpp::Rate r1(2); + r1.sleep(); + rclcpp::spin_some(node->get_node_base_interface()); + + EXPECT_FALSE(dock->isDocked()); + + // Stopped, with effort + sensor_msgs::msg::JointState msg3; + msg3.name = {"left_motor", "right_motor", "another_motor"}; + msg3.velocity = {0.0, 0.0, 0.0}; + msg3.effort = {5.1, -5.1, 0.0}; + pub->publish(msg3); + rclcpp::Rate r2(2); + r2.sleep(); + rclcpp::spin_some(node->get_node_base_interface()); + + EXPECT_TRUE(dock->isDocked()); + + dock->deactivate(); + dock->cleanup(); + dock.reset(); +} + +TEST(SimpleNonChargingDockTests, StagingPose) +{ + auto node = std::make_shared("test"); + auto dock = std::make_unique(); + + dock->configure(node, "my_dock", nullptr); + dock->activate(); + + geometry_msgs::msg::Pose pose; + std::string frame = "my_frame"; + auto staging_pose = dock->getStagingPose(pose, frame); + EXPECT_NEAR(staging_pose.pose.position.x, -0.7, 0.01); + EXPECT_NEAR(staging_pose.pose.position.y, 0.0, 0.01); + EXPECT_NEAR(tf2::getYaw(staging_pose.pose.orientation), 0.0, 0.01); + EXPECT_EQ(staging_pose.header.frame_id, frame); + + dock->deactivate(); + dock->cleanup(); + dock.reset(); +} + +TEST(SimpleNonChargingDockTests, StagingPoseWithYawOffset) +{ + // Override the parameter default + rclcpp::NodeOptions options; + options.parameter_overrides( + { + {"my_dock.staging_yaw_offset", 3.14}, + } + ); + + auto node = std::make_shared("test", options); + auto dock = std::make_unique(); + + dock->configure(node, "my_dock", nullptr); + dock->activate(); + + geometry_msgs::msg::Pose pose; + std::string frame = "my_frame"; + auto staging_pose = dock->getStagingPose(pose, frame); + // Pose should be the same as default, but pointing in opposite direction + EXPECT_NEAR(staging_pose.pose.position.x, -0.7, 0.01); + EXPECT_NEAR(staging_pose.pose.position.y, 0.0, 0.01); + EXPECT_NEAR(tf2::getYaw(staging_pose.pose.orientation), 3.14, 0.01); + EXPECT_EQ(staging_pose.header.frame_id, frame); + + dock->deactivate(); + dock->cleanup(); + dock.reset(); +} + +TEST(SimpleNonChargingDockTests, RefinedPoseTest) +{ + auto node = std::make_shared("test"); + node->declare_parameter("my_dock.use_external_detection_pose", rclcpp::ParameterValue(true)); + auto pub = node->create_publisher( + "detected_dock_pose", rclcpp::QoS(1)); + pub->on_activate(); + auto dock = std::make_unique(); + + dock->configure(node, "my_dock", nullptr); + dock->activate(); + + geometry_msgs::msg::PoseStamped pose; + + // Timestamps are outdated; this is after timeout + EXPECT_FALSE(dock->isDocked()); + EXPECT_FALSE(dock->getRefinedPose(pose, "")); + + geometry_msgs::msg::PoseStamped detected_pose; + detected_pose.header.stamp = node->now(); + detected_pose.header.frame_id = "my_frame"; + detected_pose.pose.position.x = 0.1; + detected_pose.pose.position.y = -0.5; + pub->publish(detected_pose); + rclcpp::spin_some(node->get_node_base_interface()); + + pose.header.frame_id = "my_frame"; + EXPECT_TRUE(dock->getRefinedPose(pose, "")); + EXPECT_NEAR(pose.pose.position.x, 0.1, 0.01); + EXPECT_NEAR(pose.pose.position.y, -0.3, 0.01); // Applies external_detection_translation_x, +0.2 + + dock->deactivate(); + dock->cleanup(); + dock.reset(); +} + +} // namespace opennav_docking diff --git a/nav2_docking/opennav_docking_bt/src/dock_robot.cpp b/nav2_docking/opennav_docking_bt/src/dock_robot.cpp index 2f46b8cb76..fb0c93016d 100644 --- a/nav2_docking/opennav_docking_bt/src/dock_robot.cpp +++ b/nav2_docking/opennav_docking_bt/src/dock_robot.cpp @@ -31,7 +31,8 @@ DockRobotAction::DockRobotAction( void DockRobotAction::on_tick() { // Get core inputs about what to perform - if (getInput("use_dock_id", goal_.use_dock_id)) { + [[maybe_unused]] auto res = getInput("use_dock_id", goal_.use_dock_id); + if (goal_.use_dock_id) { getInput("dock_id", goal_.dock_id); } else { getInput("dock_pose", goal_.dock_pose);