formatting files; organizing includes

source/armarx/view_selection/components/person_target_provider/Component.cpp

@@ -22,27 +22,30 @@
 
 
 #include "Component.h"
-#include "ArmarXCore/core/logging/Logging.h"
-#include "ArmarXCore/core/time/Clock.h"
-#include "RobotAPI/libraries/core/FramedPose.h"
 
-#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
+#include <cstddef>
+
 #include <Eigen/Core>
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/time/Clock.h>
 #include <ArmarXCore/core/time/StopWatch.h>
-#include <cstddef>
+#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
+
+#include "RobotAPI/libraries/core/FramedPose.h"
 
 
 namespace armarx::view_selection::components::person_target_provider
 {
 
-    const std::string
-    Component::defaultName = "person_target_provider";
+    const std::string Component::defaultName = "person_target_provider";
 
 
     armarx::PropertyDefinitionsPtr
     Component::createPropertyDefinitions()
     {
-        armarx::PropertyDefinitionsPtr def = new armarx::ComponentPropertyDefinitions(getConfigIdentifier());
+        armarx::PropertyDefinitionsPtr def =
+            new armarx::ComponentPropertyDefinitions(getConfigIdentifier());
 
         // Publish to a topic (passing the TopicListenerPrx).
         // def->topic(myTopicListener);
@@ -62,7 +65,9 @@ namespace armarx::view_selection::components::person_target_provider
         // def->optional(properties.numBoxes, "p.box.Number", "Number of boxes to draw in ArViz.");
         def->optional(properties.providerName, "p.box.ProviderName", "OpenNIPointCloudProvider");
         def->optional(properties.objectProviderDynamic, "p.objectProviderDynamic", "");
-        def->optional(properties.trackTime, "p.trackTime", "Roughly the expected average tracking time of an object in seconds.");
+        def->optional(properties.trackTime,
+                      "p.trackTime",
+                      "Roughly the expected average tracking time of an object in seconds.");
         def->optional(properties.robotName, "p.robotName", "");
         return def;
     }
@@ -148,9 +153,10 @@ namespace armarx::view_selection::components::person_target_provider
         return std::nullopt;
     }
 
