update

source/Navigation/components/Navigator/Navigator.cpp

@@ -22,79 +22,117 @@
 
 #include "Navigator.h"
 
-#include <Eigen/src/Geometry/Transform.h>
-
 // Navigation
 #include "Navigation/libraries/core/types.h"
+#include "Navigation/libraries/util/util.h"
 #include <Navigation/libraries/factories/NavigationStackFactory.h>
 #include <Navigation/libraries/server/Navigator.h>
 
-armarx::nav::components::Navigator::~Navigator()
+namespace armarx::nav::components
 {
-}
 
-void armarx::nav::components::Navigator::onInitComponent()
-{
-}
+    Navigator::~Navigator()
+    {
+    }
 
-void armarx::nav::components::Navigator::onConnectComponent()
-{
-}
+    void Navigator::onInitComponent()
+    {
+    }
 
-void armarx::nav::components::Navigator::onDisconnectComponent()
-{
-}
+    void Navigator::onConnectComponent()
+    {
+    }
 
-void armarx::nav::components::Navigator::onExitComponent()
-{
-}
+    void Navigator::onDisconnectComponent()
+    {
+    }
 
-std::string armarx::nav::components::Navigator::getDefaultName() const
-{
-    return "Navigator";
-}
+    void Navigator::onExitComponent()
+    {
+    }
+    
+    void Navigator::updateContext() 
+    {
+        ctx.staticScene = staticScene();
+        ctx.robot = getRobot();
+    }
 
-void armarx::nav::components::Navigator::moveTo(const Eigen::Matrix4f& pose,
-                                                const aron::data::AronDictPtr& stackConfig,
-                                                const std::string& navigationMode,
-                                                const Ice::Current&)
-{
+    std::string Navigator::getDefaultName() const
+    {
+        return "Navigator";
+    }
 
-    server::NavigationStack stack = fac::NavigationStackFactory::create(stackConfig, ctx);
-    server::Navigator navigator{stack, ctx};
+    void Navigator::moveTo(const Eigen::Matrix4f& pose,
+                           const aron::data::AronDictPtr& stackConfig,
+                           const std::string& navigationMode,
+                           const Ice::Current&)
+    {
 
-    navigator.moveTo(core::Pose(pose), core::NavigationFramesMap.from_name(navigationMode));
-}
+        server::NavigationStack stack = fac::NavigationStackFactory::create(stackConfig, ctx);
+        server::Navigator navigator{stack, ctx};
 
-void armarx::nav::components::Navigator::moveTowards(const Eigen::Matrix4f& direction,
-                                                     const aron::data::AronDictPtr& stackConfig,
-                                                     const std::string& navigationMode,
-                                                     const Ice::Current&)
-{
-    server::NavigationStack stack = fac::NavigationStackFactory::create(stackConfig, ctx);
-    server::Navigator navigator{stack, ctx};
+        navigator.moveTo(core::Pose(pose), core::NavigationFramesMap.from_name(navigationMode));
+    }
 
-    // navigator.moveTowards(direction); TODO
-}
+    void Navigator::moveTowards(const Eigen::Matrix4f& direction,
+                                const aron::data::AronDictPtr& stackConfig,
+                                const std::string& navigationMode,
+                                const Ice::Current&)
+    {
+        server::NavigationStack stack = fac::NavigationStackFactory::create(stackConfig, ctx);
+        server::Navigator navigator{stack, ctx};
 
-armarx::PropertyDefinitionsPtr armarx::nav::components::Navigator::createPropertyDefinitions()
-{
-    armarx::PropertyDefinitionsPtr def = new ComponentPropertyDefinitions(getConfigIdentifier());
+        // navigator.moveTowards(direction); TODO
+    }
+
+    armarx::PropertyDefinitionsPtr Navigator::createPropertyDefinitions()
+    {
+        armarx::PropertyDefinitionsPtr def =
+            new ComponentPropertyDefinitions(getConfigIdentifier());
+
+        // Publish to a topic (passing the TopicListenerPrx).
+        // def->topic(myTopicListener);
+
+        // Subscribe to a topic (passing the topic name).
+        // def->topic<PlatformUnitListener>("MyTopic");
+
+        // Use (and depend on) another component (passing the ComponentInterfacePrx).
+        def->component(platformUnit);
+
+        // Add a required property.
+        // def->required(properties.boxLayerName, "p.box.LayerName", "Name of the box layer in ArViz.");
+
+        // Add an optionalproperty.
+        // def->optional(properties.numBoxes, "p.box.Number", "Number of boxes to draw in ArViz.");
+
+        return def;
+    }
+
+    core::StaticScenePtr Navigator::staticScene()
+    {
+        core::StaticScenePtr scene(new core::StaticScene);
+
+        objpose::ObjectPoseSeq objectPoses = ObjectPoseClientPluginUser::getObjectPoses();
+
+        scene->objects = util::asSceneObjects(objectPoses);
 
-    // Publish to a topic (passing the TopicListenerPrx).
-    // def->topic(myTopicListener);
+        return scene;
+    }
+    
+    VirtualRobot::RobotPtr Navigator::getRobot() 
+    {
 
-    // Subscribe to a topic (passing the topic name).
-    // def->topic<PlatformUnitListener>("MyTopic");
+        auto robot = RemoteRobot::createLocalCloneFromFile(
+            getRobotStateComponent(), VirtualRobot::RobotIO::RobotDescription::eFull);
+        // auto robot = RemoteRobot::createLocalClone(getRobotStateComponent());
+        ARMARX_CHECK_NOT_NULL(robot);
 
-    // Use (and depend on) another component (passing the ComponentInterfacePrx).
-    // def->component(myComponentProxy)
+        // ARMARX_INFO << "Resetting robot";
+        // robot = VirtualRobot::RobotIO::loadRobot(robot->getFilename());
 
-    // Add a required property.
-    // def->required(properties.boxLayerName, "p.box.LayerName", "Name of the box layer in ArViz.");
+        RemoteRobot::synchronizeLocalClone(robot, getRobotStateComponent());
 
-    // Add an optionalproperty.
-    // def->optional(properties.numBoxes, "p.box.Number", "Number of boxes to draw in ArViz.");
+        return robot;
+    }
 
-    return def;
-}
+} // namespace armarx::nav::components
\ No newline at end of file

