dynamic_distance_to_obstacles_costmap_provider: computing navigation costs

source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/CMakeLists.txt

@@ -18,10 +18,13 @@ armarx_add_component(dynamic_distance_to_obstacle_costmap_provider
         ## ArmarXGuiComponentPlugins  # For RemoteGui plugin.
         # RobotAPI
         armem_vision
+        armem_robot_state
+        armem_robot
         ## RobotAPICore
         ## RobotAPIInterfaces
         RobotAPIComponentPlugins  # For ArViz and other plugins.
         armarx_navigation::memory
+        armarx_navigation::algorithms
     # DEPENDENCIES_LEGACY
         ## Add libraries that do not provide any targets but ${FOO_*} variables.
         # FOO

source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.cpp

@@ -39,9 +39,12 @@
 #include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
 
 #include "RobotAPI/interface/ArViz/Elements.h"
+#include "RobotAPI/libraries/armem_robot/types.h"
+#include "RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h"
 #include "RobotAPI/libraries/armem_vision/client/laser_scanner_features/Reader.h"
 
 #include "armarx/navigation/algorithms/Costmap.h"
+#include "armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.h"
 #include "armarx/navigation/memory/client/costmap/Reader.h"
 #include <armarx/navigation/conversions/eigen.h>
 
@@ -91,6 +94,7 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
 
 
     Component::Component() :
+        robotReader(memoryNameSystem()),
         costmapReader(memoryNameSystem()),
         costmapWriter(memoryNameSystem()),
         laserScannerFeaturesReader(memoryNameSystem())
@@ -139,6 +143,9 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
         */
 
         // connect memory readers and writers
+        ARMARX_INFO << "Connecting to robot state memory";
+        robotReader.connect();
+
         ARMARX_INFO << "Connecting to costmap segments";
         costmapReader.connect();
         costmapWriter.connect();
@@ -377,8 +384,49 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
 
         costmapWriter.store(dynamicCostmap, "dynamic_distance_to_obstacles", getName(), timestamp);
 
+        const auto timestamp1 = armarx::core::time::Clock::Now();
+        const auto navigationCostmap = computeNavigationCostmap(dynamicCostmap);
+        const auto timestamp2 = armarx::core::time::Clock::Now();
+
+        ARMARX_INFO << deactivateSpam(1) << "Navigation costmap. Computation took " << (timestamp2 - timestamp1).toMilliSecondsDouble() << "ms";
+
+
         // drawing
         drawCostmap(dynamicCostmap, "dynamic_costmap", 10);
+        drawCostmap(navigationCostmap, "navigation_costmap", 20);
+    }
+
+    algorithms::Costmap Component::computeNavigationCostmap(const algorithms::Costmap& obstacleDistancesCostmap)
+    {
+
+        const armem::robot::RobotDescription robotDescription{
+            .name = properties.robot.name
+        };
+
+        const auto globalPose = robotReader.queryGlobalPose(robotDescription, armarx::Clock::Now());
+        ARMARX_CHECK(globalPose.has_value());
+
+        const Eigen::Vector2f globalRobotPosition = globalPose->translation().head<2>();
+
+         // navigation costs
+        const armarx::navigation::algorithms::spfa::ShortestPathFasterAlgorithm::Parameters
+            spfaParams;
+        const armarx::navigation::algorithms::spfa::ShortestPathFasterAlgorithm spfa(
+            obstacleDistancesCostmap, spfaParams);
+
+        const armarx::navigation::algorithms::spfa::ShortestPathFasterAlgorithm::Result result =
+            spfa.spfa(globalRobotPosition);
+
+        ARMARX_VERBOSE << "max distance " << result.distances.maxCoeff();
+
+        // here, we again treat the result as a costmap
+        const armarx::navigation::algorithms::Costmap navigationPlanningCostmap(
+            result.distances,
+            obstacleDistancesCostmap.params(),
+            obstacleDistancesCostmap.getSceneBounds(),
+            result.reachable);
+
+        return navigationPlanningCostmap;
     }
 
 

source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.h

@@ -27,6 +27,7 @@
 #include <ArmarXCore/core/Component.h>
 
 #include "RobotAPI/libraries/armem/client/forward_declarations.h"
+#include "RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h"
 #include "RobotAPI/libraries/armem_vision/client/laser_scanner_features/Reader.h"
 #include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
 #include <RobotAPI/libraries/armem/client.h>
@@ -83,6 +84,9 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
                          const std::string& name,
                          float heightOffset);
 
+        algorithms::Costmap
+        computeNavigationCostmap(const algorithms::Costmap& obstacleDistancesCostmap);
+
 
         /* (Requires armarx::LightweightRemoteGuiComponentPluginUser.)
         /// This function should be called once in onConnect() or when you
@@ -130,6 +134,11 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
                 std::string name = ""; // all
             } laserScannerFeatures;
 
+            struct
+            {
+                std::string name = "Armar6";
+            } robot;
+
             int updatePeriodMs = 100;
         };
         Properties properties;
@@ -158,6 +167,8 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
         std::mutex arvizMutex;
         */
 
+        armem::robot_state::RobotReader robotReader;
+
         memory::client::costmap::Reader costmapReader;
         memory::client::costmap::Writer costmapWriter;