-    std::vector<GazeTargetWithFeatures> Component::getHumans() 
+    std::vector<GazeTargetWithFeatures>
+    Component::getHumans()
     {
-        armarx::armem::human::client::Reader::Query query {
+        armarx::armem::human::client::Reader::Query query{
             .providerName = "OpenNIPointCloudProvider",
             .timestamp = armarx::Clock::Now(),
             .maxAge = armarx::Duration::Seconds(3),
@@ -161,7 +167,8 @@ namespace armarx::view_selection::components::person_target_provider
         ARMARX_VERBOSE << result.humanPoses.size();
 
         std::vector<GazeTargetWithFeatures> gazeTargets(result.humanPoses.size());
-        for (size_t i = 0; i < result.humanPoses.size(); i++) {
+        for (size_t i = 0; i < result.humanPoses.size(); i++)
+        {
             auto human = result.humanPoses[i];
 
             // TODO check whether "Head" keypoint is available
@@ -172,10 +179,11 @@ namespace armarx::view_selection::components::person_target_provider
 
         return gazeTargets;
     }
-    
-    std::vector<GazeTargetWithFeatures> Component::getRelevantObjects() 
+
+    std::vector<GazeTargetWithFeatures>
+    Component::getRelevantObjects()
     {
-        
+
         const armarx::objpose::ObjectPoseSeq objectPosesDynamic =
             armarx::ObjectPoseClientPluginUser::getObjectPosesByProvider(
                 properties.objectProviderDynamic);
@@ -189,9 +197,11 @@ namespace armarx::view_selection::components::person_target_provider
         };
 
         std::vector<GazeTargetWithFeatures> gazeTargets;
-        for (size_t i = 0; i < objectPosesDynamic.size(); i++) {
+        for (size_t i = 0; i < objectPosesDynamic.size(); i++)
+        {
             auto object = objectPosesDynamic[i];
-            if (relevantObjects.find(object.objectID) != relevantObjects.end()) {
+            if (relevantObjects.find(object.objectID) != relevantObjects.end())
+            {
                 GazeTargetWithFeatures newTarget;
                 newTarget.position = Eigen::Isometry3f(object.objectPoseGlobal).translation();
                 newTarget.trackingName = object.objectID.str();
@@ -203,8 +213,11 @@ namespace armarx::view_selection::components::person_target_provider
         return gazeTargets;
     }
 
-    void Component::setTarget(const Eigen::Vector3f &chosenTarget) {
-        if (chosenTarget.norm() < 1e-5) {
+    void
+    Component::setTarget(const Eigen::Vector3f& chosenTarget)
+    {
+        if (chosenTarget.norm() < 1e-5)
+        {
             return;
         }
 
@@ -212,7 +225,8 @@ namespace armarx::view_selection::components::person_target_provider
         target.name = "tracking_target";
         target.position = armarx::FramedPosition(chosenTarget, GlobalFrame, robot->getName());
 
-        target.priority = gaze_targets::TargetPriority(gaze_targets::AttentionType::TaskDriven, 10'000);
+        target.priority =
+            gaze_targets::TargetPriority(gaze_targets::AttentionType::TaskDriven, 10'000);
         target.duration = armarx::Duration::Seconds(30);
         target.creationTimestamp = armarx::Clock::Now();
 
@@ -223,7 +237,8 @@ namespace armarx::view_selection::components::person_target_provider
     Component::updateHumanPoses()
     {
         // Get robot position
-        ARMARX_CHECK(virtualRobotReaderPlugin->get().synchronizeRobot(*robot, armarx::Clock::Now()));
+        ARMARX_CHECK(
+            virtualRobotReaderPlugin->get().synchronizeRobot(*robot, armarx::Clock::Now()));
         Eigen::Vector3f globalPose = robot->getRootNode()->getGlobalPosition();
         ARMARX_VERBOSE << globalPose;
 
@@ -232,21 +247,22 @@ namespace armarx::view_selection::components::person_target_provider
         std::vector<GazeTargetWithFeatures> targets = getHumans();
         {
             std::vector<GazeTargetWithFeatures> additional_targets = getRelevantObjects();
-            targets.insert(
-                targets.end(),
-                std::make_move_iterator(additional_targets.begin()),
-                std::make_move_iterator(additional_targets.end())
-            );
+            targets.insert(targets.end(),
+                           std::make_move_iterator(additional_targets.begin()),
+                           std::make_move_iterator(additional_targets.end()));
         }
-    
+
         // Calculate hotness values, hotter means more likely to be tracked
         double totalHotness = 1e-5;
         double currentTargetHotness = 0;
         Eigen::Vector3f currentTargetPosition;
-        for (auto target : targets) {
-            ARMARX_VERBOSE << "Object ID '" << target.trackingName << "' with hotness " << target.calculateHotness(globalPose);
+        for (auto target : targets)
+        {
+            ARMARX_VERBOSE << "Object ID '" << target.trackingName << "' with hotness "
+                           << target.calculateHotness(globalPose);
             totalHotness += target.calculateHotness(globalPose);
-            if (currentTarget == target.trackingName) {
+            if (currentTarget == target.trackingName)
+            {
                 currentTargetHotness = target.calculateHotness(globalPose);
                 currentTargetPosition = target.position;
             }
@@ -254,14 +270,16 @@ namespace armarx::view_selection::components::person_target_provider
 
         // Decide if a new object is going to be tracked
         double lambda = properties.trackTime * 4 - 1;
-        if (lambda < 0) {
+        if (lambda < 0)
+        {
             ARMARX_WARNING << "Value for trackTime is too small, choose at least 0.25!";
             setTarget(currentTargetPosition);
             return;
         }
         double newObjectProbability = 1.0 / (1.0 + (currentTargetHotness / totalHotness * lambda));
         ARMARX_VERBOSE << "New target lambda " << lambda << " prob " << newObjectProbability;
-        if (((rand() % 10'000'000) / 10'000'000.0) > newObjectProbability) {
+        if (((rand() % 10'000'000) / 10'000'000.0) > newObjectProbability)
+        {
             setTarget(currentTargetPosition);
             return; // No new object is going to be tracked
         }
@@ -270,10 +288,13 @@ namespace armarx::view_selection::components::person_target_provider
         // Decide which Object should be tracked
         double randomValue = (rand() % 10'000'000) / 10'000'000.0;
         double cumulatedHotness = 1e-5; // Add a small delta for floating point issues
-        for (auto chosenTarget : targets) {
+        for (auto chosenTarget : targets)
+        {
             cumulatedHotness += chosenTarget.calculateHotness(globalPose) / totalHotness;
-            if (cumulatedHotness >= randomValue) {
-                ARMARX_INFO << "Chose " << chosenTarget.trackingName << " at " << chosenTarget.position;
+            if (cumulatedHotness >= randomValue)
+            {
+                ARMARX_INFO << "Chose " << chosenTarget.trackingName << " at "
+                            << chosenTarget.position;
                 // Set new Target
                 setTarget(chosenTarget.position);
                 currentTarget = chosenTarget.trackingName;
@@ -295,9 +316,8 @@ namespace armarx::view_selection::components::person_target_provider
     {
         return Component::defaultName;
     }
-    
-    Component::Component()
-        : viewSelection(memoryNameSystem(), getName())
+
+    Component::Component() : viewSelection(memoryNameSystem(), getName())
     {
         addPlugin(virtualRobotReaderPlugin);
         addPlugin(humanPoseReaderPlugin);
@@ -387,4 +407,4 @@ namespace armarx::view_selection::components::person_target_provider
 
     ARMARX_REGISTER_COMPONENT_EXECUTABLE(Component, Component::GetDefaultName());
 
-}  // namespace armarx::view_selection::components::person_target_provider
+} // namespace armarx::view_selection::components::person_target_provider

source/armarx/view_selection/components/person_target_provider/Component.h

@@ -28,21 +28,21 @@
 
 #include <Eigen/Core>
 
-#include "ArmarXCore/core/services/tasks/TaskUtil.h"
 #include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/services/tasks/TaskUtil.h>
 
-#include "RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h"
-#include "RobotAPI/libraries/armem/client/forward_declarations.h"
-#include "RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h"
-#include "RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h"
+#include <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h>
+#include <RobotAPI/libraries/armem/client/forward_declarations.h>
 #include <RobotAPI/libraries/armem/client/plugins.h>
 #include <RobotAPI/libraries/armem/client/plugins/PluginUser.h>
+#include <RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h>
+#include <RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h>
 
 #include <VisionX/libraries/armem_human/client/HumanPoseReader.h>
 
-#include "Features.h"
 #include <armarx/view_selection/client/ViewSelection.h>
 #include <armarx/view_selection/components/person_target_provider/ComponentInterface.h>
+#include <armarx/view_selection/components/person_target_provider/Features.h>
 
 namespace armarx::view_selection::components::person_target_provider
 {