[testing sprint] final test coverage and debug logging in planner/controller servers (#1924)

* adding CLI test

* adding tests for more lines to map_server

* fix last test

* adding out of bounds and higher bounds checks

* adding planner tests for plugins working

* add cleanup

* working

* ping

* adding more testing and debug info messages

Author: SteveMacenski

nav2_controller/include/nav2_controller/nav2_controller.hpp

@@ -189,12 +189,6 @@ class ControllerServer : public nav2_util::LifecycleNode
     return twist_thresh;
   }
 
-  void pluginFailed(const std::string & name, const pluginlib::PluginlibException & ex)
-  {
-    RCLCPP_FATAL(get_logger(), "Failed to create %s. Exception: %s", name.c_str(), ex.what());
-    exit(-1);
-  }
-
   // The controller needs a costmap node
   std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
   std::unique_ptr<nav2_util::NodeThread> costmap_thread_;
@@ -234,7 +228,6 @@ class ControllerServer : public nav2_util::LifecycleNode
   double min_theta_velocity_threshold_;
 
   // Whether we've published the single controller warning yet
-  bool single_controller_warning_given_{false};
   geometry_msgs::msg::Pose end_pose_;
 };
 

nav2_controller/src/nav2_controller.cpp

@@ -113,7 +113,9 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state)
       progress_checker_id_.c_str(), progress_checker_type_.c_str());
     progress_checker_->initialize(node, progress_checker_id_);
   } catch (const pluginlib::PluginlibException & ex) {
-    pluginFailed("progress_checker", ex);
+    RCLCPP_FATAL(
+      get_logger(),
+      "Failed to create progress_checker. Exception: %s", ex.what());
   }
   try {
     goal_checker_type_ = nav2_util::get_plugin_type_param(node, goal_checker_id_);
@@ -123,7 +125,9 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state)
       goal_checker_id_.c_str(), goal_checker_type_.c_str());
     goal_checker_->initialize(node, goal_checker_id_);
   } catch (const pluginlib::PluginlibException & ex) {
-    pluginFailed("goal_checker", ex);
+    RCLCPP_FATAL(
+      get_logger(),
+      "Failed to create goal_checker. Exception: %s", ex.what());
   }
 
   for (size_t i = 0; i != controller_ids_.size(); i++) {
@@ -139,7 +143,9 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state)
         costmap_ros_->getTfBuffer(), costmap_ros_);
       controllers_.insert({controller_ids_[i], controller});
     } catch (const pluginlib::PluginlibException & ex) {
-      pluginFailed("controller", ex);
+      RCLCPP_FATAL(
+        get_logger(),
+        "Failed to create controller. Exception: %s", ex.what());
     }
   }
 
@@ -238,13 +244,10 @@ bool ControllerServer::findControllerId(
 {
   if (controllers_.find(c_name) == controllers_.end()) {
     if (controllers_.size() == 1 && c_name.empty()) {
-      if (!single_controller_warning_given_) {
-        RCLCPP_WARN(
-          get_logger(), "No controller was specified in action call."
-          " Server will use only plugin loaded %s. "
-          "This warning will appear once.", controller_ids_concat_.c_str());
-        single_controller_warning_given_ = true;
-      }
+      RCLCPP_WARN_ONCE(
+        get_logger(), "No controller was specified in action call."
+        " Server will use only plugin loaded %s. "
+        "This warning will appear once.", controller_ids_concat_.c_str());
       current_controller = controllers_.begin()->first;
     } else {
       RCLCPP_ERROR(
@@ -254,6 +257,7 @@ bool ControllerServer::findControllerId(
       return false;
     }
   } else {
+    RCLCPP_DEBUG(get_logger(), "Selected controller: %s.", c_name.c_str());
     current_controller = c_name;
   }
 
@@ -390,7 +394,12 @@ void ControllerServer::updateGlobalPath()
 void ControllerServer::publishVelocity(const geometry_msgs::msg::TwistStamped & velocity)
 {
   auto cmd_vel = std::make_unique<geometry_msgs::msg::Twist>(velocity.twist);
-  vel_publisher_->publish(std::move(cmd_vel));
+  if (
+    vel_publisher_->is_activated() &&
+    this->count_subscribers(vel_publisher_->get_topic_name()) > 0)
+  {
+    vel_publisher_->publish(std::move(cmd_vel));
+  }
 }
 
 void ControllerServer::publishZeroVelocity()

nav2_planner/src/planner_server.cpp

@@ -103,7 +103,6 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & state)
       RCLCPP_FATAL(
         get_logger(), "Failed to create global planner. Exception: %s",
         ex.what());
-      exit(-1);
     }
   }
 

nav2_system_tests/src/planning/test_planner_plugins.cpp

@@ -29,11 +29,18 @@ using nav2_util::TestCostmap;
 using ComputePathToPoseCommand = geometry_msgs::msg::PoseStamped;
 using ComputePathToPoseResult = nav_msgs::msg::Path;
 
+void callback(const nav_msgs::msg::Path::ConstSharedPtr /*grid*/)
+{
+}
+
 TEST(testPluginMap, Failures)
 {
   auto obj = std::make_shared<nav2_system_tests::NavFnPlannerTester>();
   rclcpp_lifecycle::State state;
+  obj->set_parameter(rclcpp::Parameter("expected_planner_frequency", 0.0001));
   obj->onConfigure(state);
+  obj->create_subscription<nav_msgs::msg::Path>(
+    "plan", rclcpp::SystemDefaultsQoS(), callback);
 
   geometry_msgs::msg::PoseStamped start;
   geometry_msgs::msg::PoseStamped goal;