lagadic · fspindle · May 14, 2024 · May 13, 2024 · May 14, 2024
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -1743,9 +1743,18 @@ status("")
 status("  Real robots: ")
 status("    Use Afma4:"              USE_AFMA4        THEN "yes" ELSE "no")
 status("    Use Afma6:"              USE_AFMA6        THEN "yes" ELSE "no")
+if(USE_AFMA6)
+status("    \\- Data:"               AFMA6_DATA_FOUND THEN "yes (in ${AFMA6_DATA_PATH})" ELSE "no")
+endif()
 status("    Use Franka:"             USE_FRANKA       THEN "yes (ver ${Franka_VERSION})" ELSE "no")
 status("    Use Viper650:"           USE_VIPER650     THEN "yes" ELSE "no")
+if(USE_VIPER650)
+status("    \\- Data:"               VIPER650_DATA_FOUND THEN "yes (in ${VIPER650_DATA_PATH})" ELSE "no")
+endif()
 status("    Use Viper850:"           USE_VIPER850     THEN "yes" ELSE "no")
+if(USE_VIPER850)
+status("    \\- Data:"               VIPER850_DATA_FOUND THEN "yes (in ${VIPER850_DATA_PATH})" ELSE "no")
+endif()
 status("    Use ur_rtde:"            USE_UR_RTDE      THEN "yes (ver ${ur_rtde_VERSION})" ELSE "no")
 status("    Use Kinova Jaco:"        USE_JACOSDK      THEN "yes" ELSE "no")
 status("    Use aria (Pioneer):"     USE_ARIA         THEN "yes" ELSE "no")

diff --git a/cmake/FindAfma6_data.cmake b/cmake/FindAfma6_data.cmake
@@ -45,6 +45,10 @@ find_path(AFMA6_DATA_PATH
     /home/soft/Afma6/current
     /udd/fspindle/robot/Afma6/current
     Z:/robot/Afma6/current
+    /home/soft/afma6/afma6
+    /home/soft/afma6/current
+    /udd/fspindle/robot/afma6/current
+    Z:/robot/afma6/current
   )
 
 #message("DBG AFMA6_DATA_PATH=${AFMA6_DATA_PATH}")
@@ -58,4 +62,3 @@ endif()
 mark_as_advanced(
   AFMA6_DATA_PATH
 )
-
diff --git a/cmake/FindViper650_data.cmake b/cmake/FindViper650_data.cmake
@@ -59,4 +59,3 @@ endif()
 mark_as_advanced(
   VIPER650_DATA_PATH
 )
-
diff --git a/example/servo-afma6/servoAfma62DhalfCamVelocity.cpp b/example/servo-afma6/servoAfma62DhalfCamVelocity.cpp
@@ -71,7 +71,7 @@
 #include <visp3/gui/vpDisplayOpenCV.h>
 #include <visp3/gui/vpDisplayX.h>
 #include <visp3/io/vpImageIo.h>
-#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/sensor/vpRealSense2.h>
 
 #include <visp3/core/vpHomogeneousMatrix.h>
 #include <visp3/core/vpLine.h>
@@ -99,12 +99,17 @@ int main()
   try {
     vpImage<unsigned char> I;
 
-    vp1394TwoGrabber g;
-    g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_640x480_MONO8);
-    g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_60);
-    g.open(I);
+    vpRealSense2 rs;
+    rs2::config config;
+    config.enable_stream(RS2_STREAM_COLOR, 640, 480, RS2_FORMAT_RGBA8, 30);
+    config.enable_stream(RS2_STREAM_DEPTH, 640, 480, RS2_FORMAT_Z16, 30);
+    config.enable_stream(RS2_STREAM_INFRARED, 640, 480, RS2_FORMAT_Y8, 30);
+    rs.open(config);
 
-    g.acquire(I);
+    // Warm up camera
+    for (size_t i = 0; i < 10; ++i) {
+      rs.acquire(I);
+    }
 
 #ifdef VISP_HAVE_X11
     vpDisplayX display(I, 100, 100, "Current image");
@@ -120,6 +125,7 @@ int main()
     vpServo task;
 
     vpRobotAfma6 robot;
+    robot.init(vpAfma6::TOOL_INTEL_D435_CAMERA, vpCameraParameters::perspectiveProjWithoutDistortion);
     // robot.move("zero.pos") ;
 
     vpCameraParameters cam;
@@ -271,11 +277,12 @@ int main()
     double alpha = 0.05;
     double beta = 3;
 
