From b00dfc02411d5cd9d3a799c55f1f703525301f52 Mon Sep 17 00:00:00 2001 From: Luca Bonamini Date: Fri, 18 Jun 2021 23:55:40 +0200 Subject: [PATCH] Improve coverage for waypoint_follower package (#2407) * Working on improve coverage for waypoint_follower package * Fix typo * Fix cpplint and uncrustify errors * Fix nav_2d_utils/src/conversions.cpp include order errors (#2407) * Improve coverage for wait_at_waypoint plugin * Improve coverage for photo_at_waypoint plugin --- .../nav_2d_utils/src/conversions.cpp | 4 +- .../plugins/photo_at_waypoint.cpp | 19 +++-- .../test/test_task_executors.cpp | 74 +++++++++++++++---- 3 files changed, 75 insertions(+), 22 deletions(-) diff --git a/nav2_dwb_controller/nav_2d_utils/src/conversions.cpp b/nav2_dwb_controller/nav_2d_utils/src/conversions.cpp index 996298732d9..85cae2cb699 100644 --- a/nav2_dwb_controller/nav_2d_utils/src/conversions.cpp +++ b/nav2_dwb_controller/nav_2d_utils/src/conversions.cpp @@ -33,13 +33,13 @@ */ #include "nav_2d_utils/conversions.hpp" -#include -#include #include #include #include #include #include +#include +#include #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wpedantic" #include "tf2/utils.h" diff --git a/nav2_waypoint_follower/plugins/photo_at_waypoint.cpp b/nav2_waypoint_follower/plugins/photo_at_waypoint.cpp index 598686a4392..7d1f4a9b1b4 100644 --- a/nav2_waypoint_follower/plugins/photo_at_waypoint.cpp +++ b/nav2_waypoint_follower/plugins/photo_at_waypoint.cpp @@ -12,12 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "nav2_waypoint_follower/plugins/photo_at_waypoint.hpp" #include #include #include +#include "nav2_util/node_utils.hpp" +#include "nav2_waypoint_follower/plugins/photo_at_waypoint.hpp" namespace nav2_waypoint_follower { @@ -37,14 +38,18 @@ void PhotoAtWaypoint::initialize( curr_frame_msg_ = std::make_shared(); - node->declare_parameter(plugin_name + ".enabled", rclcpp::ParameterValue(true)); - node->declare_parameter( - plugin_name + ".image_topic", + nav2_util::declare_parameter_if_not_declared( + node, plugin_name + ".enabled", + rclcpp::ParameterValue(true)); + nav2_util::declare_parameter_if_not_declared( + node, plugin_name + ".image_topic", rclcpp::ParameterValue("/camera/color/image_raw")); - node->declare_parameter( - plugin_name + ".save_dir", + nav2_util::declare_parameter_if_not_declared( + node, plugin_name + ".save_dir", rclcpp::ParameterValue("/tmp/waypoint_images")); - node->declare_parameter(plugin_name + ".image_format", rclcpp::ParameterValue("png")); + nav2_util::declare_parameter_if_not_declared( + node, plugin_name + ".image_format", + rclcpp::ParameterValue("png")); std::string save_dir_as_string; node->get_parameter(plugin_name + ".enabled", is_enabled_); diff --git a/nav2_waypoint_follower/test/test_task_executors.cpp b/nav2_waypoint_follower/test/test_task_executors.cpp index 9b881857e1c..da0a0e99443 100644 --- a/nav2_waypoint_follower/test/test_task_executors.cpp +++ b/nav2_waypoint_follower/test/test_task_executors.cpp @@ -13,6 +13,7 @@ // limitations under the License. Reserved. #include +#include #include #include #include @@ -41,18 +42,27 @@ TEST(WaypointFollowerTest, WaitAtWaypoint) node->declare_parameter("WAW.waypoint_pause_duration", 50); - nav2_waypoint_follower::WaitAtWaypoint waw; - waw.initialize(node, std::string("WAW")); + std::unique_ptr waw( + new nav2_waypoint_follower::WaitAtWaypoint + ); + waw->initialize(node, std::string("WAW")); auto start_time = node->now(); // should wait 50ms geometry_msgs::msg::PoseStamped pose; - waw.processAtWaypoint(pose, 0); + waw->processAtWaypoint(pose, 0); auto end_time = node->now(); EXPECT_NEAR((end_time - start_time).seconds(), 0.05, 0.01); + + waw.reset(new nav2_waypoint_follower::WaitAtWaypoint); + node->set_parameter(rclcpp::Parameter("WAW.enabled", false)); + waw->initialize(node, std::string("WAW")); + + // plugin is not enabled, should exit + EXPECT_TRUE(waw->processAtWaypoint(pose, 0)); } TEST(WaypointFollowerTest, InputAtWaypoint) @@ -69,14 +79,16 @@ TEST(WaypointFollowerTest, InputAtWaypoint) rclcpp::spin_some(node->shared_from_this()->get_node_base_interface()); }; - nav2_waypoint_follower::InputAtWaypoint iaw; - iaw.initialize(node, std::string("WAW")); + std::unique_ptr iaw( + new nav2_waypoint_follower::InputAtWaypoint + ); + iaw->initialize(node, std::string("IAW")); auto start_time = node->now(); // no input, should timeout geometry_msgs::msg::PoseStamped pose; - EXPECT_FALSE(iaw.processAtWaypoint(pose, 0)); + EXPECT_FALSE(iaw->processAtWaypoint(pose, 0)); auto end_time = node->now(); @@ -84,33 +96,69 @@ TEST(WaypointFollowerTest, InputAtWaypoint) // has input now, should work std::thread t1(publish_message); - EXPECT_TRUE(iaw.processAtWaypoint(pose, 0)); + EXPECT_TRUE(iaw->processAtWaypoint(pose, 0)); t1.join(); + + iaw.reset(new nav2_waypoint_follower::InputAtWaypoint); + node->set_parameter(rclcpp::Parameter("IAW.enabled", false)); + iaw->initialize(node, std::string("IAW")); + + // plugin is not enabled, should exit + EXPECT_TRUE(iaw->processAtWaypoint(pose, 0)); } TEST(WaypointFollowerTest, PhotoAtWaypoint) { auto node = std::make_shared("testWaypointNode"); - auto pub = node->create_publisher("/camer/color/image_raw", 1); + auto pub = node->create_publisher("/camera/color/image_raw", 1); pub->on_activate(); + std::condition_variable cv; + std::mutex mtx; + std::unique_lock lck(mtx, std::defer_lock); + bool data_published = false; auto publish_message = [&, this]() -> void { rclcpp::Rate(5).sleep(); auto msg = std::make_unique(); + // fill image msg data. + msg->encoding = "rgb8"; + msg->height = 240; + msg->width = 320; + msg->step = 960; + auto size = msg->height * msg->width * 3; + msg->data.reserve(size); + int fake_data = 0; + for (size_t i = 0; i < size; i++) { + msg->data.push_back(fake_data++); + } pub->publish(std::move(msg)); rclcpp::spin_some(node->shared_from_this()->get_node_base_interface()); + lck.lock(); + data_published = true; + cv.notify_one(); + lck.unlock(); }; - nav2_waypoint_follower::PhotoAtWaypoint paw; - paw.initialize(node, std::string("WAW")); + std::unique_ptr paw( + new nav2_waypoint_follower::PhotoAtWaypoint + ); + paw->initialize(node, std::string("PAW")); // no images, throws because can't write geometry_msgs::msg::PoseStamped pose; - EXPECT_FALSE(paw.processAtWaypoint(pose, 0)); + EXPECT_FALSE(paw->processAtWaypoint(pose, 0)); - // has image now, should still fail because its invalid std::thread t1(publish_message); - EXPECT_FALSE(paw.processAtWaypoint(pose, 0)); + cv.wait(lck); + // has image now, since we force waiting until image is published + EXPECT_TRUE(paw->processAtWaypoint(pose, 0)); t1.join(); + + paw.reset(new nav2_waypoint_follower::PhotoAtWaypoint); + node->set_parameter(rclcpp::Parameter("PAW.enabled", false)); + paw->initialize(node, std::string("PAW")); + + // plugin is not enabled, should exit + EXPECT_TRUE(paw->processAtWaypoint(pose, 0)); }