adding waypoint follower failure test, voxel layer integration tests, etc

nav2_bringup/bringup/params/nav2_params.yaml

@@ -164,20 +164,10 @@ local_costmap:
       height: 3
       resolution: 0.05
       robot_radius: 0.22
-      plugins: ["obstacle_layer", "voxel_layer", "inflation_layer"]
+      plugins: ["voxel_layer", "inflation_layer"]
       inflation_layer:
         plugin: "nav2_costmap_2d::InflationLayer"
         cost_scaling_factor: 3.0
-      obstacle_layer:
-        plugin: "nav2_costmap_2d::ObstacleLayer"
-        enabled: True
-        observation_sources: scan
-        scan:
-          topic: /scan
-          max_obstacle_height: 2.0
-          clearing: True
-          marking: True
-          data_type: "LaserScan"
       voxel_layer:
         plugin: "nav2_costmap_2d::VoxelLayer"
         enabled: True
@@ -187,13 +177,13 @@ local_costmap:
         z_voxels: 16
         max_obstacle_height: 2.0
         mark_threshold: 0
-        observation_sources: pointcloud
-        pointcloud:
-          topic: /intel_realsense_r200_depth/points
+        observation_sources: scan
+        scan:
+          topic: /scan
           max_obstacle_height: 2.0
           clearing: True
           marking: True
-          data_type: "PointCloud2"
+          data_type: "LaserScan"
       static_layer:
         map_subscribe_transient_local: True
       always_send_full_costmap: True
@@ -214,7 +204,7 @@ global_costmap:
       use_sim_time: True
       robot_radius: 0.22
       resolution: 0.05
-      plugins: ["static_layer", "obstacle_layer", "voxel_layer", "inflation_layer"]
+      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
       obstacle_layer:
         plugin: "nav2_costmap_2d::ObstacleLayer"
         enabled: True
@@ -225,22 +215,6 @@ global_costmap:
           clearing: True
           marking: True
           data_type: "LaserScan"
-      voxel_layer:
-        plugin: "nav2_costmap_2d::VoxelLayer"
-        enabled: True
-        publish_voxel_map: True
-        origin_z: 0.0
-        z_resolution: 0.05
-        z_voxels: 16
-        max_obstacle_height: 2.0
-        mark_threshold: 0
-        observation_sources: pointcloud
-        pointcloud:
-          topic: /intel_realsense_r200_depth/points
-          max_obstacle_height: 2.0
-          clearing: True
-          marking: True
-          data_type: "PointCloud2"
       static_layer:
         plugin: "nav2_costmap_2d::StaticLayer"
         map_subscribe_transient_local: True

nav2_controller/src/nav2_controller.cpp

@@ -63,6 +63,8 @@ ControllerServer::ControllerServer()
 ControllerServer::~ControllerServer()
 {
   RCLCPP_INFO(get_logger(), "Destroying");
+  controllers_.clear();
+  costmap_thread_.reset();
 }
 
 nav2_util::CallbackReturn
@@ -147,6 +149,10 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state)
     controller_ids_concat_ += controller_ids_[i] + std::string(" ");
   }
 
+  RCLCPP_INFO(
+    get_logger(),
+    "Controller Server has %s controllers available.", controller_ids_concat_.c_str());
+
   odom_sub_ = std::make_unique<nav_2d_utils::OdomSubscriber>(node);
   vel_publisher_ = create_publisher<geometry_msgs::msg::Twist>("cmd_vel", 1);
 
@@ -309,7 +315,7 @@ void ControllerServer::computeControl()
     return;
   }
 
-  RCLCPP_DEBUG(get_logger(), "DWB succeeded, setting result");
+  RCLCPP_DEBUG(get_logger(), "Controller succeeded, setting result");
 
   publishZeroVelocity();
 
@@ -327,7 +333,7 @@ void ControllerServer::setPlannerPath(const nav_msgs::msg::Path & path)
   }
   controllers_[current_controller_]->setPlan(path);
 
