visualizing active target

source/armarx/view_selection/components/gaze_scheduler/CMakeLists.txt

@@ -12,11 +12,15 @@ armarx_add_component(gaze_scheduler
         Scheduler.h
     DEPENDENCIES#_PUBLIC
         ArmarXCore
+        # RobotAPI
         RobotAPICore
         RobotAPIComponentPlugins
+        armem_robot_state
+        # armarx_control
+        armarx_control::client
+        # this package
         armarx_view_selection::gaze_controller
         armarx_view_selection::gaze_targets
-        armarx_control::client
     # DEPENDENCIES_PRIVATE
         # ...
     # DEPENDENCIES_LEGACY_PUBLIC

source/armarx/view_selection/components/gaze_scheduler/Scheduler.cpp

@@ -26,6 +26,7 @@
 
 #include <chrono>
 #include <thread>
+#include <SimoxUtility/color/Color.h>
 
 #include <ArmarXCore/core/time/ice_conversions.h>
 #include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
@@ -36,6 +37,8 @@
 #include <RobotAPI/libraries/armem/core/wm/ice_conversions.h>
 #include <RobotAPI/libraries/aron/common/aron_conversions/core.h>
 
+#include <armarx/view_selection/gaze_targets/AttentionType.h>
+#include <armarx/view_selection/gaze_targets/TargetPriority.h>
 #include <armarx/view_selection/gaze_targets.h>
 #include <armarx/view_selection/gaze_controller/aron/GazeControllerConfig.aron.generated.h>
 #include <armarx/view_selection/memory/constants.h>
@@ -46,6 +49,11 @@ namespace armarx::view_selection::gaze_scheduler
 
     const std::string Scheduler::defaultName = memory::constants::GazeSchedulerProviderName;
 
+    Scheduler::Scheduler()
+    {
+        addPlugin(virtualRobotReaderPlugin);
+    }
+
     armarx::PropertyDefinitionsPtr
     Scheduler::createPropertyDefinitions()
     {
@@ -69,11 +77,11 @@ namespace armarx::view_selection::gaze_scheduler
                       "properties.controllerConfig.abortIfUnreachable",
                       "If true, the controller drops a target if it gets out of reach."
                       "If false, the controller stays at the jointlimits and keeps the target");
-        def->optional(
-            properties.robotName, "properties.RobotName", "Name of the robot for framedPosition.");
-        def->optional(properties.robotFrame,
-                      "properties.robotFrameName",
-                      "Name of the local robotFrame for framedPosition.");
+        // def->optional(
+        //     properties.robotName, "properties.RobotName", "Name of the robot for framedPosition.");
+        // def->optional(properties.robotFrame,
+        //               "properties.robotFrameName",
+        //               "Name of the local robotFrame for framedPosition.");
         def->optional(properties.defaultTarget,
                       "properties.defaultTarget",
                       "Local coordinates to Position where the robot looks in idle phases.");
@@ -98,8 +106,19 @@ namespace armarx::view_selection::gaze_scheduler
         robotUnit->loadLibFromPackage("armarx_view_selection",
                                       "libarmarx_view_selection_gaze_controller.so");
 
+        // obtain info about robot
+        const std::string robotName = getControlComponentPlugin()
+                                              .getRobotUnitPlugin()
+                                              .getRobotUnit()
+                                              ->getKinematicUnit()
+                                              ->getRobotName();
+
+        robot = virtualRobotReaderPlugin->get().getRobot(
+            robotName, armarx::DateTime::Invalid(), VirtualRobot::RobotIO::RobotDescription::eStructure);
+        ARMARX_CHECK_NOT_NULL(robot);
+
         //populate the robot
-        RobotNodeNames robotNodeNames = getRobotNodeNames(properties.robotName);
+        RobotNodeNames robotNodeNames = getRobotNodeNames(robotName);
 
         gaze_controller::Config config;
         config.params.yawNodeName = robotNodeNames.yawNodeName;
@@ -205,7 +224,7 @@ namespace armarx::view_selection::gaze_scheduler
     Scheduler::submitIdleTarget()
     {
         FramedPosition viewCenter(
-            properties.defaultTarget, properties.robotFrame, properties.robotName);
+            properties.defaultTarget, robot->getRootNode()->getName(), robot->getName());
         auto infinite = armarx::Duration::MilliSeconds(-1);
         gaze_targets::TargetPriority priority(gaze_targets::AttentionType::RandomEvent, 0.0);
         // this target has lowest priority and will never be removed from the queue
@@ -217,6 +236,29 @@ namespace armarx::view_selection::gaze_scheduler
         scheduleNextTarget();
     }
     
