-
Notifications
You must be signed in to change notification settings - Fork 189
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Cherry pick Kinetic updates to the new stable melodic branch (#123)
* Patch persistent voxel decay model returning the wrong duration * Fix typo and implement acceleration in persistent decay * implementing abstract class for configurable frustum models (#93) * implementing abstract class for configurable frustum models * stoping iterator deletion from screwing up the cache * deleting costmap pointer only if created * fixing no clearing observations but allowing for global decay (#97) * fixing no clearing observations but allowing for global decay * saving a little bit of allocation * removing removed space * removing instructions tripping up people * deleting remaining comment on raw install * Vlp16 (#104) * Add support for 3D scanning lidars (Velodyne RoboSense, Ouster, etc) * various formatting cleanup (#105) * Dynamic reconfigure proposal continuing (#98) * Added primitive dynamic reconfigure configuration * Added naive callback for dynamic reconfigure * Using positive values in voxel_decay due to dynamic reconfigure limitations * using only positive values in voxel decay * Using raw poiters to follow project's convenction * We use c++17 so we have templated lambdas! * Added publishing option to dynamic reconfigure * adresing comments * added enabled param for each observation buffer (wip) - added a service to enable or disable each observation buffer - c++14 istead of 17 kinetic compatible * wip trying to make boostbind work with services * ready to test, resolved requested changes * commented reconfigure * test with no pointers * added reconfigure again * fixed compilation errors due to the merge * added service feedback * Update README.md * small changes * add a fix for not clearing local map with deactivated device * Added params to reconfigure - addressed all requested changes - fix to CMakelist closes #99 * solved bug, decay model was not beig updated in reconfigure * removed layer reset when changing camera * new param to disable services if the user doesn't want realtime disabling * cleaned some more * changed param name * Revert "changed param name" This reverts commit c195872. * Revert "cleaned some more" This reverts commit 7fe6292. * Revert "new param to disable services if the user doesn't want realtime disabling" This reverts commit 56df2a7. * adressed comments - Added clearer service names. - Added mutex lock unlock in reconfigure * changed reconfigure mutex from grid to layer * added layer_grid locks for ensuring safe interaction * still playing with mutex -trying to solve blocking errors * renamed _layer_lock to _voxel_grid_lock * advertise publish boxels always * Cleanup 98 (#106) * various cleanup from 98 * replacing locking mutexes with scoped locks * use recursive lock * Allow for 1 stream to be in 1 source vs. 1 stream in 2 sources (#109) * clearing after reading both marking and clearing * remove unnecessary comment * clear all measurements clear * Vfov padding (#113) * Update spatio_temporal_voxel_layer.cpp * Update spatio_temporal_voxel_layer.cpp * Update three_dimensional_lidar_frustum.hpp * Update three_dimensional_lidar_frustum.cpp * Update measurement_buffer.hpp * Update measurement_buffer.cpp * Update measurement_reading.h * Update spatio_temporal_voxel_grid.cpp * added missing argument * Update vlp16_config.yaml * Update three_dimensional_lidar_frustum.hpp * Update three_dimensional_lidar_frustum.hpp * Update spatio_temporal_voxel_layer.cpp * Update README.md * Update README.md * Update measurement_buffer.hpp * Update measurement_reading.h * Update measurement_buffer.cpp * adding list of environments tested in (#116) * increment up * adding vlp16 gif to readme (#119) * adding vlp16 gif to readme * oops, wrong URL * try on one line * well that didnt work... * boldface * resolve cherry pick from kinetic misses * increment up for release
- Loading branch information
1 parent
1949041
commit 3eaa79b
Showing
20 changed files
with
845 additions
and
192 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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")) |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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 |
90 changes: 90 additions & 0 deletions
90
include/spatio_temporal_voxel_layer/frustum_models/depth_camera_frustum.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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 <spatio_temporal_voxel_layer/frustum_models/frustum.hpp> | ||
|
|
||
| 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<VectorWithPt3D> _plane_normals; | ||
| Eigen::Vector3d _position; | ||
| Eigen::Quaterniond _orientation; | ||
| bool _valid_frustum; | ||
|
|
||
| #if VISUALIZE_FRUSTUM | ||
| std::vector<Eigen::Vector3d> _frustum_pts; | ||
| ros::Publisher _frustumPub; | ||
| #endif | ||
| }; | ||
|
|
||
| } // end namespace | ||
|
|
||
| #endif |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.