Merge branch 'feature/skills'

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

 armarx_add_component(view_selection_skill_provider
     ICE_FILES
-        # ComponentInterface.ice
+        ComponentInterface.ice
     ICE_DEPENDENCIES
         ArmarXCoreInterfaces
         RobotAPIInterfaces

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

@@ -22,13 +22,14 @@
 
 
 #include "Component.h"
+
+#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
+
+#include "armarx/view_selection/skills/LookAt.h"
 #include <armarx/view_selection/skills/LookForObject.h>
 #include <armarx/view_selection/skills/LookForObjectWithKinematicUnit.h>
 #include <armarx/view_selection/skills/ScanLocation.h>
 #include <armarx/view_selection/skills/constants.h>
-
-#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
-
 #include <armarx/view_selection/skills/skills.h>
 
 // Include headers you only need in function definitions in the .cpp.
@@ -40,14 +41,13 @@
 
 namespace armarx::view_selection::components::view_selection_skill_provider
 {
-    Component::Component()
+    Component::Component() : viewSelection(memoryNameSystem(), getName())
     {
         // addPlugin(virtualRobotReaderPlugin);
     }
 
-
-    const std::string Component::defaultName = view_selection::skills::constants::ViewSelectionSkillProviderName;
-
+    const std::string Component::defaultName =
+        view_selection::skills::constants::ViewSelectionSkillProviderName;
 
     armarx::PropertyDefinitionsPtr
     Component::createPropertyDefinitions()
@@ -74,7 +74,6 @@ namespace armarx::view_selection::components::view_selection_skill_provider
         return def;
     }
 
-
     void
     Component::onInitComponent()
     {
@@ -89,11 +88,11 @@ namespace armarx::view_selection::components::view_selection_skill_provider
         skills_.lookForObjectWithKinematicUnit = addSkill<skills::LookForObjectWithKinematicUnit>();
     }
 
-
     void
     Component::onConnectComponent()
     {
         // Do things after connecting to topics and components.
+        viewSelection.connect();
 
         {
             ARMARX_CHECK_NOT_NULL(skills_.scanLocation);
@@ -109,18 +108,22 @@ namespace armarx::view_selection::components::view_selection_skill_provider
             skills_.lookForObject->connect(srv);
         }
 
-         {
+        {
             ARMARX_CHECK_NOT_NULL(skills_.lookForObjectWithKinematicUnit);
 
             skills::LookForObjectWithKinematicUnit::Services srv{
                 .kinematicUnit = getRobotUnit()->getKinematicUnit(),
                 .objectPoseClient = getClient(),
-                .textToSpeech = this->textToSpeech
-            };
+                .textToSpeech = this->textToSpeech};
 
             skills_.lookForObjectWithKinematicUnit->connect(srv);
         }
 
+        {
+            addSkill(std::make_unique<skills::LookAt>(
+                skills::LookAt::Context{.viewSelectionClient = viewSelection}));
+        }
+
         // {
         //     ARMARX_CHECK_NOT_NULL(virtualRobotReaderPlugin);
         //     ARMARX_CHECK_NOT_NULL(virtualRobotReaderPlugin);
@@ -165,33 +168,28 @@ namespace armarx::view_selection::components::view_selection_skill_provider
         */
     }
 
-
     void
     Component::onDisconnectComponent()
     {
     }
 
-
     void
     Component::onExitComponent()
     {
     }
 
-
     std::string
     Component::getDefaultName() const
     {
         return Component::defaultName;
     }
 
-
     std::string
     Component::GetDefaultName()
     {
         return Component::defaultName;
     }
 
-
     /* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
     void
     Component::createRemoteGuiTab()
@@ -275,4 +273,4 @@ namespace armarx::view_selection::components::view_selection_skill_provider
 
     ARMARX_REGISTER_COMPONENT_EXECUTABLE(Component, Component::GetDefaultName());
 
-} // namespace armarx::view_selection::components::navigation_skill_provider
+} // namespace armarx::view_selection::components::view_selection_skill_provider

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

@@ -23,35 +23,36 @@
 
 #pragma once
 
-#include "RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h"
-#include "RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h"
-#include <armarx/view_selection/skills/skills.h>
 #include <ArmarXCore/core/Component.h>
 
+#include "armarx/view_selection/client/ViewSelection.h"
+#include "RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h"
+#include "RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h"
+#include <RobotAPI/interface/speech/SpeechInterface.h>
 #include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
 #include <RobotAPI/libraries/armem/client/plugins.h>
 #include <RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h>
 #include <RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h>
 #include <RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h>
 
-#include <RobotAPI/interface/speech/SpeechInterface.h>
+#include <armarx/view_selection/skills/skills.h>
 
-// #include <armarx/view_selection/components/view_selection_skill_provider/ComponentInterface.h>
+#include <armarx/view_selection/components/view_selection_skill_provider/ComponentInterface.h>
 
 namespace armarx::view_selection::components::view_selection_skill_provider
 {
 
     class Component :
         virtual public armarx::Component,
-        // virtual public ComponentInterface,
+        virtual public ComponentInterface,
         virtual public armarx::SkillProviderComponentPluginUser,
         // virtual public armarx::view_selection::client::ComponentPluginUser,
-        // virtual public armem::ListeningClientPluginUser,
         // , virtual public armarx::DebugObserverComponentPluginUser
         // , virtual public armarx::LightweightRemoteGuiComponentPluginUser
         virtual public armarx::RobotUnitComponentPluginUser,
         virtual public armarx::ObjectPoseClientPluginUser,
-        virtual public armarx::ArVizComponentPluginUser
+        virtual public armarx::ArVizComponentPluginUser,
+        virtual public armem::ListeningClientPluginUser
     {
     public:
         Component();
@@ -113,7 +114,6 @@ namespace armarx::view_selection::components::view_selection_skill_provider
     private:
         static const std::string defaultName;
 
-
         // Private member variables go here.
 
 
@@ -121,6 +121,7 @@ namespace armarx::view_selection::components::view_selection_skill_provider
         struct Properties
         {
         };
+
         Properties properties;
 
 
@@ -155,6 +156,8 @@ namespace armarx::view_selection::components::view_selection_skill_provider
 
         // ReaderWriterPlugin<armem::robot_state::VirtualRobotReader>* virtualRobotReaderPlugin =
         //     nullptr;
+        
+        client::ViewSelection viewSelection;
     };
 
-} // namespace armarx::view_selection::components::navigation_skill_provider
+} // namespace armarx::view_selection::components::view_selection_skill_provider

source/armarx/view_selection/components/view_selection_skill_provider/ComponentInterface.ice

@@ -23,14 +23,14 @@
 
 #pragma once
 
-// #include <RobotAPI/interface/armem/client/MemoryListenerInterface.ice>
+#include <RobotAPI/interface/armem/client/MemoryListenerInterface.ice>
 #include <RobotAPI/interface/skills/SkillProviderInterface.ice>
 
 module armarx {  module view_selection {  module components {  module view_selection_skill_provider 
 {
 
     interface ComponentInterface extends 
-        // armarx::armem::client::MemoryListenerInterface, 
+        armarx::armem::client::MemoryListenerInterface, 
         skills::provider::dti::SkillProviderInterface
     {
 	// Define your interface here.

source/armarx/view_selection/skills/CMakeLists.txt

@@ -3,6 +3,7 @@ armarx_add_aron_library(skills_aron
     ARON_FILES
         aron/LookForObject.xml
         aron/ScanLocation.xml
+        aron/LookAt.xml
 )
 
 armarx_add_library(skills
@@ -11,11 +12,13 @@ armarx_add_library(skills
         LookForObjectWithKinematicUnit.cpp
         LookForObject.cpp
         ScanLocation.cpp
+        LookAt.cpp
     HEADERS
         skills.h
         LookForObjectWithKinematicUnit.h
         LookForObject.h
         ScanLocation.h
+        LookAt.h
     DEPENDENCIES_PUBLIC
         # ArmarXCore
         ArmarXCoreInterfaces
@@ -26,6 +29,7 @@ armarx_add_library(skills
 
         # view_selection
         armarx_view_selection::skills_aron
+        armarx_view_selection::client
     # DEPENDENCIES_PRIVATE
         # ...
     # DEPENDENCIES_INTERFACE

source/armarx/view_selection/skills/LookAt.cpp

+#include "LookAt.h"
+
+#include <memory>
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+#include "armarx/view_selection/gaze_targets/GazeTarget.h"
+
+
+
+namespace armarx::view_selection::skills
+{
+
+
+    LookAt::LookAt(const Context& services) : Base(DefaultSkillDescription()), srv_(services)
+    {
+    }
+
+    void
+    LookAt::connect(const Context& srv)
+    {
+        srv_.emplace(srv);
+    }
+
+    ::armarx::skills::Skill::InitResult
+    LookAt::init(const Base::SpecializedInitInput& in)
+    {
+        gazeTarget.emplace(); // populate optional
+
+        fromAron(in.params.target, gazeTarget.value());
+
+        return ::armarx::skills::Skill::InitResult{
+            .status = ::armarx::skills::TerminatedSkillStatus::Succeeded};
+    }
+
+    ::armarx::skills::Skill::MainResult
+    LookAt::main(const Base::SpecializedMainInput& in)
+    {
+        ARMARX_CHECK(gazeTarget.has_value());
+
+        ARMARX_INFO << "Committing gaze target " << gazeTarget.value();
+        srv_->viewSelectionClient.commitGazeTarget(gazeTarget.value());
+
+        return ::armarx::skills::Skill::MainResult{
+            .status = ::armarx::skills::TerminatedSkillStatus::Succeeded};
+    }
+
+
+    void
+    LookAt::onStopRequested()
+    {
+        if(gazeTarget.has_value())
+        {
+            srv_->viewSelectionClient.commitGazeTarget(gazeTarget.value());
+        }
+
+    }
+
+
+} // namespace armarx::view_selection::skills

source/armarx/view_selection/skills/LookAt.h

+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author     Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date       2023
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#pragma once
+
+#include <ArmarXCore/core/ManagedIceObject.h>
+#include <ArmarXCore/core/time/Duration.h>
+
+#include <RobotAPI/libraries/skills/provider/Skill.h>
+#include <RobotAPI/libraries/skills/provider/SpecializedSkill.h>
+
+#include "armarx/view_selection/client/ViewSelection.h"
+#include <armarx/view_selection/skills/aron/LookAt.aron.generated.h>
+#include <armarx/view_selection/skills/constants.h>
+
+namespace armarx::view_selection::skills
+{
+    class LookAt : virtual public armarx::skills::SpecializedSkill<arondto::LookAtParams>
+    {
+    public:
+        using Params = arondto::LookAtParams;
+        using Base = ::armarx::skills::SpecializedSkill<Params>;
+
+        struct Context
+        {
+            client::ViewSelection& viewSelectionClient;
+        };
+
+        LookAt(const Context& services);
+
+        void connect(const Context& srv);
+
+    private:
+        ::armarx::skills::Skill::InitResult init(const Base::SpecializedInitInput& in) override;
+
+        ::armarx::skills::Skill::MainResult main(const Base::SpecializedMainInput& in) override;
+
+        void onStopRequested() override;
+
+
+    public:
+        static armarx::skills::SkillDescription
+        DefaultSkillDescription()
+        {
+            return {constants::skill_names::LookAt,
+                    "",
+                    {},
+                    armarx::core::time::Duration(),
+                    Params::ToAronType()};
+        }
+
+    protected:
+    private:
+        std::optional<Context> srv_;
+
+        // will be initialized in "init()"
+        std::optional<gaze_targets::GazeTarget> gazeTarget;
+    };
+
+} // namespace armarx::view_selection::skills

source/armarx/view_selection/skills/aron/LookAt.xml

+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+    <AronIncludes>
+        <Include include="armarx/view_selection/gaze_targets/aron/GazeTarget.xml" />
+    </AronIncludes>
+
+    <GenerateTypes>
+
+        <Object name='armarx::view_selection::skills::arondto::LookAtParams'>
+
+            <ObjectChild key='target'>
+                <armarx::view_selection::gaze_targets::arondto::GazeTarget />
+            </ObjectChild>
+
+        </Object>
+    </GenerateTypes>
+</AronTypeDefinition>

source/armarx/view_selection/skills/constants.h

@@ -31,6 +31,7 @@ namespace armarx::view_selection::skills::constants::skill_names
 
     inline const std::string LookForObjectWithKinematicUnit = "LookForObjectWithKinematicUnit";
     inline const std::string LookForObject = "LookForObject";
+    inline const std::string LookAt = "LookAt";
     inline const std::string ScanLocation = "ScanLocation";
 
 } // namespace armarx::view_selection::skills::constants::skill_names