-  auto end_pose = *(path.poses.end() - 1);
+  auto end_pose = path.poses.back();
   goal_checker_->reset();
 
   RCLCPP_DEBUG(

nav2_costmap_2d/test/integration/inflation_tests.cpp

@@ -44,7 +44,7 @@
 #include "nav2_costmap_2d/obstacle_layer.hpp"
 #include "nav2_costmap_2d/inflation_layer.hpp"
 #include "nav2_costmap_2d/observation_buffer.hpp"
-#include "nav2_costmap_2d/testing_helper.hpp"
+#include "../testing_helper.hpp"
 #include "nav2_util/node_utils.hpp"
 
 using geometry_msgs::msg::Point;

nav2_costmap_2d/test/integration/obstacle_tests.cpp

@@ -41,7 +41,7 @@
 #include "nav2_costmap_2d/costmap_2d.hpp"
 #include "nav2_costmap_2d/layered_costmap.hpp"
 #include "nav2_costmap_2d/observation_buffer.hpp"
-#include "nav2_costmap_2d/testing_helper.hpp"
+#include "../testing_helper.hpp"
 
 using std::begin;
 using std::end;

nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp

@@ -24,7 +24,7 @@
 #include "nav2_costmap_2d/static_layer.hpp"
 #include "nav2_costmap_2d/inflation_layer.hpp"
 #include "nav2_costmap_2d/costmap_2d_publisher.hpp"
-#include "nav2_costmap_2d/testing_helper.hpp"
+#include "../testing_helper.hpp"
 #include "nav2_util/robot_utils.hpp"
 #include "nav2_util/node_utils.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"

nav2_planner/src/planner_server.cpp

@@ -67,6 +67,7 @@ PlannerServer::~PlannerServer()
 {
   RCLCPP_INFO(get_logger(), "Destroying");
   planners_.clear();
+  costmap_thread_.reset();
 }
 
 nav2_util::CallbackReturn
@@ -110,6 +111,10 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & state)
     planner_ids_concat_ += planner_ids_[i] + std::string(" ");
   }
 
+  RCLCPP_INFO(
+    get_logger(),
+    "Planner Server has %s planners available.", planner_ids_concat_.c_str());
+
   double expected_planner_frequency;
   get_parameter("expected_planner_frequency", expected_planner_frequency);
   if (expected_planner_frequency > 0) {
@@ -190,6 +195,7 @@ PlannerServer::on_cleanup(const rclcpp_lifecycle::State & state)
     it->second->cleanup();
   }
   planners_.clear();
+  costmap_ = nullptr;
 
   return nav2_util::CallbackReturn::SUCCESS;
 }
@@ -225,6 +231,7 @@ 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;
     }
 
@@ -281,10 +288,12 @@ PlannerServer::computePlan()
     result->planning_time = cycle_duration;
 
     if (max_planner_duration_ && cycle_duration.seconds() > max_planner_duration_) {
+      auto planner_period = 1 / max_planner_duration_;
+      auto cycle_period = 1 / cycle_duration.seconds();
       RCLCPP_WARN(
         get_logger(),
         "Planner loop missed its desired rate of %.4f Hz. Current loop rate is %.4f Hz",
-        1 / max_planner_duration_, 1 / cycle_duration.seconds());
+        planner_period, cycle_period);
     }
 
     action_server_->succeeded_current(result);

nav2_system_tests/src/waypoint_follower/tester.py

@@ -188,6 +188,7 @@ def main(argv=sys.argv[1:]):
         rclpy.spin_once(test, timeout_sec=1.0)  # wait for poseCallback
 
     result = test.run(True)
+    assert result
 
     # preempt with new point
     test.setWaypoints([starting_pose])
@@ -200,6 +201,12 @@ def main(argv=sys.argv[1:]):
     time.sleep(2)
     test.cancel_goal()
 
+    # a failure case
+    test.setWaypoints([[100.0, 100.0]])
+    result = test.run(True)
+    assert not result
+    result = not result
+
     test.shutdown()
     test.info_msg('Done Shutting Down.')