tutorial-ros-pioneer-visual-servo.cpp

Go to the documentation of this file.

#include <iostream>

#include <visp/vpCameraParameters.h>
#include <visp/vpDisplayX.h>
#include <visp/vpDot2.h>
#include <visp/vpFeatureBuilder.h>
#include <visp/vpFeatureDepth.h>
#include <visp/vpFeaturePoint.h>
#include <visp/vpHomogeneousMatrix.h>
#include <visp/vpImage.h>
#include <visp/vpImageConvert.h>
#include <visp/vpServo.h>
#include <visp/vpVelocityTwistMatrix.h>

#include <visp_ros/vpROSGrabber.h>
#include <visp_ros/vpROSRobotPioneer.h>

#if defined( VISP_HAVE_DC1394_2 ) && defined( VISP_HAVE_X11 )
#define TEST_COULD_BE_ACHIEVED
#endif

#ifdef TEST_COULD_BE_ACHIEVED
int
main( int argc, char **argv )
{
  try
  {
    vpImage< unsigned char > I; // Create a gray level image container
    double depth  = 1.;
    double lambda = 0.6;
    double coef   = 1. / 6.77; // Scale parameter used to estimate the depth Z of the blob from its surface

    vpROSRobotPioneer robot;
    robot.setCmdVelTopic( "/RosAria/cmd_vel" );
    robot.init();

    // Wait 3 sec to be sure that the low level Aria thread used to control
    // the robot is started. Without this delay we experienced a delay (arround 2.2 sec)
    // between the velocity send to the robot and the velocity that is really applied
    // to the wheels.
    vpTime::sleepMs( 3000 );

    std::cout << "Robot connected" << std::endl;

    // Camera parameters. In this experiment we don't need a precise calibration of the camera
    vpCameraParameters cam;

    // Create a grabber based on libdc1394-2.x third party lib (for firewire cameras under Linux)
    vpROSGrabber g;
    g.setCameraInfoTopic( "/camera/camera_info" );
    g.setImageTopic( "/camera/image_raw" );
    g.setRectify( true );

    // Get camera parameters from /camera/camera_info topic
    if ( g.getCameraInfo( cam ) == false )
      cam.initPersProjWithoutDistortion( 600, 600, I.getWidth() / 2, I.getHeight() / 2 );

    g.acquire( I );

    // Create an image viewer
    vpDisplayX d( I, 10, 10, "Current frame" );
    vpDisplay::display( I );
    vpDisplay::flush( I );

    // Create a blob tracker
    vpDot2 dot;
    dot.setGraphics( true );
    dot.setComputeMoments( true );
    dot.setEllipsoidShapePrecision( 0. );       // to track a blob without any constraint on the shape
    dot.setGrayLevelPrecision( 0.9 );           // to set the blob gray level bounds for binarisation
    dot.setEllipsoidBadPointsPercentage( 0.5 ); // to be accept 50% of bad inner and outside points with bad gray level
    dot.initTracking( I );
    vpDisplay::flush( I );

    vpServo task;
    task.setServo( vpServo::EYEINHAND_L_cVe_eJe );
    task.setInteractionMatrixType( vpServo::DESIRED, vpServo::PSEUDO_INVERSE );
    task.setLambda( lambda );
    vpVelocityTwistMatrix cVe;
    cVe = robot.get_cVe();
    task.set_cVe( cVe );

    std::cout << "cVe: \n" << cVe << std::endl;

    vpMatrix eJe;
    robot.get_eJe( eJe );
    task.set_eJe( eJe );
    std::cout << "eJe: \n" << eJe << std::endl;

    // Current and desired visual feature associated to the x coordinate of the point
    vpFeaturePoint s_x, s_xd;

    // Create the current x visual feature
    vpFeatureBuilder::create( s_x, cam, dot );

    // Create the desired x* visual feature
    s_xd.buildFrom( 0, 0, depth );

    // Add the feature
    task.addFeature( s_x, s_xd );

    // Create the current log(Z/Z*) visual feature
    vpFeatureDepth s_Z, s_Zd;
    // Surface of the blob estimated from the image moment m00 and converted in meters
    double surface = 1. / sqrt( dot.m00 / ( cam.get_px() * cam.get_py() ) );
    double Z, Zd;
    // Initial depth of the blob in from of the camera
    Z = coef * surface;
    // Desired depth Z* of the blob. This depth is learned and equal to the initial depth
    Zd = Z;

    std::cout << "Z " << Z << std::endl;
    s_Z.buildFrom( s_x.get_x(), s_x.get_y(), Z, 0 );   // log(Z/Z*) = 0 that's why the last parameter is 0
    s_Zd.buildFrom( s_x.get_x(), s_x.get_y(), Zd, 0 ); // log(Z/Z*) = 0 that's why the last parameter is 0

    // Add the feature
    task.addFeature( s_Z, s_Zd );

    vpColVector v; // vz, wx

    while ( 1 )
    {
      // Acquire a new image
      g.acquire( I );

      // Set the image as background of the viewer
      vpDisplay::display( I );

      // Does the blob tracking
      dot.track( I );
      // Update the current x feature
      vpFeatureBuilder::create( s_x, cam, dot );

      // Update log(Z/Z*) feature. Since the depth Z change, we need to update the intection matrix
      surface = 1. / sqrt( dot.m00 / ( cam.get_px() * cam.get_py() ) );
      Z       = coef * surface;
      s_Z.buildFrom( s_x.get_x(), s_x.get_y(), Z, log( Z / Zd ) );

      robot.get_cVe( cVe );
      task.set_cVe( cVe );

      robot.get_eJe( eJe );
      task.set_eJe( eJe );

      // Compute the control law. Velocities are computed in the mobile robot reference frame
      v = task.computeControlLaw();

      std::cout << "Send velocity to the pionner: " << v[0] << " m/s " << vpMath::deg( v[1] ) << " deg/s" << std::endl;

      // Send the velocity to the robot
      robot.setVelocity( vpRobot::REFERENCE_FRAME, v );

      // Draw a vertical line which corresponds to the desired x coordinate of the dot cog
      vpDisplay::displayLine( I, 0, 320, 479, 320, vpColor::red );
      vpDisplay::flush( I );

      // A click in the viewer to exit
      if ( vpDisplay::getClick( I, false ) )
        break;
    }

    std::cout << "Ending robot thread..." << std::endl;

    // Kill the servo task
    task.print();
    task.kill();
  }
  catch ( vpException e )
  {
    std::cout << "Catch an exception: " << e << std::endl;
    return 1;
  }
}
#else
int
main()
{
  std::cout << "You don't have the right 3rd party libraries to run this example..." << std::endl;
}
#endif