diff --git a/source/armarx/navigation/components/Navigator/Navigator.cpp b/source/armarx/navigation/components/Navigator/Navigator.cpp
index ddb4f8118acec40f2c0fc4f44d66a6972b9d42bd..3829c503878654192f09a422d670e0e90ecaedaa 100644
--- a/source/armarx/navigation/components/Navigator/Navigator.cpp
+++ b/source/armarx/navigation/components/Navigator/Navigator.cpp
@@ -218,23 +218,6 @@ 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;
- // }
-
void
components::Navigator::initializeScene()
{
@@ -494,141 +477,6 @@ namespace armarx::navigation::components
arviz.commit(layer);
}
- // core::StaticScene
- // components::Navigator::staticScene()
- // {
- // ARMARX_TRACE;
-
- // 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"});
-
- // 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);
-
- // algorithms::CostmapBuilder costmapBuilder(
- // scene().robot,
- // objects,
- // algorithms::Costmap::Parameters{.binaryGrid = false, .cellSize = 100},
- // "Platform-navigation-colmodel");
-
- // const auto costmap = costmapBuilder.create();
-
- // ARMARX_INFO << "Storing costmap in memory";
- // costmapWriterPlugin->get().store(
- // costmap, "distance_to_obstacles", getName(), armarx::Clock::Now());
-
- // ARMARX_INFO << "Done";
-
- // ARMARX_TRACE;
- // const auto grid = costmap.getGrid();
-
- // core::StaticScene scene{.objects = objects,
- // .costmap = std::make_unique<algorithms::Costmap>(costmap)};
-
- // ARMARX_INFO << "The object scene consists of " << scene().objects->getSize() << " objects";
-
-
- // // 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()});
-
- // 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";
-
- // scene().objects->addSceneObjects(occupancyGridSceneElements);
-
- // // 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()));
- // }
-
- // ARMARX_INFO << "Creating costmap";
-
- // algorithms::CostmapBuilder costmapBuilder(
- // getRobot(),
- // scene().objects,
- // algorithms::Costmap::Parameters{.binaryGrid = false, .cellSize = 100},
- // "Platform-navigation-colmodel");
-
- // const auto costmap = costmapBuilder.create();
-
- // ARMARX_INFO << "Done";
-
- // ARMARX_TRACE;
- // ARMARX_INFO << "Saving costmap.";
- // algorithms::save(costmap, "/tmp/navigation-costmap");
-
- // arviz.commit({layer});
- // }
- // else
- // {
- // ARMARX_INFO << "Occupancy grid not available";
- // }*/
-
- // return scene;
- // }
-
-
- // void
- // components::Navigator::updateRobot()
- // {
- // const auto timestamp = armem::Time::Now();
-
- // auto& virtualRobotReader = virtualRobotReaderPlugin->get();
-
- // 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());
-
- // // 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();
- // }
-
- // scene().platformVelocity->linear = platformState->twist.linear;
- // scene().platformVelocity->angular = platformState->twist.angular;
- // }
- // }
-
server::Navigator*
components::Navigator::activeNavigator()
{