Example client: point to point movement

source/armarx/navigation/components/example_client/CMakeLists.txt

@@ -9,6 +9,8 @@ armarx_add_component(example_client
         # ArmarXGuiComponentPlugins  # For RemoteGui plugin. RobotAPI
         # RobotAPICore RobotAPIInterfaces RobotAPIArmarXObjects
         # RobotAPIComponentPlugins  # For ArViz and other plugins. Navigation
+        armem_robot_state
+        armem_robot
         armarx_navigation::client
         Simox::SimoxUtility
         Eigen3::Eigen

source/armarx/navigation/components/example_client/Component.cpp

@@ -24,10 +24,20 @@
 #include "Component.h"
 
 #include <chrono>
+#include <cmath>
 #include <thread>
 
+#include <Eigen/src/Geometry/AngleAxis.h>
+#include <Eigen/src/Geometry/Translation.h>
+
+#include "ArmarXCore/core/logging/Logging.h"
+#include "ArmarXCore/core/time/Clock.h"
+#include "ArmarXCore/core/time/ClockType.h"
+#include "ArmarXCore/core/time/forward_declarations.h"
 #include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
 
+#include "armarx/navigation/client/types.h"
+#include "armarx/navigation/global_planning/Point2Point.h"
 #include <armarx/navigation/client/PathBuilder.h>
 #include <armarx/navigation/global_planning/AStar.h>
 #include <armarx/navigation/trajectory_control/TrajectoryFollowingController.h>
@@ -36,7 +46,7 @@
 namespace armarx::navigation::components::example_client
 {
 
-    Component::Component()
+    Component::Component() : virtualRobotReader(memoryNameSystem())
     {
         // pass
     }
@@ -58,7 +68,9 @@ namespace armarx::navigation::components::example_client
     void
     Component::onConnectComponent()
     {
-        task = new armarx::RunningTask<Component>(this, &Component::exampleNavigationComplex);
+        virtualRobotReader.connect();
+
+        task = new armarx::RunningTask<Component>(this, &Component::exampleNavigationPointToPoint);
         task->start();
     }
 
@@ -264,6 +276,87 @@ namespace armarx::navigation::components::example_client
     }
 
 
+    void
+    Component::exampleNavigationPointToPoint()
+    {
+        ARMARX_INFO << "Demonstrating point to point navigation";
+
+        using namespace std::chrono_literals;
+
+        // Import relevant namespaces.
+        using namespace armarx::navigation;
+
+        ARMARX_INFO << "Configuring navigator";
+
+        // Create an example configuration valid for the following move* calls.
+        configureNavigator(
+            client::NavigationStackConfig()
+                .general({} /*{.maxVel = VelocityLimits{.linear = 400 , .angular = 0.1}}*/)
+                .globalPlanner(global_planning::Point2PointParams())
+                .trajectoryController(traj_ctrl::TrajectoryFollowingControllerParams()));
+
+
+        std::this_thread::sleep_for(1s);
+
+        ARMARX_INFO << "Moving to goal pose";
+        // Start moving to goal position using above config.
+
+        auto robot =
+            virtualRobotReader.getSynchronizedRobot(properties.robotName, armarx::Clock::Now());
+        ARMARX_CHECK_NOT_NULL(robot);
+
+        const core::Pose initialPose(robot->getGlobalPose());
+
+        const core::Pose goal1 =
+            initialPose * Eigen::Translation3f(properties.relativeMovement, 0, 0); // to the right
+
+        const core::Pose goal2 =
+            goal1 * Eigen::Translation3f(0, properties.relativeMovement, 0) *
+            Eigen::AngleAxisf(M_PI_2f32, Eigen::Vector3f::UnitZ()); // forward + 90° to the left
+
+        const core::Pose goal3 =
+            goal2 * Eigen::Translation3f(-properties.relativeMovement, 0, 0) *
+            Eigen::AngleAxisf(-M_PI_2f32,
+                              Eigen::Vector3f::UnitZ()); // left and back to initial orientation
+
+        const core::Pose& finalGoal = initialPose;
+
+        const auto moveToP2P = [&](const core::Pose& targetPose)
+        {
+            // define movement
+            client::PathBuilder pathBuilder;
+            pathBuilder.add(targetPose,
+                            armarx::navigation::client::GlobalPlanningStrategy::Point2Point);
+
+            // trigger movement
+            ARMARX_INFO << "Starting execution";
+            getNavigator().moveTo(pathBuilder, core::NavigationFrame::Absolute);
+
+            // wait for navigator to reach the target
+            ARMARX_INFO << "Waiting for navigator to reach the target";
+            armarx::navigation::client::StopEvent se = getNavigator().waitForStop();
+
+            // In case of any failure, the following will raise an exception to avoid subsequent navigation requests to be executed.
+            ARMARX_CHECK(se.isGoalReachedEvent());
+        };
+
+        ARMARX_IMPORTANT << "Moving to goal 1";
+        moveToP2P(goal1);
+
+        ARMARX_IMPORTANT << "Moving to goal 2";
+        moveToP2P(goal2);
+
+        ARMARX_IMPORTANT << "Moving to goal 3";
+        moveToP2P(goal3);
+
+        ARMARX_IMPORTANT << "Moving to final goal";
+        moveToP2P(finalGoal);
+
+        ARMARX_IMPORTANT << "Done. Pausing movement.";
+        getNavigator().pause();
+    }
+
+
     void
     Component::goalReached()
     {
@@ -276,6 +369,16 @@ namespace armarx::navigation::components::example_client
     {
         armarx::PropertyDefinitionsPtr def =
             new armarx::ComponentPropertyDefinitions(getConfigIdentifier());
+
+        def->optional(properties.robotName, "robotName");
+
+        def->optional(properties.relativeMovement,
+                      "relativeMovement",
+                      "The distance between two target poses [mm]");
+
+        virtualRobotReader.registerPropertyDefinitions(def);
+
+
         return def;
     }
 

source/armarx/navigation/components/example_client/Component.h

@@ -27,6 +27,8 @@
 #include <ArmarXCore/util/tasks.h>
 
 #include <armarx/navigation/client.h>
+#include "RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h"
+#include <RobotAPI/libraries/armem/client/plugins.h>
 
 
 namespace armarx::navigation::components::example_client
@@ -45,7 +47,8 @@ namespace armarx::navigation::components::example_client
      */
     class Component :
         virtual public armarx::Component,
-        virtual public armarx::navigation::client::ComponentPluginUser
+        virtual public armarx::navigation::client::ComponentPluginUser,
+        virtual public armem::ListeningClientPluginUser
     {
 
     public:
@@ -74,10 +77,31 @@ namespace armarx::navigation::components::example_client
         void exampleNavigation();
         void exampleNavigationComplex();
 
+        /**
+         * @brief demonstrates point to point movement.
+         * 
+         * The robot will move along a rectange in the following way:
+         * 
+         * (1) move to the right 
+         * (2) move forward and turn 90° to the left 
+         * (3) move left and turn back 90° to the right 
+         * (4) backward (this is the initial pose)
+         * 
+         */
+        void exampleNavigationPointToPoint();
+
     private:
         void goalReached();
 
+        struct{
+            std::string robotName = "Armar6";
+            float relativeMovement = 200; // [mm]
+        } properties;
+
         armarx::RunningTask<Component>::pointer_type task;
+
+        armem::robot_state::VirtualRobotReader virtualRobotReader;
+
     };
 
 } // namespace armarx::navigation::components::example_client