Improve coverage for waypoint_follower package (ros-navigation#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 (ros-navigation#2407)

* Improve coverage for wait_at_waypoint plugin

* Improve coverage for photo_at_waypoint plugin

nav2_dwb_controller/nav_2d_utils/src/conversions.cpp

@@ -33,13 +33,13 @@
  */
 
 #include "nav_2d_utils/conversions.hpp"
-#include <vector>
-#include <string>
 #include <geometry_msgs/msg/pose.hpp>
 #include <geometry_msgs/msg/pose2_d.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <geometry_msgs/msg/twist.hpp>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#include <vector>
+#include <string>
 #pragma GCC diagnostic push
 #pragma GCC diagnostic ignored "-Wpedantic"
 #include "tf2/utils.h"

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 <pluginlib/class_list_macros.hpp>
 
 #include <string>
 #include <memory>
 
+#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<sensor_msgs::msg::Image>();
 
-  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_);

nav2_waypoint_follower/test/test_task_executors.cpp

@@ -13,6 +13,7 @@
 // limitations under the License. Reserved.
 
 #include <math.h>
+#include <condition_variable>
 #include <memory>
 #include <string>
 #include <vector>
@@ -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<nav2_waypoint_follower::WaitAtWaypoint> 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,48 +79,86 @@ 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<nav2_waypoint_follower::InputAtWaypoint> 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();
 
   EXPECT_NEAR((end_time - start_time).seconds(), 10.0, 0.1);
 
   // 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<rclcpp_lifecycle::LifecycleNode>("testWaypointNode");
-  auto pub = node->create_publisher<sensor_msgs::msg::Image>("/camer/color/image_raw", 1);
+  auto pub = node->create_publisher<sensor_msgs::msg::Image>("/camera/color/image_raw", 1);
   pub->on_activate();
+  std::condition_variable cv;
+  std::mutex mtx;
+  std::unique_lock<std::mutex> lck(mtx, std::defer_lock);
+  bool data_published = false;
   auto publish_message =
     [&, this]() -> void
     {
       rclcpp::Rate(5).sleep();
       auto msg = std::make_unique<sensor_msgs::msg::Image>();
+      // 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<nav2_waypoint_follower::PhotoAtWaypoint> 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));
 }