[Test sprint] push nav2_map_server over 90% (#1922)

* 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

Author: SteveMacenski

nav2_controller/src/nav2_controller.cpp

@@ -63,8 +63,6 @@ ControllerServer::ControllerServer()
 ControllerServer::~ControllerServer()
 {
   RCLCPP_INFO(get_logger(), "Destroying");
-  controllers_.clear();
-  costmap_thread_.reset();
 }
 
 nav2_util::CallbackReturn
@@ -404,6 +402,8 @@ void ControllerServer::publishZeroVelocity()
   velocity.twist.linear.x = 0;
   velocity.twist.linear.y = 0;
   velocity.twist.linear.z = 0;
+  velocity.header.frame_id = costmap_ros_->getBaseFrameID();
+  velocity.header.stamp = now();
   publishVelocity(velocity);
 }
 

nav2_map_server/test/component/test_map_server_node.cpp

@@ -58,6 +58,7 @@ class MapServerTestFixture : public ::testing::Test
   {
     lifecycle_client_->change_state(Transition::TRANSITION_DEACTIVATE);
     lifecycle_client_->change_state(Transition::TRANSITION_CLEANUP);
+    lifecycle_client_->change_state(Transition::TRANSITION_UNCONFIGURED_SHUTDOWN);
   }
 
   template<class T>

nav2_map_server/test/map_saver_cli/test_map_saver_cli.cpp

@@ -47,6 +47,10 @@ TEST(MapSaverCLI, CLITest)
   msg->info.origin.position.y = 0.0;
   msg->info.origin.orientation.w = 1.0;
   msg->data.resize(9);
+  msg->data[0] = 0;
+  msg->data[2] = 100;
+  msg->data[1] = 101;
+  msg->data[3] = 50;
 
   RCLCPP_INFO(node->get_logger(), "Publishing occupancy grid...");
 
@@ -105,24 +109,38 @@ TEST(MapSaverCLI, CLITest)
   return_code = system(command.c_str());
   EXPECT_EQ(return_code, 0);
 
+  rclcpp::Rate(0.5).sleep();
+
   RCLCPP_INFO(node->get_logger(), "Testing invalid mode...");
   command =
     std::string(
     "ros2 run nav2_map_server map_saver_cli --mode fake_mode");
   return_code = system(command.c_str());
   EXPECT_EQ(return_code, 0);
 
+  rclcpp::Rate(0.5).sleep();
+
   RCLCPP_INFO(node->get_logger(), "Testing missing argument...");
   command =
     std::string(
     "ros2 run nav2_map_server map_saver_cli --mode");
   return_code = system(command.c_str());
   EXPECT_EQ(return_code, 65280);
 
+  rclcpp::Rate(0.5).sleep();
+
   RCLCPP_INFO(node->get_logger(), "Testing wrong argument...");
   command =
     std::string(
     "ros2 run nav2_map_server map_saver_cli --free 0 0");
   return_code = system(command.c_str());
   EXPECT_EQ(return_code, 65280);
+
+  rclcpp::Rate(0.5).sleep();
+
+  command =
+    std::string(
+    "ros2 run nav2_map_server map_saver_cli --ros-args -r __node:=map_saver_test_node");
+  return_code = system(command.c_str());
+  EXPECT_EQ(return_code, 0);
 }

nav2_navfn_planner/src/navfn_planner.cpp