source/Navigation/components/Navigator/Navigator.h

@@ -31,7 +31,12 @@
 // ArmarX
 #include <ArmarXCore/core/Component.h>
 
+#include <RobotAPI/interface/units/PlatformUnitInterface.h>
+#include <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h>
+
 // Navigation
+#include "Navigation/libraries/core/StaticScene.h"
 #include "Navigation/libraries/core/types.h"
 #include <Navigation/components/Navigator/NavigatorInterface.h>
 
@@ -49,7 +54,11 @@ namespace armarx::nav::components
      *
      * Detailed description of class Navigator.
      */
-    class Navigator : virtual public armarx::Component, virtual public NavigatorInterface
+    class Navigator :
+        virtual public armarx::Component,
+        virtual public NavigatorInterface,
+        virtual public armarx::ObjectPoseClientPluginUser,
+        virtual public armarx::RobotStateComponentPluginUser
     {
 
     public:
@@ -84,10 +93,19 @@ namespace armarx::nav::components
         /// @see armarx::ManagedIceObject::onExitComponent()
         void onExitComponent() override;
 
+        void updateContext();
+
+        core::StaticScenePtr staticScene();
+
+        VirtualRobot::RobotPtr getRobot();
+
+
     private:
         // TODO update context periodically
 
         core::Context ctx;
+
+        PlatformUnitListenerPrx platformUnit;
     };
 } // namespace armarx::nav::components
 

source/Navigation/libraries/CMakeLists.txt

@@ -7,4 +7,5 @@ add_subdirectory(client)
 add_subdirectory(algorithms)
 add_subdirectory(factories)
 add_subdirectory(safety_control)
