From 5d5b822144426338052f2ddd5361314e8bf20f54 Mon Sep 17 00:00:00 2001 From: Calder Phillips-Grafflin Date: Wed, 8 Jun 2022 20:20:56 -0400 Subject: [PATCH] Clang -Wconversion support (#46) * Switch to using omp parallel if clauses, replace size_t Size() methods * Modify maybe_test to remove surplus std::move()s * Unify LTO enablement and warnings for GCC and Clang * Rename helpers for clarity, initialize member to make GCC happy * Address all warnings with Clang -Wconversion --- CMakeLists.txt.ros1 | 7 +- CMakeLists.txt.ros2 | 7 +- .../dynamic_spatial_hashed_voxel_grid.hpp | 7 +- include/common_robotics_utilities/math.hpp | 90 ++++++------------- include/common_robotics_utilities/maybe.hpp | 14 +-- .../path_processing.hpp | 56 ++++++------ .../serialization.hpp | 3 +- .../simple_astar_search.hpp | 6 +- .../simple_graph.hpp | 49 ++++------ .../simple_graph_search.hpp | 5 +- .../simple_hierarchical_clustering.hpp | 44 +++++---- .../simple_kmeans_clustering.hpp | 71 ++++++--------- .../simple_knearest_neighbors.hpp | 6 +- .../simple_prm_planner.hpp | 66 +++++--------- .../simple_rrt_planner.hpp | 14 +-- .../simple_task_planner.hpp | 15 ++-- .../common_robotics_utilities/voxel_grid.hpp | 7 +- .../base64_helpers.cpp | 59 ++++++------ .../serialization.cpp | 3 +- test/maybe_test.cpp | 46 +++++++--- test/planning_test.cpp | 7 +- test/voxel_grid_test.cpp | 6 +- 22 files changed, 275 insertions(+), 313 deletions(-) diff --git a/CMakeLists.txt.ros1 b/CMakeLists.txt.ros1 index b993123..60923b6 100644 --- a/CMakeLists.txt.ros1 +++ b/CMakeLists.txt.ros1 @@ -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) @@ -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") diff --git a/CMakeLists.txt.ros2 b/CMakeLists.txt.ros2 index a689622..c6d267e 100644 --- a/CMakeLists.txt.ros2 +++ b/CMakeLists.txt.ros2 @@ -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) @@ -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") diff --git a/include/common_robotics_utilities/dynamic_spatial_hashed_voxel_grid.hpp b/include/common_robotics_utilities/dynamic_spatial_hashed_voxel_grid.hpp index 374ba66..d152494 100644 --- a/include/common_robotics_utilities/dynamic_spatial_hashed_voxel_grid.hpp +++ b/include/common_robotics_utilities/dynamic_spatial_hashed_voxel_grid.hpp @@ -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). - return data_[data_index]; + return data_[static_cast(data_index)]; } else { @@ -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). - return data_[data_index]; + return data_[static_cast(data_index)]; } else { @@ -240,7 +240,8 @@ class DynamicSpatialHashedVoxelGridChunk void SetCellFilledContents(const T& value) { data_.clear(); - data_.resize(sizes_.TotalCells(), value); + data_.resize(static_cast( + sizes_.TotalCells()), value); fill_status_ = DSHVGFillStatus::CELL_FILLED; } diff --git a/include/common_robotics_utilities/math.hpp b/include/common_robotics_utilities/math.hpp index 60111df..4b48321 100644 --- a/include/common_robotics_utilities/math.hpp +++ b/include/common_robotics_utilities/math.hpp @@ -467,13 +467,15 @@ GetArbitraryOrthogonalVectorToPlane( } template> -Eigen::MatrixXd BuildPairwiseDistanceMatrixParallel( +Eigen::MatrixXd BuildPairwiseDistanceMatrix( const Container& data, - const std::function& distance_fn) + const std::function& 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++) { @@ -500,62 +502,36 @@ Eigen::MatrixXd BuildPairwiseDistanceMatrixParallel( } template> -Eigen::MatrixXd BuildPairwiseDistanceMatrixSerial( +Eigen::MatrixXd BuildPairwiseDistanceMatrixParallel( const Container& data, const std::function& 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(idx), static_cast(jdx)) - = distance; - distance_matrix(static_cast(jdx), static_cast(idx)) - = distance; - } - else - { - distance_matrix(static_cast(idx), static_cast(jdx)) - = 0.0; - distance_matrix(static_cast(jdx), static_cast(idx)) - = 0.0; - } - } - } - return distance_matrix; + return BuildPairwiseDistanceMatrix( + data, distance_fn, true); } template> -Eigen::MatrixXd BuildPairwiseDistanceMatrix( +Eigen::MatrixXd BuildPairwiseDistanceMatrixSerial( const Container& data, - const std::function& distance_fn, - const bool use_parallel = false) + const std::function& distance_fn) { - if (use_parallel) - { - return BuildPairwiseDistanceMatrixParallel(data, distance_fn); - } - else - { - return BuildPairwiseDistanceMatrixSerial(data, distance_fn); - } + return BuildPairwiseDistanceMatrix( + data, distance_fn, false); } template, typename SecondContainer=std::vector> -Eigen::MatrixXd BuildPairwiseDistanceMatrixParallel( +Eigen::MatrixXd BuildPairwiseDistanceMatrix( const FirstContainer& data1, const SecondContainer& data2, const std::function& 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++) { @@ -572,41 +548,27 @@ Eigen::MatrixXd BuildPairwiseDistanceMatrixParallel( template, typename SecondContainer=std::vector> -Eigen::MatrixXd BuildPairwiseDistanceMatrixSerial( +Eigen::MatrixXd BuildPairwiseDistanceMatrixParallel( const FirstContainer& data1, const SecondContainer& data2, const std::function& 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(idx), static_cast(jdx)) - = distance; - } - } - return distance_matrix; + return BuildPairwiseDistanceMatrixParallel + ( + data1, data2, distance_fn, true); } template, typename SecondContainer=std::vector> -Eigen::MatrixXd BuildPairwiseDistanceMatrix( +Eigen::MatrixXd BuildPairwiseDistanceMatrixSerial( const FirstContainer& data1, const SecondContainer& data2, const std::function& 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 + ( + data1, data2, distance_fn, false); } class Hyperplane diff --git a/include/common_robotics_utilities/maybe.hpp b/include/common_robotics_utilities/maybe.hpp index 0e82d55..1926257 100644 --- a/include/common_robotics_utilities/maybe.hpp +++ b/include/common_robotics_utilities/maybe.hpp @@ -15,7 +15,7 @@ template struct TaggedUnion { private: - void ConstructFrom(const TaggedUnion& other) + void ConstructCopy(const TaggedUnion& other) { switch (other.type_) { @@ -34,7 +34,7 @@ struct TaggedUnion } } - void ConstructFrom(TaggedUnion&& other) + void ConstructMove(TaggedUnion&& other) { switch (other.type_) { @@ -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) {} @@ -91,9 +91,9 @@ struct TaggedUnion explicit TaggedUnion(B&& value_b) : value_b_(std::move(value_b)), type_(TYPE_B) {} - TaggedUnion(const TaggedUnion& other) { ConstructFrom(other); } + TaggedUnion(const TaggedUnion& other) { ConstructCopy(other); } - TaggedUnion(TaggedUnion&& other) { ConstructFrom(std::move(other)); } + TaggedUnion(TaggedUnion&& other) { ConstructMove(std::move(other)); } ~TaggedUnion() { Cleanup(); } @@ -103,7 +103,7 @@ struct TaggedUnion { Cleanup(); - ConstructFrom(other); + ConstructCopy(other); } return *this; } @@ -114,7 +114,7 @@ struct TaggedUnion { Cleanup(); - ConstructFrom(std::move(other)); + ConstructMove(std::move(other)); } return *this; } diff --git a/include/common_robotics_utilities/path_processing.hpp b/include/common_robotics_utilities/path_processing.hpp index 4fcd435..96a9fde 100644 --- a/include/common_robotics_utilities/path_processing.hpp +++ b/include/common_robotics_utilities/path_processing.hpp @@ -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(middle_index) + 1, + current_path.begin() + static_cast(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(start_index), + current_path.begin() + static_cast(middle_index)); + shortcut.insert( + shortcut.end(), second_half_shortcut.begin(), + second_half_shortcut.end()); } return shortcut; } @@ -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(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(end_index) + 1, + current_path.end()); } // Swap in as the new current path current_path = shortened_path; diff --git a/include/common_robotics_utilities/serialization.hpp b/include/common_robotics_utilities/serialization.hpp index cc2082f..cc6937e 100644 --- a/include/common_robotics_utilities/serialization.hpp +++ b/include/common_robotics_utilities/serialization.hpp @@ -441,7 +441,8 @@ inline uint64_t SerializeMemcpyableVectorLike( const uint64_t size = static_cast(vec_to_serialize.size()); SerializeMemcpyable(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(vec_to_serialize.size()); const size_t previous_buffer_size = buffer.size(); buffer.resize(previous_buffer_size + serialized_length, 0x00); // Serialize the contained items diff --git a/include/common_robotics_utilities/simple_astar_search.hpp b/include/common_robotics_utilities/simple_astar_search.hpp index 54b97b8..30f69d7 100644 --- a/include/common_robotics_utilities/simple_astar_search.hpp +++ b/include/common_robotics_utilities/simple_astar_search.hpp @@ -242,7 +242,7 @@ inline AstarResult PerformAstarSearch( int64_t best_start_meeting_goal_index = -1; double best_start_meeting_goal_cost = std::numeric_limits::infinity(); - for (int64_t idx = 0; idx < static_cast(start_states.size()); idx++) + for (size_t idx = 0; idx < start_states.size(); idx++) { const T& start_state = start_states.at(idx).State(); const double start_cost = start_states.at(idx).Cost(); @@ -250,7 +250,7 @@ inline AstarResult PerformAstarSearch( { if (start_cost < best_start_meeting_goal_cost) { - best_start_meeting_goal_index = idx; + best_start_meeting_goal_index = static_cast(idx); best_start_meeting_goal_cost = start_cost; } } @@ -260,7 +260,7 @@ inline AstarResult PerformAstarSearch( if (best_start_meeting_goal_index >= 0) { const auto& best_start_meeting_goal = - start_states.at(best_start_meeting_goal_index); + start_states.at(static_cast(best_start_meeting_goal_index)); Container solution; solution.push_back(best_start_meeting_goal.State()); diff --git a/include/common_robotics_utilities/simple_graph.hpp b/include/common_robotics_utilities/simple_graph.hpp index 3725ab3..d6334ff 100644 --- a/include/common_robotics_utilities/simple_graph.hpp +++ b/include/common_robotics_utilities/simple_graph.hpp @@ -347,11 +347,11 @@ class Graph std::sort(vector_nodes_to_prune.begin(), vector_nodes_to_prune.end()); // Make the pruned graph, initialized with the number of nodes we expect it // to contain. - GraphType pruned_graph(graph.Size() - nodes_to_prune.size()); + GraphType pruned_graph( + graph.Size() - static_cast(nodes_to_prune.size())); + // First, serial pass through to copy nodes to be kept - for (int64_t node_index = 0; - node_index < static_cast(graph.Size()); - node_index++) + for (int64_t node_index = 0; node_index < graph.Size(); node_index++) { if (nodes_to_prune.count(node_index) == 0) { @@ -360,8 +360,13 @@ class Graph } } pruned_graph.ShrinkToFit(); - // Loop body for second pass to update edges for the kept nodes - const auto update_edge_fn = [&] (const int64_t kept_node_index) + + // Second, optionally parallel pass to update edges for the kept nodes +#if defined(_OPENMP) +#pragma omp parallel for if (use_parallel) +#endif + for (int64_t kept_node_index = 0; kept_node_index < pruned_graph.Size(); + kept_node_index++) { GraphNodeType& kept_graph_node = pruned_graph.GetNodeMutable(kept_node_index); @@ -449,28 +454,7 @@ class Graph kept_graph_node.SetInEdges(new_in_edges); kept_graph_node.SetOutEdges(new_out_edges); }; - // Second, optionally parallel pass to update edges for the kept nodes - if (use_parallel) - { -#if defined(_OPENMP) -#pragma omp parallel for -#endif - for (int64_t kept_node_index = 0; - kept_node_index < static_cast(pruned_graph.Size()); - kept_node_index++) - { - update_edge_fn(kept_node_index); - } - } - else - { - for (int64_t kept_node_index = 0; - kept_node_index < static_cast(pruned_graph.Size()); - kept_node_index++) - { - update_edge_fn(kept_node_index); - } - } + return pruned_graph; } @@ -486,7 +470,10 @@ class Graph } } - Graph(const size_t expected_size) { nodes_.reserve(expected_size); } + Graph(const int64_t expected_size) + { + nodes_.reserve(static_cast(expected_size)); + } Graph() {} @@ -562,11 +549,11 @@ class Graph nodes_.shrink_to_fit(); } - size_t Size() const { return nodes_.size(); } + int64_t Size() const { return static_cast(nodes_.size()); } bool IndexInRange(const int64_t index) const { - if ((index >= 0) && (index < static_cast(nodes_.size()))) + if ((index >= 0) && (index < Size())) { return true; } diff --git a/include/common_robotics_utilities/simple_graph_search.hpp b/include/common_robotics_utilities/simple_graph_search.hpp index ba7787a..4f688fe 100644 --- a/include/common_robotics_utilities/simple_graph_search.hpp +++ b/include/common_robotics_utilities/simple_graph_search.hpp @@ -99,7 +99,10 @@ class DijkstrasResult return bytes_read; } - size_t Size() const { return previous_index_map_.size(); } + int64_t Size() const + { + return static_cast(previous_index_map_.size()); + } int64_t GetPreviousIndex(const int64_t node_index) const { diff --git a/include/common_robotics_utilities/simple_hierarchical_clustering.hpp b/include/common_robotics_utilities/simple_hierarchical_clustering.hpp index 4b29e46..95995e8 100644 --- a/include/common_robotics_utilities/simple_hierarchical_clustering.hpp +++ b/include/common_robotics_utilities/simple_hierarchical_clustering.hpp @@ -46,6 +46,9 @@ class Item public: Item() : index_(-1), is_cluster_(false) {} + Item(const size_t index, const bool is_cluster) + : Item(static_cast(index), is_cluster) {} + Item(const int64_t index, const bool is_cluster) : index_(index), is_cluster_(is_cluster) { @@ -122,7 +125,7 @@ inline ClosestPair GetClosestClustersParallel( const ClusterStrategy strategy) { std::vector per_thread_closest_clusters( - openmp_helpers::GetNumOmpThreads(), ClosestPair()); + static_cast(openmp_helpers::GetNumOmpThreads()), ClosestPair()); #if defined(_OPENMP) #pragma omp parallel for #endif @@ -150,8 +153,9 @@ inline ClosestPair GetClosestClustersParallel( { for (const int64_t& cluster2_index : second_cluster) { - const double distance - = distance_matrix(cluster1_index, cluster2_index); + const double distance = distance_matrix( + static_cast(cluster1_index), + static_cast(cluster2_index)); minimum_distance = std::min(minimum_distance, distance); maximum_distance = std::max(maximum_distance, distance); } @@ -159,7 +163,8 @@ inline ClosestPair GetClosestClustersParallel( const double cluster_distance = (strategy == ClusterStrategy::COMPLETE_LINK) ? maximum_distance : minimum_distance; - const int32_t thread_num = openmp_helpers::GetContextOmpThreadNum(); + const size_t thread_num = + static_cast(openmp_helpers::GetContextOmpThreadNum()); const double current_closest_distance = per_thread_closest_clusters.at(thread_num).Distance(); if (cluster_distance < current_closest_distance) @@ -219,8 +224,9 @@ inline ClosestPair GetClosestClustersSerial( { for (const int64_t& cluster2_index : second_cluster) { - const double distance - = distance_matrix(cluster1_index, cluster2_index); + const double distance = distance_matrix( + static_cast(cluster1_index), + static_cast(cluster2_index)); minimum_distance = std::min(minimum_distance, distance); maximum_distance = std::max(maximum_distance, distance); } @@ -273,7 +279,7 @@ inline ClosestPair GetClosestValueToOtherParallel( const ClusterStrategy strategy) { std::vector per_thread_closest_value_other( - openmp_helpers::GetNumOmpThreads(), ClosestPair()); + static_cast(openmp_helpers::GetNumOmpThreads()), ClosestPair()); #if defined(_OPENMP) #pragma omp parallel for #endif @@ -282,7 +288,8 @@ inline ClosestPair GetClosestValueToOtherParallel( // Make sure we're not already clustered if (datapoint_mask.at(value_idx) == 0x00) { - const int32_t thread_num = openmp_helpers::GetContextOmpThreadNum(); + const size_t thread_num = + static_cast(openmp_helpers::GetContextOmpThreadNum()); // Check against other values for (size_t other_value_idx = value_idx + 1; other_value_idx < datapoint_mask.size(); other_value_idx++) @@ -290,7 +297,9 @@ inline ClosestPair GetClosestValueToOtherParallel( // Make sure it's not already clustered if (datapoint_mask.at(other_value_idx) == 0x00) { - const double distance = distance_matrix(value_idx, other_value_idx); + const double distance = distance_matrix( + static_cast(value_idx), + static_cast(other_value_idx)); const double current_closest_distance = per_thread_closest_value_other.at(thread_num).Distance(); if (distance < current_closest_distance) @@ -314,8 +323,9 @@ inline ClosestPair GetClosestValueToOtherParallel( double maximum_distance = 0.0; for (const int64_t& cluster_element_idx : cluster) { - const double distance - = distance_matrix(value_idx, cluster_element_idx); + const double distance = distance_matrix( + static_cast(value_idx), + static_cast(cluster_element_idx)); minimum_distance = std::min(minimum_distance, distance); maximum_distance = std::max(maximum_distance, distance); } @@ -370,7 +380,9 @@ inline ClosestPair GetClosestValueToOtherSerial( // Make sure it's not already clustered if (datapoint_mask.at(other_value_idx) == 0x00) { - const double distance = distance_matrix(value_idx, other_value_idx); + const double distance = distance_matrix( + static_cast(value_idx), + static_cast(other_value_idx)); if (distance < closest_value_other.Distance()) { closest_value_other = ClosestPair(Item(value_idx, false), @@ -391,8 +403,9 @@ inline ClosestPair GetClosestValueToOtherSerial( double maximum_distance = 0.0; for (const int64_t& cluster_element_idx : cluster) { - const double distance - = distance_matrix(value_idx, cluster_element_idx); + const double distance = distance_matrix( + static_cast(value_idx), + static_cast(cluster_element_idx)); minimum_distance = std::min(minimum_distance, distance); maximum_distance = std::max(maximum_distance, distance); } @@ -518,7 +531,8 @@ inline IndexClusteringResult IndexClusterWithDistanceMatrix( { throw std::invalid_argument("distance_matrix is empty"); } - std::vector datapoint_mask(distance_matrix.rows(), 0u); + std::vector datapoint_mask( + static_cast(distance_matrix.rows()), 0u); std::vector> cluster_indices; double closest_distance = 0.0; bool complete = false; diff --git a/include/common_robotics_utilities/simple_kmeans_clustering.hpp b/include/common_robotics_utilities/simple_kmeans_clustering.hpp index 1c3997a..167436f 100644 --- a/include/common_robotics_utilities/simple_kmeans_clustering.hpp +++ b/include/common_robotics_utilities/simple_kmeans_clustering.hpp @@ -33,7 +33,11 @@ inline std::vector PerformSingleClusteringIteration( const bool use_parallel) { std::vector new_cluster_labels(data.size()); - const auto loop_body = [&] (const size_t idx) + +#if defined(_OPENMP) +#pragma omp parallel for if (use_parallel) +#endif + for (size_t idx = 0; idx < data.size(); idx++) { const DataType& datapoint = data.at(idx); const int64_t closest_cluster_index @@ -41,23 +45,7 @@ inline std::vector PerformSingleClusteringIteration( current_cluster_centers, datapoint, distance_fn, 1).at(0).Index(); new_cluster_labels.at(idx) = static_cast(closest_cluster_index); }; - if (use_parallel) - { -#if defined(_OPENMP) -#pragma omp parallel for -#endif - for (size_t idx = 0; idx < data.size(); idx++) - { - loop_body(idx); - } - } - else - { - for (size_t idx = 0; idx < data.size(); idx++) - { - loop_body(idx); - } - } + return new_cluster_labels; } @@ -82,18 +70,25 @@ inline Container ComputeClusterCentersWeighted( // TODO: improve this to avoid copies. It's possible right now, but it would // result in the average functions being more complicated and slower. // Separate the datapoints into their clusters - std::vector clustered_data(num_clusters); - std::vector> clustered_data_weights(num_clusters); + std::vector clustered_data(static_cast(num_clusters)); + std::vector> clustered_data_weights( + static_cast(num_clusters)); for (size_t idx = 0; idx < data.size(); idx++) { const DataType& datapoint = data[idx]; const int32_t label = cluster_labels.at(idx); - clustered_data.at(label).push_back(datapoint); - clustered_data_weights.at(label).push_back(data_weights.at(idx)); + clustered_data.at(static_cast(label)).push_back(datapoint); + clustered_data_weights.at( + static_cast(label)).push_back(data_weights.at(idx)); } + // Compute the center of each cluster - Container cluster_centers(num_clusters); - const auto loop_body = [&] (const size_t cluster) + Container cluster_centers(static_cast(num_clusters)); + +#if defined(_OPENMP) +#pragma omp parallel for if (use_parallel) +#endif + for (size_t cluster = 0; cluster < clustered_data.size(); cluster++) { const Container& cluster_data = clustered_data.at(cluster); const std::vector& cluster_data_weights @@ -101,23 +96,7 @@ inline Container ComputeClusterCentersWeighted( cluster_centers.at(cluster) = weighted_average_fn(cluster_data, cluster_data_weights); }; - if (use_parallel) - { -#if defined(_OPENMP) -#pragma omp parallel for -#endif - for (size_t cluster = 0; cluster < clustered_data.size(); cluster++) - { - loop_body(cluster); - } - } - else - { - for (size_t cluster = 0; cluster < clustered_data.size(); cluster++) - { - loop_body(cluster); - } - } + return cluster_centers; } else @@ -128,8 +107,9 @@ inline Container ComputeClusterCentersWeighted( /// Checks for convergence - i.e. if @param old_labels and @param new_labels are /// the same. @returns if the labels are the same. -inline bool CheckForConvergence(const std::vector& old_labels, - const std::vector& new_labels) +inline bool CheckForConvergence( + const std::vector& old_labels, + const std::vector& new_labels) { if (old_labels.size() == new_labels.size()) { @@ -203,7 +183,8 @@ inline std::vector ClusterWeighted( } } // Prepare an RNG for cluster initialization - std::mt19937_64 prng(prng_seed); + std::mt19937_64 prng( + static_cast(prng_seed)); std::uniform_int_distribution initialization_distribution( 0, data.size() - 1); // Initialize cluster centers @@ -257,7 +238,7 @@ inline std::vector ClusterWeighted( const size_t random_index = initialization_distribution(prng); index_map[random_index] = 1u; } - cluster_centers.reserve(num_clusters); + cluster_centers.reserve(static_cast(num_clusters)); for (auto itr = index_map.begin(); itr != index_map.end(); ++itr) { if (itr->second == 1u) diff --git a/include/common_robotics_utilities/simple_knearest_neighbors.hpp b/include/common_robotics_utilities/simple_knearest_neighbors.hpp index 9533c0f..8c4304a 100644 --- a/include/common_robotics_utilities/simple_knearest_neighbors.hpp +++ b/include/common_robotics_utilities/simple_knearest_neighbors.hpp @@ -75,7 +75,8 @@ inline std::vector GetKNearestNeighborsParallel( if (items.size() > K) { std::vector> per_thread_nearests( - openmp_helpers::GetNumOmpThreads(), std::vector(K)); + static_cast(openmp_helpers::GetNumOmpThreads()), + std::vector(K)); #if defined(_OPENMP) #pragma omp parallel for #endif @@ -83,7 +84,8 @@ inline std::vector GetKNearestNeighborsParallel( { const Item& item = items[idx]; const double distance = distance_fn(item, current); - const auto thread_num = openmp_helpers::GetContextOmpThreadNum(); + const size_t thread_num = + static_cast(openmp_helpers::GetContextOmpThreadNum()); std::vector& current_thread_nearests = per_thread_nearests.at(thread_num); auto itr = std::max_element(current_thread_nearests.begin(), diff --git a/include/common_robotics_utilities/simple_prm_planner.hpp b/include/common_robotics_utilities/simple_prm_planner.hpp index e8a6df5..9029a40 100644 --- a/include/common_robotics_utilities/simple_prm_planner.hpp +++ b/include/common_robotics_utilities/simple_prm_planner.hpp @@ -76,11 +76,13 @@ inline int64_t AddNodeToRoadmap( return distance_fn(query_state, node.GetValueImmutable()); }; } + // Call KNN with the distance function const auto nearest_neighbors = simple_knearest_neighbors::GetKNearestNeighbors( roadmap.GetNodesImmutable(), state, graph_distance_fn, K, use_parallel); + // Check if we already have this state in the roadmap // (and we don't want to add duplicates) if (add_duplicate_states == false) @@ -93,14 +95,20 @@ inline int64_t AddNodeToRoadmap( } } } + // Add the new node AFTER KNN is performed const int64_t new_node_index = roadmap.AddNode(state); + // Parallelize the collision-checking and distance computation // Because we don't need any special caching here, we define the loop contents // as a lambda and then call branch between parfor/for loops that call it. std::vector> nearest_neighbors_distances( nearest_neighbors.size()); - auto collision_check_and_distance_fn = [&] (const size_t idx) + +#if defined(_OPENMP) +#pragma omp parallel for if (use_parallel) +#endif + for (size_t idx = 0; idx < nearest_neighbors.size(); idx++) { const auto& nearest_neighbor = nearest_neighbors.at(idx); const int64_t nearest_neighbor_index = nearest_neighbor.Index(); @@ -149,23 +157,7 @@ inline int64_t AddNodeToRoadmap( nearest_neighbors_distances.at(idx) = std::make_pair(-1.0, -1.0); } }; - if (use_parallel) - { -#if defined(_OPENMP) -#pragma omp parallel for -#endif - for (size_t idx = 0; idx < nearest_neighbors.size(); idx++) - { - collision_check_and_distance_fn(idx); - } - } - else - { - for (size_t idx = 0; idx < nearest_neighbors.size(); idx++) - { - collision_check_and_distance_fn(idx); - } - } + // THIS MUST BE SERIAL - add edges to roadmap for (size_t idx = 0; idx < nearest_neighbors.size(); idx++) { @@ -185,6 +177,7 @@ inline int64_t AddNodeToRoadmap( nearest_neighbor_distances.second); } } + return new_node_index; } @@ -233,18 +226,18 @@ inline std::map GrowRoadMap( // Update the start time const std::chrono::time_point start_time = std::chrono::steady_clock::now(); - while (!termination_check_fn(static_cast(roadmap.Size()))) + while (!termination_check_fn(roadmap.Size())) { const T random_state = sampling_fn(); statistics["total_samples"] += 1.0; if (state_validity_check_fn(random_state)) { - const size_t pre_size = roadmap.Size(); + const int64_t pre_size = roadmap.Size(); AddNodeToRoadmap(random_state, NNDistanceDirection::ROADMAP_TO_NEW_STATE, roadmap, distance_fn, edge_validity_check_fn, K, use_parallel, distance_is_symmetric, add_duplicate_states); - const size_t post_size = roadmap.Size(); + const int64_t post_size = roadmap.Size(); if (post_size > pre_size) { statistics["successful_samples"] += 1.0; @@ -284,9 +277,12 @@ inline void UpdateRoadMapEdges( { throw std::invalid_argument("Provided roadmap has invalid linkage"); } - // Because we don't need any special caching here, we define the loop contents - // as a lambda and then call branch between parfor/for loops that call it. - auto update_node_fn = [&] (const size_t current_node_index) + +#if defined(_OPENMP) +#pragma omp parallel for if (use_parallel) +#endif + for (int64_t current_node_index = 0; current_node_index < roadmap.Size(); + current_node_index++) { simple_graph::GraphNode& current_node = roadmap.GetNodeMutable(current_node_index); @@ -310,33 +306,13 @@ inline void UpdateRoadMapEdges( // Update the other node's in edges for (auto& other_in_edge : other_node_in_edges) { - if (other_in_edge.GetFromIndex() - == static_cast(current_node_index)) + if (other_in_edge.GetFromIndex() == current_node_index) { other_in_edge.SetWeight(updated_weight); } } } }; - if (use_parallel) - { -#if defined(_OPENMP) -#pragma omp parallel for -#endif - for (size_t current_node_index = 0; current_node_index < roadmap.Size(); - current_node_index++) - { - update_node_fn(current_node_index); - } - } - else - { - for (size_t current_node_index = 0; current_node_index < roadmap.Size(); - current_node_index++) - { - update_node_fn(current_node_index); - } - } } /// Extracts the solution path from the roadmap. diff --git a/include/common_robotics_utilities/simple_rrt_planner.hpp b/include/common_robotics_utilities/simple_rrt_planner.hpp index 48e1b55..732f0b6 100644 --- a/include/common_robotics_utilities/simple_rrt_planner.hpp +++ b/include/common_robotics_utilities/simple_rrt_planner.hpp @@ -50,9 +50,8 @@ class SimpleRRTPlannerState const serialization::Deserializer& value_deserializer) { SimpleRRTPlannerState temp_state; - const uint64_t bytes_read - = temp_state.DeserializeSelf(buffer, starting_offset, - value_deserializer); + const uint64_t bytes_read = temp_state.DeserializeSelf( + buffer, starting_offset, value_deserializer); return serialization::MakeDeserialized(temp_state, bytes_read); } @@ -184,7 +183,7 @@ template class SimpleRRTPlannerTree { private: - SimpleRRTPlannerStateVector nodes_; + SimpleRRTPlannerStateVector nodes_; public: using NodeType = SimpleRRTPlannerState; @@ -233,7 +232,6 @@ class SimpleRRTPlannerTree tree, deserialized_nodes.BytesRead()); } - SimpleRRTPlannerTree() {} explicit SimpleRRTPlannerTree(const int64_t anticipated_size) @@ -340,7 +338,8 @@ class SimpleRRTPlannerTree { if (parent_index != static_cast(current_index)) { - const auto& parent_state = nodes.at(parent_index); + const auto& parent_state = + nodes.at(static_cast(parent_index)); // Make sure the parent state is initialized. if (!parent_state.IsInitialized()) { @@ -387,7 +386,8 @@ class SimpleRRTPlannerTree { if (current_child_index > static_cast(current_index)) { - const auto& child_state = nodes.at(current_child_index); + const auto& child_state = + nodes.at(static_cast(current_child_index)); if (!child_state.IsInitialized()) { std::cerr << "Tree contains uninitialized node(s) " diff --git a/include/common_robotics_utilities/simple_task_planner.hpp b/include/common_robotics_utilities/simple_task_planner.hpp index c434219..39c77ba 100644 --- a/include/common_robotics_utilities/simple_task_planner.hpp +++ b/include/common_robotics_utilities/simple_task_planner.hpp @@ -302,7 +302,8 @@ TaskStateAStarResult PlanTaskStateSequence( index < static_cast(primitive_collection.Primitives().size()); index++) { - const auto& primitive = primitive_collection.Primitives().at(index); + const auto& primitive = + primitive_collection.Primitives().at(static_cast(index)); if (primitive->IsCandidate(current_state)) { const auto outcomes = primitive->GetOutcomes(current_state); @@ -416,13 +417,11 @@ NextPrimitiveToExecute GetNextPrimitiveToExecute( const State& outcome_state = task_sequence.Path().at(0).Outcome(); int64_t best_outcome_index = -1; - for (int64_t index = 0; - index < static_cast(potential_outcome_states.size()); - index++) + for (size_t index = 0; index < potential_outcome_states.size(); index++) { if (state_equaler(outcome_state, potential_outcome_states.at(index))) { - best_outcome_index = index; + best_outcome_index = static_cast(index); break; } } @@ -435,7 +434,7 @@ NextPrimitiveToExecute GetNextPrimitiveToExecute( { const auto& next_state_and_index = task_sequence.Path().at(1); const auto& next_primitive = primitive_collection.Primitives().at( - next_state_and_index.PrimitiveIndex()); + static_cast(next_state_and_index.PrimitiveIndex())); return NextPrimitiveToExecute( next_primitive, best_outcome_index); @@ -490,8 +489,8 @@ Container PerformSingleTaskExecution( state_heuristic_fn, state_hasher, state_equaler); // Get the outcome state and add it to the execution trace. - const State& selected_outcome = - current_outcomes.at(next_to_execute.SelectedOutcomeIndex()); + const State& selected_outcome = current_outcomes.at( + static_cast(next_to_execute.SelectedOutcomeIndex())); task_state_trace.push_back(selected_outcome); // Call the user-provided callback. diff --git a/include/common_robotics_utilities/voxel_grid.hpp b/include/common_robotics_utilities/voxel_grid.hpp index 52276aa..038e313 100644 --- a/include/common_robotics_utilities/voxel_grid.hpp +++ b/include/common_robotics_utilities/voxel_grid.hpp @@ -552,7 +552,7 @@ class VoxelGridBase { // Note: do not refactor to use .at(), since not all vector-like // implementations implement it (ex thrust::host_vector). - return data_[data_index]; + return data_[static_cast(data_index)]; } else { @@ -566,7 +566,7 @@ class VoxelGridBase { // Note: do not refactor to use .at(), since not all vector-like // implementations implement it (ex thrust::host_vector). - return data_[data_index]; + return data_[static_cast(data_index)]; } else { @@ -577,7 +577,8 @@ class VoxelGridBase void SetContents(const T& value) { data_.clear(); - data_.resize(sizes_.TotalCells(), value); + data_.resize(static_cast( + sizes_.TotalCells()), value); } uint64_t BaseSerializeSelf( diff --git a/src/common_robotics_utilities/base64_helpers.cpp b/src/common_robotics_utilities/base64_helpers.cpp index 929ad52..9742480 100644 --- a/src/common_robotics_utilities/base64_helpers.cpp +++ b/src/common_robotics_utilities/base64_helpers.cpp @@ -18,7 +18,7 @@ static const std::array B64IndexTable = 25, 0, 0, 0, 0, 63, 0, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51}; -static const std::array B64ValueTable = +static const std::array B64ValueTable = {'A', 'B', 'C', 'D', 'E', 'F', 'G', 'H', 'I', 'J', 'K', 'L', 'M', 'N', 'O', 'P', 'Q', 'R', 'S', 'T', 'U', 'V', 'W', 'X', 'Y', 'Z', 'a', 'b', 'c', 'd', 'e', 'f', 'g', 'h', 'i', 'j', 'k', 'l', 'm', 'n', 'o', 'p', 'q', 'r', 's', @@ -34,22 +34,24 @@ std::vector Decode(const std::string& encoded) std::vector buffer(L / 4 * 3 + pad, 0x00); for (size_t i = 0, j = 0; i < L; i += 4) { - const int32_t n = B64IndexTable[encoded[i]] << 18 - | B64IndexTable[encoded[i + 1]] << 12 - | B64IndexTable[encoded[i + 2]] << 6 - | B64IndexTable[encoded[i + 3]]; + const int32_t n = + B64IndexTable[static_cast(encoded[i])] << 18 + | B64IndexTable[static_cast(encoded[i + 1])] << 12 + | B64IndexTable[static_cast(encoded[i + 2])] << 6 + | B64IndexTable[static_cast(encoded[i + 3])]; buffer[j++] = static_cast(n >> 16); buffer[j++] = static_cast(n >> 8 & 0xFF); buffer[j++] = static_cast(n & 0xFF); } if (pad > 0) { - int32_t n = B64IndexTable[encoded[L]] << 18 - | B64IndexTable[encoded[L + 1]] << 12; + int32_t n = + B64IndexTable[static_cast(encoded[L])] << 18 + | B64IndexTable[static_cast(encoded[L + 1])] << 12; buffer[buffer.size() - 1] = static_cast(n >> 16); if (encoded_length > L + 2 && encoded[L + 2] != '=') { - n |= B64IndexTable[encoded[L + 2]] << 6; + n |= B64IndexTable[static_cast(encoded[L + 2])] << 6; buffer.push_back(static_cast(n >> 8 & 0xFF)); } } @@ -71,35 +73,38 @@ std::string Encode(const std::vector& binary) size_t output_position = 0; while (binary_length - input_position >= 3) { - encoded[output_position++] = - B64ValueTable[binary[input_position + 0] >> 2]; - encoded[output_position++] = - B64ValueTable[((binary[input_position + 0] & 0x03) << 4) - | (binary[input_position + 1] >> 4)]; - encoded[output_position++] = - B64ValueTable[((binary[input_position + 1] & 0x0f) << 2) - | (binary[input_position + 2] >> 6)]; - encoded[output_position++] = - B64ValueTable[binary[input_position + 2] & 0x3f]; + encoded[output_position++] = B64ValueTable[ + static_cast(binary[input_position + 0] >> 2)]; + encoded[output_position++] = B64ValueTable[ + static_cast( + ((binary[input_position + 0] & 0x03) << 4) + | (binary[input_position + 1] >> 4))]; + encoded[output_position++] = B64ValueTable[ + static_cast( + ((binary[input_position + 1] & 0x0f) << 2) + | (binary[input_position + 2] >> 6))]; + encoded[output_position++] = B64ValueTable[ + static_cast(binary[input_position + 2] & 0x3f)]; input_position += 3; } if (input_position < binary_length) { - encoded[output_position++] = - B64ValueTable[binary[input_position + 0] >> 2]; + encoded[output_position++] = B64ValueTable[ + static_cast(binary[input_position + 0] >> 2)]; if ((binary_length - input_position) == 1) { - encoded[output_position++] = - B64ValueTable[(binary[input_position + 0] & 0x03) << 4]; + encoded[output_position++] = B64ValueTable[ + static_cast((binary[input_position + 0] & 0x03) << 4)]; encoded[output_position++] = '='; } else { - encoded[output_position++] = - B64ValueTable[((binary[input_position + 0] & 0x03) << 4) - | (binary[input_position + 1] >> 4)]; - encoded[output_position++] = - B64ValueTable[(binary[input_position + 1] & 0x0f) << 2]; + encoded[output_position++] = B64ValueTable[ + static_cast( + ((binary[input_position + 0] & 0x03) << 4) + | (binary[input_position + 1] >> 4))]; + encoded[output_position++] = B64ValueTable[ + static_cast((binary[input_position + 1] & 0x0f) << 2)]; } encoded[output_position++] = '='; } diff --git a/src/common_robotics_utilities/serialization.cpp b/src/common_robotics_utilities/serialization.cpp index e29b457..9f4951e 100644 --- a/src/common_robotics_utilities/serialization.cpp +++ b/src/common_robotics_utilities/serialization.cpp @@ -140,7 +140,8 @@ Deserialized DeserializeMatrixXd( { throw std::invalid_argument("Not enough room in the provided buffer"); } - Eigen::MatrixXd deserialized = Eigen::MatrixXd::Zero(rows, cols); + Eigen::MatrixXd deserialized = Eigen::MatrixXd::Zero( + static_cast(rows), static_cast(cols)); memcpy(deserialized.data(), &buffer[current_position], serialized_length); current_position += serialized_length; // Figure out how many bytes were read diff --git a/test/maybe_test.cpp b/test/maybe_test.cpp index ad9e270..7defdb7 100644 --- a/test/maybe_test.cpp +++ b/test/maybe_test.cpp @@ -23,6 +23,17 @@ class DefaultConstructibleWrapper DefaultConstructibleWrapper(const int32_t value) : value_(value) {} + DefaultConstructibleWrapper( + const DefaultConstructibleWrapper& other) = default; + + DefaultConstructibleWrapper(DefaultConstructibleWrapper&& other) = default; + + DefaultConstructibleWrapper& operator=( + const DefaultConstructibleWrapper& other) = default; + + DefaultConstructibleWrapper& operator=( + DefaultConstructibleWrapper&& other) = default; + int32_t& Value() { return value_; } const int32_t& Value() const { return value_; } @@ -33,7 +44,7 @@ GTEST_TEST(OwningMaybeTest, DefaultConstructMoveAndAssign) // Test basic construction. const OwningMaybe maybe_not; OwningMaybe maybe_default( - std::move(DefaultConstructibleWrapper())); + DefaultConstructibleWrapper{}); const OwningMaybe maybe_1( DefaultConstructibleWrapper(1)); @@ -52,8 +63,8 @@ GTEST_TEST(OwningMaybeTest, DefaultConstructMoveAndAssign) // Test copy and move constructors. const OwningMaybe copy_maybe_1(maybe_1); const OwningMaybe copy_temp_maybe( - std::move(OwningMaybe( - DefaultConstructibleWrapper(2)))); + OwningMaybe( + DefaultConstructibleWrapper(2))); EXPECT_TRUE(copy_maybe_1); EXPECT_TRUE(copy_temp_maybe); @@ -89,6 +100,18 @@ class NonDefaultConstructibleWrapper NonDefaultConstructibleWrapper(const int32_t value) : value_(value) {} + NonDefaultConstructibleWrapper( + const NonDefaultConstructibleWrapper& other) = default; + + NonDefaultConstructibleWrapper( + NonDefaultConstructibleWrapper&& other) = default; + + NonDefaultConstructibleWrapper& operator=( + const NonDefaultConstructibleWrapper& other) = default; + + NonDefaultConstructibleWrapper& operator=( + NonDefaultConstructibleWrapper&& other) = default; + int32_t& Value() { return value_; } const int32_t& Value() const { return value_; } @@ -108,8 +131,8 @@ GTEST_TEST(OwningMaybeTest, NoDefaultConstructMoveAndAssign) // Test copy and move constructors. OwningMaybe copy_maybe_1(maybe_1); OwningMaybe copy_temp_maybe( - std::move(OwningMaybe( - NonDefaultConstructibleWrapper(2)))); + OwningMaybe( + NonDefaultConstructibleWrapper(2))); EXPECT_TRUE(copy_maybe_1); EXPECT_TRUE(copy_temp_maybe); @@ -149,8 +172,8 @@ GTEST_TEST(OwningMaybeTest, ConstNoDefaultConstructMoveAndAssign) // Test copy and move constructors. const OwningMaybe copy_maybe_1(maybe_1); const OwningMaybe copy_temp_maybe( - std::move(OwningMaybe( - NonDefaultConstructibleWrapper(2)))); + OwningMaybe( + NonDefaultConstructibleWrapper(2))); EXPECT_TRUE(copy_maybe_1); EXPECT_TRUE(copy_temp_maybe); @@ -181,7 +204,7 @@ GTEST_TEST(OwningMaybeTest, NonTrivialConstructMoveAndAssign) // Test basic construction. OwningMaybe> maybe_not; OwningMaybe> maybe_default( - std::move(std::vector())); + std::vector{}); OwningMaybe> maybe_1( std::vector(1, 1)); @@ -201,8 +224,8 @@ GTEST_TEST(OwningMaybeTest, NonTrivialConstructMoveAndAssign) // Test copy and move constructors. OwningMaybe> copy_maybe_1(maybe_1); OwningMaybe> copy_temp_maybe( - std::move(OwningMaybe>( - std::vector(2, 2)))); + OwningMaybe>( + std::vector(2, 2))); EXPECT_TRUE(copy_maybe_1); EXPECT_TRUE(copy_temp_maybe); @@ -279,8 +302,7 @@ GTEST_TEST(ReferencingMaybeTest, ConstructMoveAndAssign) // Test copy and move constructors. ReferencingMaybe copy_maybe(maybe_1); - ReferencingMaybe copy_temp_maybe( - std::move(ReferencingMaybe(value_2))); + ReferencingMaybe copy_temp_maybe(ReferencingMaybe{value_2}); EXPECT_EQ(copy_maybe.Value(), value_1); EXPECT_EQ(copy_temp_maybe.Value(), value_2); diff --git a/test/planning_test.cpp b/test/planning_test.cpp index 498845a..9a31e79 100644 --- a/test/planning_test.cpp +++ b/test/planning_test.cpp @@ -467,11 +467,10 @@ GTEST_TEST(PlanningTest, Test) << "New roadmap nodes: " << loaded_roadmap.Size() << "\n" << "New roadmap binary size: " << load_buffer.size() << std::endl; - for (size_t idx = 0; idx < loaded_roadmap.Size(); idx++) + for (int64_t idx = 0; idx < loaded_roadmap.Size(); idx++) { - const auto& old_node = roadmap.GetNodeImmutable(static_cast(idx)); - const auto& new_node - = loaded_roadmap.GetNodeImmutable(static_cast(idx)); + const auto& old_node = roadmap.GetNodeImmutable(idx); + const auto& new_node = loaded_roadmap.GetNodeImmutable(idx); const Waypoint& old_waypoint = old_node.GetValueImmutable(); const Waypoint& new_waypoint = new_node.GetValueImmutable(); ASSERT_EQ(old_waypoint.first, new_waypoint.first); diff --git a/test/voxel_grid_test.cpp b/test/voxel_grid_test.cpp index ae862d0..3a2f44c 100644 --- a/test/voxel_grid_test.cpp +++ b/test/voxel_grid_test.cpp @@ -37,7 +37,7 @@ GTEST_TEST(VoxelGridTest, IndexLookup) = voxel_grid::VoxelGrid::Deserialize( buffer, 0, serialization::DeserializeMemcpyable).Value(); // Check the values - int32_t check_index = 0; + size_t check_index = 0; for (int64_t x_index = 0; x_index < read_grid.GetNumXCells(); x_index++) { for (int64_t y_index = 0; y_index < read_grid.GetNumYCells(); y_index++) @@ -88,7 +88,7 @@ GTEST_TEST(VoxelGridTest, LocationLookup) = voxel_grid::VoxelGrid::Deserialize( buffer, 0, serialization::DeserializeMemcpyable).Value(); // Check the values - int32_t check_index = 0; + size_t check_index = 0; for (double x_pos = -9.5; x_pos <= 9.5; x_pos += 1.0) { for (double y_pos = -9.5; y_pos <= 9.5; y_pos += 1.0) @@ -160,7 +160,7 @@ GTEST_TEST(VoxelGridTest, DshvgLookup) = voxel_grid::DynamicSpatialHashedVoxelGrid::Deserialize( buffer, 0, serialization::DeserializeMemcpyable).Value(); // Check the values - int32_t check_index = 0; + size_t check_index = 0; for (double x_pos = -9.5; x_pos <= 9.5; x_pos += 1.0) { for (double y_pos = -9.5; y_pos <= 9.5; y_pos += 1.0)