diff --git a/source/armarx/navigation/algorithms/Costmap.cpp b/source/armarx/navigation/algorithms/Costmap.cpp
index 6e4b244896edd253cca6da33b22c5c41e62fa2de..0a6c5a32c36fc4fa110f55bd037132e96b21b824 100644
--- a/source/armarx/navigation/algorithms/Costmap.cpp
+++ b/source/armarx/navigation/algorithms/Costmap.cpp
@@ -31,11 +31,17 @@
namespace armarx::navigation::algorithms
{
+
Costmap::Costmap(const Grid& grid,
const Parameters& parameters,
const SceneBounds& sceneBounds,
- const std::optional<Mask>& mask) :
- grid(grid), mask(mask), sceneBounds(sceneBounds), parameters(parameters)
+ const std::optional<Mask>& mask,
+ const core::Pose2D& origin) :
+ grid(grid),
+ mask(mask),
+ sceneBounds(sceneBounds),
+ parameters(parameters),
+ global_T_costmap(origin)
{
validateSizes();
}
@@ -51,15 +57,24 @@ namespace armarx::navigation::algorithms
}
Costmap::Position
- Costmap::toPosition(const Costmap::Index& index) const
+ Costmap::toPositionLocal(const Index& index) const
{
ARMARX_TRACE;
- Eigen::Vector2f pos;
- pos.x() = sceneBounds.min.x() + index.x() * parameters.cellSize + parameters.cellSize / 2;
- pos.y() = sceneBounds.min.y() + index.y() * parameters.cellSize + parameters.cellSize / 2;
+ Eigen::Vector2f posLocal;
+ posLocal.x() =
+ sceneBounds.min.x() + index.x() * parameters.cellSize + parameters.cellSize / 2;
+ posLocal.y() =
+ sceneBounds.min.y() + index.y() * parameters.cellSize + parameters.cellSize / 2;
- return pos;
+ return posLocal;
+ }
+
+ Costmap::Position
+ Costmap::toPositionGlobal(const Costmap::Index& index) const
+ {
+ const Costmap::Position costmap_P_pos = toPositionLocal(index);
+ return global_T_costmap * costmap_P_pos;
}
bool
@@ -85,12 +100,14 @@ namespace armarx::navigation::algorithms
Costmap::Vertex
- Costmap::toVertex(const Eigen::Vector2f& v) const
+ Costmap::toVertex(const Eigen::Vector2f& globalPosition) const
{
+ const auto localPosition = global_T_costmap.inverse() * globalPosition;
+
const float vX =
- (v.x() - parameters.cellSize / 2 - sceneBounds.min.x()) / parameters.cellSize;
+ (localPosition.x() - parameters.cellSize / 2 - sceneBounds.min.x()) / parameters.cellSize;
const float vY =
- (v.y() - parameters.cellSize / 2 - sceneBounds.min.y()) / parameters.cellSize;
+ (localPosition.y() - parameters.cellSize / 2 - sceneBounds.min.y()) / parameters.cellSize;
const int iX = std::round(vX - 0.01);
const int iY = std::round(vY - 0.01);
@@ -106,11 +123,11 @@ namespace armarx::navigation::algorithms
// ARMARX_CHECK_LESS_EQUAL(iX, grid.rows() - 1);
// ARMARX_CHECK_LESS_EQUAL(iY, grid.cols() - 1);
- return Vertex{.index = Index{iXSan, iYSan}, .position = v};
+ return Vertex{.index = Index{iXSan, iYSan}, .position = globalPosition};
}
const SceneBounds&
- Costmap::getSceneBounds() const noexcept
+ Costmap::getLocalSceneBounds() const noexcept
{
return sceneBounds;
}
@@ -155,7 +172,7 @@ namespace armarx::navigation::algorithms
}
}
- return {.value = minVal, .index = {row, col}, .position = toPosition({row, col})};
+ return {.value = minVal, .index = {row, col}, .position = toPositionGlobal({row, col})};
}
const Costmap::Parameters&
@@ -364,8 +381,8 @@ namespace armarx::navigation::algorithms
{
ARMARX_CHECK_EQUAL(costmaps.size() + 1, weights.size());
- const std::vector<float> costmapWeights(weights.begin(), weights.end()-1);
- ARMARX_CHECK_EQUAL(costmapWeights.size(), costmaps.size());
+ const std::vector<float> costmapWeights(weights.begin(), weights.end() - 1);
+ ARMARX_CHECK_EQUAL(costmapWeights.size(), costmaps.size());
ARMARX_INFO << "Merging into with weights " << weights;
@@ -378,7 +395,7 @@ namespace armarx::navigation::algorithms
for (int y = 0; y < mergedCostmap.getGrid().cols(); y++)
{
ARMARX_TRACE;
- const Costmap::Position position = mergedCostmap.toPosition(Index{x, y});
+ const Costmap::Position position = mergedCostmap.toPositionGlobal(Index{x, y});
// merge masks
for (const auto& costmap : costmaps)
@@ -391,7 +408,8 @@ namespace armarx::navigation::algorithms
if (mergedCostmap.isValid(Index{x, y}))
{
float newVal = 0;
- for (const auto& [weight, costmap] : ranges::views::zip(costmapWeights, costmaps))
+ for (const auto& [weight, costmap] :
+ ranges::views::zip(costmapWeights, costmaps))
{
const auto otherCostmapVal = costmap.value(position);
ARMARX_CHECK(otherCostmapVal.has_value());
@@ -401,7 +419,8 @@ namespace armarx::navigation::algorithms
newVal += weights.back() * mergedCostmap.grid(x, y);
mergedCostmap.grid(x, y) = newVal;
- }else
+ }
+ else
{
mergedCostmap.grid(x, y) = 0;
}
diff --git a/source/armarx/navigation/algorithms/Costmap.h b/source/armarx/navigation/algorithms/Costmap.h
index a59626f0f6d9daa0e2e8b1bbd6f7446223846842..c880109d486986c611c11095a9b8442918316a88 100644
--- a/source/armarx/navigation/algorithms/Costmap.h
+++ b/source/armarx/navigation/algorithms/Costmap.h
@@ -4,6 +4,7 @@
#include <Eigen/Core>
+#include <armarx/navigation/core/basic_types.h>
#include <armarx/navigation/algorithms/types.h>
namespace armarx::navigation::algorithms
@@ -36,7 +37,8 @@ namespace armarx::navigation::algorithms
Costmap(const Grid& grid,
const Parameters& parameters,
const SceneBounds& sceneBounds,
- const std::optional<Mask>& mask = std::nullopt);
+ const std::optional<Mask>& mask = std::nullopt,
+ const core::Pose2D& origin = core::Pose2D::Identity());
struct Vertex
{
@@ -44,8 +46,9 @@ namespace armarx::navigation::algorithms
Position position;
};
- Position toPosition(const Index& index) const;
- Vertex toVertex(const Position& v) const;
+ Position toPositionLocal(const Index& index) const;
+ Position toPositionGlobal(const Index& index) const;
+ Vertex toVertex(const Position& globalPosition) const;
const Grid& getGrid() const;
Grid& getMutableGrid(){ return grid; }
@@ -54,7 +57,8 @@ namespace armarx::navigation::algorithms
const Parameters& params() const noexcept;
- const SceneBounds& getSceneBounds() const noexcept;
+ // get the scene bounds in the costmap's base frame (aka 'origin')
+ const SceneBounds& getLocalSceneBounds() const noexcept;
struct Optimum
{
@@ -89,6 +93,16 @@ namespace armarx::navigation::algorithms
Costmap mergeInto(const std::vector<Costmap>& costmaps, const std::vector<float>& weights) const;
+ const core::Pose2D& origin() const
+ {
+ return global_T_costmap;
+ }
+
+ void setOrigin(const core::Pose2D& globalPose)
+ {
+ global_T_costmap = globalPose;
+ }
+
private:
void validateSizes() const;
@@ -98,6 +112,8 @@ namespace armarx::navigation::algorithms
const SceneBounds sceneBounds;
const Parameters parameters;
+
+ core::Pose2D global_T_costmap = core::Pose2D::Identity();
};
diff --git a/source/armarx/navigation/algorithms/CostmapBuilder.cpp b/source/armarx/navigation/algorithms/CostmapBuilder.cpp
index 47ec3ef47a45acd13e189675c64f71538b18ab2a..6321398e3f5b198852fd9ba7df475e653a011c05 100644
--- a/source/armarx/navigation/algorithms/CostmapBuilder.cpp
+++ b/source/armarx/navigation/algorithms/CostmapBuilder.cpp
@@ -219,7 +219,7 @@ namespace armarx::navigation::algorithms
for (unsigned int y = 0; y < c_y; y++)
{
const Costmap::Index index{x, y};
- const Costmap::Position position = costmap.toPosition(index);
+ const Costmap::Position position = costmap.toPositionGlobal(index);
costmap.grid(x, y) = computeCost(position, collisionRobot, robotCollisionModel);
}
diff --git a/source/armarx/navigation/algorithms/aron_conversions.cpp b/source/armarx/navigation/algorithms/aron_conversions.cpp
index 24cfb351e6e91e4320e876a880f7d477a0aa9e0b..155ae33296bce7073c845a7c28793fe5d09f020d 100644
--- a/source/armarx/navigation/algorithms/aron_conversions.cpp
+++ b/source/armarx/navigation/algorithms/aron_conversions.cpp
@@ -46,7 +46,8 @@ namespace armarx::navigation::algorithms
dto.cellSize = bo.params().cellSize;
dto.frame = armarx::GlobalFrame;
- dto.origin = conv::to3D(bo.getSceneBounds().min);
+ // FIXME: integrate costmap origin
+ dto.origin = conv::to3D(bo.getLocalSceneBounds().min);
auto arn = dto.toAron();
arn->addElement("mask", toAron(bo.getMask()));
diff --git a/source/armarx/navigation/algorithms/astar/AStarPlanner.cpp b/source/armarx/navigation/algorithms/astar/AStarPlanner.cpp
index e211e635dceeb0f5040d8321fdc19bf9d1ae8787..5d3c7ad0d354e7fc9323f17a86cd6bec6fc313d5 100644
--- a/source/armarx/navigation/algorithms/astar/AStarPlanner.cpp
+++ b/source/armarx/navigation/algorithms/astar/AStarPlanner.cpp
@@ -74,7 +74,7 @@ namespace armarx::navigation::algorithm::astar
{
for (size_t c = 0; c < cols; c++)
{
- const auto pos = costmap.toPosition({r, c});
+ const auto pos = costmap.toPositionGlobal({r, c});
const float obstacleDistance = costmap.getGrid()(r, c);
grid[r][c] = std::make_shared<Node>(pos, obstacleDistance);
diff --git a/source/armarx/navigation/algorithms/persistence.cpp b/source/armarx/navigation/algorithms/persistence.cpp
index e0f9713285c9df3992fd619acd32a0fb19039914..7b55a46efc652ede1c85aa5012c574d33ca7077f 100644
--- a/source/armarx/navigation/algorithms/persistence.cpp
+++ b/source/armarx/navigation/algorithms/persistence.cpp
@@ -119,10 +119,10 @@ namespace armarx::navigation::algorithms
// scene bounds
{
auto& jSceneBounds = j["scene_bounds"];
- jSceneBounds["min"] = std::vector<float>{costmap.getSceneBounds().min.x(),
- costmap.getSceneBounds().min.y()};
- jSceneBounds["max"] = std::vector<float>{costmap.getSceneBounds().max.x(),
- costmap.getSceneBounds().max.y()};
+ jSceneBounds["min"] = std::vector<float>{costmap.getLocalSceneBounds().min.x(),
+ costmap.getLocalSceneBounds().min.y()};
+ jSceneBounds["max"] = std::vector<float>{costmap.getLocalSceneBounds().max.x(),
+ costmap.getLocalSceneBounds().max.y()};
}
// grid
diff --git a/source/armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.cpp b/source/armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.cpp
index cbf80d2b664475113e50dcb2d3b9f4af116dc7bc..f0064691b02cb1fd18713592a81bd09bc6f37851 100644
--- a/source/armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.cpp
+++ b/source/armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.cpp
@@ -118,7 +118,7 @@ namespace armarx::navigation::algorithms::spfa
break;
}
- path.push_back(costmap.toPosition(parent.array()));
+ path.push_back(costmap.toPositionGlobal(parent.array()));
pt = &parent;
}
diff --git a/source/armarx/navigation/algorithms/util.cpp b/source/armarx/navigation/algorithms/util.cpp
index 0b3956df956e7fcda1789cc62176edb686cffa8e..2918d94b55b3784c905a4e9eaeec5ed13e95ac7c 100644
--- a/source/armarx/navigation/algorithms/util.cpp
+++ b/source/armarx/navigation/algorithms/util.cpp
@@ -214,10 +214,10 @@ namespace armarx::navigation::algorithms
return mergeAligned(costmaps, weights);
}
- const auto grid = CostmapBuilder::createUniformGrid(costmaps.front().getSceneBounds(),
+ const auto grid = CostmapBuilder::createUniformGrid(costmaps.front().getLocalSceneBounds(),
costmaps.front().params());
- Costmap mergedCostmap(grid, costmaps.front().params(), costmaps.front().getSceneBounds());
+ Costmap mergedCostmap(grid, costmaps.front().params(), costmaps.front().getLocalSceneBounds());
const auto addMode = [&]() -> Costmap::AddMode
{
@@ -254,10 +254,10 @@ namespace armarx::navigation::algorithms
checkSameSize(costmaps);
- const auto grid = CostmapBuilder::createUniformGrid(costmaps.front().getSceneBounds(),
+ const auto grid = CostmapBuilder::createUniformGrid(costmaps.front().getLocalSceneBounds(),
costmaps.front().params());
- Costmap mergedCostmap(grid, costmaps.front().params(), costmaps.front().getSceneBounds());
+ Costmap mergedCostmap(grid, costmaps.front().params(), costmaps.front().getLocalSceneBounds());
// foreach pair (costmap, weight): add it to there merged costmap
ranges::for_each(ranges::views::zip(costmaps, weights),
@@ -323,7 +323,7 @@ namespace armarx::navigation::algorithms
return {scaledGrid,
Costmap::Parameters{.binaryGrid = false, .cellSize = cellSize},
- costmap.getSceneBounds(),
+ costmap.getLocalSceneBounds(),
scaledMask};
}
diff --git a/source/armarx/navigation/components/Navigator/Navigator.cpp b/source/armarx/navigation/components/Navigator/Navigator.cpp
index 890ffc259c094f90e8398051aa68ddb815b22911..10f866548c0a334d30cf94f3f844a1dbd5d468bf 100644
--- a/source/armarx/navigation/components/Navigator/Navigator.cpp
+++ b/source/armarx/navigation/components/Navigator/Navigator.cpp
@@ -505,7 +505,7 @@ namespace armarx::navigation::components
for (int c = 0; c < cols; c++)
{
- verticesRow.at(c) = conv::to3D(costmap.toPosition({r, c}));
+ verticesRow.at(c) = conv::to3D(costmap.toPositionGlobal({r, c}));
colorsRow.at(c) = asColor(costmap.getGrid()(r, c));
}
}
diff --git a/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.cpp b/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.cpp
index ba64540867445c73a4fa61208630e1fdce431f2a..9e034e13c78bc9de9eb81fa238438a2c5c6dd1b8 100644
--- a/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.cpp
+++ b/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.cpp
@@ -250,7 +250,7 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
for (int c = 0; c < costmap.getGrid().cols(); c++)
{
algorithms::Costmap::Index idx(r, c);
- const auto pos = costmap.toPosition(idx);
+ const auto pos = costmap.toPositionGlobal(idx);
// if(costmap.getMask().has_value())
// {
@@ -320,7 +320,7 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
for (int c = 0; c < cols; c++)
{
verticesRow.at(c) =
- armarx::navigation::conv::to3D(costmap.toPosition({r, c})) + heightOffsetV;
+ armarx::navigation::conv::to3D(costmap.toPositionGlobal({r, c})) + heightOffsetV;
colorsRow.at(c) = [&]()
{
@@ -423,7 +423,7 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
const armarx::navigation::algorithms::Costmap navigationPlanningCostmap(
result.distances,
obstacleDistancesCostmap.params(),
- obstacleDistancesCostmap.getSceneBounds(),
+ obstacleDistancesCostmap.getLocalSceneBounds(),
result.reachable);
return navigationPlanningCostmap;
diff --git a/source/armarx/navigation/conversions/eigen.cpp b/source/armarx/navigation/conversions/eigen.cpp
index 164dd57cd84e81d48a94490b61c31d8e8a85ff95..8cbc14e6ccc9b88645ab1c04bc42db06f10dfc89 100644
--- a/source/armarx/navigation/conversions/eigen.cpp
+++ b/source/armarx/navigation/conversions/eigen.cpp
@@ -2,6 +2,8 @@
#include <algorithm>
+#include <SimoxUtility/math/convert/mat3f_to_rpy.h>
+
namespace armarx::navigation::conv
{
@@ -20,4 +22,19 @@ namespace armarx::navigation::conv
return v3;
}
+ core::Pose2D
+ to2D(const core::Pose& p3)
+ {
+ core::Pose2D p2 = armarx::navigation::core::Pose2D::Identity();
+ p2.translation() = p3.translation().head<2>();
+
+ const auto rotatedVec = p3.rotation() * Eigen::Vector3f::UnitX();
+ const float yaw = std::atan2(rotatedVec.y(), rotatedVec.x());
+
+ p2.linear() = Eigen::Rotation2Df(yaw).toRotationMatrix();
+
+ return p2;
+ }
+
+
} // namespace armarx::navigation::conv
diff --git a/source/armarx/navigation/conversions/eigen.h b/source/armarx/navigation/conversions/eigen.h
index 3c99d617417f287d7579c7fba41135024ee823d4..1e9071111f115478b7738c92df7588893ae95781 100644
--- a/source/armarx/navigation/conversions/eigen.h
+++ b/source/armarx/navigation/conversions/eigen.h
@@ -24,6 +24,7 @@
#include <vector>
+#include <armarx/navigation/core/basic_types.h>
#include <armarx/navigation/core/types.h>
namespace armarx::navigation::conv
@@ -35,6 +36,8 @@ namespace armarx::navigation::conv
return Eigen::Vector2f{v2.x(), v2.y()};
}
+ core::Pose2D to2D(const core::Pose& p3);
+
inline Eigen::Vector3f
to3D(const Eigen::Vector2f& v2)
{