Updating STVL topic parsing and namespacing to match Obstacle and Voxel layer updates (#310)

* Updating STVL topic parsing to match Obstacle and Voxel layer updates

Signed-off-by: Steve Macenski <[email protected]>

* fix other build warnings

Signed-off-by: Steve Macenski <[email protected]>

---------

Signed-off-by: Steve Macenski <[email protected]>

Author: SteveMacenski

spatio_temporal_voxel_layer/include/spatio_temporal_voxel_layer/measurement_buffer.hpp

@@ -57,7 +57,7 @@
 #include "rclcpp_lifecycle/lifecycle_node.hpp"
 // TF
 #include "tf2_ros/buffer.h"
-#include "message_filters/subscriber.h"
+#include "message_filters/subscriber.hpp"
 // msgs
 #include "sensor_msgs/msg/point_cloud2.hpp"
 #include "geometry_msgs/msg/quaternion.hpp"

spatio_temporal_voxel_layer/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_layer.hpp

@@ -73,7 +73,7 @@
 // tf
 #include "tf2_ros/transform_listener.h"
 #include "tf2_ros/message_filter.h"
-#include "message_filters/subscriber.h"
+#include "message_filters/subscriber.hpp"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 #include "tf2/buffer_core.h"
 

spatio_temporal_voxel_layer/src/spatio_temporal_voxel_grid.cpp

@@ -401,7 +401,7 @@ void SpatioTemporalVoxelGrid::ResetGridArea(
   boost::unique_lock<boost::mutex> lock(_grid_lock);
 
   openvdb::DoubleGrid::ValueOnCIter cit_grid = _grid->cbeginValueOn();
-  for (cit_grid; cit_grid.test(); ++cit_grid)
+  for (; cit_grid.test(); ++cit_grid)
   {
     const openvdb::Coord pt_index(cit_grid.getCoord());
     const openvdb::Vec3d pose_world = this->IndexToWorld(pt_index);

spatio_temporal_voxel_layer/src/spatio_temporal_voxel_layer.cpp

@@ -152,7 +152,7 @@ void SpatioTemporalVoxelLayer::onInitialize(void)
   auto save_grid_callback = std::bind(
     &SpatioTemporalVoxelLayer::SaveGridCallback, this, _1, _2, _3);
   _grid_saver = node->create_service<spatio_temporal_voxel_layer::srv::SaveGrid>(
-    "save_grid", save_grid_callback, rmw_qos_profile_services_default, callback_group_);
+    "save_grid", save_grid_callback, rclcpp::SystemDefaultsQoS(), callback_group_);
 
   _voxel_grid = std::make_unique<volume_grid::SpatioTemporalVoxelGrid>(
     node->get_clock(), _voxel_size, static_cast<double>(default_value_), _decay_model,
@@ -258,6 +258,8 @@ void SpatioTemporalVoxelLayer::onInitialize(void)
               "Only topics that use pointclouds or laser scans are supported.");
     }
 
+    topic = joinWithParentNamespace(topic);
+
     // create an observation buffer
     _observation_buffers.push_back(
       std::shared_ptr<buffer::MeasurementBuffer>(
@@ -342,7 +344,7 @@ void SpatioTemporalVoxelLayer::onInitialize(void)
       _observation_subscribers.back());
     std::string toggle_topic = source + "/toggle_enabled";
     auto server = node->create_service<std_srvs::srv::SetBool>(
-      toggle_topic, toggle_srv_callback, rmw_qos_profile_services_default, callback_group_);
+      toggle_topic, toggle_srv_callback, rclcpp::SystemDefaultsQoS(), callback_group_);
 
     _buffer_enabler_servers.push_back(server);