From 66e0811b5610013187b191681e4b282059443e62 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Tue, 4 Oct 2022 14:39:09 +0200
Subject: [PATCH] reducing verbosity
---
source/armarx/navigation/algorithms/Costmap.cpp | 8 ++------
.../navigation/algorithms/CostmapBuilder.cpp | 17 +++++++++--------
.../algorithms/astar/AStarPlanner.cpp | 6 +++---
.../navigation/algorithms/persistence.cpp | 2 +-
source/armarx/navigation/core/Trajectory.cpp | 4 ++--
.../armarx/navigation/global_planning/SPFA.cpp | 4 ++--
.../optimization/OrientationOptimizer.cpp | 10 +++++-----
.../local_planning/TimedElasticBands.cpp | 3 +++
8 files changed, 27 insertions(+), 27 deletions(-)
diff --git a/source/armarx/navigation/algorithms/Costmap.cpp b/source/armarx/navigation/algorithms/Costmap.cpp
index 0a6c5a32..5644c818 100644
--- a/source/armarx/navigation/algorithms/Costmap.cpp
+++ b/source/armarx/navigation/algorithms/Costmap.cpp
@@ -197,13 +197,11 @@ namespace armarx::navigation::algorithms
bool
Costmap::add(const Costmap& other, const float weight)
{
- ARMARX_INFO << "1";
// ensure that both grid and mask are the same size
ARMARX_TRACE;
validateSizes();
other.validateSizes();
- ARMARX_INFO << "1";
const auto startIdx = toVertex(other.sceneBounds.min);
@@ -213,7 +211,6 @@ namespace armarx::navigation::algorithms
// In this case, only add the part to this costmap that is possible.
const int rows = std::min(other.grid.rows(), grid.rows() - startIdx.index.x());
const int cols = std::min(other.grid.cols(), grid.cols() - startIdx.index.y());
- ARMARX_INFO << "1";
ARMARX_VERBOSE << "Adding other grid to region (" << startIdx.index.x() << ", "
<< startIdx.index.y() << "), "
@@ -221,7 +218,6 @@ namespace armarx::navigation::algorithms
<< ")";
ARMARX_TRACE;
- ARMARX_INFO << "1";
// add the costs of the other mask to this one by a weighting factor
grid.block(startIdx.index.x(), startIdx.index.y(), rows, cols).array() +=
@@ -369,7 +365,7 @@ namespace armarx::navigation::algorithms
std::optional<float>
Costmap::value(const Position& position) const
{
- ARMARX_INFO << "value ...";
+ ARMARX_DEBUG << "value ...";
const auto v = toVertex(position);
return value(v.index);
@@ -384,7 +380,7 @@ namespace armarx::navigation::algorithms
const std::vector<float> costmapWeights(weights.begin(), weights.end() - 1);
ARMARX_CHECK_EQUAL(costmapWeights.size(), costmaps.size());
- ARMARX_INFO << "Merging into with weights " << weights;
+ ARMARX_VERBOSE << "Merging into with weights " << weights;
Costmap mergedCostmap = *this;
diff --git a/source/armarx/navigation/algorithms/CostmapBuilder.cpp b/source/armarx/navigation/algorithms/CostmapBuilder.cpp
index 6321398e..3bd21a56 100644
--- a/source/armarx/navigation/algorithms/CostmapBuilder.cpp
+++ b/source/armarx/navigation/algorithms/CostmapBuilder.cpp
@@ -61,13 +61,13 @@ namespace armarx::navigation::algorithms
const auto sceneBounds = computeSceneBounds(obstacles);
const auto grid = createUniformGrid(sceneBounds, parameters);
- ARMARX_INFO << "Creating grid";
+ ARMARX_VERBOSE << "Creating grid";
Costmap costmap(grid, parameters, sceneBounds);
- ARMARX_INFO << "Filling grid with size (" << costmap.getGrid().rows() << "/"
- << costmap.getGrid().cols() << ")";
+ ARMARX_VERBOSE << "Filling grid with size (" << costmap.getGrid().rows() << "/"
+ << costmap.getGrid().cols() << ")";
fillGridCosts(costmap);
- ARMARX_INFO << "Filled grid";
+ ARMARX_VERBOSE << "Filled grid";
initializeMask(costmap);
@@ -79,7 +79,8 @@ namespace armarx::navigation::algorithms
{
costmap.mask = costmap.grid.array() > 0.F;
- ARMARX_INFO << "Initializing mask: Fraction of valid elements: " << costmap.mask->cast<float>().sum() / costmap.mask->size();
+ ARMARX_VERBOSE << "Initializing mask: Fraction of valid elements: "
+ << costmap.mask->cast<float>().sum() / costmap.mask->size();
}
Eigen::MatrixXf
@@ -88,15 +89,15 @@ namespace armarx::navigation::algorithms
{
ARMARX_TRACE;
- ARMARX_INFO << "Scene bounds are " << sceneBounds.min << " and " << sceneBounds.max;
+ ARMARX_VERBOSE << "Scene bounds are " << sceneBounds.min << " and " << sceneBounds.max;
//+1 for explicit rounding up
size_t c_x = (sceneBounds.max.x() - sceneBounds.min.x()) / parameters.cellSize + 1;
size_t c_y = (sceneBounds.max.y() - sceneBounds.min.y()) / parameters.cellSize + 1;
- ARMARX_INFO << "Grid size: " << c_x << ", " << c_y;
+ ARMARX_VERBOSE << "Grid size: " << c_x << ", " << c_y;
- ARMARX_INFO << "Resetting grid";
+ ARMARX_VERBOSE << "Resetting grid";
Eigen::MatrixXf grid(c_x, c_y);
grid.setZero();
diff --git a/source/armarx/navigation/algorithms/astar/AStarPlanner.cpp b/source/armarx/navigation/algorithms/astar/AStarPlanner.cpp
index 5d3c7ad0..9ce4afe6 100644
--- a/source/armarx/navigation/algorithms/astar/AStarPlanner.cpp
+++ b/source/armarx/navigation/algorithms/astar/AStarPlanner.cpp
@@ -63,7 +63,7 @@ namespace armarx::navigation::algorithm::astar
const size_t cols = costmap.getGrid().cols();
// create the grid with the correct size
- ARMARX_INFO << "Creating grid";
+ ARMARX_VERBOSE << "Creating grid";
for (size_t r = 0; r < rows; r++)
{
grid.push_back(std::vector<NodePtr>(cols));
@@ -81,7 +81,7 @@ namespace armarx::navigation::algorithm::astar
}
}
- ARMARX_INFO << "Creating graph";
+ ARMARX_VERBOSE << "Creating graph";
// Init successors
for (size_t r = 0; r < rows; r++)
@@ -118,7 +118,7 @@ namespace armarx::navigation::algorithm::astar
}
}
- ARMARX_INFO << "Done.";
+ ARMARX_VERBOSE << "Done.";
}
bool
diff --git a/source/armarx/navigation/algorithms/persistence.cpp b/source/armarx/navigation/algorithms/persistence.cpp
index a7c0e965..efcef663 100644
--- a/source/armarx/navigation/algorithms/persistence.cpp
+++ b/source/armarx/navigation/algorithms/persistence.cpp
@@ -105,7 +105,7 @@ namespace armarx::navigation::algorithms
if (not std::filesystem::exists(directory))
{
- ARMARX_INFO << "Creating directory `" << directory.string() << "`";
+ ARMARX_VERBOSE << "Creating directory `" << directory.string() << "`";
std::filesystem::create_directories(directory);
}
diff --git a/source/armarx/navigation/core/Trajectory.cpp b/source/armarx/navigation/core/Trajectory.cpp
index fc5c9b0b..2265c682 100644
--- a/source/armarx/navigation/core/Trajectory.cpp
+++ b/source/armarx/navigation/core/Trajectory.cpp
@@ -695,7 +695,7 @@ namespace armarx::navigation::core
{
const float posDiff =
(pts.at(i).waypoint.pose.translation() - referencePosition).norm();
- ARMARX_INFO << VAROUT(posDiff);
+ // ARMARX_INFO << VAROUT(posDiff);
return posDiff <= radius;
};
@@ -730,7 +730,7 @@ namespace armarx::navigation::core
}
}
- ARMARX_INFO << indices.size() << " points in range";
+ ARMARX_DEBUG << indices.size() << " points in range";
return indices;
}
diff --git a/source/armarx/navigation/global_planning/SPFA.cpp b/source/armarx/navigation/global_planning/SPFA.cpp
index 62b42068..9f309e54 100644
--- a/source/armarx/navigation/global_planning/SPFA.cpp
+++ b/source/armarx/navigation/global_planning/SPFA.cpp
@@ -119,8 +119,8 @@ namespace armarx::navigation::global_planning
}
const auto timeEnd = IceUtil::Time::now();
- ARMARX_IMPORTANT << "A* planning took " << (timeEnd - timeStart).toMilliSeconds() << " ms";
- ARMARX_IMPORTANT << "Path contains " << plan.path.size() << " points";
+ ARMARX_VERBOSE << "A* planning took " << (timeEnd - timeStart).toMilliSeconds() << " ms";
+ ARMARX_VERBOSE << "Path contains " << plan.path.size() << " points";
if (plan.path.size() < 2) // failure
{
diff --git a/source/armarx/navigation/global_planning/optimization/OrientationOptimizer.cpp b/source/armarx/navigation/global_planning/optimization/OrientationOptimizer.cpp
index d7ac3128..94a42162 100644
--- a/source/armarx/navigation/global_planning/optimization/OrientationOptimizer.cpp
+++ b/source/armarx/navigation/global_planning/optimization/OrientationOptimizer.cpp
@@ -72,7 +72,7 @@ namespace armarx::navigation::global_planning::optimization
orientations.at(i) = std::atan2(vCombined.y(), vCombined.x());
}
- ARMARX_INFO << "orientations before: " << orientations;
+ ARMARX_VERBOSE << "orientations before: " << orientations;
// const float yawStart = orientations.front();
// const float yawEnd = orientations.back();
@@ -83,7 +83,7 @@ namespace armarx::navigation::global_planning::optimization
ceres::Problem problem;
- ARMARX_INFO << orientations.size() - 2 << " orientations to optimize";
+ ARMARX_VERBOSE << orientations.size() - 2 << " orientations to optimize";
// TODO https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/slam/pose_graph_2d/pose_graph_2d.cc
// ceres::LocalParameterization* angle_local_parameterization =
@@ -199,7 +199,7 @@ namespace armarx::navigation::global_planning::optimization
ceres::Solver::Summary summary;
Solve(options, &problem, &summary);
- std::cout << summary.FullReport() << "\n";
+ ARMARX_VERBOSE << summary.FullReport() << "\n";
// std::cout << summary.BriefReport() << "\n";
const auto clampInPlace = [](auto& val)
@@ -207,8 +207,8 @@ namespace armarx::navigation::global_planning::optimization
std::for_each(orientations.begin(), orientations.end(), clampInPlace);
- ARMARX_INFO << "orientations after: " << orientations;
- ARMARX_INFO << "Optimization: " << summary.iterations.size() << " iterations";
+ ARMARX_VERBOSE << "orientations after: " << orientations;
+ ARMARX_VERBOSE << "Optimization: " << summary.iterations.size() << " iterations";
if (not summary.IsSolutionUsable())
{
diff --git a/source/armarx/navigation/local_planning/TimedElasticBands.cpp b/source/armarx/navigation/local_planning/TimedElasticBands.cpp
index 86f63c95..16f873a3 100644
--- a/source/armarx/navigation/local_planning/TimedElasticBands.cpp
+++ b/source/armarx/navigation/local_planning/TimedElasticBands.cpp
@@ -67,6 +67,8 @@ namespace armarx::navigation::local_planning
auto robot_model = boost::make_shared<teb_local_planner::CircularRobotFootprint>(
params.cfg.robot_footprint_radius);
+ ARMARX_VERBOSE << "Robot footprint is " << params.cfg.robot_footprint_radius << "m";
+
hcp_ = std::make_unique<teb_local_planner::HomotopyClassPlanner>();
hcp_->initialize(
cfg_, &teb_obstacles, robot_model, teb_local_planner::TebVisualizationPtr(), nullptr);
@@ -74,6 +76,7 @@ namespace armarx::navigation::local_planning
setTebCostmap();
if (teb_costmap)
{
+ ARMARX_VERBOSE << "Costmap available.";
// TODO: where to put all the parameters
const human::ExponentialPenaltyModel penalty{
.minDistance = 0.5, .epsilon = 0, .exponent = 1.2};
--
GitLab