Merge

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/include/nav2_controller/nav2_controller.hpp

@@ -189,6 +189,12 @@ 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_;

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
@@ -113,8 +115,7 @@ 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) {
-    RCLCPP_FATAL(get_logger(), "Failed to create controller. Exception: %s", ex.what());
-    exit(-1);
+    pluginFailed("progress_checker", ex);
   }
   try {
     goal_checker_type_ = nav2_util::get_plugin_type_param(node, goal_checker_id_);
@@ -124,8 +125,7 @@ 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) {
-    RCLCPP_FATAL(get_logger(), "Failed to create controller. Exception: %s", ex.what());
-    exit(-1);
+    pluginFailed("goal_checker", ex);
   }
 
   for (size_t i = 0; i != controller_ids_.size(); i++) {
@@ -141,15 +141,18 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state)
         costmap_ros_->getTfBuffer(), costmap_ros_);
       controllers_.insert({controller_ids_[i], controller});
     } catch (const pluginlib::PluginlibException & ex) {
-      RCLCPP_FATAL(get_logger(), "Failed to create controller. Exception: %s", ex.what());
-      exit(-1);
+      pluginFailed("controller", ex);
     }
   }
 
   for (size_t i = 0; i != controller_ids_.size(); i++) {
     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);
 
@@ -312,7 +315,7 @@ void ControllerServer::computeControl()
     return;
   }
 
-  RCLCPP_DEBUG(get_logger(), "DWB succeeded, setting result");
+  RCLCPP_DEBUG(get_logger(), "Controller succeeded, setting result");
 
   publishZeroVelocity();
 
@@ -330,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(
@@ -421,7 +424,6 @@ bool ControllerServer::getRobotPose(geometry_msgs::msg::PoseStamped & pose)
 {
   geometry_msgs::msg::PoseStamped current_pose;
   if (!costmap_ros_->getRobotPose(current_pose)) {
-    RCLCPP_ERROR(this->get_logger(), "Could not get robot pose");
     return false;
   }
   pose = current_pose;

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;
     }
 
@@ -299,15 +306,6 @@ PlannerServer::computePlan()
     //               for example: couldn't get costmap update
     action_server_->terminate_current();
     return;
-  } catch (...) {
-    RCLCPP_WARN(
-      get_logger(), "Plan calculation failed, "
-      "An unexpected error has occurred. The planner server"
-      " may not be able to continue operating correctly.");
-    // TODO(orduno): provide information about fail error to parent task,
-    //               for example: couldn't get costmap update
-    action_server_->terminate_current();
-    return;
   }
 }
 

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,13 @@ def main(argv=sys.argv[1:]):
     time.sleep(2)
     test.cancel_goal()
 
+    # a failure case
+    time.sleep(2)
+    test.setWaypoints([[100.0, 100.0]])
+    result = test.run(True)
+    assert not result
+    result = not result
+
     test.shutdown()
     test.info_msg('Done Shutting Down.')
 

nav2_voxel_grid/test/voxel_grid_tests.cpp

@@ -147,6 +147,46 @@ TEST(voxel_grid, InvalidSize) {
   EXPECT_TRUE(vg.getVoxelColumn(50, 11, 0, 0) == nav2_voxel_grid::VoxelStatus::UNKNOWN);
 }
 
+TEST(voxel_grid, MarkAndClear) {
+  int size_x = 10, size_y = 10, size_z = 10;
+  nav2_voxel_grid::VoxelGrid vg(size_x, size_y, size_z);
+  vg.markVoxelInMap(5, 5, 5, 0);
+  EXPECT_EQ(vg.getVoxel(5, 5, 5), nav2_voxel_grid::MARKED);
+  vg.clearVoxelColumn(55);
+  EXPECT_EQ(vg.getVoxel(5, 5, 5), nav2_voxel_grid::FREE);
+}
+
+TEST(voxel_grid, clearVoxelLineInMap) {
+  int size_x = 10, size_y = 10, size_z = 10;
+  nav2_voxel_grid::VoxelGrid vg(size_x, size_y, size_z);
+  vg.markVoxelInMap(0, 0, 5, 0);
+  EXPECT_EQ(vg.getVoxel(0, 0, 5), nav2_voxel_grid::MARKED);
+
+  unsigned char * map_2d = new unsigned char[100];
+  map_2d[0] = 254;
+
+  vg.clearVoxelLineInMap(0, 0, 0, 0, 0, 9, map_2d, 16, 0);
+
+  EXPECT_EQ(map_2d[0], 0);
+
+  vg.markVoxelInMap(0, 0, 5, 0);
+  vg.clearVoxelLineInMap(0, 0, 0, 0, 0, 9, nullptr, 16, 0);
+  EXPECT_EQ(vg.getVoxel(0, 0, 5), nav2_voxel_grid::FREE);
+  delete[] map_2d;
+}
+
+TEST(voxel_grid, GetVoxelData) {
+  uint32_t * data = new uint32_t[9];
+  data[4] = 255;
+  data[0] = 0;
+  EXPECT_EQ(
+    nav2_voxel_grid::VoxelGrid::getVoxel(1, 1, 1, 3, 3, 3, data), nav2_voxel_grid::UNKNOWN);
+
+  EXPECT_EQ(
+    nav2_voxel_grid::VoxelGrid::getVoxel(0, 0, 0, 3, 3, 3, data), nav2_voxel_grid::FREE);
+  delete[] data;
+}
+
 int main(int argc, char ** argv)
 {
   testing::InitGoogleTest(&argc, argv);

nav2_waypoint_follower/src/waypoint_follower.cpp

@@ -134,6 +134,8 @@ WaypointFollower::followWaypoints()
     if (action_server_->is_cancel_requested()) {
       auto cancel_future = nav_to_pose_client_->async_cancel_all_goals();
       rclcpp::spin_until_future_complete(client_node_, cancel_future);
+      // for result callback processing
+      spin_some(client_node_);
       action_server_->terminate_all();
       return;
     }