+    void Scheduler::visualizeActiveTarget(const gaze_targets::GazeTarget& target)
+    {
+        virtualRobotReaderPlugin->get().synchronizeRobot(*robot, armarx::Clock::Now());
+
+        const auto globalPosition = target.position.toGlobalEigen(robot);
+
+        std::map<gaze_targets::AttentionType, simox::Color> attentionTypeColor
+        {
+            {gaze_targets::AttentionType::RandomEvent, simox::Color::gray()},
+            {gaze_targets::AttentionType::StimulusDriven, simox::Color::blue()},
+            {gaze_targets::AttentionType::TaskDriven, simox::Color::red()}
+        };
+
+        // TODO include priority into coloring
+
+        auto l = arviz.layer("active_target");
+                l.add(armarx::viz::Sphere("target")
+                          .position(globalPosition)
+                          .radius(100)
+                          .color(attentionTypeColor.at(target.priority.attentionType)));
+        arviz.commit(l);
+    }
+    
     void Scheduler::updateControllerTarget(const gaze_targets::GazeTarget& gazeTarget)
     {
         ARMARX_INFO << "Sending new target to the controller " << gazeTarget;
@@ -329,6 +371,8 @@ namespace armarx::view_selection::gaze_scheduler
         activeTarget = true;
 
         updateControllerTarget(target);
+
+        visualizeActiveTarget(target);
     }
 
     void

source/armarx/view_selection/components/gaze_scheduler/Scheduler.h

@@ -28,6 +28,10 @@
 #include <ArmarXCore/core/Component.h>
 #include <ArmarXCore/core/time.h>
 
+#include "RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h"
+#include "RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h"
+#include "RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h"
+#include "RobotAPI/libraries/armem_robot/types.h"
 #include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h>
 #include <RobotAPI/libraries/armem/client/Reader.h>
 #include <RobotAPI/libraries/armem/client/Writer.h>
@@ -52,9 +56,12 @@ namespace armarx::view_selection::gaze_scheduler
         virtual public armarx::view_selection::gaze_scheduler::GazeSchedulerInterface,
         virtual public armarx::RobotUnitComponentPluginUser,
         virtual public armarx::armem::ListeningClientPluginUser,
-        virtual public armarx::control::client::ComponentPluginUser
+        virtual public armarx::control::client::ComponentPluginUser,
+        virtual public armarx::ArVizComponentPluginUser
     {
     public:
+        Scheduler();
+
         /// @see armarx::ManagedIceObject::getDefaultName()
         std::string getDefaultName() const override;
         /// Get the component's default name.
@@ -120,6 +127,8 @@ namespace armarx::view_selection::gaze_scheduler
         // targets are submitted
         void submitIdleTarget();
 
+        void visualizeActiveTarget(const gaze_targets::GazeTarget& target);
+
     private:
 
         void updateControllerTarget(const gaze_targets::GazeTarget& gazeTarget);
@@ -157,11 +166,14 @@ namespace armarx::view_selection::gaze_scheduler
                 GetDefaultName() + "_" + common::ControllerTypeNames.to_name(common::ControllerType::GazeController);
             ControllerConfigParams controllerConfig;
 
-            std::string robotName = "Armar6";
-            std::string robotFrame = "root";
             Eigen::Vector3f defaultTarget = Eigen::Vector3f(0, 1500, 1500);
         };
         Properties properties;
+
+        armem::client::plugins::ReaderWriterPlugin<armem::robot_state::VirtualRobotReader>*
+            virtualRobotReaderPlugin = nullptr;
+
+        VirtualRobot::RobotPtr robot = nullptr;
     };
 
 } // namespace armarx::view_selection::gaze_scheduler