update

scenarios/PlatformNavigation/config/Navigator.cfg

@@ -92,6 +92,14 @@
 # ArmarX.LoggingGroup = ""
 
 
+# ArmarX.Navigator.ArVizTopicName:  Name of the ArViz topic
+#  Attributes:
+#  - Default:            ArVizTopic
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.Navigator.ArVizTopicName = ArVizTopic
+
+
 # ArmarX.Navigator.EnableProfiling:  enable profiler which is used for logging performance events
 #  Attributes:
 #  - Default:            false

source/Navigation/components/Navigator/Navigator.cpp

@@ -15,6 +15,7 @@
  *
  * @package    Navigation::ArmarXObjects::Navigator
  * @author     Fabian Reister ( fabian dot reister at kit dot edu )
+ * @author     Christian R. G. Dreher ( c dot dreher at kit dot edu )
  * @date       2021
  * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
  *             GNU General Public License
@@ -26,12 +27,12 @@
 #include <iterator>
 
 #include <Eigen/Geometry>
-#include <Eigen/src/Geometry/Translation.h>
 
 #include <Ice/Current.h>
 
 #include <SimoxUtility/math/convert/rpy_to_mat3f.h>
 #include <VirtualRobot/MathTools.h>
+#include <VirtualRobot/Robot.h>
 
 #include "ArmarXCore/core/services/tasks/PeriodicTask.h"
 #include "ArmarXCore/core/services/tasks/TaskUtil.h"
@@ -46,17 +47,13 @@
 #include "Navigation/libraries/core/types.h"
 #include "Navigation/libraries/factories/NavigationStackFactory.h"
 #include "Navigation/libraries/global_planning/Point2Point.h"
+#include "Navigation/libraries/server/Navigator.h"
 #include "Navigation/libraries/server/execution/PlatformUnitExecutor.h"
 #include "Navigation/libraries/trajectory_control/TrajectoryFollowingController.h"
 #include "Navigation/libraries/util/util.h"
 
