From 6c71f9ae3f235cbba8983cc1546ecd29cfb4c7c4 Mon Sep 17 00:00:00 2001 From: fbrand-new Date: Wed, 28 Aug 2024 14:47:15 +0200 Subject: [PATCH 1/2] Added: - followPath interface to Navigation2D - gotoTargetByLocationName also navigates to path - Added changelog entry --- doc/release/master.md | 5 + .../fakeNavigationDevice/FakeNavigation.cpp | 7 + .../fakeNavigationDevice/FakeNavigation.h | 2 + .../INavigation2DMsgs.thrift | 1 + .../idl_generated_code/INavigation2DMsgs.cpp | 246 ++++++++++++++++++ .../idl_generated_code/INavigation2DMsgs.h | 3 + .../Navigation2D_nwc_yarp.cpp | 15 +- .../Navigation2D_nwc_yarp.h | 2 + .../Navigation2D_nwc_yarp_iNav2DTarget.cpp | 6 + .../INavigation2DServerImpl.cpp | 15 ++ .../INavigation2DServerImpl.h | 2 + src/libYARP_dev/src/yarp/dev/INavigation2D.h | 7 + 12 files changed, 310 insertions(+), 1 deletion(-) diff --git a/doc/release/master.md b/doc/release/master.md index de5401ddb77..b7ce13a493c 100644 --- 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 index 28937a6f3af..087dc298f33 100644 --- a/src/devices/fake/fakeNavigationDevice/FakeNavigation.cpp +++ b/src/devices/fake/fakeNavigationDevice/FakeNavigation.cpp @@ -11,6 +11,7 @@ #include #include #include "FakeNavigation.h" +#include "yarp/dev/Map2DPath.h" #include #include @@ -65,6 +66,12 @@ bool FakeNavigation::gotoTargetByRelativeLocation(double x, double y) return true; } +bool followPath(const yarp::dev::Nav2D::Map2DPath& locs) +{ + 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 index 8097bb813f9..ddc365ea8c9 100644 --- a/src/devices/fake/fakeNavigationDevice/FakeNavigation.h +++ b/src/devices/fake/fakeNavigationDevice/FakeNavigation.h @@ -10,6 +10,7 @@ #include #include #include +#include #include #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 index c12a251391b..9c14c1e53ae 100644 --- 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 index b6f0be31a1e..f8f474764ac 100644 --- a/src/devices/messages/INavigation2DMsgs/idl_generated_code/INavigation2DMsgs.cpp +++ b/src/devices/messages/INavigation2DMsgs/idl_generated_code/INavigation2DMsgs.cpp @@ -556,6 +556,69 @@ class INavigation2DMsgs_goto_target_by_absolute_location_RPC_helper : static constexpr const char* s_help{""}; }; +// follow_path_RPC helper class declaration +class INavigation2DMsgs_follow_path_RPC_helper : + public yarp::os::Portable +{ +public: + INavigation2DMsgs_follow_path_RPC_helper() = default; + explicit INavigation2DMsgs_follow_path_RPC_helper(const yarp::dev::Nav2D::Map2DPath& path); + bool write(yarp::os::ConnectionWriter& connection) const override; + bool read(yarp::os::ConnectionReader& connection) override; + + class Command : + public yarp::os::idl::WirePortable + { + public: + Command() = default; + explicit Command(const yarp::dev::Nav2D::Map2DPath& path); + + ~Command() override = default; + + bool write(yarp::os::ConnectionWriter& connection) const override; + bool read(yarp::os::ConnectionReader& connection) override; + + bool write(const yarp::os::idl::WireWriter& writer) const override; + bool writeTag(const yarp::os::idl::WireWriter& writer) const; + bool writeArgs(const yarp::os::idl::WireWriter& writer) const; + + bool read(yarp::os::idl::WireReader& reader) override; + bool readTag(yarp::os::idl::WireReader& reader); + bool readArgs(yarp::os::idl::WireReader& reader); + + yarp::dev::Nav2D::Map2DPath path{}; + }; + + class Reply : + public yarp::os::idl::WirePortable + { + public: + Reply() = default; + ~Reply() override = default; + + bool write(yarp::os::ConnectionWriter& connection) const override; + bool read(yarp::os::ConnectionReader& connection) override; + + bool write(const yarp::os::idl::WireWriter& writer) const override; + bool read(yarp::os::idl::WireReader& reader) override; + + bool return_helper{false}; + }; + + using funcptr_t = bool (*)(const yarp::dev::Nav2D::Map2DPath&); + void call(INavigation2DMsgs* ptr); + + Command cmd; + Reply reply; + + static constexpr const char* s_tag{"follow_path_RPC"}; + static constexpr size_t s_tag_len{3}; + static constexpr size_t s_cmd_len{4}; + static constexpr size_t s_reply_len{1}; + static constexpr const char* s_prototype{"bool INavigation2DMsgs::follow_path_RPC(const yarp::dev::Nav2D::Map2DPath& path)"}; + static constexpr const char* s_help{""}; +}; + // goto_target_by_relative_location1_RPC helper class declaration class INavigation2DMsgs_goto_target_by_relative_location1_RPC_helper : public yarp::os::Portable @@ -2332,6 +2395,160 @@ void INavigation2DMsgs_goto_target_by_absolute_location_RPC_helper::call(INaviga reply.return_helper = ptr->goto_target_by_absolute_location_RPC(cmd.loc); } +// follow_path_RPC helper class implementation +INavigation2DMsgs_follow_path_RPC_helper::INavigation2DMsgs_follow_path_RPC_helper(const yarp::dev::Nav2D::Map2DPath& path) : + cmd{path} +{ +} + +bool INavigation2DMsgs_follow_path_RPC_helper::write(yarp::os::ConnectionWriter& connection) const +{ + return cmd.write(connection); +} + +bool INavigation2DMsgs_follow_path_RPC_helper::read(yarp::os::ConnectionReader& connection) +{ + return reply.read(connection); +} + +INavigation2DMsgs_follow_path_RPC_helper::Command::Command(const yarp::dev::Nav2D::Map2DPath& path) : + path{path} +{ +} + +bool INavigation2DMsgs_follow_path_RPC_helper::Command::write(yarp::os::ConnectionWriter& connection) const +{ + yarp::os::idl::WireWriter writer(connection); + if (!writer.writeListHeader(s_cmd_len)) { + return false; + } + return write(writer); +} + +bool INavigation2DMsgs_follow_path_RPC_helper::Command::read(yarp::os::ConnectionReader& connection) +{ + yarp::os::idl::WireReader reader(connection); + if (!reader.readListHeader()) { + reader.fail(); + return false; + } + return read(reader); +} + +bool INavigation2DMsgs_follow_path_RPC_helper::Command::write(const yarp::os::idl::WireWriter& writer) const +{ + if (!writeTag(writer)) { + return false; + } + if (!writeArgs(writer)) { + return false; + } + return true; +} + +bool INavigation2DMsgs_follow_path_RPC_helper::Command::writeTag(const yarp::os::idl::WireWriter& writer) const +{ + if (!writer.writeTag(s_tag, 1, s_tag_len)) { + return false; + } + return true; +} + +bool INavigation2DMsgs_follow_path_RPC_helper::Command::writeArgs(const yarp::os::idl::WireWriter& writer) const +{ + if (!writer.writeNested(path)) { + return false; + } + return true; +} + +bool INavigation2DMsgs_follow_path_RPC_helper::Command::read(yarp::os::idl::WireReader& reader) +{ + if (!readTag(reader)) { + return false; + } + if (!readArgs(reader)) { + return false; + } + return true; +} + +bool INavigation2DMsgs_follow_path_RPC_helper::Command::readTag(yarp::os::idl::WireReader& reader) +{ + std::string tag = reader.readTag(s_tag_len); + if (reader.isError()) { + return false; + } + if (tag != s_tag) { + reader.fail(); + return false; + } + return true; +} + +bool INavigation2DMsgs_follow_path_RPC_helper::Command::readArgs(yarp::os::idl::WireReader& reader) +{ + if (reader.noMore()) { + reader.fail(); + return false; + } + if (!reader.readNested(path)) { + reader.fail(); + return false; + } + if (!reader.noMore()) { + reader.fail(); + return false; + } + return true; +} + +bool INavigation2DMsgs_follow_path_RPC_helper::Reply::write(yarp::os::ConnectionWriter& connection) const +{ + yarp::os::idl::WireWriter writer(connection); + return write(writer); +} + +bool INavigation2DMsgs_follow_path_RPC_helper::Reply::read(yarp::os::ConnectionReader& connection) +{ + yarp::os::idl::WireReader reader(connection); + return read(reader); +} + +bool INavigation2DMsgs_follow_path_RPC_helper::Reply::write(const yarp::os::idl::WireWriter& writer) const +{ + if (!writer.isNull()) { + if (!writer.writeListHeader(s_reply_len)) { + return false; + } + if (!writer.writeBool(return_helper)) { + return false; + } + } + return true; +} + +bool INavigation2DMsgs_follow_path_RPC_helper::Reply::read(yarp::os::idl::WireReader& reader) +{ + if (!reader.readListReturn()) { + return false; + } + if (reader.noMore()) { + reader.fail(); + return false; + } + if (!reader.readBool(return_helper)) { + reader.fail(); + return false; + } + return true; +} + +void INavigation2DMsgs_follow_path_RPC_helper::call(INavigation2DMsgs* ptr) +{ + reply.return_helper = ptr->follow_path_RPC(cmd.path); +} + // goto_target_by_relative_location1_RPC helper class implementation INavigation2DMsgs_goto_target_by_relative_location1_RPC_helper::INavigation2DMsgs_goto_target_by_relative_location1_RPC_helper(const double x, const double y) : cmd{x, y} @@ -3660,6 +3877,16 @@ bool INavigation2DMsgs::goto_target_by_absolute_location_RPC(const yarp::dev::Na return ok ? helper.reply.return_helper : bool{}; } +bool INavigation2DMsgs::follow_path_RPC(const yarp::dev::Nav2D::Map2DPath& path) +{ + if (!yarp().canWrite()) { + yError("Missing server method '%s'?", INavigation2DMsgs_follow_path_RPC_helper::s_prototype); + } + INavigation2DMsgs_follow_path_RPC_helper helper{path}; + bool ok = yarp().write(helper, helper); + return ok ? helper.reply.return_helper : bool{}; +} + bool INavigation2DMsgs::goto_target_by_relative_location1_RPC(const double x, const double y) { if (!yarp().canWrite()) { @@ -3756,6 +3983,7 @@ std::vector INavigation2DMsgs::help(const std::string& functionName helpString.emplace_back(INavigation2DMsgs_get_all_navigation_waypoints_RPC_helper::s_tag); helpString.emplace_back(INavigation2DMsgs_get_current_navigation_map_RPC_helper::s_tag); helpString.emplace_back(INavigation2DMsgs_goto_target_by_absolute_location_RPC_helper::s_tag); + helpString.emplace_back(INavigation2DMsgs_follow_path_RPC_helper::s_tag); helpString.emplace_back(INavigation2DMsgs_goto_target_by_relative_location1_RPC_helper::s_tag); helpString.emplace_back(INavigation2DMsgs_goto_target_by_relative_location2_RPC_helper::s_tag); helpString.emplace_back(INavigation2DMsgs_get_absolute_location_of_current_target_RPC_helper::s_tag); @@ -3793,6 +4021,9 @@ std::vector INavigation2DMsgs::help(const std::string& functionName if (functionName == INavigation2DMsgs_goto_target_by_absolute_location_RPC_helper::s_tag) { helpString.emplace_back(INavigation2DMsgs_goto_target_by_absolute_location_RPC_helper::s_prototype); } + if (functionName == INavigation2DMsgs_follow_path_RPC_helper::s_tag) { + helpString.emplace_back(INavigation2DMsgs_follow_path_RPC_helper::s_prototype); + } if (functionName == INavigation2DMsgs_goto_target_by_relative_location1_RPC_helper::s_tag) { helpString.emplace_back(INavigation2DMsgs_goto_target_by_relative_location1_RPC_helper::s_prototype); } @@ -3984,6 +4215,21 @@ bool INavigation2DMsgs::read(yarp::os::ConnectionReader& connection) reader.accept(); return true; } + if (tag == INavigation2DMsgs_follow_path_RPC_helper::s_tag) { + INavigation2DMsgs_follow_path_RPC_helper helper; + if (!helper.cmd.readArgs(reader)) { + return false; + } + + helper.call(this); + + yarp::os::idl::WireWriter writer(reader); + if (!helper.reply.write(writer)) { + return false; + } + reader.accept(); + return true; + } if (tag == INavigation2DMsgs_goto_target_by_relative_location1_RPC_helper::s_tag) { INavigation2DMsgs_goto_target_by_relative_location1_RPC_helper helper; if (!helper.cmd.readArgs(reader)) { diff --git a/src/devices/messages/INavigation2DMsgs/idl_generated_code/INavigation2DMsgs.h b/src/devices/messages/INavigation2DMsgs/idl_generated_code/INavigation2DMsgs.h index ae3aba3266d..389672aa34e 100644 --- a/src/devices/messages/INavigation2DMsgs/idl_generated_code/INavigation2DMsgs.h +++ b/src/devices/messages/INavigation2DMsgs/idl_generated_code/INavigation2DMsgs.h @@ -23,6 +23,7 @@ #include #include #include +#include class INavigation2DMsgs : public yarp::os::Wire @@ -49,6 +50,8 @@ class INavigation2DMsgs : virtual bool goto_target_by_absolute_location_RPC(const yarp::dev::Nav2D::Map2DLocation& loc); + virtual bool follow_path_RPC(const yarp::dev::Nav2D::Map2DPath& path); + virtual bool goto_target_by_relative_location1_RPC(const double x, const double y); virtual bool goto_target_by_relative_location2_RPC(const double x, const double y, const double theta); diff --git a/src/devices/networkWrappers/navigation2D_nwc_yarp/Navigation2D_nwc_yarp.cpp b/src/devices/networkWrappers/navigation2D_nwc_yarp/Navigation2D_nwc_yarp.cpp index 622b3b59a88..e41d0abf765 100644 --- 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 #include #include +#include "yarp/dev/Map2DPath.h" #include #include @@ -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 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"; diff --git a/src/devices/networkWrappers/navigation2D_nwc_yarp/Navigation2D_nwc_yarp.h b/src/devices/networkWrappers/navigation2D_nwc_yarp/Navigation2D_nwc_yarp.h index 07696e9f69f..e72bb94a091 100644 --- a/src/devices/networkWrappers/navigation2D_nwc_yarp/Navigation2D_nwc_yarp.h +++ b/src/devices/networkWrappers/navigation2D_nwc_yarp/Navigation2D_nwc_yarp.h @@ -13,6 +13,7 @@ #include #include #include +#include #include "IMap2DMsgs.h" #include "ILocalization2DMsgs.h" #include "INavigation2DMsgs.h" @@ -84,6 +85,7 @@ class Navigation2D_nwc_yarp: bool gotoTargetByLocationName(std::string location_name) override; bool gotoTargetByRelativeLocation(double x, double y, double theta) override; bool gotoTargetByRelativeLocation(double x, double y) override; + bool followPath(const yarp::dev::Nav2D::Map2DPath& locs) override; bool getAbsoluteLocationOfCurrentTarget(yarp::dev::Nav2D::Map2DLocation& loc) override; bool getNameOfCurrentTarget(std::string& location_name) override; bool getRelativeLocationOfCurrentTarget(double& x, double& y, double& theta) override; diff --git a/src/devices/networkWrappers/navigation2D_nwc_yarp/Navigation2D_nwc_yarp_iNav2DTarget.cpp b/src/devices/networkWrappers/navigation2D_nwc_yarp/Navigation2D_nwc_yarp_iNav2DTarget.cpp index 55af7cd4e4e..457dac83264 100644 --- a/src/devices/networkWrappers/navigation2D_nwc_yarp/Navigation2D_nwc_yarp_iNav2DTarget.cpp +++ b/src/devices/networkWrappers/navigation2D_nwc_yarp/Navigation2D_nwc_yarp_iNav2DTarget.cpp @@ -69,3 +69,9 @@ bool Navigation2D_nwc_yarp::getRelativeLocationOfCurrentTarget(double& x, double theta = ret.theta; return true; } + +bool Navigation2D_nwc_yarp::followPath(const yarp::dev::Nav2D::Map2DPath& locs) +{ + std::lock_guard lg(m_mutex); + return m_nav_RPC.follow_path_RPC(locs); +} diff --git a/src/devices/networkWrappers/navigation2D_nws_yarp/INavigation2DServerImpl.cpp b/src/devices/networkWrappers/navigation2D_nws_yarp/INavigation2DServerImpl.cpp index 3d7416df9ce..fbda957ed17 100644 --- a/src/devices/networkWrappers/navigation2D_nws_yarp/INavigation2DServerImpl.cpp +++ b/src/devices/networkWrappers/navigation2D_nws_yarp/INavigation2DServerImpl.cpp @@ -5,6 +5,7 @@ #include #include +#include "yarp/dev/Map2DPath.h" #include "INavigation2DServerImpl.h" /*! \file INavigation2DServerImpl.cpp */ @@ -229,6 +230,20 @@ bool INavigation2DRPCd::goto_target_by_absolute_location_and_set_name_RPC(const return true; } +bool INavigation2DRPCd::follow_path_RPC(const yarp::dev::Nav2D::Map2DPath& path) +{ + std::lock_guard lg(m_mutex); + {if (m_iNav_target == nullptr) { yCError(NAVIGATION2DSERVER, "Invalid interface"); return false; }} + + if (!m_iNav_target->followPath(path)) + { + yCError(NAVIGATION2DSERVER, "Unable to follow path"); + return false; + } + m_current_goal_name.clear(); + return true; +} + return_get_abs_loc_of_curr_target INavigation2DRPCd::get_absolute_location_of_current_target_RPC() { std::lock_guard lg(m_mutex); diff --git a/src/devices/networkWrappers/navigation2D_nws_yarp/INavigation2DServerImpl.h b/src/devices/networkWrappers/navigation2D_nws_yarp/INavigation2DServerImpl.h index 536ef8486f6..e3244a3edcd 100644 --- a/src/devices/networkWrappers/navigation2D_nws_yarp/INavigation2DServerImpl.h +++ b/src/devices/networkWrappers/navigation2D_nws_yarp/INavigation2DServerImpl.h @@ -7,6 +7,7 @@ #define YARP_DEV_NAVIGATION2DSERVERIMPL_H #include "INavigation2DMsgs.h" +#include "yarp/dev/Map2DPath.h" #include #include @@ -52,6 +53,7 @@ class INavigation2DRPCd : public INavigation2DMsgs bool goto_target_by_absolute_location_RPC(const yarp::dev::Nav2D::Map2DLocation& loc) override; bool goto_target_by_relative_location1_RPC(double x, double y) override; bool goto_target_by_relative_location2_RPC(double x, double y, double theta) override; + bool follow_path_RPC(const yarp::dev::Nav2D::Map2DPath& path) override; return_get_abs_loc_of_curr_target get_absolute_location_of_current_target_RPC() override; return_get_rel_loc_of_curr_target get_relative_location_of_current_target_RPC() override; bool goto_target_by_absolute_location_and_set_name_RPC(const yarp::dev::Nav2D::Map2DLocation& loc, const std::string& name) override; diff --git a/src/libYARP_dev/src/yarp/dev/INavigation2D.h b/src/libYARP_dev/src/yarp/dev/INavigation2D.h index d326430396a..48fb57c019d 100644 --- a/src/libYARP_dev/src/yarp/dev/INavigation2D.h +++ b/src/libYARP_dev/src/yarp/dev/INavigation2D.h @@ -105,6 +105,13 @@ class YARP_dev_API yarp::dev::Nav2D::INavigation2DTargetActions */ virtual bool gotoTargetByAbsoluteLocation(yarp::dev::Nav2D::Map2DLocation loc) = 0; + /** + * Ask the robot to navigate through a set of locations defined in the world reference frame + * @param path the locations to be reached + * @return true/false + */ + virtual bool followPath(const yarp::dev::Nav2D::Map2DPath& path) = 0; + /** * Gets the last navigation target in the world reference frame * @param loc the location of the robot From 84877a19a6f95b590d5b46f84486a36d313720c3 Mon Sep 17 00:00:00 2001 From: fbrand-new Date: Thu, 5 Sep 2024 12:47:10 +0200 Subject: [PATCH 2/2] Fixed fake navigation device --- src/devices/fake/fakeNavigationDevice/FakeNavigation.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/devices/fake/fakeNavigationDevice/FakeNavigation.cpp b/src/devices/fake/fakeNavigationDevice/FakeNavigation.cpp index 087dc298f33..3cda4f39052 100644 --- a/src/devices/fake/fakeNavigationDevice/FakeNavigation.cpp +++ b/src/devices/fake/fakeNavigationDevice/FakeNavigation.cpp @@ -66,7 +66,7 @@ bool FakeNavigation::gotoTargetByRelativeLocation(double x, double y) return true; } -bool followPath(const yarp::dev::Nav2D::Map2DPath& locs) +bool FakeNavigation::followPath(const yarp::dev::Nav2D::Map2DPath& path) { yCInfo(FAKENAVIGATION) << "followPath not yet implemented"; return true;