aron_conversions.cpp
Go to the documentation of this file.
1 #include "aron_conversions.h"
2 
7 
9 #include <armarx/navigation/algorithms/aron/ShortestPathFasterAlgorithmParams.aron.generated.h>
11 
12 
14 {
15 
17  toAron(const Costmap::Grid& grid)
18  {
20  }
21 
22 
24  toAron(const std::optional<Costmap::Mask>& mask)
25  {
26  if (not mask.has_value())
27  {
28  return nullptr; // maybe other type?
29  }
30 
32  }
33 
34 
35  inline Costmap::Mask
37  {
39  Costmap::Mask::value_type>(nav);
40  }
41 
43  toAron(const Costmap& bo)
44  {
45  arondto::Costmap dto;
46 
47  dto.cellSize = bo.params().cellSize;
48 
49  dto.frame = armarx::GlobalFrame;
50  // FIXME: integrate costmap origin
51  dto.origin = conv::to3D(bo.getLocalSceneBounds().min);
52 
53  auto arn = dto.toAron();
54  arn->setElementCopy("mask", toAron(bo.getMask()));
55  arn->setElementCopy("grid", toAron(bo.getGrid()));
56 
57  return arn;
58  }
59 
60  Costmap
61  fromAron(const armem::wm::EntityInstance& entityInstance)
62  {
63  const auto aronDto = armem::tryCast<algorithms::arondto::Costmap>(entityInstance);
64  ARMARX_CHECK(aronDto) << "Failed casting to Costmap";
65  const auto& dto = *aronDto;
66 
67  ARMARX_DEBUG << entityInstance.data()->getAllKeys();
68 
69  const Costmap::Parameters parameters{.binaryGrid = false, .cellSize = dto.cellSize};
70 
71  ARMARX_DEBUG << "Converting grid";
72  const auto gridNavigator =
73  aron::data::NDArray::DynamicCast(entityInstance.data()->getElement("grid"));
74 
75  ARMARX_CHECK_NOT_NULL(gridNavigator);
76 
77  Costmap::Grid grid =
78  aron::converter::AronEigenConverter::ConvertToDynamicMatrix<Costmap::Grid::value_type>(
79  *gridNavigator);
80 
81  ARMARX_DEBUG << "Converting mask";
82  std::optional<Costmap::Mask> mask;
83 
84  if (const auto maskNavigator =
85  aron::data::NDArray::DynamicCast(entityInstance.data()->getElement("mask")))
86  {
88  Costmap::Mask::value_type>(*maskNavigator);
89  }
90 
91  const SceneBounds sceneBounds{
92  .min = dto.origin.head<2>(),
93  .max = dto.origin.head<2>() +
94  Eigen::Vector2f{grid.rows() * dto.cellSize, grid.cols() * dto.cellSize}};
95 
96  return {grid, parameters, sceneBounds, mask};
97  }
98 
99  void
100  toAron(arondto::ShortestPathFasterAlgorithmParams& dto,
102  {
103  dto.obstacleDistanceCosts = bo.obstacleDistanceCosts;
104  dto.obstacleMaxDistance = bo.obstacleMaxDistance;
105  dto.obstacleDistanceWeight = bo.obstacleDistanceWeight;
106  dto.obstacleCostExponent = bo.obstacleCostExponent;
107  }
108 
109 
110  void
111  fromAron(const arondto::ShortestPathFasterAlgorithmParams& dto,
113  {
114  bo.obstacleDistanceCosts = dto.obstacleDistanceCosts;
115  bo.obstacleMaxDistance = dto.obstacleMaxDistance;
116  bo.obstacleDistanceWeight = dto.obstacleDistanceWeight;
117  bo.obstacleCostExponent = dto.obstacleCostExponent;
118  }
119 
120 
121 } // namespace armarx::navigation::algorithms
armarx::navigation::algorithms::spfa::ShortestPathFasterAlgorithm::Parameters
Definition: ShortestPathFasterAlgorithm.h:36
armarx::armem::wm::EntityInstance
Client-side working entity instance.
Definition: memory_definitions.h:32
aron_conversions.h
armarx::aron::data::NDArray
Definition: NDArray.h:48
armarx::navigation::algorithms::fromAron
Costmap fromAron(const armem::wm::EntityInstance &entityInstance)
Definition: aron_conversions.cpp:61
ARMARX_CHECK_NOT_NULL
#define ARMARX_CHECK_NOT_NULL(ptr)
This macro evaluates whether ptr is not null and if it turns out to be false it will throw an Express...
Definition: ExpressionException.h:206
armarx::aron::data::converter::AronEigenConverter::ConvertFromMatrix
static data::NDArrayPtr ConvertFromMatrix(const Eigen::Matrix< T, Eigen::Dynamic, Eigen::Dynamic > &mat)
Definition: EigenConverter.h:151
armarx::GlobalFrame
const std::string GlobalFrame
Definition: FramedPose.h:62
armarx::max
std::vector< T > max(const std::vector< T > &v1, const std::vector< T > &v2)
Definition: VectorHelpers.h:267
armarx::navigation::algorithms
This file is part of ArmarX.
Definition: aron_conversions.cpp:13
armarx::aron::data::NDArrayPtr
std::shared_ptr< NDArray > NDArrayPtr
Definition: NDArray.h:46
armarx::navigation::algorithms::Costmap::Grid
Eigen::MatrixXf Grid
Definition: Costmap.h:48
armarx::navigation::algorithms::Costmap::Mask
Eigen::Matrix< bool, Eigen::Dynamic, Eigen::Dynamic > Mask
Definition: Costmap.h:51
ARMARX_CHECK
#define ARMARX_CHECK(expression)
Shortcut for ARMARX_CHECK_EXPRESSION.
Definition: ExpressionException.h:82
armarx::aron::data::detail::SpecializedVariantBase< data::dto::NDArray, NDArray >::DynamicCast
static PointerType DynamicCast(const VariantPtr &n)
Definition: SpecializedVariant.h:117
Costmap.h
armarx::navigation::algorithms::Costmap::Parameters::binaryGrid
bool binaryGrid
Definition: Costmap.h:24
FramedPose.h
armarx::navigation::algorithms::fronAron
Costmap::Mask fronAron(const aron::data::NDArray &nav)
Definition: aron_conversions.cpp:36
ARMARX_DEBUG
#define ARMARX_DEBUG
Definition: Logging.h:177
armarx::navigation::algorithms::toAron
aron::data::NDArrayPtr toAron(const Costmap::Grid &grid)
Definition: aron_conversions.cpp:17
armarx::aron::data::converter::AronEigenConverter::ConvertToDynamicMatrix
static Eigen::Matrix< T, Eigen::Dynamic, Eigen::Dynamic > ConvertToDynamicMatrix(const data::NDArray &nav)
Definition: EigenConverter.h:109
EigenConverter.h
armarx::navigation::algorithms::SceneBounds
Definition: types.h:29
memory_definitions.h
armarx::aron::data::DictPtr
std::shared_ptr< Dict > DictPtr
Definition: Dict.h:41
armarx::navigation::algorithms::SceneBounds::min
Eigen::Vector2f min
Definition: types.h:31
armarx::navigation::conv::to3D
std::vector< Eigen::Vector3f > to3D(const std::vector< Eigen::Vector2f > &v)
Definition: eigen.cpp:11
armarx::navigation::algorithms::Costmap::Parameters
Definition: Costmap.h:21
util.h
eigen.h
armarx::armem::base::EntityInstanceBase::data
const DataT & data() const
Definition: EntityInstanceBase.h:129
armarx::aron::bo
const std::optional< BoT > & bo
Definition: aron_conversions.h:168
armarx::navigation::algorithms::Costmap
Definition: Costmap.h:13