Skip to content

Commit

Permalink
Merge branch 'calderpg:master' into master
Browse files Browse the repository at this point in the history
  • Loading branch information
@calderpg-tri
calderpg-tri authored Jun 9, 2022
2 parents 052ef48 + 5d5b822 commit 433d858
Show file tree
Hide file tree
Showing 22 changed files with 275 additions and 313 deletions.
7 changes: 5 additions & 2 deletions CMakeLists.txt.ros1
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,9 @@ include_directories(include SYSTEM ${catkin_INCLUDE_DIRS}
${Eigen3_INCLUDE_DIRS})

## Build options
set(CMAKE_INTERPROCEDURAL_OPTIMIZATION ON)
cmake_policy(SET CMP0069 NEW)

add_compile_options(-std=c++11)
add_compile_options(-Wall)
add_compile_options(-Wextra)
Expand All @@ -50,10 +53,10 @@ add_compile_options(-Wconversion)
add_compile_options(-Wshadow)
add_compile_options(-O3)
add_compile_options(-g)
add_compile_options(-flto)
add_compile_options(-Werror=non-virtual-dtor)
add_compile_options(-Wold-style-cast)
add_compile_options(-Wmaybe-uninitialized)
add_compile_options(-Wpessimizing-move)
add_compile_options(-Wuninitialized)

if(drake_FOUND)
message(STATUS "Drake found, disabling -march=native")
Expand Down
7 changes: 5 additions & 2 deletions CMakeLists.txt.ros2
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,9 @@ find_package(drake QUIET)
include_directories(include SYSTEM ${Eigen3_INCLUDE_DIRS} ${ZLIB_INCLUDE_DIRS})

## Build options
set(CMAKE_INTERPROCEDURAL_OPTIMIZATION ON)
cmake_policy(SET CMP0069 NEW)

add_compile_options(-std=c++14)
add_compile_options(-Wall)
add_compile_options(-Wextra)
Expand All @@ -32,10 +35,10 @@ add_compile_options(-Wconversion)
add_compile_options(-Wshadow)
add_compile_options(-O3)
add_compile_options(-g)
add_compile_options(-flto)
add_compile_options(-Werror=non-virtual-dtor)
add_compile_options(-Wold-style-cast)
add_compile_options(-Wmaybe-uninitialized)
add_compile_options(-Wpessimizing-move)
add_compile_options(-Wuninitialized)

