Merge pull request #3128 from fbrand-new/feat/navThroughPoses

Navigation through path
robotology · Sep 5, 2024 · f6ae5af · f6ae5af
2 parents 1fde37e + 84877a1
commit f6ae5af
Show file tree

Hide file tree

Showing 12 changed files with 310 additions and 1 deletion.
diff --git a/doc/release/master.md b/doc/release/master.md
@@ -70,6 +70,11 @@ New Features
 * Added LLM_Message data type to propagate LLM answers
 * Added refreshConversation feature in the interface to allow users to restart the conversation mantaining the same prompt.
 
+#### Navigation2D
+
+* Added followPath functionality
+* Added paths as possible arguments in gotoLocation in Navigation2D_nwc
+
 ### GUIs
 
 #### yarpopencvdisplay

diff --git a/src/devices/fake/fakeNavigationDevice/FakeNavigation.cpp b/src/devices/fake/fakeNavigationDevice/FakeNavigation.cpp
@@ -11,6 +11,7 @@
 #include <yarp/os/LogStream.h>
 #include <yarp/dev/ControlBoardInterfaces.h>
 #include "FakeNavigation.h"
+#include "yarp/dev/Map2DPath.h"
 #include <math.h>
 #include <cmath>
 
@@ -65,6 +66,12 @@ bool FakeNavigation::gotoTargetByRelativeLocation(double x, double y)
     return true;
 }
 
+bool FakeNavigation::followPath(const yarp::dev::Nav2D::Map2DPath& path)
+{
+    yCInfo(FAKENAVIGATION) << "followPath not yet implemented";
+    return true;
+}
+
 bool FakeNavigation::applyVelocityCommand(double x_vel, double y_vel, double theta_vel, double timeout)
 {
     m_control_out.linear_xvel = x_vel;

diff --git a/src/devices/fake/fakeNavigationDevice/FakeNavigation.h b/src/devices/fake/fakeNavigationDevice/FakeNavigation.h
@@ -10,6 +10,7 @@
 #include <yarp/dev/INavigation2D.h>
 #include <yarp/dev/ControlBoardInterfaces.h>
 #include <yarp/os/PeriodicThread.h>
+#include <yarp/dev/Map2DPath.h>
 #include <math.h>
 
 #include "FakeNavigation_ParamsParser.h"
@@ -57,6 +58,7 @@ class FakeNavigation :
     bool gotoTargetByAbsoluteLocation(yarp::dev::Nav2D::Map2DLocation loc) override;
     bool gotoTargetByRelativeLocation(double x, double y, double theta) override;
     bool gotoTargetByRelativeLocation(double x, double y) override;
+    bool followPath(const yarp::dev::Nav2D::Map2DPath& path) override;
     bool getAbsoluteLocationOfCurrentTarget(yarp::dev::Nav2D::Map2DLocation& target) override;
     bool getRelativeLocationOfCurrentTarget(double& x, double& y, double& theta) override;
     bool getNavigationStatus(yarp::dev::Nav2D::NavigationStatusEnum& status) override;

diff --git a/src/devices/messages/INavigation2DMsgs/INavigation2DMsgs.thrift b/src/devices/messages/INavigation2DMsgs/INavigation2DMsgs.thrift
@@ -108,6 +108,7 @@ service INavigation2DMsgs
   return_get_current_nav_map         get_current_navigation_map_RPC    (1:yarp_dev_Nav2D_NavigationMapTypeEnum map_type);
 
   bool                               goto_target_by_absolute_location_RPC         (1:yarp_dev_Nav2D_Map2DLocation loc);
+  bool                               follow_path_RPC  (1:yarp_dev_Nav2D_Map2DPath path);
   bool                               goto_target_by_relative_location1_RPC        (1:double x, 2:double y);
   bool                               goto_target_by_relative_location2_RPC        (1:double x, 2:double y, 3: double theta);
   return_get_abs_loc_of_curr_target  get_absolute_location_of_current_target_RPC  ();

diff --git a/src/devices/messages/INavigation2DMsgs/idl_generated_code/INavigation2DMsgs.cpp b/src/devices/messages/INavigation2DMsgs/idl_generated_code/INavigation2DMsgs.cpp
diff --git a/src/devices/messages/INavigation2DMsgs/idl_generated_code/INavigation2DMsgs.h b/src/devices/messages/INavigation2DMsgs/idl_generated_code/INavigation2DMsgs.h
diff --git a/src/devices/networkWrappers/navigation2D_nwc_yarp/Navigation2D_nwc_yarp.cpp b/src/devices/networkWrappers/navigation2D_nwc_yarp/Navigation2D_nwc_yarp.cpp
@@ -8,6 +8,7 @@
 #include <yarp/os/Log.h>
 #include <yarp/os/LogComponent.h>
 #include <yarp/os/LogStream.h>
+#include "yarp/dev/Map2DPath.h"
 #include <mutex>
 #include <cmath>
 
@@ -257,6 +258,7 @@ bool Navigation2D_nwc_yarp::gotoTargetByLocationName(std::string location_name)
 {
     Map2DLocation loc;
     Map2DArea area;
+    Map2DPath path;
 
     //first of all, ask to the location server if location_name exists as a location_name...
     bool found = this->getLocation(location_name, loc);
@@ -271,7 +273,18 @@ bool Navigation2D_nwc_yarp::gotoTargetByLocationName(std::string location_name)
         }
     }
 
-    //...if it is neither a location, nor an area then quit...
+    //...if it is neither a location, nor an area then check if it is a path...
+    if (found == false)
+    {
+        if(this->getPath(location_name, path))
+        {
+            // We still need to handle path navigation differently
+            std::lock_guard <std::mutex> lg(m_mutex);
+            return m_nav_RPC.follow_path_RPC(path);
+        }
+    }
+
+    //...if it is neither a location, nor an area, nor a path then quit...
     if (found == false)
     {
         yCError(NAVIGATION2D_NWC_YARP) << "Location not found, stopping navigation";