updates from ros2 port for melodic (#149)

Author: SteveMacenski

src/spatio_temporal_voxel_grid.cpp

@@ -65,6 +65,11 @@ SpatioTemporalVoxelGrid::~SpatioTemporalVoxelGrid(void)
   {
     delete _cost_map;    
   }
+
+  if (_grid_points)
+  {
+    delete _grid_points;
+  }
 }
 
 /*****************************************************************************/

src/spatio_temporal_voxel_layer.cpp

@@ -266,22 +266,23 @@ void SpatioTemporalVoxelLayer::onInitialize(void)
           boost::bind(&SpatioTemporalVoxelLayer::PointCloud2Callback, this, _1, \
                                                    _observation_buffers.back()));
 
-      ros::ServiceServer server;
-      boost::function < bool(std_srvs::SetBool::Request&, \
-                             std_srvs::SetBool::Response&) > serv_callback;
-
-      serv_callback = boost::bind(&SpatioTemporalVoxelLayer::BufferEnablerCallback, \
-                                  this, _1, _2, _observation_buffers.back(),  \
-                                  _observation_subscribers.back());
-
-      std::string topic = source +  "/toggle_enabled";
-      server = nh.advertiseService(topic, serv_callback);
-
-      _buffer_enabler_servers.push_back(server);
       _observation_subscribers.push_back(sub);
       _observation_notifiers.push_back(filter);
     }
 
+    ros::ServiceServer server;
+    boost::function < bool(std_srvs::SetBool::Request&, \
+                           std_srvs::SetBool::Response&) > serv_callback;
+
+    serv_callback = boost::bind(&SpatioTemporalVoxelLayer::BufferEnablerCallback, \
+                                this, _1, _2, _observation_buffers.back(),  \
+                                _observation_subscribers.back());
+
+    std::string topic = source +  "/toggle_enabled";
+    server = nh.advertiseService(topic, serv_callback);
+
+    _buffer_enabler_servers.push_back(server);
+
     if (sensor_frame != "")
     {
       std::vector<std::string> target_frames;