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 Dec 18, 2021
2 parents aae030d + ced374c commit 17aabe9
Show file tree
Hide file tree
Showing 4 changed files with 140 additions and 109 deletions.
8 changes: 4 additions & 4 deletions include/common_robotics_utilities/print.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,7 @@ namespace common_robotics_utilities
{
namespace print
{
namespace detail
namespace internal
{
#if COMMON_ROBOTICS_UTILITIES__SUPPORTED_ROS_VERSION == 2
struct ROSMessagePrinter
Expand All @@ -48,7 +48,7 @@ struct GenericPrinter
return strm.str();
}
};
} // namespace detail
} // namespace internal

// Base template function for printing types
template <typename T>
Expand All @@ -61,9 +61,9 @@ inline std::string Print(const T& toprint,
#if COMMON_ROBOTICS_UTILITIES__SUPPORTED_ROS_VERSION == 2
using Printer = typename std::conditional<
rosidl_generator_traits::is_message<T>::value,
detail::ROSMessagePrinter, detail::GenericPrinter>::type;
internal::ROSMessagePrinter, internal::GenericPrinter>::type;
#else
using Printer = detail::GenericPrinter;
using Printer = internal::GenericPrinter;
#endif
return Printer::Print(toprint);
}
Expand Down
29 changes: 14 additions & 15 deletions include/common_robotics_utilities/simple_task_planner.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -53,8 +53,8 @@ class ActionPrimitiveInterface

/// Typedef of shared pointer to primitive.
template<typename State, typename Container=std::vector<State>>
using ActionPrimitivePtr
= std::shared_ptr<ActionPrimitiveInterface<State, Container>>;
using ActionPrimitiveSharedPtr =
std::shared_ptr<ActionPrimitiveInterface<State, Container>>;

/// Wrapper type to generate action primitive types from std::functions
/// Use this if you want to assemble primitives from a number of existing
Expand Down Expand Up @@ -114,10 +114,7 @@ template<typename State, typename Container=std::vector<State>>
class ActionPrimitiveCollection
{
public:
using ActionPrimitivePtrType = ActionPrimitivePtr<State, Container>;
using ActionPrimitivePtrTypeVector = std::vector<ActionPrimitivePtrType>;

ActionPrimitiveCollection(
explicit ActionPrimitiveCollection(
const LoggingFunction& logging_fn = MakeCoutLoggingFunction())
: logging_fn_(logging_fn) {}

Expand All @@ -134,7 +131,8 @@ class ActionPrimitiveCollection
}
}

