Skip to content
Snippets Groups Projects

RT controller and simplification

Merged RT controller and simplification
chore/rt-controller-and-simplification into master
Merged Fabian Reister requested to merge chore/rt-controller-and-simplification into master
Viewing commit 1b34e305
Prev Next
Show latest version
1 file
+ 0
− 152
Compare changes
  • Side-by-side
  • Inline
source/armarx/navigation/components/Navigator/Navigator.cpp
+ 0
− 152
@@ -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()
{