Add death tests and build in debug mode

beluga_amcl/include/beluga_amcl/amcl_nodelet.hpp

@@ -110,7 +110,7 @@ class AmclNodelet : public nodelet::Nodelet {
 
   void update_covariance_diagnostics(diagnostic_updater::DiagnosticStatusWrapper& status);
 
-  std::mutex mutex_;
+  mutable std::mutex mutex_;
 
   ros::Timer particle_cloud_timer_;
   ros::Publisher particle_cloud_pub_;

beluga_amcl/src/amcl_nodelet.cpp

@@ -245,6 +245,7 @@ auto AmclNodelet::make_particle_filter(const nav_msgs::OccupancyGrid::ConstPtr&
 }
 
 void AmclNodelet::map_timer_callback(const ros::TimerEvent&) {
+  std::lock_guard<std::mutex> lock(mutex_);
   if (!get_map_client_.exists()) {
     NODELET_INFO_THROTTLE(10, "Waiting for map service to be available");
     return;

beluga_amcl/test/test_amcl_nodelet.cpp

@@ -55,13 +55,22 @@ void spin_for(const std::chrono::duration<Rep, Period>& duration) {
 class AmclNodeletUnderTest : public beluga_amcl::AmclNodelet {
  public:
   /// Get particle filter pointer.
-  const auto& particle_filter() { return particle_filter_; }
+  const auto& particle_filter() {
+    std::lock_guard<std::mutex> lock(mutex_);
+    return particle_filter_;
+  }
 
   /// Return true if the particle filter has been initialized.
-  bool is_initialized() const { return particle_filter_ != nullptr; }
+  bool is_initialized() const {
+    std::lock_guard<std::mutex> lock(mutex_);
+    return particle_filter_ != nullptr;
+  }
 
   /// Return the last known estimate. Throws if there is no estimate.
-  const auto& estimate() { return last_known_estimate_.value(); }
+  const auto& estimate() {
+    std::lock_guard<std::mutex> lock(mutex_);
+    return last_known_estimate_.value();
+  }
 
   /// Retrieve nodelet default configuration (may fail).
   bool default_config(beluga_amcl::AmclConfig& config) {