diff --git a/source/armarx/navigation/components/Navigator/Navigator.cpp b/source/armarx/navigation/components/Navigator/Navigator.cpp
index d47853a797eafa6b2270b226a105a1e5652e8d9b..26ae33d2adc6b0302559d5b8b1568491b62878d9 100644
--- a/source/armarx/navigation/components/Navigator/Navigator.cpp
+++ b/source/armarx/navigation/components/Navigator/Navigator.cpp
@@ -118,7 +118,7 @@ namespace armarx::navigation::components
components::Navigator::Navigator() : parameterizationService(nullptr, nullptr)
// publisher(&resultsWriter, &eventsWriter)
{
- // scene.timeServer = &timeServer;
+ // scene().timeServer = &timeServer;
addPlugin(parameterizationReaderPlugin);
addPlugin(parameterizationWriterPlugin);
@@ -126,7 +126,7 @@ namespace armarx::navigation::components
addPlugin(resultsWriterPlugin);
addPlugin(graphReaderPlugin);
addPlugin(costmapWriterPlugin);
-
+
addPlugin(virtualRobotReaderPlugin);
parameterizationService = server::MemoryParameterizationService(
@@ -160,25 +160,31 @@ namespace armarx::navigation::components
virtualRobotReaderPlugin->get().setSyncTimeout(Duration::MilliSeconds(20));
virtualRobotReaderPlugin->get().setSleepAfterSyncFailure(Duration::MilliSeconds(10));
- // initialize scene
+ // initialize scene provider
ARMARX_TRACE;
+ {
+ const server::scene_provider::SceneProvider::InjectedServices srv{
+ .graphReader = &graphReaderPlugin->get(),
+ .costmapReader = &costmapReaderPlugin->get(),
+ .virtualRobotReader = &virtualRobotReaderPlugin->get(),
+ .objectPoseClient = ObjectPoseClientPluginUser::getClient()};
+
+ const std::string robotName = getControlComponentPlugin()
+ .getRobotUnitPlugin()
+ .getRobotUnit()
+ ->getKinematicUnit()
+ ->getRobotName();
+
+ const server::scene_provider::SceneProvider::Config cfg{.robotName = robotName};
+ sceneProvider = server::scene_provider::SceneProvider(srv, cfg);
+ }
+
initializeScene();
executor.emplace(server::PlatformControllerExecutor(getControlComponentPlugin()));
- introspector = server::ArvizIntrospector(getArvizClient(), scene.robot);
+ introspector = server::ArvizIntrospector(getArvizClient(), scene().robot);
- // TODO dynamic scene
- // TODO memory
- // TODO param (10)
-
-
- // robotStateUpdateTask = new PeriodicTask<Navigator>(this,
- // &Navigator::updateRobot,
- // params.robotUpdatePeriodMs,
- // false,
- // "RobotStateUpdateTask");
- // robotStateUpdateTask->start();
navRemoteGui = std::make_unique<NavigatorRemoteGui>(remoteGui, *this);
navRemoteGui->enable();
@@ -192,12 +198,6 @@ namespace armarx::navigation::components
components::Navigator::onReconnectComponent()
{
ARMARX_TRACE;
-
- robotStateUpdateTask->start();
-
- // TODO not in all cases meaningful
- //resume();
-
navRemoteGui->enable();
}
@@ -206,10 +206,7 @@ namespace armarx::navigation::components
{
ARMARX_TRACE;
- robotStateUpdateTask->stop();
-
stopAll();
-
navRemoteGui->disable();
}
@@ -218,41 +215,28 @@ namespace armarx::navigation::components
{
}
- VirtualRobot::RobotPtr
- components::Navigator::getRobot()
- {
- ARMARX_TRACE;
- auto robot = virtualRobotReaderPlugin->get().getSynchronizedRobot(
- getControlComponentPlugin().getRobotUnitPlugin().getRobotUnit()->getKinematicUnit()->getRobotName(),
- VirtualRobot::RobotIO::RobotDescription::eCollisionModel,
- true);
-
- ARMARX_CHECK_NOT_NULL(robot);
- return robot;
- }
+ // VirtualRobot::RobotPtr
+ // components::Navigator::getRobot()
+ // {
+ // ARMARX_TRACE;
+ // auto robot = virtualRobotReaderPlugin->get().getSynchronizedRobot(
+ // getControlComponentPlugin()
+ // .getRobotUnitPlugin()
+ // .getRobotUnit()
+ // ->getKinematicUnit()
+ // ->getRobotName(),
+ // VirtualRobot::RobotIO::RobotDescription::eCollisionModel,
+ // true);
+
+ // ARMARX_CHECK_NOT_NULL(robot);
+ // return robot;
+ // }
void
components::Navigator::initializeScene()
{
ARMARX_TRACE;
-
- scene.robot = getRobot();
-
- {
- if (not scene.staticScene.has_value())
- {
- scene.staticScene = staticScene();
-
- // cv::Mat1f mat;
- // cv::eigen2cv(scene.staticScene->costmap->getGrid(), mat);
- // cv::imwrite("/tmp/grid.exr", mat);
- }
- }
-
- // TODO dynamic scene
- {
- scene.graph = core::SceneGraph{.subgraphs = graphReaderPlugin->get().graphs()};
- }
+ sceneProvider->initialize(armarx::Clock::Now());
}
std::string
@@ -270,9 +254,10 @@ namespace armarx::navigation::components
ARMARX_TRACE;
ARMARX_INFO << "Creating config for caller '" << callerId << "'";
- parameterizationService.store(stackConfig, callerId, timeServer.now());
+ parameterizationService.store(stackConfig, callerId, armarx::Clock::Now());
- server::NavigationStack stack = fac::NavigationStackFactory::create(stackConfig, scene);
+ server::NavigationStack stack =
+ fac::NavigationStackFactory::create(stackConfig, sceneProvider->scene());
memoryIntrospectors.emplace_back(
std::make_unique<server::MemoryIntrospector>(resultsWriterPlugin->get(), callerId));
@@ -288,12 +273,12 @@ namespace armarx::navigation::components
std::forward_as_tuple(callerId),
std::forward_as_tuple(
server::Navigator::Config{.stack = std::move(stack),
- .scene = &scene,
.general = server::Navigator::Config::General{}},
server::Navigator::InjectedServices{.executor = &executor.value(),
.publisher =
memoryPublishers.at(callerId).get(),
- .introspector = &(introspector.value())}));
+ .introspector = &(introspector.value()),
+ .sceneProvider = &sceneProvider.value()}));
}
void
@@ -341,7 +326,6 @@ namespace armarx::navigation::components
ARMARX_CHECK(navigators.count(callerId) > 0)
<< "Navigator config for caller `" << callerId << "` not registered!";
- visualizeSPFA();
navigators.at(callerId).moveTo(wps, core::NavigationFrameNames.from_name(navigationMode));
}
@@ -389,8 +373,8 @@ namespace armarx::navigation::components
navigators.at(configId).stop();
// emit UserAbortTriggered event
- const core::UserAbortTriggeredEvent event{{timeServer.now()},
- core::Pose(scene.robot->getGlobalPose())};
+ const core::UserAbortTriggeredEvent event{{armarx::Clock::Now()},
+ core::Pose(scene().robot->getGlobalPose())};
memoryPublishers.at(configId)->userAbortTriggered(event);
}
@@ -406,8 +390,8 @@ namespace armarx::navigation::components
}
// emit UserAbortTriggered event
- const core::UserAbortTriggeredEvent event{{timeServer.now()},
- core::Pose(scene.robot->getGlobalPose())};
+ const core::UserAbortTriggeredEvent event{{armarx::Clock::Now()},
+ core::Pose(scene().robot->getGlobalPose())};
for (auto& [callerId, memoryPublisher] : memoryPublishers)
{
memoryPublisher->userAbortTriggered(event);
@@ -423,6 +407,13 @@ namespace armarx::navigation::components
return navigators.at(configId).isPaused();
}
+ const core::Scene&
+ Navigator::scene() const
+ {
+ ARMARX_CHECK_NOT_NULL(sceneProvider);
+ return sceneProvider->scene();
+ }
+
bool
components::Navigator::isStopped(const std::string& configId, const Ice::Current&)
{
@@ -500,159 +491,140 @@ namespace armarx::navigation::components
arviz.commit(layer);
}
- core::StaticScene
- components::Navigator::staticScene()
- {
- ARMARX_TRACE;
+ // core::StaticScene
+ // components::Navigator::staticScene()
+ // {
+ // ARMARX_TRACE;
- const objpose::ObjectPoseSeq objectPoses = ObjectPoseClientPluginUser::getObjectPoses();
+ // const objpose::ObjectPoseSeq objectPoses = ObjectPoseClientPluginUser::getObjectPoses();
- // remove those objects that belong to an object dataset. the manipulation object / distance computation is broken
- const auto objectPosesStatic =
- armarx::navigation::util::filterObjects(objectPoses, {"KIT", "HOPE", "MDB", "YCB"});
+ // // remove those objects that belong to an object dataset. the manipulation object / distance computation is broken
+ // const auto objectPosesStatic =
+ // armarx::navigation::util::filterObjects(objectPoses, {"KIT", "HOPE", "MDB", "YCB"});
- const auto objects = armarx::navigation::util::asSceneObjects(objectPosesStatic);
- ARMARX_INFO << objects->getSize() << " objects in the scene";
+ // const auto objects = armarx::navigation::util::asSceneObjects(objectPosesStatic);
+ // ARMARX_INFO << objects->getSize() << " objects in the scene";
- ARMARX_INFO << "Creating costmap";
- ARMARX_CHECK_NOT_NULL(scene.robot);
+ // ARMARX_INFO << "Creating costmap";
+ // ARMARX_CHECK_NOT_NULL(scene().robot);
- algorithms::CostmapBuilder costmapBuilder(
- scene.robot,
- objects,
- algorithms::Costmap::Parameters{.binaryGrid = false, .cellSize = 100},
- "Platform-navigation-colmodel");
+ // algorithms::CostmapBuilder costmapBuilder(
+ // scene().robot,
+ // objects,
+ // algorithms::Costmap::Parameters{.binaryGrid = false, .cellSize = 100},
+ // "Platform-navigation-colmodel");
- const auto costmap = costmapBuilder.create();
+ // const auto costmap = costmapBuilder.create();
- ARMARX_INFO << "Storing costmap in memory";
- costmapWriterPlugin->get().store(
- costmap, "distance_to_obstacles", getName(), armarx::Clock::Now());
+ // ARMARX_INFO << "Storing costmap in memory";
+ // costmapWriterPlugin->get().store(
+ // costmap, "distance_to_obstacles", getName(), armarx::Clock::Now());
- ARMARX_INFO << "Done";
+ // ARMARX_INFO << "Done";
- ARMARX_TRACE;
- const auto grid = costmap.getGrid();
+ // ARMARX_TRACE;
+ // const auto grid = costmap.getGrid();
- core::StaticScene scene{.objects = objects,
- .costmap = std::make_unique<algorithms::Costmap>(costmap)};
+ // core::StaticScene scene{.objects = objects,
+ // .costmap = std::make_unique<algorithms::Costmap>(costmap)};
- ARMARX_INFO << "The object scene consists of " << scene.objects->getSize() << " objects";
+ // ARMARX_INFO << "The object scene consists of " << scene().objects->getSize() << " objects";
- visualize(costmap, arviz, "distance");
+ // // visualize(costmap, arviz, "distance");
- /*
- const armem::vision::occupancy_grid::client::Reader::Result result =
- occupancyGridReader.query(armem::vision::occupancy_grid::client::Reader::Query{
- .providerName = "CartographerMappingAndLocalization",
- .timestamp = armem::Time::Now()});
+ // /*
+ // const armem::vision::occupancy_grid::client::Reader::Result result =
+ // occupancyGridReader.query(armem::vision::occupancy_grid::client::Reader::Query{
+ // .providerName = "CartographerMappingAndLocalization",
+ // .timestamp = armem::Time::Now()});
- if (result and result.occupancyGrid.has_value() and false) // Feature disabled on master ...
- {
- ARMARX_INFO << "Occupancy grid available!";
+ // if (result and result.occupancyGrid.has_value() and false) // Feature disabled on master ...
+ // {
+ // ARMARX_INFO << "Occupancy grid available!";
- const auto occupancyGridSceneElements = util::asSceneObjects(
- result.occupancyGrid.value(),
- OccupancyGridHelper::Params{.freespaceThreshold = 0.45F,
- .occupiedThreshold = params.occupiedGridThreshold});
- ARMARX_INFO << occupancyGridSceneElements->getSize()
- << " scene elements from occupancy grid";
+ // const auto occupancyGridSceneElements = util::asSceneObjects(
+ // result.occupancyGrid.value(),
+ // OccupancyGridHelper::Params{.freespaceThreshold = 0.45F,
+ // .occupiedThreshold = params.occupiedGridThreshold});
+ // ARMARX_INFO << occupancyGridSceneElements->getSize()
+ // << " scene elements from occupancy grid";
- scene.objects->addSceneObjects(occupancyGridSceneElements);
+ // scene().objects->addSceneObjects(occupancyGridSceneElements);
- // draw
- auto layer = arviz.layer("occupancy_grid");
+ // // draw
+ // auto layer = arviz.layer("occupancy_grid");
- for (const auto& sceneObject : occupancyGridSceneElements->getSceneObjects())
- {
- const Eigen::Isometry3f world_T_obj(sceneObject->getGlobalPose());
- ARMARX_INFO << world_T_obj.translation();
- ARMARX_INFO << layer.size();
- layer.add(viz::Box("box_" + std::to_string(layer.size()))
- .pose(world_T_obj)
- .size(result.occupancyGrid->resolution)
- .color(viz::Color::orange()));
- }
+ // for (const auto& sceneObject : occupancyGridSceneElements->getSceneObjects())
+ // {
+ // const Eigen::Isometry3f world_T_obj(sceneObject->getGlobalPose());
+ // ARMARX_INFO << world_T_obj.translation();
+ // ARMARX_INFO << layer.size();
+ // layer.add(viz::Box("box_" + std::to_string(layer.size()))
+ // .pose(world_T_obj)
+ // .size(result.occupancyGrid->resolution)
+ // .color(viz::Color::orange()));
+ // }
- ARMARX_INFO << "Creating costmap";
+ // ARMARX_INFO << "Creating costmap";
- algorithms::CostmapBuilder costmapBuilder(
- getRobot(),
- scene.objects,
- algorithms::Costmap::Parameters{.binaryGrid = false, .cellSize = 100},
- "Platform-navigation-colmodel");
+ // algorithms::CostmapBuilder costmapBuilder(
+ // getRobot(),
+ // scene().objects,
+ // algorithms::Costmap::Parameters{.binaryGrid = false, .cellSize = 100},
+ // "Platform-navigation-colmodel");
- const auto costmap = costmapBuilder.create();
+ // const auto costmap = costmapBuilder.create();
- ARMARX_INFO << "Done";
+ // ARMARX_INFO << "Done";
- ARMARX_TRACE;
- ARMARX_INFO << "Saving costmap.";
- algorithms::save(costmap, "/tmp/navigation-costmap");
+ // ARMARX_TRACE;
+ // ARMARX_INFO << "Saving costmap.";
+ // algorithms::save(costmap, "/tmp/navigation-costmap");
- arviz.commit({layer});
- }
- else
- {
- ARMARX_INFO << "Occupancy grid not available";
- }*/
+ // arviz.commit({layer});
+ // }
+ // else
+ // {
+ // ARMARX_INFO << "Occupancy grid not available";
+ // }*/
- return scene;
- }
+ // return scene;
+ // }
- void
- components::Navigator::visualizeSPFA()
- {
- algorithms::spfa::ShortestPathFasterAlgorithm::Parameters spfaParams{
- .obstacleDistanceCosts = true};
- ARMARX_INFO << "spfa...";
- algorithms::spfa::ShortestPathFasterAlgorithm spfa(*scene.staticScene->costmap, spfaParams);
- const auto result123 = spfa.spfa(getRobot()->getGlobalPosition().head<2>());
- ARMARX_INFO << "spfa done...";
+ // void
+ // components::Navigator::updateRobot()
+ // {
+ // const auto timestamp = armem::Time::Now();
- const auto sceneBounds = algorithms::computeSceneBounds(scene.staticScene->objects);
+ // auto& virtualRobotReader = virtualRobotReaderPlugin->get();
- ARMARX_INFO << "Costmap SPFA";
- algorithms::Costmap cm(result123.distances, algorithms::Costmap::Parameters{}, sceneBounds);
- visualize(cm, arviz, "spfa");
- ARMARX_INFO << "Done.";
- }
+ // ARMARX_TRACE;
+ // ARMARX_CHECK(virtualRobotReader.synchronizeRobot(*scene().robot, timestamp));
+ // ARMARX_TRACE;
+ // const auto robotDescription =
+ // virtualRobotReader.queryDescription(scene().robot->getType(), timestamp);
+ // ARMARX_CHECK(robotDescription.has_value());
- void
- components::Navigator::updateRobot()
- {
- const auto timestamp = armem::Time::Now();
-
- auto& virtualRobotReader = virtualRobotReaderPlugin->get();
-
- ARMARX_TRACE;
- ARMARX_CHECK(virtualRobotReader.synchronizeRobot(*scene.robot, timestamp));
+ // // synchronizing platform state
+ // ARMARX_TRACE;
+ // const std::optional<armem::robot::PlatformState> platformState =
+ // virtualRobotReader.queryPlatformState(robotDescription.value(), armem::Time::Now());
+ // ARMARX_CHECK(platformState.has_value());
- ARMARX_TRACE;
- const auto robotDescription =
- virtualRobotReader.queryDescription(scene.robot->getType(), timestamp);
- ARMARX_CHECK(robotDescription.has_value());
-
- // synchronizing platform state
- ARMARX_TRACE;
- const std::optional<armem::robot::PlatformState> platformState =
- virtualRobotReader.queryPlatformState(robotDescription.value(), armem::Time::Now());
- ARMARX_CHECK(platformState.has_value());
+ // // TODO / FIXME make this thread safe!
+ // {
+ // if (not scene().platformVelocity.has_value())
+ // {
+ // scene().platformVelocity = core::Twist();
+ // }
- // TODO / FIXME make this thread safe!
- {
- if (not scene.platformVelocity.has_value())
- {
- scene.platformVelocity = core::Twist();
- }
-
- scene.platformVelocity->linear = platformState->twist.linear;
- scene.platformVelocity->angular = platformState->twist.angular;
- }
- }
+ // scene().platformVelocity->linear = platformState->twist.linear;
+ // scene().platformVelocity->angular = platformState->twist.angular;
+ // }
+ // }
server::Navigator*
components::Navigator::activeNavigator()
diff --git a/source/armarx/navigation/components/Navigator/Navigator.h b/source/armarx/navigation/components/Navigator/Navigator.h
index 6ce999004c4112f6ccf503b9f131349f576bd5a6..d1f37ccc77659299859e00ff0210f770ca0656e6 100644
--- a/source/armarx/navigation/components/Navigator/Navigator.h
+++ b/source/armarx/navigation/components/Navigator/Navigator.h
@@ -87,7 +87,7 @@ namespace armarx::navigation::components
virtual public ObjectPoseClientPluginUser,
virtual public ArVizComponentPluginUser,
virtual public armarx::control::client::ComponentPluginUser
- // virtual public armem::ListeningClientPluginUser
+ // virtual public armem::ListeningClientPluginUser
{
public:
@@ -131,11 +131,7 @@ namespace armarx::navigation::components
bool isStopped(const std::string& callerId,
const Ice::Current& c = Ice::emptyCurrent) override;
- const core::Scene&
- getScene() const
- {
- return scene;
- }
+ const core::Scene& scene() const;
protected:
/// @see PropertyUser::createPropertyDefinitions()
@@ -156,10 +152,10 @@ namespace armarx::navigation::components
void onReconnectComponent();
void initializeScene();
- core::StaticScene staticScene();
+ // core::StaticScene staticScene();
- [[nodiscard]] VirtualRobot::RobotPtr getRobot();
- void updateRobot();
+ // [[nodiscard]] VirtualRobot::RobotPtr getRobot();
+ // void updateRobot();
server::Navigator* activeNavigator();
@@ -168,16 +164,16 @@ namespace armarx::navigation::components
RemoteGuiInterfacePrx remoteGui;
- core::Scene scene;
+ // core::Scene scene;
std::optional<server::PlatformControllerExecutor> executor;
std::optional<server::ArvizIntrospector> introspector;
+ std::optional<server::scene_provider::SceneProvider> sceneProvider;
+
std::unordered_map<std::string, server::Navigator> navigators;
std::mutex propertiesMutex;
- PeriodicTask<Navigator>::pointer_type robotStateUpdateTask;
-
// TODO maybe as optional, but requires some effort
std::unique_ptr<NavigatorRemoteGui> navRemoteGui;
@@ -186,17 +182,26 @@ namespace armarx::navigation::components
std::vector<std::unique_ptr<server::MemoryIntrospector>> memoryIntrospectors;
// `navigation` memory reader and writer
- armem::client::plugins::ReaderWriterPlugin<memory::client::param::Reader>* parameterizationReaderPlugin = nullptr;
- armem::client::plugins::ReaderWriterPlugin<memory::client::param::Writer>* parameterizationWriterPlugin = nullptr;
- armem::client::plugins::ReaderWriterPlugin<memory::client::events::Writer>* eventsWriterPlugin = nullptr;
- armem::client::plugins::ReaderWriterPlugin<memory::client::stack_result::Writer>* resultsWriterPlugin = nullptr;
- armem::client::plugins::ReaderWriterPlugin<memory::client::graph::Reader>* graphReaderPlugin = nullptr;
- armem::client::plugins::ReaderWriterPlugin<memory::client::costmap::Writer>* costmapWriterPlugin = nullptr;
+ armem::client::plugins::ReaderWriterPlugin<memory::client::param::Reader>*
+ parameterizationReaderPlugin = nullptr;
+ armem::client::plugins::ReaderWriterPlugin<memory::client::param::Writer>*
+ parameterizationWriterPlugin = nullptr;
+ armem::client::plugins::ReaderWriterPlugin<memory::client::events::Writer>*
+ eventsWriterPlugin = nullptr;
+ armem::client::plugins::ReaderWriterPlugin<memory::client::stack_result::Writer>*
+ resultsWriterPlugin = nullptr;
+ armem::client::plugins::ReaderWriterPlugin<memory::client::graph::Reader>*
+ graphReaderPlugin = nullptr;
+ armem::client::plugins::ReaderWriterPlugin<memory::client::costmap::Reader>*
+ costmapReaderPlugin = nullptr;
+ armem::client::plugins::ReaderWriterPlugin<memory::client::costmap::Writer>*
+ costmapWriterPlugin = nullptr;
// armem::vision::occupancy_grid::client::Reader occupancyGridReader;
// `robot_state` memory reader and writer
std::optional<armem::robot::RobotDescription> robotDescription;
- armem::client::plugins::ReaderWriterPlugin<armem::robot_state::VirtualRobotReader>* virtualRobotReaderPlugin = nullptr;
+ armem::client::plugins::ReaderWriterPlugin<armem::robot_state::VirtualRobotReader>*
+ virtualRobotReaderPlugin = nullptr;
server::MemoryParameterizationService parameterizationService;
@@ -205,14 +210,9 @@ namespace armarx::navigation::components
// key is `callerId`
std::unordered_map<std::string, std::unique_ptr<server::MemoryPublisher>> memoryPublishers;
- core::ChronoMonotonicTimeServer timeServer;
-
-
struct Parameters
{
float occupiedGridThreshold{0.55F};
-
- int robotUpdatePeriodMs = 10;
};
Parameters params;