Merge pull request #1 from SteveMacenski/ros2

update to use changes in main repo

Author: JakubWarkocki

openvdb_vendor/openvdb_vendor-extras.cmake

@@ -5,4 +5,5 @@ endif()
 message( STATUS "CMAKE_BUILD_TYPE: ${CMAKE_BUILD_TYPE}" )
 list(APPEND CMAKE_MODULE_PATH "${openvdb_vendor_DIR}/../../../opt/openvdb_vendor/lib/cmake/OpenVDB")
 list(APPEND CMAKE_MODULE_PATH "/usr/lib/x86_64-linux-gnu/cmake/OpenVDB")
+list(APPEND CMAKE_MODULE_PATH "/usr/lib/aarch64-linux-gnu/cmake/OpenVDB")
 find_package(OpenVDB REQUIRED)

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"
 #include "tf2_ros/buffer.h"

spatio_temporal_voxel_layer/src/measurement_buffer.cpp

@@ -272,7 +272,7 @@ void MeasurementBuffer::RemoveStaleObservations(void)
   }
 
   for (it = _observation_list.begin(); it != _observation_list.end(); ++it) {
-    const rclcpp::Duration time_diff = _last_updated - it->_cloud->header.stamp;
+    const rclcpp::Duration time_diff = clock_->now() - it->_cloud->header.stamp;
 
     if (time_diff > _observation_keep_time) {
       _observation_list.erase(it, _observation_list.end());

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

@@ -158,7 +158,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,
@@ -275,6 +275,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>(
@@ -359,7 +361,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);