Skip to content

Commit

Permalink
Migrate afma6 examples to Realsense D435 camera usage
Browse files Browse the repository at this point in the history
  • Loading branch information
@fspindle
fspindle committed May 14, 2024
1 parent 622297f commit 65d040e
Show file tree
Hide file tree
Showing 14 changed files with 381 additions and 184 deletions.
29 changes: 20 additions & 9 deletions example/servo-afma6/servoAfma62DhalfCamVelocity.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down Expand Up @@ -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");
Expand All @@ -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;
Expand Down Expand Up @@ -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();
Expand Down Expand Up @@ -339,7 +346,6 @@ int main()

v = task.computeControlLaw();

vpDisplay::flush(I);
std::cout << v.sumSquare() << std::endl;
if (iter == 0)
vpDisplay::getClick(I);
Expand All @@ -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;
Expand Down
43 changes: 29 additions & 14 deletions example/servo-afma6/servoAfma6Cylinder2DCamVelocity.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand All @@ -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");
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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);
Expand All @@ -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
Expand All @@ -205,7 +214,6 @@ int main()
pd[i].display(cam, I, vpColor::green);
}

vpDisplay::flush(I);

// Adaptative gain
double gain;
Expand All @@ -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());
Expand Down
56 changes: 37 additions & 19 deletions example/servo-afma6/servoAfma6Cylinder2DCamVelocitySecondaryTask.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand All @@ -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");
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand All @@ -212,8 +219,6 @@ int main()
pd[i].display(cam, I, vpColor::green);
}

vpDisplay::flush(I);

// Adaptative gain
double gain;
{
Expand All @@ -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);
Expand All @@ -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
Expand All @@ -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
Expand Down Expand Up @@ -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());
Expand Down
Loading

0 comments on commit 65d040e

Please sign in to comment.