-armarx::nav::components::Navigator::Navigator()
-{
-}
-
-armarx::nav::components::Navigator::~Navigator()
-{
-}
+armarx::nav::components::Navigator::Navigator() = default;
+armarx::nav::components::Navigator::~Navigator() = default;
 
 void armarx::nav::components::Navigator::onInitComponent()
 {
@@ -64,11 +61,14 @@ void armarx::nav::components::Navigator::onInitComponent()
 
 void armarx::nav::components::Navigator::onConnectComponent()
 {
-    executor = server::PlatformUnitExecutor(platformUnit);
+
     scene.robot = getRobot();
     scene.staticScene = staticScene();
 
-    // TODO scene
+    executor = server::PlatformUnitExecutor(platformUnit);
+    introspector = server::ArvizIntrospector(getArvizClient(), scene.robot);
+
+    // TODO dynamic scene
     // TODO memory
     // TODO param (10)
 
@@ -92,8 +92,9 @@ void armarx::nav::components::Navigator::onExitComponent()
 
 void armarx::nav::components::Navigator::updateContext()
 {
-    scene.staticScene = staticScene();
     scene.robot = getRobot();
+    scene.staticScene = staticScene();
+    // TODO dynamic scene
 }
 
 std::string armarx::nav::components::Navigator::getDefaultName() const
@@ -124,8 +125,12 @@ void armarx::nav::components::Navigator::createConfig(const aron::data::AronDict
     ARMARX_TRACE;
 
     server::NavigationStack stack = fac::NavigationStackFactory::create(stackConfig, scene);
+    server::Navigator navigator{server::NavigatorConfig{.stack = std::move(stack),
+                                .scene = &scene,
+                                .executor = &executor.value(),
+                                .introspector = &(introspector.value())}};
 
-    navigators.emplace(callerId, server::Navigator{stack, scene, executor.value()});
+    navigators.emplace(callerId, std::move(navigator));
 }
 
 void armarx::nav::components::Navigator::moveTo(const std::vector<Eigen::Matrix4f>& waypoints,
@@ -152,10 +157,16 @@ void armarx::nav::components::Navigator::moveTowards(const Eigen::Vector3f& dire
 
 void armarx::nav::components::Navigator::pauseMovement(const Ice::Current&)
 {
+    // TODO
+    //  find active navigator
+    //  pause navigator
 }
 
 void armarx::nav::components::Navigator::resumeMovement(const Ice::Current&)
 {
+    // TODO
+    //  find active navigator
+    //  resume navigator
 }
 
 armarx::PropertyDefinitionsPtr armarx::nav::components::Navigator::createPropertyDefinitions()

source/Navigation/components/Navigator/Navigator.h

@@ -15,6 +15,7 @@
  *
  * @package    Navigation::ArmarXObjects::Navigator
  * @author     Fabian Reister ( fabian dot reister at kit dot edu )
+ * @author     Christian R. G. Dreher ( c dot dreher at kit dot edu )
  * @date       2021
  * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
  *             GNU General Public License
@@ -39,6 +40,7 @@
 
 #include <RobotAPI/interface/units/PlatformUnitInterface.h>
 #include <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
 #include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h>
 
 #include "Navigation/libraries/core/StaticScene.h"
@@ -46,6 +48,7 @@
 #include <Navigation/components/Navigator/NavigatorInterface.h>
 #include <Navigation/libraries/server/Navigator.h>
 #include <Navigation/libraries/server/execution/PlatformUnitExecutor.h>
+#include <Navigation/libraries/server/introspection/ArvizIntrospector.h>
 
 namespace armarx::nav::components
 {
@@ -66,7 +69,8 @@ namespace armarx::nav::components
         virtual public NavigatorInterface,
         virtual public armarx::ObjectPoseClientPluginUser,
         virtual public armarx::RobotStateComponentPluginUser,
-        virtual public armarx::LightweightRemoteGuiComponentPluginUser
+        virtual public armarx::LightweightRemoteGuiComponentPluginUser,
+        virtual public armarx::ArVizComponentPluginUser
     {
 
     public:
@@ -127,6 +131,7 @@ namespace armarx::nav::components
 
         core::Scene scene;
         std::optional<server::PlatformUnitExecutor> executor;
+        std::optional<server::ArvizIntrospector> introspector;
         std::map<std::string, server::Navigator> navigators;
 
         std::mutex propertiesMutex;

source/Navigation/components/Navigator/NavigatorInterface.ice

@@ -14,7 +14,7 @@
  * along with this program. If not, see <http://www.gnu.org/licenses/>.
  *
  * @package    armarx::nav::components::Navigator
- * @author     Christian R. G. Dreher <c.dreher@kit.edu>
+ * @author     Christian R. G. Dreher ( c dot dreher at kit dot edu )
  * @date       2021
  * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
  *             GNU General Public License

source/Navigation/components/Navigator/test/NavigatorTest.cpp

@@ -15,6 +15,7 @@
  *
  * @package    Navigation::ArmarXObjects::Navigator
  * @author     Fabian Reister ( fabian dot reister at kit dot edu )
+ * @author     Christian R. G. Dreher ( c dot dreher at kit dot edu )
  * @date       2021
  * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
  *             GNU General Public License

source/Navigation/libraries/algorithms/algorithms.cpp

@@ -15,6 +15,7 @@
  *
  * @package    Navigation::ArmarXObjects::algorithms
  * @author     Fabian Reister ( fabian dot reister at kit dot edu )
+ * @author     Christian R. G. Dreher ( c dot dreher at kit dot edu )
  * @date       2021
  * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
  *             GNU General Public License

source/Navigation/libraries/algorithms/algorithms.h

@@ -15,6 +15,7 @@
  *
  * @package    Navigation::ArmarXObjects::algorithms
  * @author     Fabian Reister ( fabian dot reister at kit dot edu )
+ * @author     Christian R. G. Dreher ( c dot dreher at kit dot edu )
  * @date       2021
  * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
  *             GNU General Public License

source/Navigation/libraries/algorithms/smoothing/ChainApproximation.h

@@ -14,6 +14,7 @@
  * along with this program. If not, see <http://www.gnu.org/licenses/>.
  *
  * @author     Fabian Reister ( fabian dot reister at kit dot edu )
+ * @author     Christian R. G. Dreher ( c dot dreher at kit dot edu )
  * @date       2021
  * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
  *             GNU General Public License

source/Navigation/libraries/algorithms/smoothing/CircularPathSmoothing.h

@@ -14,17 +14,18 @@
  * along with this program. If not, see <http://www.gnu.org/licenses/>.
  *
  * @author     Fabian Reister ( fabian dot reister at kit dot edu )
+ * @author     Christian R. G. Dreher ( c dot dreher at kit dot edu )
  * @date       2021
  * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
  *             GNU General Public License
  */
- 
- #pragma once
- 
- namespace armarx::nav::algo
- {
- 
-class CircularPathSmoothing
+
+#pragma once
+
+namespace armarx::nav::algo
+{
+
+    class CircularPathSmoothing
     {
     public:
         // TODO(fabian.reister): add param
@@ -60,7 +61,7 @@ class CircularPathSmoothing
 
                 // const float phi = phiIn + (phiOut - phiIn) / 2;
 
-                // the angle between the lines 
+                // the angle between the lines
                 const float phi = getAngleBetweenVectors(vIn, vOut);
 
                 // const Eigen::Vector2f at_dir = Eigen::Rotation2Df(phi) * Eigen::Vector2f::UnitX();
@@ -100,4 +101,4 @@ class CircularPathSmoothing
         }
     };
 
- } // namespace armarx::nav::algo
+} // namespace armarx::nav::algo

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

-
-# Libs required for the tests
-SET(LIBS ${LIBS} ArmarXCore algorithms)
- 
-armarx_add_test(algorithmsTest algorithmsTest.cpp "${LIBS}")
\ No newline at end of file
+armarx_add_test(
+    TEST_NAME
+        algorithmsTest
+    TEST_FILE
+        algorithmsTest.cpp
+    DEPENDEND_LIBS
+        ArmarXCore
+        algorithms
+)

source/Navigation/libraries/algorithms/test/algorithmsTest.cpp

@@ -15,6 +15,7 @@
  *
  * @package    Navigation::ArmarXObjects::algorithms
  * @author     Fabian Reister ( fabian dot reister at kit dot edu )
+ * @author     Christian R. G. Dreher ( c dot dreher at kit dot edu )
  * @date       2021
  * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
  *             GNU General Public License

source/Navigation/libraries/client/NavigationStackConfig.h

@@ -14,6 +14,7 @@
  * along with this program. If not, see <http://www.gnu.org/licenses/>.
  *
  * @author     Fabian Reister ( fabian dot reister at kit dot edu )
+ * @author     Christian R. G. Dreher ( c dot dreher at kit dot edu )
  * @date       2021
  * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
  *             GNU General Public License

source/Navigation/libraries/client/Navigator.h

@@ -14,6 +14,7 @@
  * along with this program. If not, see <http://www.gnu.org/licenses/>.
  *
  * @author     Fabian Reister ( fabian dot reister at kit dot edu )
+ * @author     Christian R. G. Dreher ( c dot dreher at kit dot edu )
  * @date       2021
  * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
  *             GNU General Public License

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

-# Libs required for the tests TODO remove Navigation::server
-set(
-    LIBS
-    ${LIBS}
-    ArmarXCore
-    Navigation::client
-    Navigation::factories
-    Navigation::server
-)
 
 armarx_add_test(
-    clientTest
-    clientTest.cpp
-    "${LIBS}"
+    TEST_NAME
+        clientTest
+    TEST_FILE
+        clientTest.cpp
+    DEPENDEND_LIBS
+        ArmarXCore
+        Navigation::Client
+        Navigation::Factories
+        Navigation::Server
 )

source/Navigation/libraries/client/test/clientTest.cpp

@@ -15,6 +15,7 @@
  *
  * @package    Navigation::ArmarXObjects::client
  * @author     Fabian Reister ( fabian dot reister at kit dot edu )
+ * @author     Christian R. G. Dreher ( c dot dreher at kit dot edu )
  * @date       2021
  * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
  *             GNU General Public License

source/Navigation/libraries/conversions/eigen.h

@@ -14,6 +14,7 @@
  * along with this program. If not, see <http://www.gnu.org/licenses/>.
  *
  * @author     Fabian Reister ( fabian dot reister at kit dot edu )
+ * @author     Christian R. G. Dreher ( c dot dreher at kit dot edu )
  * @date       2021
  * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
  *             GNU General Public License

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

-# Libs required for the tests
-set(
-    LIBS
-    ${LIBS}
-    ArmarXCore
-    Navigation::Conversions
-)
 
 armarx_add_test(
-    conversionsTest
-    conversionsTest.cpp
-    "${LIBS}"
+    TEST_NAME
+        conversionsTest
+    TEST_FILE
+        conversionsTest.cpp
+    DEPENDEND_LIBS
+        ArmarXCore
+        Navigation::Conversions
 )

source/Navigation/libraries/conversions/test/conversionsTest.cpp

@@ -15,6 +15,7 @@
  *
  * @package    Navigation::ArmarXObjects::conversions
  * @author     Fabian Reister ( fabian dot reister at kit dot edu )
+ * @author     Christian R. G. Dreher ( c dot dreher at kit dot edu )
  * @date       2021
  * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
  *             GNU General Public License

source/Navigation/libraries/core/DynamicScene.h

@@ -14,6 +14,7 @@
  * along with this program. If not, see <http://www.gnu.org/licenses/>.
  *
  * @author     Fabian Reister ( fabian dot reister at kit dot edu )
+ * @author     Christian R. G. Dreher ( c dot dreher at kit dot edu )
  * @date       2021
  * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
  *             GNU General Public License

source/Navigation/libraries/core/MemoryReferencedElement.h

@@ -14,6 +14,7 @@
  * along with this program. If not, see <http://www.gnu.org/licenses/>.
  *
  * @author     Fabian Reister ( fabian dot reister at kit dot edu )
+ * @author     Christian R. G. Dreher ( c dot dreher at kit dot edu )
  * @date       2021
  * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
  *             GNU General Public License