diff --git a/source/armarx/navigation/components/Navigator/Navigator.cpp b/source/armarx/navigation/components/Navigator/Navigator.cpp index 659a39aa3bc9dc9fc92a84b7655df3e99a6e982f..7090cb6ef73a6303e69290386e6b69b79b0a86ac 100644 --- a/source/armarx/navigation/components/Navigator/Navigator.cpp +++ b/source/armarx/navigation/components/Navigator/Navigator.cpp @@ -46,6 +46,7 @@ #include "ArmarXCore/core/exceptions/local/ExpressionException.h" #include "ArmarXCore/core/time/Clock.h" +#include "ArmarXCore/core/time/Duration.h" #include <ArmarXCore/core/Component.h> #include <ArmarXCore/core/exceptions/LocalException.h> #include <ArmarXCore/core/logging/LogSender.h> @@ -113,19 +114,22 @@ namespace armarx::navigation namespace armarx::navigation::components { - components::Navigator::Navigator() : - parameterizationReader(memoryNameSystem()), - parameterizationWriter(memoryNameSystem()), - eventsWriter(memoryNameSystem()), - resultsWriter(memoryNameSystem()), - graphReader(memoryNameSystem()), - // occupancyGridReader(memoryNameSystem()), - costmapWriter(memoryNameSystem()), - virtualRobotReader(memoryNameSystem()), - parameterizationService(¶meterizationReader, ¶meterizationWriter) + components::Navigator::Navigator() : parameterizationService(nullptr, nullptr) // publisher(&resultsWriter, &eventsWriter) { scene.timeServer = &timeServer; + + addPlugin(parameterizationReaderPlugin); + addPlugin(parameterizationWriterPlugin); + addPlugin(eventsWriterPlugin); + addPlugin(resultsWriterPlugin); + addPlugin(graphReaderPlugin); + addPlugin(costmapWriterPlugin); + + addPlugin(virtualRobotReaderPlugin); + + parameterizationService = server::MemoryParameterizationService( + ¶meterizationReaderPlugin->get(), ¶meterizationWriterPlugin->get()); } @@ -151,16 +155,9 @@ namespace armarx::navigation::components return; } - // connect to all memories - ARMARX_TRACE; - resultsWriter.connect(); - eventsWriter.connect(); - // parameterizationReader.connect(); - parameterizationWriter.connect(); - graphReader.connect(); - // occupancyGridReader.connect(); - costmapWriter.connect(); - virtualRobotReader.connect(); + + virtualRobotReaderPlugin->get().setSyncTimeout(Duration::MilliSeconds(20)); + virtualRobotReaderPlugin->get().setSleepAfterSyncFailure(Duration::MilliSeconds(10)); // initialize scene ARMARX_TRACE; @@ -175,8 +172,11 @@ namespace armarx::navigation::components // TODO param (10) - robotStateUpdateTask = new PeriodicTask<Navigator>( - this, &Navigator::updateRobot, params.robotUpdatePeriodMs, false, "RobotStateUpdateTask"); + robotStateUpdateTask = new PeriodicTask<Navigator>(this, + &Navigator::updateRobot, + params.robotUpdatePeriodMs, + false, + "RobotStateUpdateTask"); robotStateUpdateTask->start(); navRemoteGui = std::make_unique<NavigatorRemoteGui>(remoteGui, *this); @@ -196,14 +196,6 @@ namespace armarx::navigation::components // TODO not in all cases meaningful //resume(); - resultsWriter.connect(); - eventsWriter.connect(); - // parameterizationReader.connect(); - parameterizationWriter.connect(); - graphReader.connect(); - virtualRobotReader.connect(); - // occupancyGridReader.connect(); - costmapWriter.connect(); navRemoteGui->enable(); } @@ -229,7 +221,11 @@ namespace armarx::navigation::components components::Navigator::getRobot() { ARMARX_TRACE; - auto robot = virtualRobotReader.getSynchronizedRobot(params.robotName, armarx::core::time::Clock::Now(), VirtualRobot::RobotIO::RobotDescription::eCollisionModel, true ); + auto robot = virtualRobotReaderPlugin->get().getSynchronizedRobot( + params.robotName, + armarx::core::time::Clock::Now(), + VirtualRobot::RobotIO::RobotDescription::eCollisionModel, + true); ARMARX_CHECK_NOT_NULL(robot); return robot; @@ -258,7 +254,7 @@ namespace armarx::navigation::components // TODO dynamic scene { std::lock_guard g{scene.graphMtx}; - scene.graph = core::SceneGraph{.subgraphs = graphReader.graphs()}; + scene.graph = core::SceneGraph{.subgraphs = graphReaderPlugin->get().graphs()}; } } @@ -282,12 +278,13 @@ namespace armarx::navigation::components server::NavigationStack stack = fac::NavigationStackFactory::create(stackConfig, scene); memoryIntrospectors.emplace_back( - std::make_unique<server::MemoryIntrospector>(resultsWriter, callerId)); + std::make_unique<server::MemoryIntrospector>(resultsWriterPlugin->get(), callerId)); // keep track of all memory publishers memoryPublishers.emplace( callerId, - std::make_unique<server::MemoryPublisher>(&resultsWriter, &eventsWriter, callerId)); + std::make_unique<server::MemoryPublisher>( + &resultsWriterPlugin->get(), &eventsWriterPlugin->get(), callerId)); navigators.emplace( std::piecewise_construct, @@ -466,15 +463,6 @@ namespace armarx::navigation::components "Threshold for each cell to be considered occupied. Increase this value to " "reduce noise."); - resultsWriter.registerPropertyDefinitions(def); - eventsWriter.registerPropertyDefinitions(def); - // parameterizationReader.registerPropertyDefinitions(def); - parameterizationWriter.registerPropertyDefinitions(def); - graphReader.registerPropertyDefinitions(def); - virtualRobotReader.registerPropertyDefinitions(def); - // occupancyGridReader.registerPropertyDefinitions(def); - costmapWriter.registerPropertyDefinitions(def); - return def; } @@ -545,7 +533,8 @@ namespace armarx::navigation::components const auto costmap = costmapBuilder.create(); ARMARX_INFO << "Storing costmap in memory"; - costmapWriter.store(costmap, "distance_to_obstacles", getName(), armarx::Clock::Now()); + costmapWriterPlugin->get().store( + costmap, "distance_to_obstacles", getName(), armarx::Clock::Now()); ARMARX_INFO << "Done"; @@ -639,17 +628,19 @@ namespace armarx::navigation::components } - 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(params.robotName, timestamp); + const auto robotDescription = + virtualRobotReader.queryDescription(params.robotName, timestamp); ARMARX_CHECK(robotDescription.has_value()); // synchronizing platform state diff --git a/source/armarx/navigation/components/Navigator/Navigator.h b/source/armarx/navigation/components/Navigator/Navigator.h index 5a7c32ec756a7b136253062e956a289a9fec0548..6fa6459d03cf26b18d4735adff071787ed6e7ec7 100644 --- a/source/armarx/navigation/components/Navigator/Navigator.h +++ b/source/armarx/navigation/components/Navigator/Navigator.h @@ -39,6 +39,7 @@ #include <ArmarXGui/interface/RemoteGuiInterface.h> #include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h> +#include "RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h" #include <RobotAPI/interface/units/PlatformUnitInterface.h> #include <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h> #include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h> @@ -82,8 +83,8 @@ namespace armarx::navigation::components virtual public Component, virtual public client::NavigatorInterface, virtual public ObjectPoseClientPluginUser, - virtual public ArVizComponentPluginUser, - virtual public armem::ListeningClientPluginUser + virtual public ArVizComponentPluginUser + // virtual public armem::ListeningClientPluginUser { public: @@ -187,17 +188,17 @@ namespace armarx::navigation::components std::vector<std::unique_ptr<server::MemoryIntrospector>> memoryIntrospectors; // `navigation` memory reader and writer - memory::client::param::Reader parameterizationReader; - memory::client::param::Writer parameterizationWriter; - memory::client::events::Writer eventsWriter; - memory::client::stack_result::Writer resultsWriter; - memory::client::graph::Reader graphReader; + 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::vision::occupancy_grid::client::Reader occupancyGridReader; - memory::client::costmap::Writer costmapWriter; // `robot_state` memory reader and writer std::optional<armem::robot::RobotDescription> robotDescription; - armem::robot_state::VirtualRobotReader virtualRobotReader; + armem::client::plugins::ReaderWriterPlugin<armem::robot_state::VirtualRobotReader>* virtualRobotReaderPlugin = nullptr; server::MemoryParameterizationService parameterizationService; diff --git a/source/armarx/navigation/memory/client/graph/Reader.cpp b/source/armarx/navigation/memory/client/graph/Reader.cpp index cb986d2db0149e9f84411018404df0226d990d7d..51a9b890c30a73ed0cddc2ed66a84cba80cfae36 100644 --- a/source/armarx/navigation/memory/client/graph/Reader.cpp +++ b/source/armarx/navigation/memory/client/graph/Reader.cpp @@ -196,6 +196,7 @@ namespace armarx::navigation::memory::client::graph // Wait for the memory to become available and add it as dependency. ARMARX_IMPORTANT << "Reader: Waiting for memory '" << navigation::graph::coreSegmentID.memoryName << "' ..."; + try { ARMARX_INFO << "graphs ..."; diff --git a/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.cpp b/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.cpp index 82a370638a43e0521826e42458142fdc4fd140a4..1997cb8fc631a6d360c04ab70e336541c0bf168f 100644 --- a/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.cpp +++ b/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.cpp @@ -93,9 +93,11 @@ namespace armarx::navigation::traj_ctrl core::Twist TrajectoryFollowingController::applyTwistLimits(core::Twist twist) { - core::Twist limits; - limits.linear = Eigen::Vector3f::Ones() * params.limits.linear; - limits.angular = Eigen::Vector3f::Ones() * params.limits.angular; + const core::Twist limits + { + .linear = Eigen::Vector3f::Ones() * params.limits.linear, + .angular = Eigen::Vector3f::Ones() * params.limits.angular + }; // for all entries, scale should be less than 1 const auto scalePos = twist.linear.cwiseAbs().cwiseQuotient(limits.linear.cwiseAbs()); @@ -113,12 +115,12 @@ namespace armarx::navigation::traj_ctrl twist.angular /= scaleMax; // pedantic checks - ARMARX_CHECK(std::abs(twist.linear.x()) <= params.limits.linear); - ARMARX_CHECK(std::abs(twist.linear.y()) <= params.limits.linear); - ARMARX_CHECK(std::abs(twist.linear.z()) <= params.limits.linear); - ARMARX_CHECK(std::abs(twist.angular.x()) <= params.limits.angular); - ARMARX_CHECK(std::abs(twist.angular.y()) <= params.limits.angular); - ARMARX_CHECK(std::abs(twist.angular.z()) <= params.limits.angular); + ARMARX_CHECK_LESS_EQUAL(std::abs(twist.linear.x()), params.limits.linear); + ARMARX_CHECK_LESS_EQUAL(std::abs(twist.linear.y()), params.limits.linear); + ARMARX_CHECK_LESS_EQUAL(std::abs(twist.linear.z()), params.limits.linear); + ARMARX_CHECK_LESS_EQUAL(std::abs(twist.angular.x()), params.limits.angular); + ARMARX_CHECK_LESS_EQUAL(std::abs(twist.angular.y()), params.limits.angular); + ARMARX_CHECK_LESS_EQUAL(std::abs(twist.angular.z()), params.limits.angular); return twist; }