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};
169 .costmapReader = &costmapReaderPlugin->get(),
170 .virtualRobotReader = &virtualRobotReaderPlugin->get(),
171 .humanReader = &humanReaderPlugin->get(),
172 .laserScannerFeaturesReader = &laserScannerFeaturesReaderPlugin->get(),
175 sceneProvider.emplace(srv, params.sceneCfg);
182 if (not params.disableExecutor)
188 .rtUnitDynamicallyLoadLibraries = params.rtUnitDynamicallyLoadLibraries}));
197 navRemoteGui.emplace(remoteGui, *
this);
198 navRemoteGui->enable();
202 ARMARX_INFO <<
"Initialized. Will now respond to navigation requests.";
209 navRemoteGui->enable();
218 navRemoteGui->disable();
247 const std::string& callerId,
252 ARMARX_INFO <<
"Creating config for caller '" << callerId <<
"'";
265 memoryIntrospectors.emplace_back(
266 std::make_unique<server::MemoryIntrospector>(resultsWriterPlugin->get(), callerId));
269 memoryPublishers.emplace(
271 std::make_unique<server::MemoryPublisher>(
272 &resultsWriterPlugin->get(), &eventsWriterPlugin->get(), callerId));
275 if (executor.has_value())
277 executorPtr = &executor.value();
281 navigators.erase(callerId);
283 std::piecewise_construct,
284 std::forward_as_tuple(callerId),
285 std::forward_as_tuple(
290 .publisher = memoryPublishers.at(callerId).get(),
291 .introspector = introspector.has_value() ? &(introspector.value()) :
nullptr,
294 sceneProvider.has_value() ? &sceneProvider.value() :
nullptr}));
299 const std::string& navigationMode,
300 const std::string& callerId,
305 ARMARX_INFO <<
"moveTo() requested by caller '" << callerId <<
"' with navigation mode `"
306 << navigationMode <<
"`.";
308 <<
"Navigator config for caller `" << callerId <<
"` not registered!";
312 navigators.at(callerId).moveTo(
convert(waypoints),
321 <<
"Movement will be paused.";
329 const std::string& navigationMode,
330 const std::string& callerId,
331 const Ice::Current& )
335 ARMARX_INFO <<
"updateMoveTo() requested by caller '" << callerId <<
"'";
337 <<
"Navigator config for caller `" << callerId <<
"` not registered!";
339 const auto checkIsActiveNavigator = [&](
const std::string& callerId)
342 if (not activeNavigatorCallerId.has_value())
348 return activeNavigatorCallerId == callerId;
352 <<
"The navigator with id `" << callerId <<
"` is not active!";
354 navigators.at(callerId).update(
convert(waypoints),
360 const std::string& navigationMode,
361 const std::string& callerId,
362 const Ice::Current&
c)
368 const std::vector<client::WaypointTarget> wps =
client::defrost(waypoints);
371 <<
"Navigator config for caller `" << callerId <<
"` not registered!";
378 const std::string& callerId,
379 const Ice::Current&
c)
382 ARMARX_INFO <<
"MoveToLocation `" << location <<
"` requested by caller '" << callerId
386 <<
"Navigator config for caller `" << callerId <<
"` not registered!";
388 navigators.at(callerId).moveToLocation(location);
393 const std::string& navigationMode,
394 const std::string& callerId,
399 ARMARX_INFO <<
"MoveTowards requested by caller '" << callerId <<
"'";
402 <<
"Navigator config for caller `" << callerId <<
"` not registered!";
404 navigators.at(callerId).moveTowards(direction,
412 <<
"Navigator config for caller `" << configId <<
"` not registered!";
413 navigators.at(configId).pause();
421 <<
"Navigator config for caller `" << configId <<
"` not registered!";
422 navigators.at(configId).resume();
431 <<
"Navigator config for caller `" << configId <<
"` not registered!";
432 navigators.at(configId).stop();
437 memoryPublishers.at(configId)->userAbortTriggered(event);
446 for (
auto& [_, navigator] : navigators)
454 for (
auto& [callerId, memoryPublisher] : memoryPublishers)
456 memoryPublisher->userAbortTriggered(event);
465 <<
"Navigator config for caller `" << configId <<
"` not registered!";
466 return navigators.at(configId).isPaused();
473 return sceneProvider->scene();
481 <<
"Navigator config for caller `" << configId <<
"` not registered!";
482 return navigators.at(configId).isStopped();
499 def->component(remoteGui,
"RemoteGuiProvider");
504 def->optional(params.occupiedGridThreshold,
505 "p.occupancy_grid.occopied_threshold",
506 "Threshold for each cell to be considered occupied. Increase this value to "
509 def->optional(params.disableExecutor,
511 "If the executor is disabled, the navigator will only plan the trajectory "
512 "but won't execute it.");
514 def->required(params.sceneCfg.robotName,
"p.scene.robotName");
515 def->optional(params.sceneCfg.staticCostmapProviderName,
516 "p.scene.staticCostmapProviderName");
517 def->optional(params.sceneCfg.staticCostmapName,
"p.scene.staticCostmapName");
518 def->optional(params.sceneCfg.humanProviderName,
"p.scene.humanProviderName");
519 def->optional(params.sceneCfg.laserScannerFeaturesProviderName,
520 "p.scene.laserScannerFeaturesProviderName");
522 def->optional(params.rtUnitDynamicallyLoadLibraries,
523 "p.rtUnitDynamicallyLoadLibraries",
524 "If enabled, the controller library will automatically be loaded in the "
525 "RobotUnit. If disabled (desired), you have to make sure that the controller "
526 "library is listed in the RobotUnit's property `LoadLibraries`");
534 const auto cmap = simox::color::cmaps::viridis();
535 const float vmax = costmap.
getGrid().array().maxCoeff();
539 const auto color = cmap.at(
distance, 0.
F, vmax);
540 return {color.a, color.r, color.g, color.b};
543 auto layer = arviz.
layer(
"costmap_" + name);
545 const std::int64_t cols = costmap.
getGrid().cols();
546 const std::int64_t rows = costmap.
getGrid().rows();
550 std::vector<std::vector<Eigen::Vector3f>> vertices;
551 std::vector<std::vector<viz::data::Color>> colors;
553 for (
int r = 0; r < rows; r++)
555 auto& verticesRow = vertices.emplace_back(cols);
556 auto& colorsRow = colors.emplace_back(cols);
558 for (
int c = 0;
c < cols;
c++)
561 colorsRow.at(
c) = asColor(costmap.
getGrid()(r,
c));
565 mesh.grid2D(vertices, colors);
577 if (not navigatorId.has_value())
582 return &navigators.at(navigatorId.value());
585 std::optional<std::string>
590 const auto isActive = [](
const auto& nameNavPair) ->
bool
592 const auto& [name, navigator] = nameNavPair;
593 return not navigator.isPaused();
596 const auto it = std::find_if(navigators.begin(), navigators.end(), isActive);
599 if (it == navigators.end())