Merge branch 'calderpg:master' into master

ToyotaResearchInstitute · Mar 22, 2022 · f59b15d · f59b15d
2 parents d4f5519 + 5d8c557
commit f59b15d
Show file tree

Hide file tree

Showing 4 changed files with 19 additions and 27 deletions.
diff --git a/include/common_robotics_utilities/dynamic_spatial_hashed_voxel_grid.hpp b/include/common_robotics_utilities/dynamic_spatial_hashed_voxel_grid.hpp
@@ -284,8 +284,7 @@ class DynamicSpatialHashedVoxelGridChunk
     }
   }
 
-  DynamicSpatialHashedVoxelGridChunk()
-      : fill_status_(DSHVGFillStatus::NOT_FILLED) {}
+  DynamicSpatialHashedVoxelGridChunk() = default;
 
   uint64_t SerializeSelf(
       std::vector<uint8_t>& buffer,
@@ -778,8 +777,6 @@ class DynamicSpatialHashedVoxelGridBase
     {
       origin_transform_ = origin_transform;
       inverse_origin_transform_ = origin_transform_.inverse();
-      utility::RequireEigenAlignment(origin_transform_);
-      utility::RequireEigenAlignment(inverse_origin_transform_);
       chunk_sizes_ = chunk_sizes;
       default_value_ = default_value;
       chunks_.reserve(expected_chunks);
@@ -791,14 +788,7 @@ class DynamicSpatialHashedVoxelGridBase
     }
   }
 
-  DynamicSpatialHashedVoxelGridBase()
-  {
-    origin_transform_ = Eigen::Isometry3d::Identity();
-    inverse_origin_transform_ = origin_transform_.inverse();
-    utility::RequireEigenAlignment(origin_transform_);
-    utility::RequireEigenAlignment(inverse_origin_transform_);
-    initialized_ = false;
-  }
+  DynamicSpatialHashedVoxelGridBase() = default;
 
   virtual ~DynamicSpatialHashedVoxelGridBase() {}
 

diff --git a/include/common_robotics_utilities/path_processing.hpp b/include/common_robotics_utilities/path_processing.hpp
@@ -1,8 +1,12 @@
 #pragma once
 
+#include <algorithm>
 #include <cmath>
 #include <cstdint>
 #include <functional>
+#include <iostream>
+#include <random>
+#include <stdexcept>
 #include <vector>
 
 #include <common_robotics_utilities/utility.hpp>

diff --git a/include/common_robotics_utilities/simple_task_planner.hpp b/include/common_robotics_utilities/simple_task_planner.hpp
@@ -476,6 +476,8 @@ Container PerformSingleTaskExecution(
     const std::function<void(
         const State&, const ActionPrimitiveSharedPtr<State, Container>&)>&
             user_pre_action_callback_fn = {},
+    const std::function<void(const Container&)>&
+        user_post_execution_callback = {},
     const std::function<void(const Container&, int64_t)>&
         user_post_outcome_callback_fn = {})
 {
@@ -537,9 +539,13 @@ Container PerformSingleTaskExecution(
 
     // Execute the primitive.
     num_primitive_executions++;
-    const auto primitive_outcomes = next_primitive->Execute(selected_outcome);
+    current_outcomes = next_primitive->Execute(selected_outcome);
 
-    current_outcomes = primitive_outcomes;
+    // Call the user-provided callback.
+    if (user_post_execution_callback)
+    {
+      user_post_execution_callback(current_outcomes);
+    }
   }
 
   return task_state_trace;
@@ -560,6 +566,8 @@ Container PerformSingleTaskExecution(
     const std::function<void(
         const State&, const ActionPrimitiveSharedPtr<State, Container>&)>&
             user_pre_action_callback_fn = {},
+    const std::function<void(const Container&)>&
+        user_post_execution_callback = {},
     const std::function<void(const Container&, int64_t)>&
         user_post_outcome_callback_fn = {})
 {
@@ -569,7 +577,7 @@ Container PerformSingleTaskExecution(
   return PerformSingleTaskExecution<State, Container, StateHash, StateEqual>(
       primitive_collection, task_sequence_complete_fn, start_states,
       max_primitive_executions, single_step, state_heuristic_fn, state_hasher,
-      state_equaler, user_pre_action_callback_fn,
+      state_equaler, user_pre_action_callback_fn, user_post_execution_callback,
       user_post_outcome_callback_fn);
 }
 }  // namespace simple_task_planner

diff --git a/include/common_robotics_utilities/voxel_grid.hpp b/include/common_robotics_utilities/voxel_grid.hpp
@@ -649,15 +649,7 @@ class VoxelGridBase
     Initialize(sizes, default_value, oob_value);
   }
 
-  VoxelGridBase()
-  {
-    origin_transform_ = Eigen::Isometry3d::Identity();
-    inverse_origin_transform_ = origin_transform_.inverse();
-    utility::RequireEigenAlignment(origin_transform_);
-    utility::RequireEigenAlignment(inverse_origin_transform_);
-    data_.clear();
-    initialized_ = false;
-  }
+  VoxelGridBase() = default;
 
   virtual ~VoxelGridBase() {}
 
@@ -668,13 +660,11 @@ class VoxelGridBase
   {
     if (sizes.Valid())
     {
-      sizes_ = sizes;
       origin_transform_ = origin_transform;
       inverse_origin_transform_ = origin_transform_.inverse();
-      utility::RequireEigenAlignment(origin_transform_);
-      utility::RequireEigenAlignment(inverse_origin_transform_);
       default_value_ = default_value;
       oob_value_ = oob_value;
+      sizes_ = sizes;
       SetContents(default_value_);
       initialized_ = true;
     }