Add Locations and Graph to Navigation Memory and Redesign Location Graph Editor

source/armarx/navigation/components/ExampleClient/ExampleClient.cpp

@@ -20,166 +20,178 @@
  *             GNU General Public License
  */
 
+#include <chrono>
+#include <thread>
+
+#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
+
 #include <armarx/navigation/components/ExampleClient/ExampleClient.h>
 #include <armarx/navigation/global_planning/AStar.h>
 #include <armarx/navigation/trajectory_control/TrajectoryFollowingController.h>
 
-// STD/STL
-#include <chrono>
-#include <thread>
 
-exns::ExampleClient::ExampleClient()
+namespace armarx::navigation::examples
 {
-    // pass
-}
 
-exns::ExampleClient::~ExampleClient()
-{
-    // pass
-}
+    ARMARX_DECOUPLED_REGISTER_COMPONENT(ExampleClient);
 
-void
-exns::ExampleClient::onInitComponent()
-{
-    // pass
-}
 
-void
-exns::ExampleClient::onConnectComponent()
-{
-    task = new armarx::RunningTask<ExampleClient>(this, &ExampleClient::exampleNavigation);
-    task->start();
-}
+    ExampleClient::ExampleClient()
+    {
+        // pass
+    }
 
-void
-exns::ExampleClient::onDisconnectComponent()
-{
-    const bool join = true;
-    task->stop(join);
-}
+    ExampleClient::~ExampleClient()
+    {
+        // pass
+    }
 
-void
-exns::ExampleClient::onExitComponent()
-{
-    // pass
-}
+    void
+    ExampleClient::onInitComponent()
+    {
+        // pass
+    }
 
-std::string
-exns::ExampleClient::getDefaultName() const
-{
-    return "ExampleClient";
-}
+    void
+    ExampleClient::onConnectComponent()
+    {
+        task = new armarx::RunningTask<ExampleClient>(this, &ExampleClient::exampleNavigation);
+        task->start();
+    }
 
