diff --git a/CMakeLists.txt b/CMakeLists.txt index 7c5d92f4..06c73ad7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,6 +1,7 @@ cmake_minimum_required(VERSION 2.8.3) project(spatio_temporal_voxel_layer) -add_compile_options(-std=c++17) + +add_definitions (-std=c++17) set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PROJECT_SOURCE_DIR}/cmake/") @@ -18,15 +19,22 @@ find_package(catkin REQUIRED COMPONENTS tf2_ros visualization_msgs message_generation + dynamic_reconfigure ) add_service_files(DIRECTORY msgs FILES SaveGrid.srv ) + +generate_dynamic_reconfigure_options( + cfg/SpatioTemporalVoxelLayer.cfg +) + generate_messages( DEPENDENCIES std_msgs + dynamic_reconfigure ) find_package(PCL REQUIRED) @@ -70,6 +78,7 @@ catkin_package( costmap_2d tf2_ros visualization_msgs + dynamic_reconfigure DEPENDS TBB OpenVDB @@ -82,10 +91,11 @@ add_library(${PROJECT_NAME} src/spatio_temporal_voxel_layer.cpp src/spatio_temporal_voxel_grid.cpp src/measurement_buffer.cpp - src/frustum.cpp + src/frustum_models/depth_camera_frustum.cpp + src/frustum_models/three_dimensional_lidar_frustum.cpp src/vdb2pc.cpp ) -add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_generate_messages_cpp) +add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_generate_messages_cpp ${PROJECT_NAME}_gencfg) target_link_libraries(${PROJECT_NAME} ${Boost_LIBRARIES} ${EIGEN3_LIBRARIES} diff --git a/README.md b/README.md index a525a010..60eb7e33 100644 --- a/README.md +++ b/README.md @@ -6,12 +6,22 @@ This package sits on top of [OpenVDB](http://www.openvdb.org/), an open-source C Leveraging OpenVDB, we have the ability to efficiently maintain a 3 dimensional voxel-representative world space. We wrap this with ROS tools and interfaces to the [navigation stack](http://wiki.ros.org/navigation) to allow for use of this layer in standard ROS configurations. It is certainly possible to utilize this package without ROS/Navigation and I invite other competing methodologies to develop here and create interfaces. -An simple example video can be seen [here](https://www.youtube.com/watch?v=8YIFPiI1vrg&feature=youtu.be), another more complex example can be seen [here](https://youtu.be/hpWxa6M1AR8) with global decay and frustum decay while moving with dynamic objects. A short clip from this is shown below. +Sample videos are shown below of a robot using **7 depth cameras** with less than 50% of a core, and another robot using a **VLP-16**. -![ezgif com-video-to-gif](https://user-images.githubusercontent.com/14944147/37010885-b18fe1f8-20bb-11e8-8c28-5b31e65f2844.gif) +7 Depth Cameras | VLP-16 LIDAR +:-------------------------:|:-------------------------: +![ezgif com-video-to-gif](https://user-images.githubusercontent.com/14944147/37010885-b18fe1f8-20bb-11e8-8c28-5b31e65f2844.gif) | ![vlp16](https://github.com/nickovaras/gifs/blob/master/follow.gif?raw=true) We found in experimental trials with **6** 7hz dense stereo RGBD cameras we ran the major move_base process at **20-50%** nominal from **80-110%** on a 5th gen i7 CPU in the global costmap updating using the existing `voxel_layer`. +We've received feedback from users and have robots operating in the following environments with STVL: +- Retail +- Warehouses +- Factories +- Libraries +- Hospitals +- Hospitality + Steve spoke at ROSCon 2018 about STVL and his presentation is [linked here](https://vimeo.com/292699571) (or click on image). [![IMAGE ALT TEXT](https://user-images.githubusercontent.com/14944147/46768837-987c9280-cc9e-11e8-99ea-788d3d590dd8.png)](https://vimeo.com/292699571) @@ -28,7 +38,7 @@ Below is an example a size of map that is **trivial** for the Spatio-Temportal V ## -**Temporal** The Temporal in this package is the novel concept of `voxel_decay` whereas we have configurable functions that expire voxels and their occupation over time. Infrasture was created to store times in each voxel after which the voxel will disappear from the map. This is combined with checking inclusion of voxels in current measurement frustums to accelerate the decay of those voxels that do not have measurements but should if still in the scene and remain marked. This is done rather than simply clearing them naively or via costly raytracing. The time it takes to clear depends on the configured functions and acceleration factors. -Voxel acceleration uses given FOV to compute traditional 6-planed cubical frustums. Sensors that cannot be modelled by traditional frustums (i.e. 360 lidars or sensors without flat back planes) the frustum acceleration mathematics breakdown, **do not use frustum acceleration for these class of sensors**. PRs to implement curved frustums are welcome and encouraged. +Voxel acceleration uses given FOV to compute the frustum geometry. Depth cameras (e.g. Intel Realsense) are modeled with traditional 6-planed cubical frustums. 3D lidars (e.g. Velodyne VLP 16) are modeled with their hourglass-shaped FOV. Although many 3D lidars have 360 degree horizontal FOV, it is possible to use a narrower angle for the clearing frustum by setting the hFOV parameter accordingly. Future extensions will also to query a static map and determine which connected components belong to the map, not in the map, or moving. Each of these three classes of blobs will have configurable models to control the time they persist, and if these things are reported to the user. @@ -64,32 +74,8 @@ sudo apt-get install ros-kinetic-spatio-temporal-voxel-layer ### Install from source Required dependencies ROS Kinetic, navigation, OpenVDB, TBB. -If you are familiar with ROS, I have setup rosdep to work with all the necessary libraries, no need to install manually. In your workspace: - `sudo rosdep init && rosdep update && rosdep install --from-paths src --ignore-src -r -y` -If you are not familiar or cannot get rosdep to work, the dependencies and docs are listed below. - -#### ROS - -[See install instructions here.](http://wiki.ros.org/kinetic/Installation) - -#### Navigation - -`sudo apt-get install ros-kinetic-navigation` - -#### OpenVDB - -`sudo apt-get install libopenvdb3.1 libopenvdb-dev libopenvdb-tools` - -#### TBB - -`sudo apt-get install libtbb-dev libtbb2` - -#### OpenEXR - -`sudo apt-get install libopenexr-dev` - ## Configuration and Running ### costmap_common_params.yaml @@ -138,7 +124,9 @@ rgbd_obstacle_layer: vertical_fov_angle: 0.7 #default 0.7, radians horizontal_fov_angle: 1.04 #default 1.04, radians decay_acceleration: 1. #default 0, 1/s^2. If laser scanner MUST be 0 + model_type: 0 #default 0 (depth camera). Use 1 for 3D Lidar ``` +More configuration samples are included in the example folder, including a 3D lidar one. ### local/global_costmap_params.yaml @@ -151,7 +139,20 @@ Add this plugin to your costmap params file. `roslaunch [navigation_pkg] move_base.launch` +### Enabing/disabling observation_sources real-time + +To enable/disable observation sources use a ros service for each source: +~rgbd_obstacle_layer/$(source_name)/toggle_enabled (std_srvs/SetBool) + - request.data = true // Enable observation source + - request.data = false // Disable observation source + + Example: + ``` + rosservice call /move_base/global_costmap/rgbd_obstacle_layer/rgbd_back/toggle_enabled "data: true" + rosservice call /move_base/local_costmap/rgbd_obstacle_layer/rgbd_back/toggle_enabled "data: false" + ``` + ### Debug and Model Fitting I have made the frustum transformations available for visualization and debugging. You may enable them by the `VISUALIZE_FRUSTUM` macro, though be aware it takes a substantial decrease on performance since we're creating and destroying a ros publisher at a non-trivial rate. diff --git a/cfg/SpatioTemporalVoxelLayer.cfg b/cfg/SpatioTemporalVoxelLayer.cfg new file mode 100755 index 00000000..c39c8fcc --- /dev/null +++ b/cfg/SpatioTemporalVoxelLayer.cfg @@ -0,0 +1,29 @@ +#!/usr/bin/env python +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() + +combination_method = [] +combination_method.append(gen.const("largest", int_t, 0, "uses the highest of the layers in the stack")) +combination_method.append(gen.const("overwrite", int_t, 1, "overrides lower layers than this with these values")) +combination_method_enum = gen.enum(combination_method, "Combination methods") + +decay_model = [] +decay_model.append(gen.const("Linear", int_t, 0, "Linear")) +decay_model.append(gen.const("Exponential", int_t, 1, "Exponential")) +decay_model.append(gen.const("Permanent", int_t, 2, "Permanent")) +decay_model_enum = gen.enum(decay_model, "Decay models") + +gen.add("enabled", bool_t, 0, "Enabled or not", True) +gen.add("publish_voxel_map", bool_t, 0, "Publishes the voxel map", False) +gen.add("voxel_size", double_t, 0, "Size of the voxel in meters", 0.05, 0.001, 10.0) +gen.add("combination_method", int_t, 0, "Combination methods", 1, edit_method=combination_method_enum) +gen.add("mark_threshold", double_t, 0, "Over this threshold, the measurement will be marked", 0.0, 0.0, 200.0) +gen.add("update_footprint_enabled", bool_t, 0, "Cleans where the robot's footprint is", True) +gen.add("track_unknown_space", bool_t, 0, "If true, marks will be UNKNOWN (255) otherwise, FREE (0)", True) +gen.add("decay_model", int_t, 0, "Decay models", 0, edit_method=decay_model_enum) +gen.add("voxel_decay", double_t, 0, "Seconds if linear, e^n if exponential", 10.0, -1, 200.0) +gen.add("mapping_mode", bool_t, 0, "Mapping mode", False) +gen.add("map_save_duration", double_t, 60, "f mapping, how often to save a map for safety", 60.0, 1.0, 2000.0) + +exit(gen.generate("spatio_temporal_voxel_layer", "spatio_temporal_voxel_layer", "SpatioTemporalVoxelLayer")) diff --git a/example/constrained_indoor_environment_config.yaml b/example/constrained_indoor_environment_config.yaml index e7342987..6ff22433 100644 --- a/example/constrained_indoor_environment_config.yaml +++ b/example/constrained_indoor_environment_config.yaml @@ -38,3 +38,4 @@ rgbd_obstacle_layer: vertical_fov_angle: 0.8745 # default 0.7, radians horizontal_fov_angle: 1.048 # default 1.04, radians decay_acceleration: 15.0 # default 0, 1/s^2. If laser scanner MUST be 0 + model_type: 0 # default 0, model type for frustum. 0=depth camera, 1=3d lidar like VLP16 or similar diff --git a/example/outdoor_environment_config.yaml b/example/outdoor_environment_config.yaml index 75bef4e8..f281de11 100644 --- a/example/outdoor_environment_config.yaml +++ b/example/outdoor_environment_config.yaml @@ -38,3 +38,4 @@ rgbd_obstacle_layer: vertical_fov_angle: 0.8745 # default 0.7, radians horizontal_fov_angle: 1.048 # default 1.04, radians decay_acceleration: 1.0 # default 0, 1/s^2. If laser scanner MUST be 0 + model_type: 0 # default 0, model type for frustum. 0=depth camera, 1=3d lidar like VLP16 or similar diff --git a/example/standard_indoor_environment_config.yaml b/example/standard_indoor_environment_config.yaml index 761a49f5..a0feae5b 100644 --- a/example/standard_indoor_environment_config.yaml +++ b/example/standard_indoor_environment_config.yaml @@ -38,3 +38,4 @@ rgbd_obstacle_layer: vertical_fov_angle: 0.8745 # default 0.7, radians horizontal_fov_angle: 1.048 # default 1.04, radians decay_acceleration: 5.0 # default 0, 1/s^2. If laser scanner MUST be 0 + model_type: 0 # default 0, model type for frustum. 0=depth camera, 1=3d lidar like VLP16 or similar diff --git a/example/vlp16_config.yaml b/example/vlp16_config.yaml new file mode 100644 index 00000000..601d51b8 --- /dev/null +++ b/example/vlp16_config.yaml @@ -0,0 +1,42 @@ +rgbd_obstacle_layer: + enabled: true + voxel_decay: 15 # seconds if linear, e^n if exponential + decay_model: 0 # 0=linear, 1=exponential, -1=persistent + voxel_size: 0.05 # meters + track_unknown_space: true # default space is known + max_obstacle_height: 2.0 # meters + unknown_threshold: 15 # voxel height + mark_threshold: 0 # voxel height + update_footprint_enabled: true + combination_method: 1 # 1=max, 0=override + obstacle_range: 3.0 # meters + origin_z: 0.0 # meters + publish_voxel_map: false # default off + transform_tolerance: 0.2 # seconds + mapping_mode: false # default off, saves map not for navigation + map_save_duration: 60 # default 60s, how often to autosave + observation_sources: rgbd1_mark rgbd1_clear + rgbd1_mark: + data_type: PointCloud2 + topic: /velodyne_points + marking: true + clearing: false + min_obstacle_height: 0.3 # default 0, meters + max_obstacle_height: 2.0 # default 3, meters + expected_update_rate: 0.0 # default 0, if not updating at this rate at least, remove from buffer + observation_persistence: 0.0 # default 0, use all measurements taken during now-value, 0=latest + inf_is_valid: false # default false, for laser scans + voxel_filter: false # default off, apply voxel filter to sensor, recommend on + clear_after_reading: true # default false, clear the buffer after the layer gets readings from it + rgbd1_clear: + data_type: PointCloud2 + topic: /velodyne_points + marking: false + clearing: true + max_z: 8.0 # default 0, meters + min_z: 1.0 # default 10, meters + vertical_fov_angle: 0.523 # default 0.7, radians. For 3D lidars it's the symmetric FOV about the planar axis. + vertical_fov_padding: 0.05 # 3D Lidar only. Default 0, in meters + horizontal_fov_angle: 6.29 # 3D lidar scanners like the VLP16 have 360 deg horizontal FOV. + decay_acceleration: 5.0 # default 0, 1/s^2. + model_type: 1 # default 0, model type for frustum. 0=depth camera, 1=3d lidar like VLP16 or similar diff --git a/include/spatio_temporal_voxel_layer/frustum_models/depth_camera_frustum.hpp b/include/spatio_temporal_voxel_layer/frustum_models/depth_camera_frustum.hpp new file mode 100644 index 00000000..661705d5 --- /dev/null +++ b/include/spatio_temporal_voxel_layer/frustum_models/depth_camera_frustum.hpp @@ -0,0 +1,90 @@ +/********************************************************************* + * + * Software License Agreement + * + * Copyright (c) 2018, Simbe Robotics, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Simbe Robotics, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Steve Macenski (steven.macenski@simberobotics.com) + * Purpose: Structure for handling camera FOVs to construct frustums + * and associated methods + *********************************************************************/ + +#ifndef DEPTH_FRUSTUM_H_ +#define DEPTH_FRUSTUM_H_ + +// STVL +#include + +namespace geometry +{ + +// visualize the frustum should someone other than me care +#define VISUALIZE_FRUSTUM 0 + +// A class to model a depth sensor frustum in world space +class DepthCameraFrustum : public Frustum +{ +public: + DepthCameraFrustum(const double& vFOV, const double& hFOV, + const double& min_dist, const double& max_dist); + virtual ~DepthCameraFrustum(void); + + // transform plane normals by depth camera pose + virtual void TransformModel(void); + + // determine if a point is inside of the transformed frustum + virtual bool IsInside(const openvdb::Vec3d& pt); + + // set pose of depth camera in global space + virtual void SetPosition(const geometry_msgs::Point& origin); + virtual void SetOrientation(const geometry_msgs::Quaternion& quat); + +private: + // utils to find useful frustum metadata + void ComputePlaneNormals(void); + double Dot(const VectorWithPt3D&, const openvdb::Vec3d&) const; + double Dot(const VectorWithPt3D&, const Eigen::Vector3d&) const; + + double _vFOV, _hFOV, _min_d, _max_d; + std::vector _plane_normals; + Eigen::Vector3d _position; + Eigen::Quaterniond _orientation; + bool _valid_frustum; + + #if VISUALIZE_FRUSTUM + std::vector _frustum_pts; + ros::Publisher _frustumPub; + #endif +}; + +} // end namespace + +#endif diff --git a/include/spatio_temporal_voxel_layer/frustum.hpp b/include/spatio_temporal_voxel_layer/frustum_models/frustum.hpp similarity index 78% rename from include/spatio_temporal_voxel_layer/frustum.hpp rename to include/spatio_temporal_voxel_layer/frustum_models/frustum.hpp index af767e29..b5657ec5 100644 --- a/include/spatio_temporal_voxel_layer/frustum.hpp +++ b/include/spatio_temporal_voxel_layer/frustum_models/frustum.hpp @@ -33,8 +33,7 @@ * POSSIBILITY OF SUCH DAMAGE. * * Author: Steve Macenski (steven.macenski@simberobotics.com) - * Purpose: Structure for handling camera FOVs to construct frustums - * and associated methods + * Purpose: Abstract class for a frustum model implementation *********************************************************************/ #ifndef FRUSTUM_H_ @@ -60,9 +59,6 @@ namespace geometry { -// visualize the frustum should someone other than me care -#define VISUALIZE_FRUSTUM 0 - // A structure for maintaining vectors and points in world spaces struct VectorWithPt3D { @@ -99,36 +95,23 @@ struct VectorWithPt3D class Frustum { public: - Frustum(const double& vFOV, const double& hFOV, - const double& min_dist, const double& max_dist); - ~Frustum(void); - - // transform plane normals by depth camera pose - void TransformPlaneNormals(void); + Frustum() {}; + virtual ~Frustum(void) {}; // determine if a point is inside of the transformed frustum - bool IsInside(const openvdb::Vec3d& pt); + virtual bool IsInside(const openvdb::Vec3d& pt) = 0; // set pose of depth camera in global space - void SetPosition(const geometry_msgs::Point& origin); - void SetOrientation(const geometry_msgs::Quaternion& quat); + virtual void SetPosition(const geometry_msgs::Point& origin) = 0; + virtual void SetOrientation(const geometry_msgs::Quaternion& quat) = 0; -private: - // utils to find useful frustum metadata - void ComputePlaneNormals(void); - double Dot(const VectorWithPt3D&, const openvdb::Vec3d&) const; - double Dot(const VectorWithPt3D&, const Eigen::Vector3d&) const; + // transform model to the current coordinates + virtual void TransformModel(void) = 0; - double _vFOV, _hFOV, _min_d, _max_d; - std::vector _plane_normals; +private: Eigen::Vector3d _position; Eigen::Quaterniond _orientation; bool _valid_frustum; - - #if VISUALIZE_FRUSTUM - std::vector _frustum_pts; - ros::Publisher _frustumPub; - #endif }; } // end namespace diff --git a/include/spatio_temporal_voxel_layer/frustum_models/three_dimensional_lidar_frustum.hpp b/include/spatio_temporal_voxel_layer/frustum_models/three_dimensional_lidar_frustum.hpp new file mode 100644 index 00000000..86d2a8c0 --- /dev/null +++ b/include/spatio_temporal_voxel_layer/frustum_models/three_dimensional_lidar_frustum.hpp @@ -0,0 +1,89 @@ +/********************************************************************* + * + * Software License Agreement + * + * Copyright (c) 2018, Simbe Robotics, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Simbe Robotics, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Nicolas Varas (nicovaras@gmail.com) + * Purpose: Structure for handling 3D Lidar FOVs to construct frustums + * and associated methods + *********************************************************************/ + +#ifndef THREE_DIMENSIONAL_LIDAR_FRUSTUM_H_ +#define THREE_DIMENSIONAL_LIDAR_FRUSTUM_H_ + +// STVL +#include +// M_PI +#include + +namespace geometry +{ + +// A class to model a spinning 3D Lidar frustum in world space +class ThreeDimensionalLidarFrustum : public Frustum +{ +public: + ThreeDimensionalLidarFrustum(const double& vFOV, const double& vFOVPadding, + const double& hFOV, const double& min_dist, const double& max_dist); + virtual ~ThreeDimensionalLidarFrustum(void); + + // Does nothing in 3D lidar model + virtual void TransformModel(void); + + // determine if a point is inside of the transformed frustum + virtual bool IsInside(const openvdb::Vec3d& pt); + + // set pose of 3d lidar in global space + virtual void SetPosition(const geometry_msgs::Point& origin); + virtual void SetOrientation(const geometry_msgs::Quaternion& quat); + +private: + // utils to find useful frustum metadata + double Dot(const VectorWithPt3D&, const openvdb::Vec3d&) const; + double Dot(const VectorWithPt3D&, const Eigen::Vector3d&) const; + + double _vFOV, _vFOVPadding, _hFOV, _min_d, _max_d; + double _hFOVhalf; + double _min_d_squared, _max_d_squared; + double _tan_vFOVhalf; + double _tan_vFOVhalf_squared; + Eigen::Vector3d _position; + Eigen::Quaterniond _orientation; + Eigen::Quaterniond _orientation_conjugate; + bool _valid_frustum; + bool _full_hFOV; + +}; + +} // end namespace + +#endif diff --git a/include/spatio_temporal_voxel_layer/measurement_buffer.hpp b/include/spatio_temporal_voxel_layer/measurement_buffer.hpp index 6c7e122b..fa1bb2c3 100644 --- a/include/spatio_temporal_voxel_layer/measurement_buffer.hpp +++ b/include/spatio_temporal_voxel_layer/measurement_buffer.hpp @@ -75,26 +75,29 @@ typedef sensor_msgs::PointCloud2::Ptr point_cloud_ptr; class MeasurementBuffer { public: - MeasurementBuffer(const std::string& topic_name, \ - const double& observation_keep_time, \ - const double& expected_update_rate, \ - const double& min_obstacle_height, \ - const double& max_obstacle_height, \ - const double& obstacle_range, \ - tf2_ros::Buffer& tf, \ - const std::string& global_frame, \ - const std::string& sensor_frame, \ - const double& tf_tolerance, \ - const double& min_d, \ - const double& max_d, \ - const double& vFOV, \ - const double& hFOV, \ - const double& decay_acceleration, \ - const bool& marking, \ - const bool& clearing, \ - const double& voxel_size, \ - const bool& voxel_filter, \ - const bool& clear_buffer_after_reading); + MeasurementBuffer(const std::string& topic_name, \ + const double& observation_keep_time, \ + const double& expected_update_rate, \ + const double& min_obstacle_height, \ + const double& max_obstacle_height, \ + const double& obstacle_range, \ + tf2_ros::Buffer& tf, \ + const std::string& global_frame, \ + const std::string& sensor_frame, \ + const double& tf_tolerance, \ + const double& min_d, \ + const double& max_d, \ + const double& vFOV, \ + const double& vFOVPadding, \ + const double& hFOV, \ + const double& decay_acceleration, \ + const bool& marking, \ + const bool& clearing, \ + const double& voxel_size, \ + const bool& voxel_filter, \ + const bool& enabled, \ + const bool& clear_buffer_after_reading, \ + const ModelType& model_type); ~MeasurementBuffer(void); @@ -104,9 +107,15 @@ class MeasurementBuffer // Get measurements from the buffer void GetReadings(std::vector& observations); + // enabler setter getter + bool IsEnabled(void) const; + void SetEnabled(const bool& enabled); + // State knoweldge if sensors are operating as expected bool UpdatedAtExpectedRate(void) const; void ResetLastUpdatedTime(void); + void ResetAllMeasurements(void); + bool ClearAfterReading(void); // Public mutex locks void Lock(void); @@ -123,9 +132,10 @@ class MeasurementBuffer std::string _global_frame, _topic_name, _sensor_frame; std::list _observation_list; double _min_obstacle_height, _max_obstacle_height, _obstacle_range, _tf_tolerance; - double _min_z, _max_z, _vertical_fov, _horizontal_fov, _decay_acceleration; - double _voxel_size; - bool _marking, _clearing, _voxel_filter, _clear_buffer_after_reading; + double _min_z, _max_z, _vertical_fov, _vertical_fov_padding, _horizontal_fov; + double _decay_acceleration, _voxel_size; + bool _marking, _clearing, _voxel_filter, _clear_buffer_after_reading, _enabled; + ModelType _model_type; }; } // namespace buffer diff --git a/include/spatio_temporal_voxel_layer/measurement_reading.h b/include/spatio_temporal_voxel_layer/measurement_reading.h index ae6b83c0..90c76882 100644 --- a/include/spatio_temporal_voxel_layer/measurement_reading.h +++ b/include/spatio_temporal_voxel_layer/measurement_reading.h @@ -44,6 +44,12 @@ #include #include +enum ModelType +{ + DEPTH_CAMERA = 0, + THREE_DIMENSIONAL_LIDAR = 1 +}; + namespace observation { @@ -57,20 +63,23 @@ struct MeasurementReading } /*****************************************************************************/ - MeasurementReading(geometry_msgs::Point& origin, sensor_msgs::PointCloud2 cloud, \ - double obstacle_range, double min_z, double max_z, double vFOV, double hFOV, - double decay_acceleration, bool marking, bool clearing) : + MeasurementReading(geometry_msgs::Point& origin, sensor_msgs::PointCloud2 cloud, \ + double obstacle_range, double min_z, double max_z, double vFOV, \ + double vFOVPadding, double hFOV, double decay_acceleration, bool marking, \ + bool clearing, ModelType model_type) : /*****************************************************************************/ _origin(origin), \ - _cloud(new sensor_msgs::PointCloud2(cloud)), \ + _cloud(new sensor_msgs::PointCloud2(cloud)), \ _obstacle_range_in_m(obstacle_range), \ _min_z_in_m(min_z), \ _max_z_in_m(max_z), \ _vertical_fov_in_rad(vFOV), \ + _vertical_fov_padding_in_m(vFOVPadding), \ _horizontal_fov_in_rad(hFOV), \ _decay_acceleration(decay_acceleration), \ _marking(marking), \ - _clearing(clearing) + _clearing(clearing), \ + _model_type(model_type) { } @@ -91,11 +100,13 @@ struct MeasurementReading _min_z_in_m(obs._min_z_in_m), \ _max_z_in_m(obs._max_z_in_m), \ _vertical_fov_in_rad(obs._vertical_fov_in_rad), \ + _vertical_fov_padding_in_m(obs._vertical_fov_padding_in_m),\ _horizontal_fov_in_rad(obs._horizontal_fov_in_rad), \ _marking(obs._marking), \ _clearing(obs._clearing), \ _orientation(obs._orientation), \ - _decay_acceleration(obs._decay_acceleration) + _decay_acceleration(obs._decay_acceleration), \ + _model_type(obs._model_type) { } @@ -103,8 +114,9 @@ struct MeasurementReading geometry_msgs::Quaternion _orientation; sensor_msgs::PointCloud2::Ptr _cloud; double _obstacle_range_in_m, _min_z_in_m, _max_z_in_m; - double _vertical_fov_in_rad, _horizontal_fov_in_rad; + double _vertical_fov_in_rad, _vertical_fov_padding_in_m, _horizontal_fov_in_rad; double _marking, _clearing, _decay_acceleration; + ModelType _model_type; }; diff --git a/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_grid.hpp b/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_grid.hpp index ada93580..e4deef4d 100644 --- a/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_grid.hpp +++ b/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_grid.hpp @@ -65,14 +65,22 @@ #include // measurement struct and buffer #include -#include -// Mutex -#include -#include +#include +#include +// Mutex and locks +#include +#include namespace volume_grid { +enum GlobalDecayModel +{ + LINEAR = 0, + EXPONENTIAL = 1, + PERSISTENT = 2 +}; + // Structure for an occupied cell for map struct occupany_cell { @@ -92,11 +100,18 @@ struct occupany_cell // Structure for wrapping frustum model and necessary metadata struct frustum_model { - frustum_model(geometry::Frustum _frustum, const double& _factor) : + frustum_model(geometry::Frustum* _frustum, const double& _factor) : frustum(_frustum), accel_factor(_factor) { } - geometry::Frustum frustum; + ~frustum_model() + { + if (frustum) + { + delete frustum; + } + } + geometry::Frustum* frustum; const double accel_factor; }; diff --git a/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_layer.hpp b/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_layer.hpp index 9321c8f2..77ae8bba 100644 --- a/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_layer.hpp +++ b/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_layer.hpp @@ -43,9 +43,11 @@ // voxel grid #include +#include // ROS #include #include +#include // costmap #include #include @@ -64,6 +66,7 @@ #include #include #include +#include // projector #include // tf @@ -79,6 +82,8 @@ namespace spatio_temporal_voxel_layer // conveniences for line lengths typedef std::vector >::iterator observation_subscribers_iter; typedef std::vector >::iterator observation_buffers_iter; +typedef spatio_temporal_voxel_layer::SpatioTemporalVoxelLayerConfig dynamicReconfigureType; +typedef dynamic_reconfigure::Server dynamicReconfigureServerType; // Core ROS voxel layer class class SpatioTemporalVoxelLayer : public costmap_2d::CostmapLayer @@ -102,9 +107,11 @@ class SpatioTemporalVoxelLayer : public costmap_2d::CostmapLayer virtual void activate(void); virtual void deactivate(void); + // Functions for sensor feeds bool GetMarkingObservations(std::vector& marking_observations) const; bool GetClearingObservations(std::vector& marking_observations) const; + void ObservationsResetAfterReading() const; // Functions to interact with maps void UpdateROSCostmap(double* min_x, double* min_y, double* max_x, double* max_y); @@ -129,12 +136,24 @@ class SpatioTemporalVoxelLayer : public costmap_2d::CostmapLayer bool AddStaticObservations(const observation::MeasurementReading& obs); bool RemoveStaticObservations(void); + // Dynamic reconfigure + void DynamicReconfigureCallback(dynamicReconfigureType &config, uint32_t level); + + // Enable/Disable callback + bool BufferEnablerCallback( std_srvs::SetBool::Request & request, \ + std_srvs::SetBool::Response & response, \ + boost::shared_ptr& buffer, \ + boost::shared_ptr& subcriber); + + laser_geometry::LaserProjection _laser_projector; std::vector > _observation_subscribers; std::vector > _observation_notifiers; std::vector > _observation_buffers; std::vector > _marking_buffers; std::vector > _clearing_buffers; + std::vector _buffer_enabler_servers; + dynamicReconfigureServerType* _dynamic_reconfigure_server; bool _publish_voxels, _mapping_mode; ros::Publisher _voxel_pub; @@ -142,12 +161,14 @@ class SpatioTemporalVoxelLayer : public costmap_2d::CostmapLayer ros::Duration _map_save_duration; ros::Time _last_map_save_time; std::string _global_frame; - double _voxel_size; + double _voxel_size, _voxel_decay; int _combination_method, _mark_threshold; + volume_grid::GlobalDecayModel _decay_model; bool _update_footprint_enabled, _enabled; std::vector _transformed_footprint; std::vector _static_observations; volume_grid::SpatioTemporalVoxelGrid* _voxel_grid; + boost::recursive_mutex _voxel_grid_lock; }; }; // end namespace diff --git a/package.xml b/package.xml index 208cf0ef..7f0f358b 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ spatio_temporal_voxel_layer - 1.1.4 + 1.3.1 The spatio-temporal 3D obstacle costmap package Steve Macenski @@ -17,7 +17,6 @@ laser_geometry pcl_ros pcl_conversions - libpcl-all-dev roscpp message_filters tf2_ros @@ -27,6 +26,7 @@ libopenvdb tbb libopenexr-dev + dynamic_reconfigure costmap_2d laser_geometry message_filters @@ -43,8 +43,7 @@ libopenvdb tbb libopenexr-dev - libpcl-all - + dynamic_reconfigure diff --git a/src/frustum.cpp b/src/frustum_models/depth_camera_frustum.cpp similarity index 93% rename from src/frustum.cpp rename to src/frustum_models/depth_camera_frustum.cpp index 54619f1c..3ae5aa01 100644 --- a/src/frustum.cpp +++ b/src/frustum_models/depth_camera_frustum.cpp @@ -35,16 +35,15 @@ * Author: Steve Macenski (steven.macenski@simberobotics.com) *********************************************************************/ -#include +#include namespace geometry { /*****************************************************************************/ -Frustum::Frustum(const double& vFOV, const double& hFOV, \ - const double& min_dist, const double& max_dist) : - _vFOV(vFOV), _hFOV(hFOV), \ - _min_d(min_dist), _max_d(max_dist) +DepthCameraFrustum::DepthCameraFrustum(const double& vFOV, const double& hFOV, + const double& min_dist, const double& max_dist) : + _vFOV(vFOV), _hFOV(hFOV), _min_d(min_dist), _max_d(max_dist) /*****************************************************************************/ { _valid_frustum = false; @@ -59,13 +58,13 @@ Frustum::Frustum(const double& vFOV, const double& hFOV, \ } /*****************************************************************************/ -Frustum::~Frustum(void) +DepthCameraFrustum::~DepthCameraFrustum(void) /*****************************************************************************/ { } /*****************************************************************************/ -void Frustum::ComputePlaneNormals(void) +void DepthCameraFrustum::ComputePlaneNormals(void) /*****************************************************************************/ { // give ability to construct with bogus values @@ -159,7 +158,7 @@ void Frustum::ComputePlaneNormals(void) } /*****************************************************************************/ -void Frustum::TransformPlaneNormals(void) +void DepthCameraFrustum::TransformModel(void) /*****************************************************************************/ { if (!_valid_frustum) @@ -290,7 +289,7 @@ void Frustum::TransformPlaneNormals(void) } /*****************************************************************************/ -bool Frustum::IsInside(const openvdb::Vec3d& pt) +bool DepthCameraFrustum::IsInside(const openvdb::Vec3d& pt) /*****************************************************************************/ { if (!_valid_frustum) @@ -315,21 +314,21 @@ bool Frustum::IsInside(const openvdb::Vec3d& pt) } /*****************************************************************************/ -void Frustum::SetPosition(const geometry_msgs::Point& origin) +void DepthCameraFrustum::SetPosition(const geometry_msgs::Point& origin) /*****************************************************************************/ { _position = Eigen::Vector3d(origin.x, origin.y, origin.z); } /*****************************************************************************/ -void Frustum::SetOrientation(const geometry_msgs::Quaternion& quat) +void DepthCameraFrustum::SetOrientation(const geometry_msgs::Quaternion& quat) /*****************************************************************************/ { _orientation = Eigen::Quaterniond(quat.w, quat.x, quat.y, quat.z); } /*****************************************************************************/ -double Frustum::Dot(const VectorWithPt3D& plane_pt, \ +double DepthCameraFrustum::Dot(const VectorWithPt3D& plane_pt, \ const openvdb::Vec3d& query_pt) const /*****************************************************************************/ { @@ -337,7 +336,7 @@ double Frustum::Dot(const VectorWithPt3D& plane_pt, \ } /*****************************************************************************/ -double Frustum::Dot(const VectorWithPt3D& plane_pt, \ +double DepthCameraFrustum::Dot(const VectorWithPt3D& plane_pt, \ const Eigen::Vector3d& query_pt) const /*****************************************************************************/ { diff --git a/src/frustum_models/three_dimensional_lidar_frustum.cpp b/src/frustum_models/three_dimensional_lidar_frustum.cpp new file mode 100644 index 00000000..1095b1ea --- /dev/null +++ b/src/frustum_models/three_dimensional_lidar_frustum.cpp @@ -0,0 +1,161 @@ +/********************************************************************* + * + * Software License Agreement + * + * Copyright (c) 2018, Simbe Robotics, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Simbe Robotics, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Nicolas Varas (nicovaras@gmail.com) + *********************************************************************/ + +#include + +namespace geometry +{ + +/*****************************************************************************/ +ThreeDimensionalLidarFrustum::ThreeDimensionalLidarFrustum(const double& vFOV, + const double& vFOVPadding, + const double& hFOV, + const double& min_dist, + const double& max_dist) + : _vFOV(vFOV), + _vFOVPadding(vFOVPadding), + _hFOV(hFOV), + _min_d(min_dist), + _max_d(max_dist) +/*****************************************************************************/ +{ + _hFOVhalf = _hFOV / 2.0; + _tan_vFOVhalf = tan(_vFOV / 2.0); + _tan_vFOVhalf_squared = _tan_vFOVhalf * _tan_vFOVhalf; + _min_d_squared = _min_d * _min_d; + _max_d_squared = _max_d * _max_d; + _full_hFOV = false; + if (_hFOV > 6.27) + { + _full_hFOV = true; + } +} + +/*****************************************************************************/ +ThreeDimensionalLidarFrustum::~ThreeDimensionalLidarFrustum(void) +/*****************************************************************************/ +{ +} + +/*****************************************************************************/ +void ThreeDimensionalLidarFrustum::TransformModel(void) +/*****************************************************************************/ +{ + _orientation_conjugate = _orientation.conjugate(); + _valid_frustum = true; +} + +/*****************************************************************************/ +bool ThreeDimensionalLidarFrustum::IsInside(const openvdb::Vec3d &pt) +/*****************************************************************************/ +{ + + Eigen::Vector3d point_in_global_frame(pt[0], pt[1], pt[2]); + Eigen::Vector3d transformed_pt = + _orientation_conjugate * (point_in_global_frame - _position); + + const double radial_distance_squared = \ + (transformed_pt[0] * transformed_pt[0]) + \ + (transformed_pt[1] * transformed_pt[1]); + + // Check if inside frustum valid range + if (radial_distance_squared > _max_d_squared || + radial_distance_squared < _min_d_squared) + { + return false; + } + + // // Check if inside frustum valid vFOV + const double v_padded = fabs(transformed_pt[2]) + _vFOVPadding; + if (( v_padded * v_padded / radial_distance_squared) > _tan_vFOVhalf_squared) + { + return false; + } + + // Check if inside frustum valid hFOV, unless hFOV is full-circle (360 degree) + if (!_full_hFOV) + { + if (transformed_pt[0] > 0) + { + if (fabs(atan(transformed_pt[1] / transformed_pt[0])) > _hFOVhalf) + { + return false; + } + } + else if ( \ + fabs(atan(transformed_pt[0] / transformed_pt[1])) + M_PI/2 > _hFOVhalf) + { + return false; + } + } + + return true; +} + +/*****************************************************************************/ +void ThreeDimensionalLidarFrustum::SetPosition(const \ + geometry_msgs::Point& origin) +/*****************************************************************************/ +{ + _position = Eigen::Vector3d(origin.x, origin.y, origin.z); +} + +/*****************************************************************************/ +void ThreeDimensionalLidarFrustum::SetOrientation(const \ + geometry_msgs::Quaternion& quat) +/*****************************************************************************/ +{ + _orientation = Eigen::Quaterniond(quat.w, quat.x, quat.y, quat.z); +} + +/*****************************************************************************/ +double ThreeDimensionalLidarFrustum::Dot(const VectorWithPt3D &plane_pt, + const openvdb::Vec3d &query_pt) const +/*****************************************************************************/ +{ + return plane_pt.x*query_pt[0]+plane_pt.y*query_pt[1]+plane_pt.z*query_pt[2]; +} + +/*****************************************************************************/ +double ThreeDimensionalLidarFrustum::Dot(const VectorWithPt3D &plane_pt, + const Eigen::Vector3d &query_pt) const +/*****************************************************************************/ +{ + return plane_pt.x*query_pt[0]+plane_pt.y*query_pt[1]+plane_pt.z*query_pt[2]; +} + +} // namespace geometry diff --git a/src/measurement_buffer.cpp b/src/measurement_buffer.cpp index 749347cd..88067641 100644 --- a/src/measurement_buffer.cpp +++ b/src/measurement_buffer.cpp @@ -41,26 +41,29 @@ namespace buffer { /*****************************************************************************/ -MeasurementBuffer::MeasurementBuffer(const std::string& topic_name, \ - const double& observation_keep_time, \ - const double& expected_update_rate, \ - const double& min_obstacle_height, \ - const double& max_obstacle_height, \ - const double& obstacle_range, \ - tf2_ros::Buffer& tf, \ - const std::string& global_frame, \ - const std::string& sensor_frame, \ - const double& tf_tolerance, \ - const double& min_d, \ - const double& max_d, \ - const double& vFOV, \ - const double& hFOV, \ - const double& decay_acceleration, \ - const bool& marking, \ - const bool& clearing, \ - const double& voxel_size, \ - const bool& voxel_filter, \ - const bool& clear_buffer_after_reading) : +MeasurementBuffer::MeasurementBuffer(const std::string& topic_name, \ + const double& observation_keep_time, \ + const double& expected_update_rate, \ + const double& min_obstacle_height, \ + const double& max_obstacle_height, \ + const double& obstacle_range, \ + tf2_ros::Buffer& tf, \ + const std::string& global_frame, \ + const std::string& sensor_frame, \ + const double& tf_tolerance, \ + const double& min_d, \ + const double& max_d, \ + const double& vFOV, \ + const double& vFOVPadding, \ + const double& hFOV, \ + const double& decay_acceleration, \ + const bool& marking, \ + const bool& clearing, \ + const double& voxel_size, \ + const bool& voxel_filter, \ + const bool& enabled, \ + const bool& clear_buffer_after_reading, \ + const ModelType& model_type) : /*****************************************************************************/ _buffer(tf), _observation_keep_time(observation_keep_time), _expected_update_rate(expected_update_rate),_last_updated(ros::Time::now()), @@ -68,10 +71,12 @@ MeasurementBuffer::MeasurementBuffer(const std::string& topic_name, \ _topic_name(topic_name), _min_obstacle_height(min_obstacle_height), _max_obstacle_height(max_obstacle_height), _obstacle_range(obstacle_range), _tf_tolerance(tf_tolerance), _min_z(min_d), _max_z(max_d), - _vertical_fov(vFOV), _horizontal_fov(hFOV), - _decay_acceleration(decay_acceleration), _marking(marking), - _clearing(clearing), _voxel_size(voxel_size), _voxel_filter(voxel_filter), - _clear_buffer_after_reading(clear_buffer_after_reading) + _vertical_fov(vFOV), _vertical_fov_padding(vFOVPadding), + _horizontal_fov(hFOV), _decay_acceleration(decay_acceleration), + _marking(marking), _clearing(clearing), _voxel_size(voxel_size), + _voxel_filter(voxel_filter), _enabled(enabled), + _clear_buffer_after_reading(clear_buffer_after_reading), + _model_type(model_type) { } @@ -118,10 +123,12 @@ void MeasurementBuffer::BufferROSCloud(const sensor_msgs::PointCloud2& cloud) _observation_list.front()._min_z_in_m = _min_z; _observation_list.front()._max_z_in_m = _max_z; _observation_list.front()._vertical_fov_in_rad = _vertical_fov; + _observation_list.front()._vertical_fov_padding_in_m = _vertical_fov_padding; _observation_list.front()._horizontal_fov_in_rad = _horizontal_fov; _observation_list.front()._decay_acceleration = _decay_acceleration; _observation_list.front()._clearing = _clearing; _observation_list.front()._marking = _marking; + _observation_list.front()._model_type = _model_type; if (_clearing && !_marking) { @@ -194,12 +201,6 @@ void MeasurementBuffer::GetReadings( \ { observations.push_back(*it); } - - // clear from buffer so we don't over report - if (_clear_buffer_after_reading || _observation_keep_time.toSec() == 0.) - { - _observation_list.clear(); - } } /*****************************************************************************/ @@ -231,6 +232,20 @@ void MeasurementBuffer::RemoveStaleObservations(void) } } +/*****************************************************************************/ +void MeasurementBuffer::ResetAllMeasurements(void) +/*****************************************************************************/ +{ + _observation_list.clear(); +} + +/*****************************************************************************/ +bool MeasurementBuffer::ClearAfterReading(void) +/*****************************************************************************/ +{ + return _clear_buffer_after_reading; +} + /*****************************************************************************/ bool MeasurementBuffer::UpdatedAtExpectedRate(void) const /*****************************************************************************/ @@ -251,6 +266,20 @@ bool MeasurementBuffer::UpdatedAtExpectedRate(void) const return current; } +/*****************************************************************************/ +bool MeasurementBuffer::IsEnabled(void) const +/*****************************************************************************/ +{ + return _enabled; +} + +/*****************************************************************************/ +void MeasurementBuffer::SetEnabled(const bool& enabled) +/*****************************************************************************/ +{ + _enabled = enabled; +} + /*****************************************************************************/ void MeasurementBuffer::ResetLastUpdatedTime(void) /*****************************************************************************/ diff --git a/src/spatio_temporal_voxel_grid.cpp b/src/spatio_temporal_voxel_grid.cpp index 11aa1025..1e49f7ef 100644 --- a/src/spatio_temporal_voxel_grid.cpp +++ b/src/spatio_temporal_voxel_grid.cpp @@ -61,7 +61,10 @@ SpatioTemporalVoxelGrid::~SpatioTemporalVoxelGrid(void) /*****************************************************************************/ { // pcl pointclouds free themselves - delete _cost_map; + if (_cost_map) + { + delete _cost_map; + } } /*****************************************************************************/ @@ -109,7 +112,6 @@ void SpatioTemporalVoxelGrid::ClearFrustums(const \ if(clearing_readings.size() == 0) { - obs_frustums.push_back(frustum_model(geometry::Frustum(0.,0.,0.,0.), 0.)); TemporalClearAndGenerateCostmap(obs_frustums); return; } @@ -120,13 +122,33 @@ void SpatioTemporalVoxelGrid::ClearFrustums(const \ clearing_readings.begin(); for (it; it != clearing_readings.end(); ++it) { - geometry::Frustum frustum(it->_vertical_fov_in_rad, \ - it->_horizontal_fov_in_rad, \ - it->_min_z_in_m, \ - it->_max_z_in_m); - frustum.SetPosition(it->_origin); - frustum.SetOrientation(it->_orientation); - frustum.TransformPlaneNormals(); + geometry::Frustum* frustum; + if (it->_model_type == DEPTH_CAMERA) + { + frustum = new geometry::DepthCameraFrustum(it->_vertical_fov_in_rad, + it->_horizontal_fov_in_rad, + it->_min_z_in_m, + it->_max_z_in_m); + } + else if (it->_model_type == THREE_DIMENSIONAL_LIDAR) + { + frustum = new geometry::ThreeDimensionalLidarFrustum( \ + it->_vertical_fov_in_rad, + it->_vertical_fov_padding_in_m, + it->_horizontal_fov_in_rad, + it->_min_z_in_m, + it->_max_z_in_m); + } + else + { + // add else if statement for each implemented model + delete frustum; + continue; + } + + frustum->SetPosition(it->_origin); + frustum->SetOrientation(it->_orientation); + frustum->TransformModel(); obs_frustums.emplace_back(frustum, it->_decay_acceleration); } TemporalClearAndGenerateCostmap(obs_frustums); @@ -143,7 +165,7 @@ void SpatioTemporalVoxelGrid::TemporalClearAndGenerateCostmap( \ // check each point in the grid for inclusion in a frustum openvdb::DoubleGrid::ValueOnCIter cit_grid = _grid->cbeginValueOn(); - for (cit_grid; cit_grid; ++cit_grid) + for (cit_grid; cit_grid.test(); ++cit_grid) { const openvdb::Coord pt_index(cit_grid.getCoord()); @@ -156,7 +178,12 @@ void SpatioTemporalVoxelGrid::TemporalClearAndGenerateCostmap( \ for(frustum_it; frustum_it != frustums.end(); ++frustum_it) { - if ( frustum_it->frustum.IsInside(this->IndexToWorld(pt_index)) ) + if (!frustum_it->frustum) + { + continue; + } + + if ( frustum_it->frustum->IsInside(this->IndexToWorld(pt_index)) ) { frustum_cycle = true; @@ -165,7 +192,7 @@ void SpatioTemporalVoxelGrid::TemporalClearAndGenerateCostmap( \ const double time_until_decay = base_duration_to_decay - \ frustum_acceleration; - if (time_until_decay <= 0) + if (time_until_decay < 0.) { // expired by acceleration if(!this->ClearGridPoint(pt_index)) @@ -189,10 +216,9 @@ void SpatioTemporalVoxelGrid::TemporalClearAndGenerateCostmap( \ // if not inside any, check against nominal decay model if(!frustum_cycle) { - const double time_until_decay = base_duration_to_decay; - if (time_until_decay <= 0) + if (base_duration_to_decay < 0.) { - // expired by acceleration + // expired by temporal clearing if(!this->ClearGridPoint(pt_index)) { std::cout << "Failed to clear point." << std::endl; @@ -314,7 +340,7 @@ double SpatioTemporalVoxelGrid::GetTemporalClearingDuration(const double& time_d { return _voxel_decay * std::exp(-time_delta); } - return 0.; // permanent + return _voxel_decay; // PERSISTENT } /*****************************************************************************/ @@ -323,20 +349,9 @@ double SpatioTemporalVoxelGrid::GetFrustumAcceleration( \ const double& acceleration_factor) /*****************************************************************************/ { - if (_decay_model == 0) // linear - { - const double acceleration = 1. / 6. * \ - acceleration_factor * (time_delta * time_delta * time_delta); - return acceleration; - } - else if (_decay_model == 1) // exponential - { - const double acceleration = 1. / \ - (acceleration_factor * acceleration_factor) * \ - std::exp(acceleration_factor * time_delta); - return acceleration; - } - return 0.; // permanent + const double acceleration = 1. / 6. * acceleration_factor * \ + (time_delta * time_delta * time_delta); + return acceleration; } /*****************************************************************************/ diff --git a/src/spatio_temporal_voxel_layer.cpp b/src/spatio_temporal_voxel_layer.cpp index f1ada56f..d7a7c45d 100644 --- a/src/spatio_temporal_voxel_layer.cpp +++ b/src/spatio_temporal_voxel_layer.cpp @@ -49,8 +49,14 @@ SpatioTemporalVoxelLayer::SpatioTemporalVoxelLayer(void) SpatioTemporalVoxelLayer::~SpatioTemporalVoxelLayer(void) /*****************************************************************************/ { - // I prefer to manage my own memory, I know others like std::unique_ptr - delete _voxel_grid; + if (_dynamic_reconfigure_server) + { + delete _dynamic_reconfigure_server; + } + if (_voxel_grid) + { + delete _voxel_grid; + } } /*****************************************************************************/ @@ -68,9 +74,9 @@ void SpatioTemporalVoxelLayer::onInitialize(void) getName().c_str(), _global_frame.c_str()); bool track_unknown_space; - double transform_tolerance, voxel_decay, map_save_time; + double transform_tolerance, map_save_time; std::string topics_string; - int decay_model; + int decay_model_int; // source names nh.param("observation_sources", topics_string, std::string("")); // timeout in seconds for transforms @@ -91,13 +97,15 @@ void SpatioTemporalVoxelLayer::onInitialize(void) // keep tabs on unknown space nh.param("track_unknown_space", track_unknown_space, \ layered_costmap_->isTrackingUnknown()); - nh.param("decay_model", decay_model, 0); + nh.param("decay_model", decay_model_int, 0); + _decay_model = static_cast(decay_model_int); // decay param - nh.param("voxel_decay", voxel_decay, -1.); + nh.param("voxel_decay", _voxel_decay, -1.); // whether to map or navigate nh.param("mapping_mode", _mapping_mode, false); // if mapping, how often to save a map for safety nh.param("map_save_duration", map_save_time, 60.); + ROS_INFO("%s loaded parameters from parameter server.", getName().c_str()); if (_mapping_mode) { @@ -108,25 +116,25 @@ void SpatioTemporalVoxelLayer::onInitialize(void) if (track_unknown_space) { default_value_ = costmap_2d::NO_INFORMATION; - } else { - default_value_ = costmap_2d::FREE_SPACE; } - - if (_publish_voxels) + else { - _voxel_pub = nh.advertise("voxel_grid", 1); + default_value_ = costmap_2d::FREE_SPACE; } + + _voxel_pub = nh.advertise("voxel_grid", 1); _grid_saver = nh.advertiseService("spatiotemporal_voxel_grid/save_grid", \ &SpatioTemporalVoxelLayer::SaveGridCallback, \ this); _voxel_grid = new volume_grid::SpatioTemporalVoxelGrid(_voxel_size, \ (double)default_value_, \ - decay_model, \ - voxel_decay, \ + _decay_model, \ + _voxel_decay, \ _publish_voxels); matchSize(); current_ = true; + ROS_INFO("%s created underlying voxel grid.", getName().c_str()); const std::string tf_prefix = tf::getPrefixParam(prefix_nh); std::stringstream ss(topics_string); @@ -137,7 +145,8 @@ void SpatioTemporalVoxelLayer::onInitialize(void) // get the parameters for the specific topic double observation_keep_time, expected_update_rate, min_obstacle_height; - double max_obstacle_height, min_z, max_z, vFOV, hFOV, decay_acceleration; + double max_obstacle_height, min_z, max_z, vFOV, vFOVPadding; + double hFOV, decay_acceleration; std::string topic, sensor_frame, data_type; bool inf_is_valid, clearing, marking, voxel_filter, clear_after_reading; @@ -157,6 +166,8 @@ void SpatioTemporalVoxelLayer::onInitialize(void) source_node.param("max_z", max_z, 10.); // vertical FOV angle in rad source_node.param("vertical_fov_angle", vFOV, 0.7); + // vertical FOV padding in meters (3D lidar frustum only) + source_node.param("vertical_fov_padding", vFOVPadding, 0.0); // horizontal FOV angle in rad source_node.param("horizontal_fov_angle", hFOV, 1.04); // acceleration scales the model's decay in presence of readings @@ -165,6 +176,10 @@ void SpatioTemporalVoxelLayer::onInitialize(void) source_node.param("voxel_filter", voxel_filter, false); // clears measurement buffer after reading values from it source_node.param("clear_after_reading", clear_after_reading, false); + // model type - default depth camera frustum model + int model_type_int; + source_node.param("model_type", model_type_int, 0); + ModelType model_type = static_cast(model_type_int); if (!sensor_frame.empty()) { @@ -184,15 +199,16 @@ void SpatioTemporalVoxelLayer::onInitialize(void) source_node.getParam(obstacle_range_param_name, obstacle_range); } + bool enabled = true; // create an observation buffer _observation_buffers.push_back( boost::shared_ptr - (new buffer::MeasurementBuffer(topic, observation_keep_time, \ - expected_update_rate, min_obstacle_height, max_obstacle_height, \ - obstacle_range, *tf_, _global_frame, \ - sensor_frame, transform_tolerance, min_z, max_z, vFOV, \ - hFOV, decay_acceleration, marking, clearing, _voxel_size, \ - voxel_filter, clear_after_reading))); + (new buffer::MeasurementBuffer(topic, observation_keep_time, \ + expected_update_rate, min_obstacle_height, max_obstacle_height, \ + obstacle_range, *tf_, _global_frame, sensor_frame, \ + transform_tolerance, min_z, max_z, vFOV, vFOVPadding, hFOV, \ + decay_acceleration, marking, clearing, _voxel_size, \ + voxel_filter, enabled, clear_after_reading, model_type))); // Add buffer to marking observation buffers if (marking == true) @@ -249,6 +265,18 @@ 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); } @@ -262,6 +290,14 @@ void SpatioTemporalVoxelLayer::onInitialize(void) _observation_notifiers.back()->setTargetFrames(target_frames); } } + + // Dynamic reconfigure + dynamic_reconfigure::Server::CallbackType f; + f = boost::bind(&SpatioTemporalVoxelLayer::DynamicReconfigureCallback, \ + this, _1, _2); + _dynamic_reconfigure_server = new dynamicReconfigureServerType(nh); + _dynamic_reconfigure_server->setCallback(f); + ROS_INFO("%s initialization complete!", getName().c_str()); } @@ -334,6 +370,40 @@ void SpatioTemporalVoxelLayer::PointCloud2Callback( \ buffer->Unlock(); } +/*****************************************************************************/ +bool SpatioTemporalVoxelLayer::BufferEnablerCallback( \ + std_srvs::SetBool::Request& request, \ + std_srvs::SetBool::Response& response, \ + boost::shared_ptr& buffer, \ + boost::shared_ptr& subcriber) +/*****************************************************************************/ +{ + buffer->Lock(); + if (buffer->IsEnabled() != request.data) + { + buffer->SetEnabled(request.data); + if (request.data) + { + subcriber->subscribe(); + buffer->ResetLastUpdatedTime(); + response.message = "Enabling sensor"; + } + else if (subcriber) + { + subcriber->unsubscribe(); + response.message = "Disabling sensor"; + } + } + else + { + response.message = "Sensor already in the required state doing nothing"; + } + buffer->Unlock(); + response.success = true; + return response.success; +} + + /*****************************************************************************/ bool SpatioTemporalVoxelLayer::GetMarkingObservations( \ std::vector& marking_observations) const @@ -342,7 +412,7 @@ bool SpatioTemporalVoxelLayer::GetMarkingObservations( \ // get marking observations and static marked areas bool current = true; - for (unsigned int i=0; i!=_marking_buffers.size(); ++i) //1 + for (unsigned int i = 0; i != _marking_buffers.size(); ++i) { _marking_buffers[i]->Lock(); _marking_buffers[i]->GetReadings(marking_observations); @@ -362,7 +432,7 @@ bool SpatioTemporalVoxelLayer::GetClearingObservations( \ { // get clearing observations bool current = true; - for (unsigned int i = 0; i < _clearing_buffers.size(); ++i) + for (unsigned int i = 0; i != _clearing_buffers.size(); ++i) { _clearing_buffers[i]->Lock(); _clearing_buffers[i]->GetReadings(clearing_observations); @@ -372,6 +442,32 @@ bool SpatioTemporalVoxelLayer::GetClearingObservations( \ return current; } +/*****************************************************************************/ +void SpatioTemporalVoxelLayer::ObservationsResetAfterReading() const +/*****************************************************************************/ +{ + for (unsigned int i = 0; i != _clearing_buffers.size(); ++i) + { + _clearing_buffers[i]->Lock(); + if (_clearing_buffers[i]->ClearAfterReading()) + { + _clearing_buffers[i]->ResetAllMeasurements(); + } + _clearing_buffers[i]->Unlock(); + } + + for (unsigned int i = 0; i != _marking_buffers.size(); ++i) + { + _marking_buffers[i]->Lock(); + if (_marking_buffers[i]->ClearAfterReading()) + { + _marking_buffers[i]->ResetAllMeasurements(); + } + _marking_buffers[i]->Unlock(); + } + return; +} + /*****************************************************************************/ bool SpatioTemporalVoxelLayer::updateFootprint(double robot_x, double robot_y, \ double robot_yaw, double* min_x,\ @@ -434,6 +530,7 @@ void SpatioTemporalVoxelLayer::deactivate(void) void SpatioTemporalVoxelLayer::reset(void) /*****************************************************************************/ { + boost::recursive_mutex::scoped_lock lock(_voxel_grid_lock); // reset layer Costmap2D::resetMaps(); this->ResetGrid(); @@ -479,6 +576,48 @@ bool SpatioTemporalVoxelLayer::RemoveStaticObservations(void) } } +/*****************************************************************************/ +void SpatioTemporalVoxelLayer::DynamicReconfigureCallback( \ + SpatioTemporalVoxelLayerConfig& config, uint32_t level) +/*****************************************************************************/ +{ + boost::recursive_mutex::scoped_lock lock(_voxel_grid_lock); + bool update_grid(false); + auto updateFlagIfChanged = [&update_grid](auto& own, const auto& reference) + { + if (static_cast(std::abs(own - reference)) >= FLT_EPSILON) + { + own = reference; + update_grid = true; + } + }; + + auto default_value = (config.track_unknown_space) ? \ + costmap_2d::NO_INFORMATION : costmap_2d::FREE_SPACE; + updateFlagIfChanged(default_value_, default_value); + updateFlagIfChanged(_voxel_size, config.voxel_size); + updateFlagIfChanged(_voxel_decay, config.voxel_decay); + int decay_model_int = (int)_decay_model; + updateFlagIfChanged(decay_model_int, config.decay_model); + updateFlagIfChanged(_publish_voxels, config.publish_voxel_map); + + _enabled = config.enabled; + _combination_method = config.combination_method; + _mark_threshold = config.mark_threshold; + _update_footprint_enabled = config.update_footprint_enabled; + _mapping_mode = config.mapping_mode; + _map_save_duration = ros::Duration(config.map_save_duration); + _decay_model = static_cast(decay_model_int); + + if (update_grid) + { + delete _voxel_grid; + _voxel_grid = new volume_grid::SpatioTemporalVoxelGrid(_voxel_size, \ + static_cast(default_value_), _decay_model, \ + _voxel_decay, _publish_voxels); + } +} + /*****************************************************************************/ void SpatioTemporalVoxelLayer::ResetGrid(void) /*****************************************************************************/ @@ -560,6 +699,8 @@ void SpatioTemporalVoxelLayer::updateBounds( \ return; } + boost::recursive_mutex::scoped_lock lock(_voxel_grid_lock); + // Steve's Note June 22, 2018 // I dislike this necessity, I can't remove the master grid's knowledge about // STVL on the fly so I have play games with the API even though this isn't @@ -576,6 +717,7 @@ void SpatioTemporalVoxelLayer::updateBounds( \ clearing_observations; current = GetMarkingObservations(marking_observations) && current; current = GetClearingObservations(clearing_observations) && current; + ObservationsResetAfterReading(); current_ = current; // navigation mode: clear observations, mapping mode: save maps and publish @@ -625,7 +767,9 @@ bool SpatioTemporalVoxelLayer::SaveGridCallback( \ spatio_temporal_voxel_layer::SaveGrid::Response& resp) /*****************************************************************************/ { + boost::recursive_mutex::scoped_lock lock(_voxel_grid_lock); double map_size_bytes; + if( _voxel_grid->SaveGrid(req.file_name.data, map_size_bytes) ) { ROS_INFO( \ @@ -635,6 +779,7 @@ bool SpatioTemporalVoxelLayer::SaveGridCallback( \ resp.status = true; return true; } + ROS_WARN("SpatioTemporalVoxelGrid: Failed to save grid."); resp.status = false; return false;