Merge pull request #3006 from randaz81/improved_fakeNav

improved fakeNavigation device

src/devices/fakeNavigationDevice/fakeNavigationDev.cpp

@@ -53,29 +53,27 @@ bool fakeNavigation :: open(yarp::os::Searchable& config)
     Property p;
     p.fromString(config.toString());
 #endif
-    navThread = new fakeNavigationThread(0.010, p);
 
-    if (!navThread->start())
+    if (rf.check("navigation_time"))
     {
-        delete navThread;
-        return false;
+        m_navig_duration_param = rf.find ("navigation_time").asInt32();
+    }
+    if (rf.check("reached_time"))
+    {
+        m_reached_duration_param = rf.find("reached_time").asInt32();
     }
 
+    this->start();
     return true;
 }
 
-fakeNavigation::fakeNavigation()
+fakeNavigation::fakeNavigation() : yarp::os::PeriodicThread(0.010)
 {
-    navThread =NULL;
 }
 
 //module cleanup
 bool fakeNavigation:: close()
 {
-    navThread->stop();
-    delete navThread;
-    navThread =NULL;
-
     return true;
 }
 
@@ -85,6 +83,7 @@ bool fakeNavigation::gotoTargetByAbsoluteLocation(Map2DLocation loc)
     {
         m_status = NavigationStatusEnum::navigation_status_moving;
         m_absgoal_loc = loc;
+        m_time_counter=m_navig_duration_param;
     }
     return true;
 }
@@ -190,21 +189,39 @@ bool fakeNavigation::getRelativeLocationOfCurrentTarget(double& x, double& y, do
     return true;
 }
 
-fakeNavigationThread::fakeNavigationThread(double _period, yarp::os::Searchable& _cfg) : PeriodicThread(_period)
-{
-}
-
-bool fakeNavigationThread::threadInit()
+bool fakeNavigation::threadInit()
 {
     return true;
 }
 
-void fakeNavigationThread::threadRelease()
+void fakeNavigation::threadRelease()
 {
 
 }
 
-void fakeNavigationThread::run()
+void fakeNavigation::run()
 {
-
+    if (m_status == NavigationStatusEnum::navigation_status_moving)
+    {
+        if (m_time_counter>0)
+        {
+            m_time_counter--;
+        }
+        else
+        {
+            m_status = NavigationStatusEnum::navigation_status_goal_reached;
+            m_time_counter = m_reached_duration_param;
+        }
+    }
+    if (m_status == NavigationStatusEnum::navigation_status_goal_reached)
+    {
+        if (m_time_counter > 0)
+        {
+            m_time_counter--;
+        }
+        else
+        {
+            m_status = NavigationStatusEnum::navigation_status_idle;
+        }
+    }
 }

src/devices/fakeNavigationDevice/fakeNavigationDev.h

@@ -12,17 +12,6 @@
 #include <yarp/os/PeriodicThread.h>
 #include <math.h>
 
-
-class fakeNavigationThread :
-        public yarp::os::PeriodicThread
-{
-public:
-    fakeNavigationThread(double _period, yarp::os::Searchable& _cfg);
-    virtual bool threadInit() override;
-    virtual void threadRelease() override;
-    virtual void run() override;
-};
-
 /**
  * @ingroup dev_impl_fake dev_impl_navigation
  *
@@ -32,7 +21,8 @@ class fakeNavigation :
         public yarp::dev::DeviceDriver,
         public yarp::dev::Nav2D::INavigation2DTargetActions,
         public yarp::dev::Nav2D::INavigation2DControlActions,
-        public yarp::dev::Nav2D::INavigation2DVelocityActions
+        public yarp::dev::Nav2D::INavigation2DVelocityActions,
+        public yarp::os::PeriodicThread
 {
 private:
     yarp::dev::Nav2D::NavigationStatusEnum m_status = yarp::dev::Nav2D::NavigationStatusEnum::navigation_status_idle;
@@ -47,8 +37,9 @@ class fakeNavigation :
     }
     m_control_out;
 
-public:
-    fakeNavigationThread  *navThread=nullptr;
+    int m_reached_duration_param = 100;
+    int m_navig_duration_param = 500;
+    int m_time_counter= m_navig_duration_param;
 
 public:
     virtual bool open(yarp::os::Searchable& config) override;
@@ -75,4 +66,8 @@ class fakeNavigation :
     bool recomputeCurrentNavigationPath() override;
     bool applyVelocityCommand(double x_vel, double y_vel, double theta_vel, double timeout = 0.1) override;
     bool getLastVelocityCommand(double& x_vel, double& y_vel, double& theta_vel) override;
+
+    bool threadInit() override;
+    void threadRelease() override;
+    void run() override;
 };

src/devices/fakeNavigationDevice/tests/fakeNavigation_test.cpp

@@ -46,6 +46,7 @@ TEST_CASE("dev::FakeNavigationTest", "[yarp::dev]")
         // Do tests
         //yarp::dev::tests::exec_iNav2D_test_1(inav, imap);
         //yarp::dev::tests::exec_iNav2D_test_2(inav, imap);
+        yarp::dev::tests::exec_iNav2D_test_3(inav_target, inav_control);
 
         //"Close all polydrivers and check"
         {

src/libYARP_dev/src/yarp/dev/tests/INavigation2DTest.h

@@ -181,6 +181,39 @@ namespace yarp::dev::tests
         b1 = inav->gotoTargetByLocationName("non-existing-loc"); CHECK(b1 == false);
         b1 = inav->getAbsoluteLocationOfCurrentTarget(gloc1); CHECK(b1); CHECK(gloc1 == gloc_empty);
     }
+
+    inline void exec_iNav2D_test_3(INavigation2DTargetActions* inav_trgt, INavigation2DControlActions* inav_ctl)
+    {
+        //////////"Checking INavigation2D methods
+        bool b;
+        Map2DLocation loc("test", 1, 2, 3);
+        yarp::dev::Nav2D::NavigationStatusEnum status;
+
+        b = inav_ctl->getNavigationStatus(status); CHECK(b);
+        CHECK(status== yarp::dev::Nav2D::NavigationStatusEnum::navigation_status_idle);
+
+        b = inav_trgt->gotoTargetByAbsoluteLocation(loc); CHECK(b);
+        b = inav_ctl->getNavigationStatus(status); CHECK(b);
+        CHECK(status == yarp::dev::Nav2D::NavigationStatusEnum::navigation_status_moving);
+
+        size_t count=0;
+        do
+        {
+            b = inav_ctl->getNavigationStatus(status); CHECK(b);
+            if (status == yarp::dev::Nav2D::NavigationStatusEnum::navigation_status_goal_reached)
+            {
+                break;
+            }
+            yarp::os::Time::delay(0.1);
+            count++;
+            if (count>200) {CHECK(0); break; }
+        }
+        while(1);
+
+        yarp::os::Time::delay(2);
+        b = inav_ctl->getNavigationStatus(status); CHECK(b);
+        CHECK(status == yarp::dev::Nav2D::NavigationStatusEnum::navigation_status_idle);
+    }
 }
 
 #endif