Skip to content

Commit

Permalink
Dashing tested working in ROS2 (#150)
Browse files Browse the repository at this point in the history 
* fixing typecast errors

* temporarily disable toggle callbacks in dashing
  • Loading branch information
@SteveMacenski
SteveMacenski authored Dec 14, 2019
1 parent aaac01b commit c48d698
Show file tree
Hide file tree
Showing 6 changed files with 38 additions and 26 deletions.
11 changes: 9 additions & 2 deletions CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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}
Expand All @@ -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)
Expand Down
4 changes: 2 additions & 2 deletions example/constrained_indoor_environment_config.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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
Expand Down
4 changes: 2 additions & 2 deletions example/outdoor_environment_config.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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
Expand Down
4 changes: 2 additions & 2 deletions example/standard_indoor_environment_config.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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
Expand Down
4 changes: 2 additions & 2 deletions example/vlp16_config.yaml
View file
Original file line number Diff line number Diff line change
@@ -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
Expand All @@ -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
Expand Down
37 changes: 21 additions & 16 deletions src/spatio_temporal_voxel_layer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -118,21 +118,22 @@ void SpatioTemporalVoxelLayer::onInitialize(void)
if (_mapping_mode) {
_map_save_duration = std::make_unique<rclcpp::Duration>(
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<sensor_msgs::msg::PointCloud2>(
_voxel_pub = rclcpp_node_->create_publisher<sensor_msgs::msg::PointCloud2>(
"voxel_grid", rclcpp::QoS(1));

auto save_grid_callback = std::bind(
&SpatioTemporalVoxelLayer::SaveGridCallback, this, _1, _2, _3);
_grid_saver = node_->create_service<spatio_temporal_voxel_layer::srv::SaveGrid>(
_grid_saver = rclcpp_node_->create_service<spatio_temporal_voxel_layer::srv::SaveGrid>(
"save_grid", save_grid_callback);

_voxel_grid = std::make_unique<volume_grid::SpatioTemporalVoxelGrid>(
Expand Down Expand Up @@ -282,19 +283,20 @@ void SpatioTemporalVoxelLayer::onInitialize(void)
_observation_notifiers.push_back(filter);
}

std::function<void(const std::shared_ptr<rmw_request_id_t>,
const std::shared_ptr<std_srvs::srv::SetBool::Request>,
std::shared_ptr<std_srvs::srv::SetBool::Response>)> toggle_srv_callback;
// TODO(stevemacenski)
// std::function<void(const std::shared_ptr<rmw_request_id_t>,
// const std::shared_ptr<std_srvs::srv::SetBool::Request>,
// std::shared_ptr<std_srvs::srv::SetBool::Response>)> 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<std_srvs::srv::SetBool>(
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<std_srvs::srv::SetBool>(
// toggle_topic, toggle_srv_callback);

_buffer_enabler_servers.push_back(server);
// _buffer_enabler_servers.push_back(server);

if (sensor_frame != "") {
std::vector<std::string> target_frames;
Expand Down Expand Up @@ -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;
Expand Down

0 comments on commit c48d698

Please sign in to comment.