-add_subdirectory(server)
+add_subdirectory(server)
\ No newline at end of file
+add_subdirectory(util)
\ No newline at end of file

source/Navigation/libraries/core/StaticScene.h

@@ -22,14 +22,15 @@
 #pragma once
 
 #include <memory>
+
+#include <VirtualRobot/VirtualRobot.h>
+
 namespace armarx::nav::core
 {
 
-    class StaticScene
+    struct StaticScene
     {
-    public:
-    protected:
-    private:
+        VirtualRobot::SceneObjectSetPtr objects;
     };
 
     using StaticScenePtr = std::shared_ptr<StaticScene>;

source/Navigation/libraries/util/CMakeLists.txt

+set(LIB_NAME       util)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+armarx_add_library(
+    LIBS     
+        ArmarXCoreInterfaces
+        ArmarXCore
+    SOURCES  
+        ./util.cpp
+    HEADERS  
+        ./util.h
+)
+
+add_library(Navigation::util ALIAS util)
+
+#find_package(MyLib QUIET)
+#armarx_build_if(MyLib_FOUND "MyLib not available")
+# all target_include_directories must be guarded by if(Xyz_FOUND)
+# for multiple libraries write: if(X_FOUND AND Y_FOUND)....
+#if(MyLib_FOUND)
+#    target_include_directories(util PUBLIC ${MyLib_INCLUDE_DIRS})
+#endif()
+
+# add unit tests
+add_subdirectory(test)

source/Navigation/libraries/util/test/CMakeLists.txt

+
+# Libs required for the tests
+SET(LIBS ${LIBS} ArmarXCore util)
+ 
+armarx_add_test(utilTest utilTest.cpp "${LIBS}")
\ No newline at end of file

source/Navigation/libraries/util/test/utilTest.cpp

+/*
+ * 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/>.
+ *
+ * @package    Navigation::ArmarXObjects::util
+ * @author     Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date       2021
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#define BOOST_TEST_MODULE Navigation::ArmarXLibraries::util
+
+#define ARMARX_BOOST_TEST
+
+#include <Navigation/Test.h>
+#include "../util.h"
+
+#include <iostream>
+
+BOOST_AUTO_TEST_CASE(testExample)
+{
+
+    BOOST_CHECK_EQUAL(true, true);
+}

source/Navigation/libraries/util/util.cpp

+/*
+ * 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/>.
+ *
+ * @package    Navigation::ArmarXObjects::util
+ * @author     Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date       2021
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#include "util.h"
+
+#include <VirtualRobot/SceneObjectSet.h>
+#include <VirtualRobot/ManipulationObject.h>
+
+#include "RobotAPI/libraries/ArmarXObjects/ObjectFinder.h"
+
+namespace armarx::nav::util
+{
+
+    VirtualRobot::SceneObjectSetPtr asSceneObjects(const objpose::ObjectPoseSeq& objectPoses)
+    {
+        ObjectFinder finder;
+
+        VirtualRobot::SceneObjectSetPtr sceneObjects(new VirtualRobot::SceneObjectSet);
+        for (const auto& objectPose : objectPoses)
+        {
+            if (auto obstacle = finder.loadManipulationObject(objectPose))
+            {
+                obstacle->setGlobalPose(objectPose.objectPoseGlobal);
+                sceneObjects->addSceneObject(obstacle);
+            }
+        }
+
+        return sceneObjects;
+    }
+} // namespace armarx::nav::util

source/Navigation/libraries/util/util.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       2021
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#pragma once
+
+#include <VirtualRobot/VirtualRobot.h>
+
+#include "RobotAPI/libraries/ArmarXObjects/ObjectPose.h"
+
+namespace armarx::nav::util
+{
+
+    VirtualRobot::SceneObjectSetPtr asSceneObjects(const objpose::ObjectPoseSeq& objectPoses);
+
+} // namespace armarx::nav::util
\ No newline at end of file