if(drake_FOUND)
message(STATUS "Drake found, disabling -march=native")
Expand Down
7 changes: 4 additions & 3 deletions include/common_robotics_utilities/dynamic_spatial_hashed_voxel_grid.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -215,7 +215,7 @@ class DynamicSpatialHashedVoxelGridChunk
{
// Note: do not refactor to use .at(), since not all vector-like
// implementations implement it (ex thrust::host_vector<T>).
return data_[data_index];
return data_[static_cast<typename BackingStore::size_type>(data_index)];
}
else
{
Expand All @@ -229,7 +229,7 @@ class DynamicSpatialHashedVoxelGridChunk
{
// Note: do not refactor to use .at(), since not all vector-like
// implementations implement it (ex thrust::host_vector<T>).
return data_[data_index];
return data_[static_cast<typename BackingStore::size_type>(data_index)];
}
else
{
Expand All @@ -240,7 +240,8 @@ class DynamicSpatialHashedVoxelGridChunk
void SetCellFilledContents(const T& value)
{
data_.clear();
data_.resize(sizes_.TotalCells(), value);
data_.resize(static_cast<typename BackingStore::size_type>(
sizes_.TotalCells()), value);
fill_status_ = DSHVGFillStatus::CELL_FILLED;
}

Expand Down
90 changes: 26 additions & 64 deletions include/common_robotics_utilities/math.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -467,13 +467,15 @@ GetArbitraryOrthogonalVectorToPlane(
}

template<typename DataType, typename Container=std::vector<DataType>>
Eigen::MatrixXd BuildPairwiseDistanceMatrixParallel(
Eigen::MatrixXd BuildPairwiseDistanceMatrix(
const Container& data,
const std::function<double(const DataType&, const DataType&)>& distance_fn)
const std::function<double(const DataType&, const DataType&)>& distance_fn,
const bool use_parallel = false)
{
Eigen::MatrixXd distance_matrix(data.size(), data.size());

#if defined(_OPENMP)
#pragma omp parallel for
#pragma omp parallel for if (use_parallel)
#endif
for (size_t idx = 0; idx < data.size(); idx++)
{
Expand All @@ -500,62 +502,36 @@ Eigen::MatrixXd BuildPairwiseDistanceMatrixParallel(
}

template<typename DataType, typename Container=std::vector<DataType>>
Eigen::MatrixXd BuildPairwiseDistanceMatrixSerial(
Eigen::MatrixXd BuildPairwiseDistanceMatrixParallel(
const Container& data,
const std::function<double(const DataType&, const DataType&)>& distance_fn)
{
Eigen::MatrixXd distance_matrix(data.size(), data.size());
for (size_t idx = 0; idx < data.size(); idx++)
{
for (size_t jdx = idx; jdx < data.size(); jdx++)
{
if (idx != jdx)
{
const double distance = distance_fn(data[idx], data[jdx]);
distance_matrix(static_cast<ssize_t>(idx), static_cast<ssize_t>(jdx))
= distance;
distance_matrix(static_cast<ssize_t>(jdx), static_cast<ssize_t>(idx))
= distance;
}
else
{
distance_matrix(static_cast<ssize_t>(idx), static_cast<ssize_t>(jdx))
= 0.0;
distance_matrix(static_cast<ssize_t>(jdx), static_cast<ssize_t>(idx))
= 0.0;
}
}
}
return distance_matrix;
return BuildPairwiseDistanceMatrix<DataType, Container>(
data, distance_fn, true);
}

template<typename DataType, typename Container=std::vector<DataType>>
Eigen::MatrixXd BuildPairwiseDistanceMatrix(
Eigen::MatrixXd BuildPairwiseDistanceMatrixSerial(
const Container& data,
const std::function<double(const DataType&, const DataType&)>& distance_fn,
const bool use_parallel = false)
const std::function<double(const DataType&, const DataType&)>& distance_fn)
{
if (use_parallel)
{
return BuildPairwiseDistanceMatrixParallel(data, distance_fn);
}
else
{
return BuildPairwiseDistanceMatrixSerial(data, distance_fn);
}
return BuildPairwiseDistanceMatrix<DataType, Container>(
data, distance_fn, false);
}

template<typename FirstDataType, typename SecondDataType,
typename FirstContainer=std::vector<FirstDataType>,
typename SecondContainer=std::vector<SecondDataType>>
Eigen::MatrixXd BuildPairwiseDistanceMatrixParallel(
Eigen::MatrixXd BuildPairwiseDistanceMatrix(
const FirstContainer& data1, const SecondContainer& data2,
const std::function<double(const FirstDataType&,
const SecondDataType&)>& distance_fn)
const SecondDataType&)>& distance_fn,
const bool use_parallel = false)
{
Eigen::MatrixXd distance_matrix(data1.size(), data2.size());

#if defined(_OPENMP)
#pragma omp parallel for
#pragma omp parallel for if (use_parallel)
#endif
for (size_t idx = 0; idx < data1.size(); idx++)
{
Expand All @@ -572,41 +548,27 @@ Eigen::MatrixXd BuildPairwiseDistanceMatrixParallel(
template<typename FirstDataType, typename SecondDataType,
typename FirstContainer=std::vector<FirstDataType>,
typename SecondContainer=std::vector<SecondDataType>>
Eigen::MatrixXd BuildPairwiseDistanceMatrixSerial(
Eigen::MatrixXd BuildPairwiseDistanceMatrixParallel(
const FirstContainer& data1, const SecondContainer& data2,
const std::function<double(const FirstDataType&,
const SecondDataType&)>& distance_fn)
{
Eigen::MatrixXd distance_matrix(data1.size(), data2.size());
for (size_t idx = 0; idx < data1.size(); idx++)
{
for (size_t jdx = 0; jdx < data2.size(); jdx++)
{
const double distance = distance_fn(data1[idx], data2[jdx]);
distance_matrix(static_cast<ssize_t>(idx), static_cast<ssize_t>(jdx))
= distance;
}
}
return distance_matrix;
return BuildPairwiseDistanceMatrixParallel
<FirstDataType, SecondDataType, FirstContainer, SecondContainer>(
data1, data2, distance_fn, true);
}

template<typename FirstDataType, typename SecondDataType,
typename FirstContainer=std::vector<FirstDataType>,
typename SecondContainer=std::vector<SecondDataType>>
Eigen::MatrixXd BuildPairwiseDistanceMatrix(
Eigen::MatrixXd BuildPairwiseDistanceMatrixSerial(
const FirstContainer& data1, const SecondContainer& data2,
const std::function<double(const FirstDataType&,
const SecondDataType&)>& distance_fn,
const bool use_parallel = false)
const SecondDataType&)>& distance_fn)
{
if (use_parallel)
{
return BuildPairwiseDistanceMatrixParallel(data1, data2, distance_fn);
}
else
{
return BuildPairwiseDistanceMatrixSerial(data1, data2, distance_fn);
}
return BuildPairwiseDistanceMatrixParallel
<FirstDataType, SecondDataType, FirstContainer, SecondContainer>(
data1, data2, distance_fn, false);
}

class Hyperplane
Expand Down
14 changes: 7 additions & 7 deletions include/common_robotics_utilities/maybe.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,7 @@ template<typename A, typename B>
struct TaggedUnion
{
private:
void ConstructFrom(const TaggedUnion<A, B>& other)
void ConstructCopy(const TaggedUnion<A, B>& other)
{
switch (other.type_)
{
Expand All @@ -34,7 +34,7 @@ struct TaggedUnion
}
}

void ConstructFrom(TaggedUnion<A, B>&& other)
void ConstructMove(TaggedUnion<A, B>&& other)
{
switch (other.type_)
{
Expand Down Expand Up @@ -77,7 +77,7 @@ struct TaggedUnion
B value_b_;
};

enum {TYPE_A, TYPE_B} type_;
enum {TYPE_A, TYPE_B} type_{};

explicit TaggedUnion(const A& value_a)
: value_a_(value_a), type_(TYPE_A) {}
Expand All @@ -91,9 +91,9 @@ struct TaggedUnion
explicit TaggedUnion(B&& value_b)
: value_b_(std::move(value_b)), type_(TYPE_B) {}

TaggedUnion(const TaggedUnion<A, B>& other) { ConstructFrom(other); }
TaggedUnion(const TaggedUnion<A, B>& other) { ConstructCopy(other); }

TaggedUnion(TaggedUnion<A, B>&& other) { ConstructFrom(std::move(other)); }
TaggedUnion(TaggedUnion<A, B>&& other) { ConstructMove(std::move(other)); }

~TaggedUnion() { Cleanup(); }

Expand All @@ -103,7 +103,7 @@ struct TaggedUnion
{
Cleanup();

ConstructFrom(other);
ConstructCopy(other);
}
return *this;
}
Expand All @@ -114,7 +114,7 @@ struct TaggedUnion
{
Cleanup();

ConstructFrom(std::move(other));
ConstructMove(std::move(other));
}
return *this;
}
Expand Down
56 changes: 29 additions & 27 deletions include/common_robotics_utilities/path_processing.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -146,37 +146,39 @@ inline Container AttemptShortcut(
Container shortcut;
if (first_half_shortcut.size() > 0 && second_half_shortcut.size() > 0)
{
shortcut.insert(shortcut.end(),
first_half_shortcut.begin(),
first_half_shortcut.end());
shortcut.insert(
shortcut.end(), first_half_shortcut.begin(),
first_half_shortcut.end());
// Skip the first configuration, since this is a duplicate of the last
// configuration in the first half shortcut
shortcut.insert(shortcut.end(),
second_half_shortcut.begin() + 1,
second_half_shortcut.end());
shortcut.insert(
shortcut.end(), second_half_shortcut.begin() + 1,
second_half_shortcut.end());
}
else if (first_half_shortcut.size() > 0)
{
shortcut.insert(shortcut.end(),
first_half_shortcut.begin(),
first_half_shortcut.end());
shortcut.insert(
shortcut.end(), first_half_shortcut.begin(),
first_half_shortcut.end());
// Skip the middle configuration, since this is a duplicate of the
// last configuration in the first half shortcut, but include the end
// index
shortcut.insert(shortcut.end(),
current_path.begin() + middle_index + 1,
current_path.begin() + end_index + 1);
shortcut.insert(
shortcut.end(),
current_path.begin() + static_cast<ptrdiff_t>(middle_index) + 1,
current_path.begin() + static_cast<ptrdiff_t>(end_index) + 1);
}
else if (second_half_shortcut.size() > 0)
{
// Skip the middle configuration, since this is a duplicate of the
// first configuration in the second half shortcut
shortcut.insert(shortcut.end(),
current_path.begin() + start_index,
current_path.begin() + middle_index);
shortcut.insert(shortcut.end(),
second_half_shortcut.begin(),
second_half_shortcut.end());
shortcut.insert(
shortcut.end(),
current_path.begin() + static_cast<ptrdiff_t>(start_index),
current_path.begin() + static_cast<ptrdiff_t>(middle_index));
shortcut.insert(
shortcut.end(), second_half_shortcut.begin(),
second_half_shortcut.end());
}
return shortcut;
}
Expand Down Expand Up @@ -251,20 +253,20 @@ inline Container ShortcutSmoothPath(
if (start_index > 0)
{
// Copy the path before the shortcut (excluding start_index)
shortened_path.insert(shortened_path.end(),
current_path.begin(),
current_path.begin() + start_index);
shortened_path.insert(
shortened_path.end(), current_path.begin(),
current_path.begin() + static_cast<ptrdiff_t>(start_index));
}
// Copy the shortcut
shortened_path.insert(shortened_path.end(),
shortcut.begin(),
shortcut.end());
shortened_path.insert(
shortened_path.end(), shortcut.begin(), shortcut.end());
if (end_index < current_path.size() - 1)
{
// Copy the path after the shortcut (excluding end_index)
shortened_path.insert(shortened_path.end(),
current_path.begin() + end_index + 1,
current_path.end());
shortened_path.insert(
shortened_path.end(),
current_path.begin() + static_cast<ptrdiff_t>(end_index) + 1,
current_path.end());
}
// Swap in as the new current path
current_path = shortened_path;
Expand Down
3 changes: 2 additions & 1 deletion include/common_robotics_utilities/serialization.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -441,7 +441,8 @@ inline uint64_t SerializeMemcpyableVectorLike(
const uint64_t size = static_cast<uint64_t>(vec_to_serialize.size());
SerializeMemcpyable<uint64_t>(size, buffer);
// Expand the buffer to handle everything
const size_t serialized_length = sizeof(T) * vec_to_serialize.size();
const size_t serialized_length =
sizeof(T) * static_cast<size_t>(vec_to_serialize.size());
const size_t previous_buffer_size = buffer.size();
buffer.resize(previous_buffer_size + serialized_length, 0x00);
// Serialize the contained items
Expand Down
Loading

0 comments on commit 433d858

Please sign in to comment.