-    for (;;) {
+    bool quit = false;
+    while (!quit) {
       std::cout << "---------------------------------------------" << iter << std::endl;
 
       try {
-        g.acquire(I);
+        rs.acquire(I);
         vpDisplay::display(I);
 
         pose.clearPoint();
@@ -339,7 +346,6 @@ int main()
 
         v = task.computeControlLaw();
 
-        vpDisplay::flush(I);
         std::cout << v.sumSquare() << std::endl;
         if (iter == 0)
           vpDisplay::getClick(I);
@@ -352,6 +358,11 @@ int main()
 
         robot.setVelocity(vpRobot::CAMERA_FRAME, v);
 
+        vpDisplay::displayText(I, 20, 20, "Click to quit...", vpColor::red);
+        if (vpDisplay::getClick(I, false)) {
+          quit = true;
+        }
+        vpDisplay::flush(I);
       }
       catch (...) {
         v = 0;

diff --git a/example/servo-afma6/servoAfma6Cylinder2DCamVelocity.cpp b/example/servo-afma6/servoAfma6Cylinder2DCamVelocity.cpp
@@ -61,7 +61,7 @@
 #include <visp3/gui/vpDisplayOpenCV.h>
 #include <visp3/gui/vpDisplayX.h>
 #include <visp3/io/vpImageIo.h>
-#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/sensor/vpRealSense2.h>
 
 #include <visp3/core/vpCylinder.h>
 #include <visp3/core/vpHomogeneousMatrix.h>
@@ -82,12 +82,17 @@ int main()
   try {
     vpImage<unsigned char> I;
 
-    vp1394TwoGrabber g;
-    g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_640x480_MONO8);
-    g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_60);
-    g.open(I);
+    vpRealSense2 rs;
+    rs2::config config;
+    config.enable_stream(RS2_STREAM_COLOR, 640, 480, RS2_FORMAT_RGBA8, 30);
+    config.enable_stream(RS2_STREAM_DEPTH, 640, 480, RS2_FORMAT_Z16, 30);
+    config.enable_stream(RS2_STREAM_INFRARED, 640, 480, RS2_FORMAT_Y8, 30);
+    rs.open(config);
 
-    g.acquire(I);
+    // Warm up camera
+    for (size_t i = 0; i < 10; ++i) {
+      rs.acquire(I);
+    }
 
 #ifdef VISP_HAVE_X11
     vpDisplayX display(I, 100, 100, "Current image");
@@ -131,16 +136,18 @@ int main()
     }
 
     vpRobotAfma6 robot;
-    // robot.move("zero.pos") ;
+    robot.init(vpAfma6::TOOL_INTEL_D435_CAMERA, vpCameraParameters::perspectiveProjWithoutDistortion);
+    // robot.move("zero.pos");
 
     vpCameraParameters cam;
     // Update camera parameters
     robot.getCameraParameters(cam, I);
 
     vpTRACE("sets the current position of the visual feature ");
     vpFeatureLine p[nbline];
-    for (i = 0; i < nbline; i++)
+    for (i = 0; i < nbline; i++) {
       vpFeatureBuilder::create(p[i], cam, line[i]);
+    }
 
     vpTRACE("sets the desired position of the visual feature ");
     vpCylinder cyld(0, 1, 0, 0, 0, 0, 0.04);
@@ -168,8 +175,9 @@ int main()
 
     vpTRACE("\t we want to see a two lines on two lines..");
     std::cout << std::endl;
-    for (i = 0; i < nbline; i++)
+    for (i = 0; i < nbline; i++) {
       task.addFeature(p[i], pd[i]);
+    }
 
     vpTRACE("\t set the gain");
     task.setLambda(0.2);
@@ -186,11 +194,12 @@ int main()
     double lambda_av = 0.05;
     double alpha = 0.2;
     double beta = 3;
-    for (;;) {
+    bool quit = false;
+    while (!quit) {
       std::cout << "---------------------------------------------" << iter << std::endl;
 
       try {
-        g.acquire(I);
+        rs.acquire(I);
         vpDisplay::display(I);
 
         // Track the two edges and update the features
@@ -205,7 +214,6 @@ int main()
           pd[i].display(cam, I, vpColor::green);
         }
 
-        vpDisplay::flush(I);
 
         // Adaptative gain
         double gain;
@@ -220,15 +228,22 @@ int main()
 
         v = task.computeControlLaw();
 
-        if (iter == 0)
+        if (iter == 0) {
           vpDisplay::getClick(I);
+        }
         robot.setVelocity(vpRobot::CAMERA_FRAME, v);
+
+        vpDisplay::displayText(I, 20, 20, "Click to quit...", vpColor::red);
+        if (vpDisplay::getClick(I, false)) {
+          quit = true;
+        }
+        vpDisplay::flush(I);
       }
       catch (...) {
         v = 0;
         robot.setVelocity(vpRobot::CAMERA_FRAME, v);
         robot.stopMotion();
-        exit(1);
+        return EXIT_FAILURE;
       }
 
       vpTRACE("\t\t || s - s* || = %f ", (task.getError()).sumSquare());

diff --git a/example/servo-afma6/servoAfma6Cylinder2DCamVelocitySecondaryTask.cpp b/example/servo-afma6/servoAfma6Cylinder2DCamVelocitySecondaryTask.cpp
@@ -65,7 +65,7 @@
 #include <visp3/gui/vpDisplayOpenCV.h>
 #include <visp3/gui/vpDisplayX.h>
 #include <visp3/io/vpImageIo.h>
-#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/sensor/vpRealSense2.h>
 
 #include <visp3/core/vpCylinder.h>
 #include <visp3/core/vpHomogeneousMatrix.h>
@@ -86,12 +86,17 @@ int main()
   try {
     vpImage<unsigned char> I;
 
-    vp1394TwoGrabber g;
-    g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_640x480_MONO8);
-    g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_60);
-    g.open(I);
+    vpRealSense2 rs;
+    rs2::config config;
+    config.enable_stream(RS2_STREAM_COLOR, 640, 480, RS2_FORMAT_RGBA8, 30);
+    config.enable_stream(RS2_STREAM_DEPTH, 640, 480, RS2_FORMAT_Z16, 30);
+    config.enable_stream(RS2_STREAM_INFRARED, 640, 480, RS2_FORMAT_Y8, 30);
+    rs.open(config);
 
-    g.acquire(I);
+    // Warm up camera
+    for (size_t i = 0; i < 10; ++i) {
+      rs.acquire(I);
+    }
 
 #ifdef VISP_HAVE_X11
     vpDisplayX display(I, 100, 100, "Current image");
@@ -136,7 +141,8 @@ int main()
     }
 
     vpRobotAfma6 robot;
