diff --git a/source/armarx/navigation/components/navigator/Component.cpp b/source/armarx/navigation/components/navigator/Component.cpp
index 402c90e44c9fbd1f8f91a5166f94d05721931673..34d27ae7372d410a97cdab175d67543b0f4ad074 100644
--- a/source/armarx/navigation/components/navigator/Component.cpp
+++ b/source/armarx/navigation/components/navigator/Component.cpp
@@ -67,9 +67,6 @@
#include <RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
-#include <armarx/navigation/memory/client/costmap/Reader.h>
-#include <armarx/navigation/server/execution/ExecutorInterface.h>
-#include <armarx/navigation/server/scene_provider/SceneProvider.h>
#include <armarx/navigation/algorithms/Costmap.h>
#include <armarx/navigation/algorithms/CostmapBuilder.h>
#include <armarx/navigation/algorithms/astar/util.h>
@@ -84,9 +81,12 @@
#include <armarx/navigation/conversions/eigen.h>
#include <armarx/navigation/core/types.h>
#include <armarx/navigation/factories/NavigationStackFactory.h>
+#include <armarx/navigation/memory/client/costmap/Reader.h>
#include <armarx/navigation/server/Navigator.h>
#include <armarx/navigation/server/event_publishing/MemoryPublisher.h>
+#include <armarx/navigation/server/execution/ExecutorInterface.h>
#include <armarx/navigation/server/introspection/MemoryIntrospector.h>
+#include <armarx/navigation/server/scene_provider/SceneProvider.h>
#include <armarx/navigation/util/util.h>
namespace armarx::navigation::components::navigator
@@ -257,6 +257,9 @@ namespace armarx::navigation::components::navigator
server::NavigationStack stack =
fac::NavigationStackFactory::create(stackConfig, sceneProvider->scene());
+ // set visualization of LocalPlanner
+ stack.localPlanner->setVisualization(&getArvizClient());
+
memoryIntrospectors.emplace_back(
std::make_unique<server::MemoryIntrospector>(resultsWriterPlugin->get(), callerId));
@@ -278,12 +281,12 @@ namespace armarx::navigation::components::navigator
std::forward_as_tuple(
server::Navigator::Config{.stack = std::move(stack),
.general = server::Navigator::Config::General{}},
- server::Navigator::InjectedServices{.executor = executorPtr,
- .publisher =
- memoryPublishers.at(callerId).get(),
- .introspector = &(introspector.value()),
- .debugObserverHelper = &getDebugObserverComponentPlugin(),
- .sceneProvider = &sceneProvider.value()}));
+ server::Navigator::InjectedServices{
+ .executor = executorPtr,
+ .publisher = memoryPublishers.at(callerId).get(),
+ .introspector = &(introspector.value()),
+ .debugObserverHelper = &getDebugObserverComponentPlugin(),
+ .sceneProvider = &sceneProvider.value()}));
}
void
@@ -319,7 +322,7 @@ namespace armarx::navigation::components::navigator
Component::updateMoveTo(const std::vector<Eigen::Matrix4f>& waypoints,
const std::string& navigationMode,
const std::string& callerId,
- const Ice::Current& /*c*/)
+ const Ice::Current& /*c*/)
{
ARMARX_TRACE;
@@ -535,9 +538,9 @@ namespace armarx::navigation::components::navigator
server::Navigator*
Component::activeNavigator()
{
-
+
const auto navigatorId = activeNavigatorId();
- if(not navigatorId.has_value())
+ if (not navigatorId.has_value())
{
return nullptr;
}
diff --git a/source/armarx/navigation/local_planning/LocalPlanner.cpp b/source/armarx/navigation/local_planning/LocalPlanner.cpp
index b98a0a533ffea74451f9106414bfc69f937bd113..4b7ea7ed74a41cec3e7f01c31c320d77ade4a813 100644
--- a/source/armarx/navigation/local_planning/LocalPlanner.cpp
+++ b/source/armarx/navigation/local_planning/LocalPlanner.cpp
@@ -5,4 +5,10 @@ namespace armarx::navigation::local_planning
LocalPlanner::LocalPlanner(const core::Scene& context) : context(context)
{
}
+
+ void
+ LocalPlanner::setVisualization(viz::Client* vis)
+ {
+ arviz = vis;
+ }
} // namespace armarx::navigation::local_planning
diff --git a/source/armarx/navigation/local_planning/LocalPlanner.h b/source/armarx/navigation/local_planning/LocalPlanner.h
index 8c2554d67787c2611b2710d0121e8513b3711936..f2f6641b622a78101a4fd5892d0170ce764c4661 100644
--- a/source/armarx/navigation/local_planning/LocalPlanner.h
+++ b/source/armarx/navigation/local_planning/LocalPlanner.h
@@ -26,13 +26,14 @@
#include <VirtualRobot/VirtualRobot.h>
+#include <RobotAPI/components/ArViz/Client/ScopedClient.h>
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
-#include <armarx/navigation/local_planning/core.h>
#include <armarx/navigation/core/DynamicScene.h>
#include <armarx/navigation/core/StaticScene.h>
#include <armarx/navigation/core/Trajectory.h>
#include <armarx/navigation/core/types.h>
+#include <armarx/navigation/local_planning/core.h>
namespace armarx::navigation::local_planning
{
@@ -57,7 +58,11 @@ namespace armarx::navigation::local_planning
virtual std::optional<LocalPlannerResult> plan(const core::GlobalTrajectory& goal) = 0;
+ void setVisualization(viz::Client* vis);
+
protected:
+ viz::Client* arviz;
+
private:
const core::Scene& context;
};
diff --git a/source/armarx/navigation/local_planning/TimedElasticBands.cpp b/source/armarx/navigation/local_planning/TimedElasticBands.cpp
index b9a9d0b3abefe7ec13ac6b185feb2da0495c2627..2633583dafd120cf02f10839d8cc26b131b90aae 100644
--- a/source/armarx/navigation/local_planning/TimedElasticBands.cpp
+++ b/source/armarx/navigation/local_planning/TimedElasticBands.cpp
@@ -1,5 +1,6 @@
#include "TimedElasticBands.h"
+#include <SimoxUtility/algorithm/apply.hpp>
#include <SimoxUtility/json/json.hpp>
#include <VirtualRobot/MathTools.h>
#include <VirtualRobot/Robot.h>
@@ -128,15 +129,34 @@ namespace armarx::navigation::local_planning
core::LocalTrajectory best = conv::fromRos(hcp_->findBestTeb()->teb());
- return {{.trajectory = best}};
- }
+ // visualize path alternatives
+ if (arviz != nullptr)
+ {
+ auto layer = arviz->layer("local_planner_path_alternatives");
- void
- TimedElasticBands::setVisualization(viz::ScopedClient* vis)
- {
- arviz = vis;
- }
+ int i = 0;
+ int bestTebIdx = hcp_->bestTebIdx();
+ for (const auto& teb : hcp_->getTrajectoryContainer())
+ {
+ if (i == bestTebIdx)
+ {
+ continue;
+ }
+ const core::LocalTrajectory trajectory = conv::fromRos(teb->teb());
+ const std::vector<Eigen::Vector3f> points =
+ simox::alg::apply(trajectory.points(),
+ [](const core::LocalTrajectoryPoint& pt) -> Eigen::Vector3f
+ { return pt.pose.translation(); });
+ layer.add(viz::Path("path_alternative_" + std::to_string(i))
+ .points(points)
+ .color(simox::Color::gray()));
+ i++;
+ }
+ arviz->commit(layer);
+ }
+ return {{.trajectory = best}};
+ }
void
TimedElasticBands::fillObstacles()
@@ -165,7 +185,7 @@ namespace armarx::navigation::local_planning
ARMARX_CHECK(scene.dynamicScene.has_value());
for (const auto& obst : scene.dynamicScene.value().humans)
{
- obstManager.addHumanObstacle(obst, visPtr);
+ obstManager.addHumanObstacle(obst, visPtr);
}
}
diff --git a/source/armarx/navigation/local_planning/TimedElasticBands.h b/source/armarx/navigation/local_planning/TimedElasticBands.h
index 2bf54057cfd695c5c64501f6bb1aa97d81e9560a..9eba4d52739af83a79e0adcbc6cd8b0b19386b27 100644
--- a/source/armarx/navigation/local_planning/TimedElasticBands.h
+++ b/source/armarx/navigation/local_planning/TimedElasticBands.h
@@ -22,7 +22,6 @@
#pragma once
-#include <RobotAPI/components/ArViz/Client/ScopedClient.h>
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
#include "armarx/navigation/core/basic_types.h"
@@ -59,8 +58,6 @@ namespace armarx::navigation::local_planning
std::optional<LocalPlannerResult> plan(const core::GlobalTrajectory& goal) override;
- void setVisualization(viz::ScopedClient* vis);
-
private:
void readDefaultConfig(arondto::TimedElasticBandsParams& target);
void fillObstacles();
@@ -76,7 +73,5 @@ namespace armarx::navigation::local_planning
TebObstacleManager obstManager;
teb_local_planner::PoseSequence teb_globalPath;
std::unique_ptr<teb_local_planner::HomotopyClassPlanner> hcp_{nullptr};
-
- viz::ScopedClient* arviz;
};
} // namespace armarx::navigation::local_planning