planner_cspace: fix flaky navigation tests (#768)

- tweak trajectory_tracker parameter
- use default switch back wait duration
- fix parameterized rostest result
- wait topics on init
- fix local map publish timing on stuck tests

Author: at-wat

planner_cspace/test/src/test_navigate.cpp

@@ -96,29 +96,53 @@ class Navigate : public ::testing::Test
 
   virtual void SetUp()
   {
-    test_scope_ = "[" + std::to_string(getpid()) + "] ";
+    test_scope_ =
+        "[" + std::to_string(getpid()) + "/" +
+        ::testing::UnitTest::GetInstance()->current_test_info()->name() + "] ";
 
-    srv_forget_.waitForExistence(ros::Duration(10.0));
     ros::Rate rate(10.0);
 
+    const ros::Time deadline = ros::Time::now() + ros::Duration(15.0);
+    while (ros::ok())
+    {
+      rate.sleep();
+      ASSERT_LT(ros::Time::now(), deadline)
+          << test_scope_ << "Initialization timeout: "
+          << "sub_map:" << sub_map_.getNumPublishers() << " "
+          << "sub_map_local:" << sub_map_local_.getNumPublishers() << " "
+          << "sub_costmap:" << sub_costmap_.getNumPublishers() << " "
+          << "sub_status:" << sub_status_.getNumPublishers() << " "
+          << "pub_map:" << pub_map_.getNumSubscribers() << " "
+          << "pub_map_local:" << pub_map_local_.getNumSubscribers() << " "
+          << "pub_initial_pose:" << pub_initial_pose_.getNumSubscribers() << " "
+          << "pub_patrol_nodes:" << pub_patrol_nodes_.getNumSubscribers() << " ";
+      if (sub_map_.getNumPublishers() > 0 &&
+          sub_map_local_.getNumPublishers() > 0 &&
+          sub_costmap_.getNumPublishers() > 0 &&
+          sub_status_.getNumPublishers() > 0 &&
+          pub_map_.getNumSubscribers() > 0 &&
+          pub_map_local_.getNumSubscribers() > 0 &&
+          pub_initial_pose_.getNumSubscribers() > 0 &&
+          pub_patrol_nodes_.getNumSubscribers() > 0)
+      {
+        break;
+      }
+    }
+    ASSERT_TRUE(srv_forget_.waitForExistence(ros::Duration(10.0)));
+
     geometry_msgs::PoseWithCovarianceStamped pose;
     pose.header.frame_id = "map";
     pose.pose.pose.position.x = 2.5;
     pose.pose.pose.position.y = 0.45;
     pose.pose.pose.orientation.z = 1.0;
     pub_initial_pose_.publish(pose);
 
-    const ros::Time deadline = ros::Time::now() + ros::Duration(15);
-
     while (ros::ok())
     {
       ros::spinOnce();
       rate.sleep();
       const ros::Time now = ros::Time::now();
-      if (now > deadline)
-      {
-        FAIL() << test_scope_ << now << " SetUp: transform timeout" << std::endl;
-      }
+      ASSERT_LT(now, deadline) << test_scope_ << "Initial transform timeout";
       if (tfbuf_.canTransform("map", "base_link", now, ros::Duration(0.5)))
       {
         break;
@@ -129,24 +153,23 @@ class Navigate : public ::testing::Test
     {
       ros::spinOnce();
       rate.sleep();
-      const ros::Time now = ros::Time::now();
-      if (now > deadline)
-      {
-        FAIL() << test_scope_ << now << " SetUp: map timeout" << std::endl;
-      }
+      ASSERT_LT(ros::Time::now(), deadline) << test_scope_ << "Initial map timeout";
     }
     pub_map_.publish(map_);
     std::cerr << test_scope_ << ros::Time::now() << " Map applied." << std::endl;
 
+    while (ros::ok() && !map_local_)
+    {
+      ros::spinOnce();
+      rate.sleep();
+      ASSERT_LT(ros::Time::now(), deadline) << test_scope_ << "Initial local map timeout";
+    }
+
     while (ros::ok() && !costmap_)
     {
       ros::spinOnce();
       rate.sleep();
-      const ros::Time now = ros::Time::now();
-      if (now > deadline)
-      {
-        FAIL() << test_scope_ << now << " SetUp: costmap timeout" << std::endl;
-      }
+      ASSERT_LT(ros::Time::now(), deadline) << test_scope_ << "Initial costmap timeout";
     }
 
     std_srvs::EmptyRequest req;
@@ -167,6 +190,10 @@ class Navigate : public ::testing::Test
   }
   void cbMapLocal(const nav_msgs::OccupancyGrid::ConstPtr& msg)
   {
+    if (map_local_)
+    {
+      return;
+    }
     map_local_.reset(new nav_msgs::OccupancyGrid(*msg));
     std::cerr << test_scope_ << msg->header.stamp << " Local map received." << std::endl;
   }
@@ -198,11 +225,22 @@ class Navigate : public ::testing::Test
   }
   void pubMapLocal()
   {
-    if (map_local_)
+    if (!map_local_)
     {
-      pub_map_local_.publish(map_local_);
-      if ((local_map_apply_cnt_++) % 30 == 0)
-        std::cerr << test_scope_ << " Local map applied." << std::endl;
+      return;
+    }
+    pub_map_local_.publish(map_local_);
+    if ((local_map_apply_cnt_++) % 30 == 0)
+    {
+      int num_occupied = 0;
+      for (const auto& c : map_local_->data)
+      {
+        if (c == 100)
+        {
+          num_occupied++;
+        }
+      }
+      std::cerr << test_scope_ << " Local map applied. occupied grids:" << num_occupied << std::endl;
     }
   }
   tf2::Stamped<tf2::Transform> lookupRobotTrans(const ros::Time& now)
@@ -557,6 +595,16 @@ TEST_F(Navigate, GoalIsInRockRecovered)
   ASSERT_TRUE(static_cast<bool>(map_));
   ASSERT_TRUE(static_cast<bool>(map_local_));
 
+  for (int x = 10; x <= 16; ++x)
+  {
+    for (int y = 22; y <= 26; ++y)
+    {
+      const int pos = x + y * map_local_->info.width;
+      map_local_->data[pos] = 100;
+    }
+  }
+  pubMapLocal();
+
   nav_msgs::Path path;
   path.poses.resize(1);
   path.header.frame_id = "map";
@@ -566,19 +614,12 @@ TEST_F(Navigate, GoalIsInRockRecovered)
   path.poses[0].pose.orientation.w = 1.0;
   pub_patrol_nodes_.publish(path);
 
-  for (int x = 12; x <= 14; ++x)
-  {
-    for (int y = 24; y <= 26; ++y)
-    {
-      const int pos = x + y * map_local_->info.width;
-      map_local_->data[pos] = 100;
-    }
-  }
   waitForPlannerStatus("Got stuck", planner_cspace_msgs::PlannerStatus::PATH_NOT_FOUND);
+  ASSERT_FALSE(::testing::Test::HasFailure());
 
-  for (int x = 12; x <= 14; ++x)
+  for (int x = 10; x <= 16; ++x)
   {
-    for (int y = 24; y <= 26; ++y)
+    for (int y = 22; y <= 26; ++y)
     {
       const int pos = x + y * map_local_->info.width;
       map_local_->data[pos] = 0;
@@ -593,6 +634,16 @@ TEST_F(Navigate, RobotIsInRockOnRecovered)
   ASSERT_TRUE(static_cast<bool>(map_));
   ASSERT_TRUE(static_cast<bool>(map_local_));
 
+  for (int x = 21; x <= 28; ++x)
+  {
+    for (int y = 2; y <= 8; ++y)
+    {
+      const int pos = x + y * map_local_->info.width;
+      map_local_->data[pos] = 100;
+    }
+  }
+  pubMapLocal();
+
   nav_msgs::Path path;
   path.poses.resize(1);
   path.header.frame_id = "map";
@@ -602,19 +653,12 @@ TEST_F(Navigate, RobotIsInRockOnRecovered)
   path.poses[0].pose.orientation.w = 1.0;
   pub_patrol_nodes_.publish(path);
 
-  for (int x = 23; x <= 26; ++x)
-  {
-    for (int y = 4; y <= 6; ++y)
-    {
-      const int pos = x + y * map_local_->info.width;
-      map_local_->data[pos] = 100;
-    }
-  }
   waitForPlannerStatus("Got stuck", planner_cspace_msgs::PlannerStatus::IN_ROCK);
+  ASSERT_FALSE(::testing::Test::HasFailure());
 
-  for (int x = 23; x <= 26; ++x)
+  for (int x = 21; x <= 28; ++x)
   {
-    for (int y = 4; y <= 6; ++y)
+    for (int y = 2; y <= 8; ++y)
     {
       const int pos = x + y * map_local_->info.width;
       map_local_->data[pos] = 0;

planner_cspace/test/test/navigation_compat_rostest.test

@@ -3,7 +3,7 @@
   <env name="GCOV_PREFIX" value="/tmp/gcov/planner_cspace_navigation_compat" />
   <param name="neonavigation_compatible" value="0" />
 
-  <test test-name="test_navigate" pkg="planner_cspace" type="test_navigate" time-limit="200.0">
+  <test test-name="test_navigate_compat" pkg="planner_cspace" type="test_navigate" time-limit="200.0">
     <remap from="patrol_nodes" to="/patrol/path" />
     <remap from="forget_planning_cost" to="/planner_3d/forget" />
     <remap from="map" to="/map" />

planner_cspace/test/test/navigation_remember_rostest.test

@@ -6,7 +6,7 @@
   <env name="GCOV_PREFIX" value="/tmp/gcov/planner_cspace_navigation_remember" />
   <param name="neonavigation_compatible" value="1" />
 
-  <test test-name="test_navigate" pkg="planner_cspace" type="test_navigate_remember" time-limit="200.0" />
+  <test test-name="test_navigate_remember" pkg="planner_cspace" type="test_navigate_remember" time-limit="200.0" />
 
   <node pkg="costmap_cspace" type="costmap_3d" name="costmap_3d">
     <rosparam param="footprint">[[0.2, -0.1], [0.2, 0.1], [-0.2, 0.1], [-0.2, -0.1]]</rosparam>

planner_cspace/test/test/navigation_rostest.test

@@ -6,7 +6,7 @@
   <env name="GCOV_PREFIX" value="/tmp/gcov/planner_cspace_navigation_$(arg antialias_start)_$(arg fast_map_update)_$(arg with_tolerance)" />
   <param name="neonavigation_compatible" value="1" />
 
-  <test test-name="test_navigate" pkg="planner_cspace" type="test_navigate" time-limit="200.0" />
+  <test test-name="test_navigate_$(arg antialias_start)_$(arg fast_map_update)_$(arg with_tolerance)" pkg="planner_cspace" type="test_navigate" time-limit="200.0" />
 
   <node pkg="costmap_cspace" type="costmap_3d" name="costmap_3d">
     <rosparam param="footprint">[[0.2, -0.1], [0.2, 0.1], [-0.2, 0.1], [-0.2, -0.1]]</rosparam>
@@ -29,7 +29,6 @@
     <param name="max_vel" value="0.1" />
     <param name="max_ang_vel" value="0.3" />
     <param name="goal_tolerance_lin" value="0.025" />
-    <param name="sw_wait" value="0.2" />
     <param name="antialias_start" value="$(arg antialias_start)" />
     <param name="fast_map_update" value="$(arg fast_map_update)" />
   </node>
@@ -41,7 +40,7 @@
 
     <param name="curv_forward" value="0.1" />
     <param name="look_forward" value="0.0" />
-    <param name="k_dist" value="4.5" />
+    <param name="k_dist" value="5.5" />
     <param name="k_ang" value="3.0" />
     <param name="k_avel" value="4.0" />