void RegisterPrimitive(const ActionPrimitivePtrType& new_primitive)
void RegisterPrimitive(
const ActionPrimitiveSharedPtr<State, Container>& new_primitive)
{
for (const auto& primitive : primitives_)
{
Expand All @@ -152,7 +150,7 @@ class ActionPrimitiveCollection

void UnregisterPrimitivesByName(const std::unordered_set<std::string>& names)
{
ActionPrimitivePtrTypeVector primitives_to_keep;
std::vector<ActionPrimitiveSharedPtr<State, Container>> primitives_to_keep;
for (const auto& primitive : primitives_)
{
if (names.count(primitive->Name()) == 0)
Expand All @@ -174,10 +172,11 @@ class ActionPrimitiveCollection

void ClearPrimitives() { primitives_.clear(); }

const ActionPrimitivePtrTypeVector& Primitives() const { return primitives_; }
const std::vector<ActionPrimitiveSharedPtr<State, Container>>&
Primitives() const { return primitives_; }

private:
ActionPrimitivePtrTypeVector primitives_;
std::vector<ActionPrimitiveSharedPtr<State, Container>> primitives_;
LoggingFunction logging_fn_;
};

Expand Down Expand Up @@ -369,7 +368,7 @@ class NextPrimitiveToExecute
{
public:
NextPrimitiveToExecute(
const ActionPrimitivePtr<State, Container>& action_primitive,
const ActionPrimitiveSharedPtr<State, Container>& action_primitive,
const int64_t selected_outcome_index)
: action_primitive_(action_primitive),
selected_outcome_index_(selected_outcome_index)
Expand All @@ -383,15 +382,15 @@ class NextPrimitiveToExecute
explicit NextPrimitiveToExecute(const int64_t selected_outcome_index)
: NextPrimitiveToExecute({}, selected_outcome_index) {}

const ActionPrimitivePtr<State, Container>& ActionPrimitive() const
const ActionPrimitiveSharedPtr<State, Container>& ActionPrimitive() const
{
return action_primitive_;
}

int64_t SelectedOutcomeIndex() const { return selected_outcome_index_; }

private:
ActionPrimitivePtr<State, Container> action_primitive_;
ActionPrimitiveSharedPtr<State, Container> action_primitive_;
int64_t selected_outcome_index_ = -1;
};

Expand Down Expand Up @@ -475,7 +474,7 @@ Container PerformSingleTaskExecution(
const StateHash& state_hasher = StateHash(),
const StateEqual& state_equaler = StateEqual(),
const std::function<void(
const State&, const ActionPrimitivePtr<State, Container>&)>&
const State&, const ActionPrimitiveSharedPtr<State, Container>&)>&
user_pre_action_callback_fn = {},
const std::function<void(const Container&, int64_t)>&
user_post_outcome_callback_fn = {})
Expand Down Expand Up @@ -559,7 +558,7 @@ Container PerformSingleTaskExecution(
const StateHash& state_hasher = StateHash(),
const StateEqual& state_equaler = StateEqual(),
const std::function<void(
const State&, const ActionPrimitivePtr<State, Container>&)>&
const State&, const ActionPrimitiveSharedPtr<State, Container>&)>&
user_pre_action_callback_fn = {},
const std::function<void(const Container&, int64_t)>&
user_post_outcome_callback_fn = {})
Expand Down
210 changes: 122 additions & 88 deletions include/common_robotics_utilities/voxel_grid.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -72,28 +72,112 @@ class GridSizes
int64_t stride2_ = 0;
bool valid_ = false;

static void CheckPositiveValid(const double param)
static bool CheckPositiveValid(const double param)
{
if (param <= 0.0)
{
throw std::invalid_argument("param must be positive and non-zero");
}
if (std::isnan(param))
return (std::isfinite(param) && (param > 0.0));
}

static bool CheckPositiveValid(const int64_t param)
{
return (param > 0);
}

bool Initialize(
const double cell_x_size,
const double cell_y_size,
const double cell_z_size,
const double x_size,
const double y_size,
const double z_size)
{
// Safety check
if (CheckPositiveValid(cell_x_size) && CheckPositiveValid(cell_y_size) &&
CheckPositiveValid(cell_z_size) && CheckPositiveValid(x_size) &&
CheckPositiveValid(y_size) && CheckPositiveValid(z_size))
{
throw std::invalid_argument("param must not be NaN");
// Set
cell_x_size_ = cell_x_size;
cell_y_size_ = cell_y_size;
cell_z_size_ = cell_z_size;
inv_cell_x_size_ = 1.0 / cell_x_size_;
inv_cell_y_size_ = 1.0 / cell_y_size_;
inv_cell_z_size_ = 1.0 / cell_z_size_;
num_x_cells_
= static_cast<int64_t>(std::ceil(x_size / cell_x_size));
num_y_cells_
= static_cast<int64_t>(std::ceil(y_size / cell_y_size));
num_z_cells_
= static_cast<int64_t>(std::ceil(z_size / cell_z_size));
x_size_ = static_cast<double>(num_x_cells_) * cell_x_size_;
y_size_ = static_cast<double>(num_y_cells_) * cell_y_size_;
z_size_ = static_cast<double>(num_z_cells_) * cell_z_size_;
stride1_ = num_y_cells_ * num_z_cells_;
stride2_ = num_z_cells_;
valid_ = true;

return true;
}
if (std::isinf(param) != 0)
else
{
throw std::invalid_argument("param must not be INF");
return false;
}
}

static void CheckPositiveValid(const int64_t param)
bool Initialize(
const double cell_x_size,
const double cell_y_size,
const double cell_z_size,
const int64_t num_x_cells,
const int64_t num_y_cells,
const int64_t num_z_cells)
{
if (param <= 0)
// Safety check
if (CheckPositiveValid(cell_x_size) && CheckPositiveValid(cell_y_size) &&
CheckPositiveValid(cell_z_size) && CheckPositiveValid(num_x_cells) &&
CheckPositiveValid(num_y_cells) && CheckPositiveValid(num_z_cells))
{
throw std::invalid_argument("param must be positive and non-zero");
// Set
cell_x_size_ = cell_x_size;
cell_y_size_ = cell_y_size;
cell_z_size_ = cell_z_size;
inv_cell_x_size_ = 1.0 / cell_x_size_;
inv_cell_y_size_ = 1.0 / cell_y_size_;
inv_cell_z_size_ = 1.0 / cell_z_size_;
num_x_cells_ = num_x_cells;
num_y_cells_ = num_y_cells;
num_z_cells_ = num_z_cells;
x_size_ = static_cast<double>(num_x_cells_) * cell_x_size_;
y_size_ = static_cast<double>(num_y_cells_) * cell_y_size_;
z_size_ = static_cast<double>(num_z_cells_) * cell_z_size_;
stride1_ = num_y_cells_ * num_z_cells_;
stride2_ = num_z_cells_;
valid_ = true;

return true;
}
else
{
return false;
}
}

void DefaultInitialize()
{
cell_x_size_ = 0.0;
cell_y_size_ = 0.0;
cell_z_size_ = 0.0;
inv_cell_x_size_ = 0.0;
inv_cell_y_size_ = 0.0;
inv_cell_z_size_ = 0.0;
num_x_cells_ = 0;
num_y_cells_ = 0;
num_z_cells_ = 0;
x_size_ = 0.0;
y_size_ = 0.0;
z_size_ = 0.0;
stride1_ = 0.0;
stride2_ = 0.0;
valid_ = false;
}

public:
Expand Down Expand Up @@ -132,32 +216,14 @@ class GridSizes
const double y_size,
const double z_size)
{
// Safety check
CheckPositiveValid(cell_x_size);
CheckPositiveValid(cell_y_size);
CheckPositiveValid(cell_z_size);
CheckPositiveValid(x_size);
CheckPositiveValid(y_size);
CheckPositiveValid(z_size);
// Set
cell_x_size_ = cell_x_size;
cell_y_size_ = cell_y_size;
cell_z_size_ = cell_z_size;
inv_cell_x_size_ = 1.0 / cell_x_size_;
inv_cell_y_size_ = 1.0 / cell_y_size_;
inv_cell_z_size_ = 1.0 / cell_z_size_;
num_x_cells_
= static_cast<int64_t>(std::ceil(x_size / cell_x_size));
num_y_cells_
= static_cast<int64_t>(std::ceil(y_size / cell_y_size));
num_z_cells_
= static_cast<int64_t>(std::ceil(z_size / cell_z_size));
x_size_ = static_cast<double>(num_x_cells_) * cell_x_size_;
y_size_ = static_cast<double>(num_y_cells_) * cell_y_size_;
z_size_ = static_cast<double>(num_z_cells_) * cell_z_size_;
stride1_ = num_y_cells_ * num_z_cells_;
stride2_ = num_z_cells_;
valid_ = true;
const bool initialized = Initialize(
cell_x_size, cell_y_size, cell_z_size, x_size, y_size, z_size);

if (!initialized)
{
throw std::invalid_argument(
"All size parameters must be positive, non-zero, and finite");
}
}

