allowing for subscriber toggle callback (#153)

SteveMacenski · Dec 16, 2019 · 3bdf9f4 · 3bdf9f4
1 parent ba1bd68
commit 3bdf9f4
Show file tree

Hide file tree

Showing 3 changed files with 15 additions and 13 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -26,6 +26,7 @@ find_package(tf2_geometry_msgs REQUIRED)
 find_package(visualization_msgs REQUIRED)
 find_package(builtin_interfaces REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
+find_package(std_srvs REQUIRED)
 
 find_package(OpenVDB REQUIRED)
 find_package(TBB REQUIRED)
@@ -56,6 +57,7 @@ set(dependencies
   pluginlib
   sensor_msgs
   std_msgs
+  std_srvs
   laser_geometry
   message_filters
   pcl_conversions

diff --git a/package.xml b/package.xml
@@ -21,6 +21,7 @@
   <depend>pluginlib</depend>
   <depend>sensor_msgs</depend>
   <depend>std_msgs</depend>
+  <depend>std_srvs</depend>
   <depend>laser_geometry</depend>
   <depend>pcl</depend>
   <depend>pcl_conversions</depend>

diff --git a/src/spatio_temporal_voxel_layer.cpp b/src/spatio_temporal_voxel_layer.cpp
@@ -283,20 +283,19 @@ void SpatioTemporalVoxelLayer::onInitialize(void)
       _observation_notifiers.push_back(filter);
     }
 
-    // TODO(stevemacenski)
-    // std::function<void(const std::shared_ptr<rmw_request_id_t>,
-    //   const std::shared_ptr<std_srvs::srv::SetBool::Request>,
-    //   std::shared_ptr<std_srvs::srv::SetBool::Response>)> toggle_srv_callback;
+    std::function<void(const std::shared_ptr<rmw_request_id_t>,
+      std_srvs::srv::SetBool::Request::SharedPtr,
+      std_srvs::srv::SetBool::Response::SharedPtr)> toggle_srv_callback;
 
-    // toggle_srv_callback = std::bind(
-    //   &SpatioTemporalVoxelLayer::BufferEnablerCallback, this,
-    //   _1, _2, _3, _observation_buffers.back(),
-    //   _observation_subscribers.back());
-    // std::string toggle_topic = source + "/toggle_enabled";
-    // auto server = rclcpp_node_->create_service<std_srvs::srv::SetBool>(
-    //   toggle_topic, toggle_srv_callback);
+    toggle_srv_callback = std::bind(
+      &SpatioTemporalVoxelLayer::BufferEnablerCallback, this,
+      _1, _2, _3, _observation_buffers.back(),
+      _observation_subscribers.back());
+    std::string toggle_topic = source + "/toggle_enabled";
+    auto server = rclcpp_node_->create_service<std_srvs::srv::SetBool>(
+      toggle_topic, toggle_srv_callback);
 
-    // _buffer_enabler_servers.push_back(server);
+    _buffer_enabler_servers.push_back(server);
 
     if (sensor_frame != "") {
       std::vector<std::string> target_frames;
@@ -670,7 +669,7 @@ void SpatioTemporalVoxelLayer::updateBounds(
   // navigation mode: clear observations, mapping mode: save maps and publish
   bool should_save = false;
   if (_map_save_duration) {
-      should_save = node_->now() - _last_map_save_time > *_map_save_duration;
+    should_save = node_->now() - _last_map_save_time > *_map_save_duration;
   }
   if (!_mapping_mode) {
     _voxel_grid->ClearFrustums(clearing_observations);