@@ -152,6 +152,9 @@ NavfnPlanner::makePlan(
   // clear the plan, just in case
   plan.poses.clear();
 
+  plan.header.stamp = node_->now();
+  plan.header.frame_id = global_frame_;
+
   // TODO(orduno): add checks for start and goal reference frame -- should be in global frame
 
   double wx = start.position.x;
@@ -327,9 +330,6 @@ NavfnPlanner::getPlanFromPotential(
   float * y = planner_->getPathY();
   int len = planner_->getPathLen();
 
-  plan.header.stamp = node_->now();
-  plan.header.frame_id = global_frame_;
-
   for (int i = len - 1; i >= 0; --i) {
     // convert the plan to world coordinates
     double world_x, world_y;

nav2_planner/include/nav2_planner/planner_server.hpp

@@ -58,6 +58,17 @@ class PlannerServer : public nav2_util::LifecycleNode
 
   using PlannerMap = std::unordered_map<std::string, nav2_core::GlobalPlanner::Ptr>;
 
+  /**
+   * @brief Method to get plan from the desired plugin
+   * @param start starting pose
+   * @param goal goal request
+   * @return Path
+   */
+  nav_msgs::msg::Path getPlan(
+    const geometry_msgs::msg::PoseStamped & start,
+    const geometry_msgs::msg::PoseStamped & goal,
+    const std::string & planner_id);
+
 protected:
   /**
    * @brief Configure member variables and initializes planner
@@ -90,7 +101,8 @@ class PlannerServer : public nav2_util::LifecycleNode
    */
   nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override;
 
-  using ActionServer = nav2_util::SimpleActionServer<nav2_msgs::action::ComputePathToPose>;
+  using ActionT = nav2_msgs::action::ComputePathToPose;
+  using ActionServer = nav2_util::SimpleActionServer<ActionT>;
 
   // Our action server implements the ComputePathToPose action
   std::unique_ptr<ActionServer> action_server_;
@@ -129,9 +141,6 @@ class PlannerServer : public nav2_util::LifecycleNode
 
   // Publishers for the path
   rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>::SharedPtr plan_publisher_;
-
-  // Whether we've published the single planner warning yet
-  bool single_planner_warning_given_{false};
 };
 
 }  // namespace nav2_planner

nav2_planner/src/planner_server.cpp

@@ -120,12 +120,11 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & state)
   if (expected_planner_frequency > 0) {
     max_planner_duration_ = 1 / expected_planner_frequency;
   } else {
-    max_planner_duration_ = 0.0;
-
     RCLCPP_WARN(
       get_logger(),
-      "The expected planner frequency parameter is %.4f Hz. The value has to be greater"
-      " than 0.0 to turn on displaying warning messages", expected_planner_frequency);
+      "The expected planner frequency parameter is %.4f Hz. The value should to be greater"
+      " than 0.0 to turn on duration overrrun warning messages", expected_planner_frequency);
+    max_planner_duration_ = 0.0;
   }
 
   // Initialize pubs & subs
@@ -230,7 +229,6 @@ PlannerServer::computePlan()
 
     geometry_msgs::msg::PoseStamped start;
     if (!costmap_ros_->getRobotPose(start)) {
-      RCLCPP_ERROR(this->get_logger(), "Could not get robot pose");
       action_server_->terminate_current();
       return;
     }
@@ -240,30 +238,7 @@ PlannerServer::computePlan()
       goal = action_server_->accept_pending_goal();
     }
 
-    RCLCPP_DEBUG(
-      get_logger(), "Attempting to a find path from (%.2f, %.2f) to "
-      "(%.2f, %.2f).", start.pose.position.x, start.pose.position.y,
-      goal->pose.pose.position.x, goal->pose.pose.position.y);
-
-    if (planners_.find(goal->planner_id) != planners_.end()) {
-      result->path = planners_[goal->planner_id]->createPlan(start, goal->pose);
-    } else {
-      if (planners_.size() == 1 && goal->planner_id.empty()) {
-        if (!single_planner_warning_given_) {
-          single_planner_warning_given_ = true;
-          RCLCPP_WARN(
-            get_logger(), "No planners specified in action call. "
-            "Server will use only plugin %s in server."
-            " This warning will appear once.", planner_ids_concat_.c_str());
-        }
-        result->path = planners_[planners_.begin()->first]->createPlan(start, goal->pose);
-      } else {
-        RCLCPP_ERROR(
-          get_logger(), "planner %s is not a valid planner. "
-          "Planner names are: %s", goal->planner_id.c_str(),
-          planner_ids_concat_.c_str());
-      }
-    }
+    result->path = getPlan(start, goal->pose, goal->planner_id);
 
     if (result->path.poses.size() == 0) {
       RCLCPP_WARN(
@@ -281,7 +256,6 @@ PlannerServer::computePlan()
       goal->pose.pose.position.y);
 
     // Publish the plan for visualization purposes
-    RCLCPP_DEBUG(get_logger(), "Publishing the valid path");
     publishPlan(result->path);
 
     auto cycle_duration = steady_clock_.now() - start_time;
@@ -295,8 +269,6 @@ PlannerServer::computePlan()
     }
 
     action_server_->succeeded_current(result);