GridSizes(const double cell_x_size,
Expand All @@ -167,50 +233,19 @@ class GridSizes
const int64_t num_y_cells,
const int64_t num_z_cells)
{
// Safety check
CheckPositiveValid(cell_x_size);
CheckPositiveValid(cell_y_size);
CheckPositiveValid(cell_z_size);
CheckPositiveValid(num_x_cells);
CheckPositiveValid(num_y_cells);
CheckPositiveValid(num_z_cells);
// Set
cell_x_size_ = cell_x_size;
cell_y_size_ = cell_y_size;
cell_z_size_ = cell_z_size;
inv_cell_x_size_ = 1.0 / cell_x_size_;
inv_cell_y_size_ = 1.0 / cell_y_size_;
inv_cell_z_size_ = 1.0 / cell_z_size_;
num_x_cells_ = num_x_cells;
num_y_cells_ = num_y_cells;
num_z_cells_ = num_z_cells;
x_size_ = static_cast<double>(num_x_cells_) * cell_x_size_;
y_size_ = static_cast<double>(num_y_cells_) * cell_y_size_;
z_size_ = static_cast<double>(num_z_cells_) * cell_z_size_;
stride1_ = num_y_cells_ * num_z_cells_;
stride2_ = num_z_cells_;
valid_ = true;
}