-void
-exns::ExampleClient::exampleNavigation()
-{
-    using namespace std::chrono_literals;
+    void
+    ExampleClient::onDisconnectComponent()
+    {
+        const bool join = true;
+        task->stop(join);
+    }
 
-    // Import relevant namespaces.
-    using namespace armarx::navigation;
+    void
+    ExampleClient::onExitComponent()
+    {
+        // pass
+    }
 
-    ARMARX_INFO << "Configuring navigator";
+    std::string
+    ExampleClient::getDefaultName() const
+    {
+        return "ExampleClient";
+    }
 
-    // Create an example configuration valid for the following move* calls.
-    configureNavigator(
-        client::NavigationStackConfig()
-            .general({} /*{.maxVel = VelocityLimits{.linear = 400 , .angular = 0.1}}*/)
-            .globalPlanner(glob_plan::AStarParams())
-            .trajectoryController(traj_ctrl::TrajectoryFollowingControllerParams()));
+    void
+    ExampleClient::exampleNavigation()
+    {
+        using namespace std::chrono_literals;
 
-    // Example of registering a lambda as callback.
-    navigator.onGoalReached([&]() { ARMARX_IMPORTANT << "Goal reached! (lambda-style)"; });
+        // Import relevant namespaces.
+        using namespace armarx::navigation;
 
-    // Example of registering a method as callback.
-    navigator.onGoalReached([this] { goalReached(); });
+        ARMARX_INFO << "Configuring navigator";
 
-    std::this_thread::sleep_for(1s);
+        // Create an example configuration valid for the following move* calls.
+        configureNavigator(
+            client::NavigationStackConfig()
+                .general({} /*{.maxVel = VelocityLimits{.linear = 400 , .angular = 0.1}}*/)
+                .globalPlanner(glob_plan::AStarParams())
+                .trajectoryController(traj_ctrl::TrajectoryFollowingControllerParams()));
 
-    ARMARX_INFO << "Moving to goal pose";
-    // Start moving to goal position using above config.
-    core::Pose goal = core::Pose::Identity();
-    goal.translation() << 2000, 1000, 0;
+        // Example of registering a lambda as callback.
+        navigator.onGoalReached([&]() { ARMARX_IMPORTANT << "Goal reached! (lambda-style)"; });
 
-    navigator.moveTo(goal, core::NavigationFrame::Absolute);
+        // Example of registering a method as callback.
+        navigator.onGoalReached([this] { goalReached(); });
 
-    std::this_thread::sleep_for(15s);
+        std::this_thread::sleep_for(1s);
 
-    // Wait until goal is reached
-    armarx::navigation::client::StopEvent se = navigator.waitForStop();
-    if (se)
-    {
-        ARMARX_INFO << "Goal 1 reached.";
-    }
-    else
-    {
-        if (se.isSafetyStopTriggeredEvent())
+        ARMARX_INFO << "Moving to goal pose";
+        // Start moving to goal position using above config.
+        core::Pose goal = core::Pose::Identity();
+        goal.translation() << 2000, 1000, 0;
+
+        navigator.moveTo(goal, core::NavigationFrame::Absolute);
+
+        std::this_thread::sleep_for(15s);
+
+        // Wait until goal is reached
+        armarx::navigation::client::StopEvent se = navigator.waitForStop();
+        if (se)
         {
-            ARMARX_ERROR << "Safety stop was triggered!";
+            ARMARX_INFO << "Goal 1 reached.";
         }
-        else if (se.isUserAbortTriggeredEvent())
+        else
         {
-            ARMARX_ERROR << "Aborted by user!";
+            if (se.isSafetyStopTriggeredEvent())
+            {
+                ARMARX_ERROR << "Safety stop was triggered!";
+            }
+            else if (se.isUserAbortTriggeredEvent())
+            {
+                ARMARX_ERROR << "Aborted by user!";
+            }
+            else if (se.isInternalErrorEvent())
+            {
+                ARMARX_ERROR << "Unknown internal error occured! "
+                             << se.toInternalErrorEvent().message;
+            }
         }
-        else if (se.isInternalErrorEvent())
+
+        goal.translation() << -1500, 1000, 0;
+        navigator.moveTo(goal, core::NavigationFrame::Absolute);
+
+        std::this_thread::sleep_for(15s);
+
+        // Wait until goal is reached
+        se = navigator.waitForStop();
+        if (se)
         {
-            ARMARX_ERROR << "Unknown internal error occured! " << se.toInternalErrorEvent().message;
+            ARMARX_INFO << "Goal 2 reached.";
+        }
+        else
+        {
+            ARMARX_ERROR << "Could not reach goal 2!";
         }
-    }
-
-    goal.translation() << -1500, 1000, 0;
-    navigator.moveTo(goal, core::NavigationFrame::Absolute);
 
-    std::this_thread::sleep_for(15s);
+        goal.translation() << 4500, 4500, 0;
+        navigator.moveTo(goal, core::NavigationFrame::Absolute);
 
-    // Wait until goal is reached
-    se = navigator.waitForStop();
-    if (se)
-    {
-        ARMARX_INFO << "Goal 2 reached.";
-    }
-    else
-    {
-        ARMARX_ERROR << "Could not reach goal 2!";
-    }
+        // Wait until goal is reached
+        se = navigator.waitForStop();
+        if (se)
+        {
+            ARMARX_INFO << "Goal 3 reached.";
+        }
+        else
+        {
+            ARMARX_ERROR << "Could not reach goal 3!";
+        }
 
-    goal.translation() << 4500, 4500, 0;
-    navigator.moveTo(goal, core::NavigationFrame::Absolute);
+        std::this_thread::sleep_for(15s);
 
-    // Wait until goal is reached
-    se = navigator.waitForStop();
-    if (se)
-    {
-        ARMARX_INFO << "Goal 3 reached.";
-    }
-    else
-    {
-        ARMARX_ERROR << "Could not reach goal 3!";
-    }
+        // TODO example with waypoints
 
-    std::this_thread::sleep_for(15s);
+        ARMARX_INFO << "Moving into certain direction.";
+        // Start moving towards a direction
+        navigator.moveTowards(core::Direction(100, 100, 0), core::NavigationFrame::Relative);
 
-    // TODO example with waypoints
+        std::this_thread::sleep_for(3s);
 
-    ARMARX_INFO << "Moving into certain direction.";
-    // Start moving towards a direction
-    navigator.moveTowards(core::Direction(100, 100, 0), core::NavigationFrame::Relative);
+        ARMARX_INFO << "Pausing movement.";
+        navigator.pause();
+    }
 
-    std::this_thread::sleep_for(3s);
+    void
+    ExampleClient::goalReached()
+    {
+        ARMARX_IMPORTANT << "Goal reached! (method-style)";
+    }
 
-    ARMARX_INFO << "Pausing movement.";
-    navigator.pause();
-}
+    armarx::PropertyDefinitionsPtr
+    ExampleClient::createPropertyDefinitions()
+    {
+        armarx::PropertyDefinitionsPtr def =
+            new armarx::ComponentPropertyDefinitions(getConfigIdentifier());
+        return def;
+    }
 
-void
-exns::ExampleClient::goalReached()
-{
-    ARMARX_IMPORTANT << "Goal reached! (method-style)";
-}
 
-armarx::PropertyDefinitionsPtr
-exns::ExampleClient::createPropertyDefinitions()
-{
-    armarx::PropertyDefinitionsPtr def =
-        new armarx::ComponentPropertyDefinitions(getConfigIdentifier());
-    return def;
-}
+} // namespace armarx::navigation::examples