-
-    return;
   } catch (std::exception & ex) {
     RCLCPP_WARN(
       get_logger(), "%s plugin failed to plan calculation to (%.2f, %.2f): \"%s\"",
@@ -305,15 +277,50 @@ PlannerServer::computePlan()
     // TODO(orduno): provide information about fail error to parent task,
     //               for example: couldn't get costmap update
     action_server_->terminate_current();
-    return;
   }
 }
 
+nav_msgs::msg::Path
+PlannerServer::getPlan(
+  const geometry_msgs::msg::PoseStamped & start,
+  const geometry_msgs::msg::PoseStamped & goal,
+  const std::string & planner_id)
+{
+  RCLCPP_DEBUG(
+    get_logger(), "Attempting to a find path from (%.2f, %.2f) to "
+    "(%.2f, %.2f).", start.pose.position.x, start.pose.position.y,
+    goal.pose.position.x, goal.pose.position.y);
+
+  if (planners_.find(planner_id) != planners_.end()) {
+    return planners_[planner_id]->createPlan(start, goal);
+  } else {
+    if (planners_.size() == 1 && planner_id.empty()) {
+      RCLCPP_WARN_ONCE(
+        get_logger(), "No planners specified in action call. "
+        "Server will use only plugin %s in server."
+        " This warning will appear once.", planner_ids_concat_.c_str());
+      return planners_[planners_.begin()->first]->createPlan(start, goal);
+    } else {
+      RCLCPP_ERROR(
+        get_logger(), "planner %s is not a valid planner. "
+        "Planner names are: %s", planner_id.c_str(),
+        planner_ids_concat_.c_str());
+    }
+  }
+
+  return nav_msgs::msg::Path();
+}
+
 void
 PlannerServer::publishPlan(const nav_msgs::msg::Path & path)
 {
   auto msg = std::make_unique<nav_msgs::msg::Path>(path);
-  plan_publisher_->publish(std::move(msg));
+  if (
+    plan_publisher_->is_activated() &&
+    this->count_subscribers(plan_publisher_->get_topic_name()) > 0)
+  {
+    plan_publisher_->publish(std::move(msg));
+  }
 }
 
 }  // namespace nav2_planner

nav2_system_tests/src/planning/CMakeLists.txt

@@ -45,3 +45,13 @@ ament_add_test(test_planner_random
     TEST_EXECUTABLE=$<TARGET_FILE:${test_planner_random_exec}>
     TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map.pgm
 )
+
+ament_add_gtest(test_planner_plugin_failures
+  test_planner_plugins.cpp
+)
+
+ament_target_dependencies(test_planner_plugin_failures rclcpp geometry_msgs nav2_msgs ${dependencies})
+
+target_link_libraries(test_planner_plugin_failures
+  stdc++fs
+)

nav2_system_tests/src/planning/test_planner_plugins.cpp

@@ -0,0 +1,64 @@
+// Copyright (c) 2020 Samsung Research
+//
+// 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. Reserved.
+
+#include <gtest/gtest.h>
+#include <memory>
+#include <vector>
+#include <string>
+
+#include "rclcpp/rclcpp.hpp"
+#include "planner_tester.hpp"
+#include "nav2_util/lifecycle_utils.hpp"
+
+using namespace std::chrono_literals;
+
+using nav2_system_tests::PlannerTester;
+using nav2_util::TestCostmap;
+
+using ComputePathToPoseCommand = geometry_msgs::msg::PoseStamped;
+using ComputePathToPoseResult = nav_msgs::msg::Path;
+
+TEST(testPluginMap, Failures)
+{
+  auto obj = std::make_shared<nav2_system_tests::NavFnPlannerTester>();
+  rclcpp_lifecycle::State state;
+  obj->onConfigure(state);
+
+  geometry_msgs::msg::PoseStamped start;
+  geometry_msgs::msg::PoseStamped goal;
+  std::string plugin_fake = "fake";
+  std::string plugin_none = "";
+  auto path = obj->getPlan(start, goal, plugin_none);
+  EXPECT_EQ(path.header.frame_id, std::string("map"));
+
+  path = obj->getPlan(start, goal, plugin_fake);
+  EXPECT_EQ(path.poses.size(), 0ul);
+
+  obj->onCleanup(state);
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  // initialize ROS
+  rclcpp::init(argc, argv);
+
+  bool all_successful = RUN_ALL_TESTS();
+
+  // shutdown ROS
+  rclcpp::shutdown();
+
+  return all_successful;
+}