From ba1bd6870481f22cd26951c8bd49c3979279d0ba Mon Sep 17 00:00:00 2001 From: Steven Macenski Date: Fri, 13 Dec 2019 16:03:35 -0800 Subject: [PATCH 1/4] tested working in eloquent (#151) --- CMakeLists.txt | 11 +++++- ...constrained_indoor_environment_config.yaml | 4 +- example/outdoor_environment_config.yaml | 4 +- .../standard_indoor_environment_config.yaml | 4 +- example/vlp16_config.yaml | 4 +- src/spatio_temporal_voxel_layer.cpp | 37 +++++++++++-------- 6 files changed, 38 insertions(+), 26 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 1ae8f918..77873107 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -87,7 +87,7 @@ include_directories( ${TBB_INCLUDE_DIRS} ) -add_library(${library_name} +add_library(${library_name} SHARED src/spatio_temporal_voxel_layer.cpp src/spatio_temporal_voxel_grid.cpp src/measurement_buffer.cpp @@ -97,6 +97,7 @@ add_library(${library_name} ) rosidl_target_interfaces(${library_name} ${PROJECT_NAME} "rosidl_typesupport_cpp") +target_compile_definitions(${library_name} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") target_link_libraries(${library_name} ${Boost_LIBRARIES} @@ -114,13 +115,19 @@ if(BUILD_TESTING) endif() install(TARGETS ${library_name} - DESTINATION share + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} ) install(DIRECTORY example DESTINATION share ) +install(FILES costmap_plugins.xml + DESTINATION share +) + ament_export_dependencies(rosidl_default_runtime) pluginlib_export_plugin_description_file(nav2_costmap_2d costmap_plugins.xml) ament_export_include_directories(include) diff --git a/example/constrained_indoor_environment_config.yaml b/example/constrained_indoor_environment_config.yaml index 53503394..1bfd39dc 100644 --- a/example/constrained_indoor_environment_config.yaml +++ b/example/constrained_indoor_environment_config.yaml @@ -3,7 +3,7 @@ global_costmap: ros__parameters: rgbd_obstacle_layer: enabled: true - voxel_decay: 15 # seconds if linear, e^n if exponential + voxel_decay: 15.0 # 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 @@ -17,7 +17,7 @@ global_costmap: 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 + map_save_duration: 60.0 # default 60s, how often to autosave observation_sources: rgbd1_mark rgbd1_clear rgbd1_mark: data_type: PointCloud2 diff --git a/example/outdoor_environment_config.yaml b/example/outdoor_environment_config.yaml index 3afb22c0..9268b1b2 100644 --- a/example/outdoor_environment_config.yaml +++ b/example/outdoor_environment_config.yaml @@ -3,7 +3,7 @@ global_costmap: ros__parameters: rgbd_obstacle_layer: enabled: true - voxel_decay: 15 # seconds if linear, e^n if exponential + voxel_decay: 15.0 # 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 @@ -17,7 +17,7 @@ global_costmap: 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 + map_save_duration: 60.0 # default 60s, how often to autosave observation_sources: rgbd1_mark rgbd1_clear rgbd1_mark: data_type: PointCloud2 diff --git a/example/standard_indoor_environment_config.yaml b/example/standard_indoor_environment_config.yaml index 626c4fa4..96731d9b 100644 --- a/example/standard_indoor_environment_config.yaml +++ b/example/standard_indoor_environment_config.yaml @@ -3,7 +3,7 @@ global_costmap: ros__parameters: rgbd_obstacle_layer: enabled: true - voxel_decay: 15 # seconds if linear, e^n if exponential + voxel_decay: 15.0 # 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 @@ -17,7 +17,7 @@ global_costmap: 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 + map_save_duration: 60.0 # default 60s, how often to autosave observation_sources: rgbd1_mark rgbd1_clear rgbd1_mark: data_type: PointCloud2 diff --git a/example/vlp16_config.yaml b/example/vlp16_config.yaml index c62386f0..c053848e 100644 --- a/example/vlp16_config.yaml +++ b/example/vlp16_config.yaml @@ -1,6 +1,6 @@ rgbd_obstacle_layer: enabled: true - voxel_decay: 15 # seconds if linear, e^n if exponential + voxel_decay: 15.0 # 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 @@ -14,7 +14,7 @@ rgbd_obstacle_layer: 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 + map_save_duration: 60.0 # default 60s, how often to autosave observation_sources: rgbd1_mark rgbd1_clear rgbd1_mark: data_type: PointCloud2 diff --git a/src/spatio_temporal_voxel_layer.cpp b/src/spatio_temporal_voxel_layer.cpp index 8b7b0569..52edd869 100644 --- a/src/spatio_temporal_voxel_layer.cpp +++ b/src/spatio_temporal_voxel_layer.cpp @@ -118,21 +118,22 @@ void SpatioTemporalVoxelLayer::onInitialize(void) if (_mapping_mode) { _map_save_duration = std::make_unique( map_save_time, 0.0); - _last_map_save_time = node_->now() - *_map_save_duration; } + _last_map_save_time = node_->now(); + if (track_unknown_space) { default_value_ = nav2_costmap_2d::NO_INFORMATION; } else { default_value_ = nav2_costmap_2d::FREE_SPACE; } - _voxel_pub = node_->create_publisher( + _voxel_pub = rclcpp_node_->create_publisher( "voxel_grid", rclcpp::QoS(1)); auto save_grid_callback = std::bind( &SpatioTemporalVoxelLayer::SaveGridCallback, this, _1, _2, _3); - _grid_saver = node_->create_service( + _grid_saver = rclcpp_node_->create_service( "save_grid", save_grid_callback); _voxel_grid = std::make_unique( @@ -282,19 +283,20 @@ void SpatioTemporalVoxelLayer::onInitialize(void) _observation_notifiers.push_back(filter); } - std::function, - const std::shared_ptr, - std::shared_ptr)> toggle_srv_callback; + // TODO(stevemacenski) + // std::function, + // const std::shared_ptr, + // std::shared_ptr)> 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 = node_->create_service( - 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( + // toggle_topic, toggle_srv_callback); - _buffer_enabler_servers.push_back(server); + // _buffer_enabler_servers.push_back(server); if (sensor_frame != "") { std::vector target_frames; @@ -666,10 +668,13 @@ void SpatioTemporalVoxelLayer::updateBounds( current_ = current; // navigation mode: clear observations, mapping mode: save maps and publish - bool should_save = node_->now() - _last_map_save_time > *_map_save_duration; + bool should_save = false; + if (_map_save_duration) { + should_save = node_->now() - _last_map_save_time > *_map_save_duration; + } if (!_mapping_mode) { _voxel_grid->ClearFrustums(clearing_observations); - } else if (_map_save_duration && should_save) { + } else if (should_save) { _last_map_save_time = node_->now(); time_t rawtime; struct tm * timeinfo; From 3bdf9f476c1e77d084311c352dabff383798f3f9 Mon Sep 17 00:00:00 2001 From: Steven Macenski Date: Mon, 16 Dec 2019 13:08:22 -0800 Subject: [PATCH 2/4] allowing for subscriber toggle callback (#153) --- CMakeLists.txt | 2 ++ package.xml | 1 + src/spatio_temporal_voxel_layer.cpp | 25 ++++++++++++------------- 3 files changed, 15 insertions(+), 13 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 77873107..268e426b 100644 --- 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 index 5338c444..1ddc2bed 100644 --- a/package.xml +++ b/package.xml @@ -21,6 +21,7 @@ pluginlib sensor_msgs std_msgs + std_srvs laser_geometry pcl pcl_conversions diff --git a/src/spatio_temporal_voxel_layer.cpp b/src/spatio_temporal_voxel_layer.cpp index 52edd869..e2f1a473 100644 --- 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, - // const std::shared_ptr, - // std::shared_ptr)> toggle_srv_callback; + std::function, + 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( - // 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( + toggle_topic, toggle_srv_callback); - // _buffer_enabler_servers.push_back(server); + _buffer_enabler_servers.push_back(server); if (sensor_frame != "") { std::vector 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); From 95bb9f29fc6c22888cf4d76384726cbca62f3487 Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Mon, 16 Dec 2019 13:11:25 -0800 Subject: [PATCH 3/4] 2.0.0 to 2.1.0 --- package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/package.xml b/package.xml index 1ddc2bed..2ae36bf9 100644 --- a/package.xml +++ b/package.xml @@ -2,7 +2,7 @@ spatio_temporal_voxel_layer - 2.0.0 + 2.1.0 The spatio-temporal 3D obstacle costmap package Steve Macenski From 290619da3694bb5beb7b008f237fafe1c6426ad7 Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Mon, 16 Dec 2019 13:14:41 -0800 Subject: [PATCH 4/4] eloquent fix rosdep keys for pcl --- package.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/package.xml b/package.xml index 2ae36bf9..91df713c 100644 --- a/package.xml +++ b/package.xml @@ -2,7 +2,7 @@ spatio_temporal_voxel_layer - 2.1.0 + 2.1.1 The spatio-temporal 3D obstacle costmap package Steve Macenski @@ -23,7 +23,7 @@ std_msgs std_srvs laser_geometry - pcl + libpcl-all pcl_conversions rclcpp message_filters