-    // robot.move("zero.pos") ;
+    robot.init(vpAfma6::TOOL_INTEL_D435_CAMERA, vpCameraParameters::perspectiveProjWithoutDistortion);
+    // robot.move("zero.pos");
 
     vpCameraParameters cam;
     // Update camera parameters
@@ -192,13 +198,14 @@ int main()
     double alpha = 0.02;
     double beta = 3;
     double erreur = 1;
+    bool quit = false;
 
     // First loop to reach the convergence position
-    while (erreur > 0.00001) {
+    while ((erreur > 0.00001) && (!quit)) {
       std::cout << "---------------------------------------------" << iter << std::endl;
 
       try {
-        g.acquire(I);
+        rs.acquire(I);
         vpDisplay::display(I);
 
         // Track the two edges and update the features
@@ -212,8 +219,6 @@ int main()
           pd[i].display(cam, I, vpColor::green);
         }
 
-        vpDisplay::flush(I);
-
         // Adaptative gain
         double gain;
         {
@@ -227,14 +232,23 @@ int main()
 
         v = task.computeControlLaw();
 
-        if (iter == 0)
+        if (iter == 0) {
           vpDisplay::getClick(I);
+        }
+
+        robot.setVelocity(vpRobot::CAMERA_FRAME, v);
+
+        vpDisplay::displayText(I, 20, 20, "Click to quit...", vpColor::red);
+        if (vpDisplay::getClick(I, false)) {
+          quit = true;
+        }
+        vpDisplay::flush(I);
       }
       catch (...) {
         v = 0;
         robot.setVelocity(vpRobot::CAMERA_FRAME, v);
         robot.stopMotion();
-        exit(1);
+        return EXIT_FAILURE;
       }
 
       robot.setVelocity(vpRobot::CAMERA_FRAME, v);
@@ -257,12 +271,12 @@ int main()
     double rapport = 0;
     double vitesse = 0.02;
     unsigned int tempo = 1200;
-
-    for (;;) {
+    quit = false;
+    while (!quit) {
       std::cout << "---------------------------------------------" << iter << std::endl;
 
       try {
-        g.acquire(I);
+        rs.acquire(I);
         vpDisplay::display(I);
 
         // Track the two edges and update the features
@@ -276,8 +290,6 @@ int main()
           pd[i].display(cam, I, vpColor::green);
         }
 
-        vpDisplay::flush(I);
-
         v = task.computeControlLaw();
 
         // Compute the new control law corresponding to the secondary task
@@ -321,12 +333,18 @@ int main()
         }
 
         robot.setVelocity(vpRobot::CAMERA_FRAME, v);
+
+        vpDisplay::displayText(I, 20, 20, "Click to quit...", vpColor::red);
+        if (vpDisplay::getClick(I, false)) {
+          quit = true;
+        }
+        vpDisplay::flush(I);
       }
       catch (...) {
         v = 0;
         robot.setVelocity(vpRobot::CAMERA_FRAME, v);
         robot.stopMotion();
-        exit(1);
+        return EXIT_FAILURE;
       }
 
       vpTRACE("\t\t || s - s* || = %f ", (task.getError()).sumSquare());
Original file line number	Diff line number	Diff line change
Expand Up		@@ -59,4 +59,3 @@ endif()
		mark_as_advanced(
		VIPER650_DATA_PATH
		)