Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Map editing tools and improved local mapping #92

Draft
wants to merge 43 commits into
base: main
Choose a base branch
from
Draft
Changes from 1 commit
Commits
Show all changes
43 commits
Select commit Hold shift + click to select a range
53778a9
Add method to crop map at voxel resolution and update crop_map_operation
victorreijgwart Nov 12, 2024
c6759c6
Add option to use most recent available ROS TF, ignoring timestamps
victorreijgwart Nov 13, 2024
3bbd1b8
Update config validation schemas for CropMapOperation
victorreijgwart Nov 13, 2024
097b6dc
Extend automatic unit conversions with additional units
victorreijgwart Nov 13, 2024
6fb3d81
Implement map decay operation for temporally local mapping
victorreijgwart Nov 29, 2024
f46ea6c
Add option to multi-thread crop_map and decay_map operations
victorreijgwart Nov 29, 2024
7bd403b
Minor refactoring for consistency
victorreijgwart Dec 7, 2024
8e1e10a
Draft method to resample map into another coordinate frame
victorreijgwart Dec 7, 2024
f3034d7
Prune the blocks after dense sampling to restore sparsity
victorreijgwart Dec 7, 2024
dd42058
Make sure all config getters are marked const
victorreijgwart Dec 7, 2024
e87d0e5
Improve map transform method
victorreijgwart Dec 7, 2024
e6afc67
Add occupancy map to PLY conversion script
victorreijgwart Dec 11, 2024
435e45f
Expose method to convert node indices to their center point in Python
victorreijgwart Dec 11, 2024
b656e0a
Add Python methods to get all occupied leaf nodes or cell center points
victorreijgwart Dec 11, 2024
fff8b77
Build the C++ Library in Release mode by default
victorreijgwart Dec 11, 2024
1092dc2
Merge branch 'feature/map_conversions' into feature/improve_local_map…
victorreijgwart Dec 12, 2024
7bf1525
Merge branch 'feature/default_build_release' into feature/improve_loc…
victorreijgwart Dec 12, 2024
21c91e2
Preselect last loaded map in file dialog to enable easy reloads in Rviz
victorreijgwart Dec 12, 2024
fa8a4eb
Make sure cell sizes are updated when map min cell width changes
victorreijgwart Dec 12, 2024
f37f7f9
Update local test script to include C++ examples and Python API
victorreijgwart Dec 6, 2024
a815bc0
Add map edit module to Python API and expose crop_to_sphere methods
victorreijgwart Dec 12, 2024
43003a4
Expose map multiplication method and add exponential forgetting example
victorreijgwart Dec 12, 2024
ba9ae88
Expose map transformation method
victorreijgwart Dec 12, 2024
c3b5f4b
Generalize sampling method to also allow summing, not only overwriting
victorreijgwart Dec 13, 2024
565864c
Separate definitions from declarations for readability
victorreijgwart Dec 13, 2024
7392f41
Add method to sum (merge) maps together
victorreijgwart Dec 13, 2024
a908112
Add map editing examples for C++ API
victorreijgwart Dec 13, 2024
c72e9ae
Reduce code duplication by using sampler to implement transform
victorreijgwart Dec 13, 2024
10e80fb
Explicitly use default copy and move constructors for QueryAccelerators
victorreijgwart Dec 14, 2024
1728931
Refactor and move AABB to utils/geometry to accommodate new colliders
victorreijgwart Dec 15, 2024
d0db0de
Make C++ examples more consistent
victorreijgwart Dec 15, 2024
1c90971
Minor refactoring
victorreijgwart Dec 15, 2024
759669b
Add method to sum values to all cells within a given shape
victorreijgwart Dec 15, 2024
68bf7a9
Expose methods to sum shapes into map through Python API
victorreijgwart Dec 16, 2024
4d6d707
Generalize map crop method to other shapes
victorreijgwart Dec 18, 2024
e1384ba
Improve intersection tests
victorreijgwart Dec 18, 2024
e0700f1
Expose new crop functions in Python
victorreijgwart Dec 18, 2024
3d81e74
Update Python examples
victorreijgwart Dec 18, 2024
c19f5bb
Speed up masked sums using multi-resolution
victorreijgwart Dec 18, 2024
345f198
Update crop method argument names to be consistent with sum
victorreijgwart Dec 18, 2024
8f1f01b
Add Python example on generating random obstacle maps
victorreijgwart Dec 18, 2024
28803ee
Add Rviz voxel filter option that includes occupied|unknown surfaces
victorreijgwart Dec 30, 2024
12b4728
Update random map generation script
victorreijgwart Dec 30, 2024
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
Add option to use most recent available ROS TF, ignoring timestamps
victorreijgwart committed Nov 28, 2024
commit c6759c6ae942c6b427c1c932974739a4ddaefd59
Original file line number Diff line number Diff line change
@@ -25,15 +25,18 @@ struct CropMapOperationConfig : public ConfigBase<CropMapOperationConfig, 5> {
//! `radius` from this point are deleted.
std::string body_frame = "body";

// TODO(victorr): Explain
Seconds<FloatingPoint> tf_delay = 0.0f;
//! Time offset applied when retrieving the transform from body_frame to
//! world_frame. Set to -1 to use the most recent transform available in ROS
//! TF, ignoring timestamps (default). If set to a non-negative value, the
//! transform lookup uses a timestamp of `ros::Time::now() - tf_time_offset`.
Seconds<FloatingPoint> tf_time_offset = -1.f;

//! Distance beyond which to remove nodes from the map.
//! Distance beyond which to remove cells from the map.
Meters<FloatingPoint> radius;

//! Maximum resolution at which to crop the map. Defaults to the maximum
//! resolution. Can be reduced to save computation time, at the cost of more
//! jagged edges.
//! Maximum resolution at which the crop is applied. Set to 0 to match the
//! map's maximum resolution (default). Setting a higher value reduces
//! computation but produces more jagged borders.
Meters<FloatingPoint> max_update_resolution = 0.f;

static MemberMap memberMap;
Original file line number Diff line number Diff line change
@@ -13,7 +13,7 @@ namespace wavemap {
DECLARE_CONFIG_MEMBERS(CropMapOperationConfig,
(once_every)
(body_frame)
(tf_delay)
(tf_time_offset)
(radius)
(max_update_resolution));

@@ -22,7 +22,10 @@ bool CropMapOperationConfig::isValid(bool verbose) const {

all_valid &= IS_PARAM_GT(once_every, 0.f, verbose);
all_valid &= IS_PARAM_NE(body_frame, "", verbose);
all_valid &=
IS_PARAM_TRUE(tf_time_offset == -1.f || 0.f <= tf_time_offset, verbose);
all_valid &= IS_PARAM_GT(radius, 0.f, verbose);
all_valid &= IS_PARAM_GE(max_update_resolution, 0.f, verbose);

return all_valid;
}
@@ -52,15 +55,27 @@ void CropMapOperation::run(bool force_run) {
return;
}

const ros::Time timestamp = current_time - ros::Duration(config_.tf_delay);
const bool use_most_recent_transform = config_.tf_time_offset < 0.f;
const ros::Time timestamp =
use_most_recent_transform
? ros::Time::UNINITIALIZED
: current_time - ros::Duration(config_.tf_time_offset);
const auto T_W_B = transformer_->lookupTransform(
world_frame_, config_.body_frame, timestamp);
if (!T_W_B) {
ROS_WARN_STREAM(
"Could not look up center point for map cropping. TF lookup of "
"body_frame \""
<< config_.body_frame << "\" w.r.t. world_frame \"" << world_frame_
<< "\" at time " << timestamp << " failed.");
if (use_most_recent_transform) {
ROS_WARN_STREAM(
"Could not look up center point for map cropping. No TF from "
"body_frame \""
<< config_.body_frame << "\" to world_frame \"" << world_frame_
<< "\".");
} else {
ROS_WARN_STREAM(
"Could not look up center point for map cropping. TF lookup from "
"body_frame \""
<< config_.body_frame << "\" to world_frame \"" << world_frame_
<< "\" at time " << timestamp << " failed.");
}
return;
}

3 changes: 2 additions & 1 deletion interfaces/ros1/wavemap_ros/src/utils/tf_transformer.cc
Original file line number Diff line number Diff line change
@@ -23,7 +23,8 @@ bool TfTransformer::waitForTransform(const std::string& to_frame_id,
std::optional<Transformation3D> TfTransformer::lookupLatestTransform(
const std::string& to_frame_id, const std::string& from_frame_id) {
return lookupTransformImpl(sanitizeFrameId(to_frame_id),
sanitizeFrameId(from_frame_id), {});
sanitizeFrameId(from_frame_id),
ros::Time::UNINITIALIZED);
}

std::optional<Transformation3D> TfTransformer::lookupTransform(