GridSizes()
{
cell_x_size_ = 0.0;
cell_y_size_ = 0.0;
cell_z_size_ = 0.0;
inv_cell_x_size_ = 0.0;
inv_cell_y_size_ = 0.0;
inv_cell_z_size_ = 0.0;
num_x_cells_ = 0;
num_y_cells_ = 0;
num_z_cells_ = 0;
x_size_ = 0.0;
y_size_ = 0.0;
z_size_ = 0.0;
stride1_ = 0.0;
stride2_ = 0.0;
valid_ = false;
const bool initialized = Initialize(
cell_x_size, cell_y_size, cell_z_size,
num_x_cells, num_y_cells, num_z_cells);

if (!initialized)
{
throw std::invalid_argument(
"All size parameters must be positive, non-zero, and finite");
}
}

GridSizes() { DefaultInitialize(); }

bool Valid() const { return valid_; }

bool UniformCellSize() const
Expand Down Expand Up @@ -268,16 +303,15 @@ class GridSizes
current_position);
const int64_t num_z_cells = num_z_cells_deserialized.Value();
current_position += num_z_cells_deserialized.BytesRead();
try
{
*this = GridSizes(cell_x_size, cell_y_size, cell_z_size,
num_x_cells, num_y_cells, num_z_cells);
}
catch (const std::invalid_argument&)

const bool initialized = Initialize(
cell_x_size, cell_y_size, cell_z_size,
num_x_cells, num_y_cells, num_z_cells);
if (!initialized)
{
// The only reason the constructor can fail is if the params are
// The only reason Initialize can fail is if the params are
// default-initialized or invalid. We make sure to zero and set invalid.
*this = GridSizes();
DefaultInitialize();
}
// Figure out how many bytes were read
const uint64_t bytes_read = current_position - starting_offset;
Expand Down
2 changes: 0 additions & 2 deletions test/task_planning_test.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -120,8 +120,6 @@ using PutInBoxStatesWithCosts = OutcomesWithCosts<PutInBoxState>;
using PutInBoxStateContainer = std::vector<PutInBoxState>;
using PutInBoxStatePrimitiveType =
ActionPrimitiveInterface<PutInBoxState, PutInBoxStateContainer>;
using PutInBoxStatePrimitivePtrType =
ActionPrimitivePtr<PutInBoxState, PutInBoxStateContainer>;
using PutInBoxStatePrimitiveCollection =
ActionPrimitiveCollection<PutInBoxState, PutInBoxStateContainer>;

Expand Down

0 comments on commit 17aabe9

Please sign in to comment.