35 #include <Eigen/Geometry>
37 #include <opencv2/core/eigen.hpp>
38 #include <opencv2/imgcodecs.hpp>
40 #include <Ice/Current.h>
42 #include <SimoxUtility/color/ColorMap.h>
43 #include <SimoxUtility/color/cmaps/colormaps.h>
44 #include <SimoxUtility/math/convert/rpy_to_mat3f.h>
45 #include <VirtualRobot/MathTools.h>
46 #include <VirtualRobot/Robot.h>
78 #include <armarx/navigation/client/ice/NavigatorInterface.h>
100 std::vector<core::Pose>
101 convert(
const std::vector<Eigen::Matrix4f>& wps)
103 std::vector<core::Pose> p;
104 p.reserve(wps.size());
107 std::back_inserter(p),
108 [](
const auto& p) { return core::Pose(p); });
129 addPlugin(laserScannerFeaturesReaderPlugin);
134 ¶meterizationReaderPlugin->get(), ¶meterizationWriterPlugin->get());
150 static bool initialized{
false};
171 .costmapReader = &costmapReaderPlugin->get(),
172 .virtualRobotReader = &virtualRobotReaderPlugin->get(),
173 .humanReader = &humanReaderPlugin->get(),
174 .laserScannerFeaturesReader = &laserScannerFeaturesReaderPlugin->get(),
177 sceneProvider.emplace(srv, params.sceneCfg);
184 if (not params.disableExecutor)
190 .rtUnitDynamicallyLoadLibraries = params.rtUnitDynamicallyLoadLibraries});
199 navRemoteGui.emplace(remoteGui, *
this);
200 navRemoteGui->enable();
204 ARMARX_INFO <<
"Initialized. Will now respond to navigation requests.";
211 navRemoteGui->enable();
220 navRemoteGui->disable();
249 const std::string& callerId,
254 ARMARX_INFO <<
"Creating config for caller '" << callerId <<
"'";
267 memoryIntrospectors.emplace_back(
268 std::make_unique<server::MemoryIntrospector>(resultsWriterPlugin->get(), callerId));
271 memoryPublishers.emplace(
273 std::make_unique<server::MemoryPublisher>(
274 &resultsWriterPlugin->get(), &eventsWriterPlugin->get(), callerId));
277 if (executor.has_value())
279 executorPtr = &executor.value();
283 navigators.erase(callerId);
285 std::piecewise_construct,
286 std::forward_as_tuple(callerId),
287 std::forward_as_tuple(
292 .publisher = memoryPublishers.at(callerId).get(),
293 .introspector = introspector.has_value() ? &(introspector.value()) :
nullptr,
296 sceneProvider.has_value() ? &sceneProvider.value() :
nullptr}));
301 const std::string& navigationMode,
302 const std::string& callerId,
307 ARMARX_INFO <<
"moveTo() requested by caller '" << callerId <<
"' with navigation mode `"
308 << navigationMode <<
"`.";
310 <<
"Navigator config for caller `" << callerId <<
"` not registered!";
314 navigators.at(callerId).moveTo(
convert(waypoints),
323 <<
"Movement will be paused.";
331 const std::string& navigationMode,
332 const std::string& callerId,
333 const Ice::Current& )
337 ARMARX_INFO <<
"updateMoveTo() requested by caller '" << callerId <<
"'";
339 <<
"Navigator config for caller `" << callerId <<
"` not registered!";
341 const auto checkIsActiveNavigator = [&](
const std::string& callerId)
344 if (not activeNavigatorCallerId.has_value())
350 return activeNavigatorCallerId == callerId;
354 <<
"The navigator with id `" << callerId <<
"` is not active!";
356 navigators.at(callerId).update(
convert(waypoints),
362 const std::string& navigationMode,
363 const std::string& callerId,
364 const Ice::Current&
c)
370 const std::vector<client::WaypointTarget> wps =
client::defrost(waypoints);
373 <<
"Navigator config for caller `" << callerId <<
"` not registered!";
380 const std::string& callerId,
381 const Ice::Current&
c)
384 ARMARX_INFO <<
"MoveToLocation `" << location <<
"` requested by caller '" << callerId
388 <<
"Navigator config for caller `" << callerId <<
"` not registered!";
390 navigators.at(callerId).moveToLocation(location);
395 const std::string& navigationMode,
396 const std::string& callerId,
401 ARMARX_INFO <<
"MoveTowards requested by caller '" << callerId <<
"'";
404 <<
"Navigator config for caller `" << callerId <<
"` not registered!";
406 navigators.at(callerId).moveTowards(direction,
414 <<
"Navigator config for caller `" << configId <<
"` not registered!";
415 navigators.at(configId).pause();
423 <<
"Navigator config for caller `" << configId <<
"` not registered!";
424 navigators.at(configId).resume();
433 <<
"Navigator config for caller `" << configId <<
"` not registered!";
434 navigators.at(configId).stop();
439 memoryPublishers.at(configId)->userAbortTriggered(event);
448 for (
auto& [_, navigator] : navigators)
456 for (
auto& [callerId, memoryPublisher] : memoryPublishers)
458 memoryPublisher->userAbortTriggered(event);
467 <<
"Navigator config for caller `" << configId <<
"` not registered!";
468 return navigators.at(configId).isPaused();
475 return sceneProvider->scene();
483 <<
"Navigator config for caller `" << configId <<
"` not registered!";
484 return navigators.at(configId).isStopped();
501 def->component(remoteGui,
"RemoteGuiProvider");
506 def->optional(params.occupiedGridThreshold,
507 "p.occupancy_grid.occopied_threshold",
508 "Threshold for each cell to be considered occupied. Increase this value to "
511 def->optional(params.disableExecutor,
513 "If the executor is disabled, the navigator will only plan the trajectory "
514 "but won't execute it.");
516 def->required(params.sceneCfg.robotName,
"p.scene.robotName");
517 def->optional(params.sceneCfg.staticCostmapProviderName,
518 "p.scene.staticCostmapProviderName");
519 def->optional(params.sceneCfg.staticCostmapName,
"p.scene.staticCostmapName");
520 def->optional(params.sceneCfg.humanProviderName,
"p.scene.humanProviderName");
521 def->optional(params.sceneCfg.laserScannerFeaturesProviderName,
522 "p.scene.laserScannerFeaturesProviderName");
524 def->optional(params.rtUnitDynamicallyLoadLibraries,
525 "p.rtUnitDynamicallyLoadLibraries",
526 "If enabled, the controller library will automatically be loaded in the "
527 "RobotUnit. If disabled (desired), you have to make sure that the controller "
528 "library is listed in the RobotUnit's property `LoadLibraries`");
536 const auto cmap = simox::color::cmaps::viridis();
537 const float vmax = costmap.
getGrid().array().maxCoeff();
541 const auto color = cmap.at(
distance, 0.
F, vmax);
542 return {color.a, color.r, color.g, color.b};
545 auto layer = arviz.
layer(
"costmap_" + name);
547 const std::int64_t cols = costmap.
getGrid().cols();
548 const std::int64_t rows = costmap.
getGrid().rows();
552 std::vector<std::vector<Eigen::Vector3f>> vertices;
553 std::vector<std::vector<viz::data::Color>> colors;
555 for (
int r = 0; r < rows; r++)
557 auto& verticesRow = vertices.emplace_back(cols);
558 auto& colorsRow = colors.emplace_back(cols);
560 for (
int c = 0;
c < cols;
c++)
563 colorsRow.at(
c) = asColor(costmap.
getGrid()(r,
c));
567 mesh.grid2D(vertices, colors);
579 if (not navigatorId.has_value())
584 return &navigators.at(navigatorId.value());
587 std::optional<std::string>
592 const auto isActive = [](
const auto& nameNavPair) ->
bool
594 const auto& [name, navigator] = nameNavPair;
595 return not navigator.isPaused();
598 const auto it = std::find_if(navigators.begin(), navigators.end(), isActive);
601 if (it == navigators.end())