visual_servo_nodelet.cpp

Go to the documentation of this file.

/****************************************************************************
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2019 by INRIA. All rights reserved.
 *
 * This software is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * ("GPL") version 2 as published by the Free Software Foundation.
 * See the file LICENSE.txt at the root directory of this source
 * distribution for additional information about the GNU GPL.
 *
 * For using ViSP with software that can not be combined with the GNU
 * GPL, please contact INRIA about acquiring a ViSP Professional
 * Edition License.
 *
 * See http://visp.inria.fr/ for more information.
 *
 * This software was developed at:
 * INRIA Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 * https://team.inria.fr/rainbow/fr/
 *
 * If you have questions regarding the use of this file, please contact
 * INRIA at visp@inria.fr
 *
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 * Description:
 * Visual servoing nodelet to control Parrot Bebop 2 drone.
 *
 * Authors:
 * Gatien Gaumerais
 * Fabien Spindler
 *
 *****************************************************************************/

#include <ros/ros.h>

#include <nodelet/nodelet.h>
#include <pluginlib/class_list_macros.h>

#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseArray.h>
#include <geometry_msgs/Twist.h>

#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <sensor_msgs/image_encodings.h>

#include <visp3/core/vpConfig.h>
#include <visp3/core/vpExponentialMap.h>
#include <visp3/core/vpImageConvert.h>
#include <visp3/core/vpMomentAreaNormalized.h>
#include <visp3/core/vpMomentBasic.h>
#include <visp3/core/vpMomentCentered.h>
#include <visp3/core/vpMomentDatabase.h>
#include <visp3/core/vpMomentGravityCenter.h>
#include <visp3/core/vpMomentGravityCenterNormalized.h>
#include <visp3/core/vpMomentObject.h>
#include <visp3/core/vpPixelMeterConversion.h>
#include <visp3/core/vpPoint.h>
#include <visp3/core/vpTime.h>
#include <visp3/detection/vpDetectorAprilTag.h>
#include <visp3/gui/vpDisplayX.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/visual_features/vpFeatureMomentAreaNormalized.h>
#include <visp3/visual_features/vpFeatureMomentGravityCenterNormalized.h>
#include <visp3/visual_features/vpFeatureVanishingPoint.h>
#include <visp3/vs/vpServo.h>
#include <visp3/vs/vpServoDisplay.h>

#include <iostream>
#include <string>

#if VISP_VERSION_INT > VP_VERSION_INT( 3, 2, 0 ) // ViSP >= 3.0.0

static double
getTagSize( ros::NodeHandle nh )
{
  double res = 0;

  if ( nh.hasParam( "/vs/tagSize" ) )
  {
    nh.getParam( "/vs/tagSize", res );
    return res;
  }
  else
  {
    ROS_WARN( "No tag size parameter has been found, using default value : 0.14 meters." );
    return 0.14;
  }
}

static double
getDistanceToTag( ros::NodeHandle nh )
{
  double res = 0;
  if ( nh.hasParam( "/vs/distanceToTag" ) )
  {
    nh.getParam( "/vs/distanceToTag", res );
    return res;
  }
  else
  {
    ROS_WARN( "No distance to tag parameter has been found, using default value : 1.5 meters." );
    return 1.5;
  }
}

bool
compareImagePoint( std::pair< size_t, vpImagePoint > p1, std::pair< size_t, vpImagePoint > p2 )
{
  return ( p1.second.get_v() < p2.second.get_v() );
}

class bebopVSNodelet : public nodelet::Nodelet
{
public:
  bebopVSNodelet();
  virtual ~bebopVSNodelet();

  virtual void onInit();

  void imageCallback( const sensor_msgs::ImageConstPtr &msg );

private:
  ros::NodeHandle m_nh;
  ros::Publisher m_pubTwist;
  ros::Subscriber m_subImg;
  ros::Publisher m_pubCam;

  bool bebop_has_been_setup;

  vpDisplayX m_display;
  bool m_display_setup;
  vpImage< unsigned char > I;

  double m_tagSize;
  double m_Z_d;

  double m_cameraTilt;
  double m_cameraPan;

  vpServo m_task;
  vpDetectorAprilTag m_detector;
  vpCameraParameters m_cam;

  bool m_vec_ip_has_been_sorted;
  std::vector< std::pair< size_t, vpImagePoint > > m_vec_ip_sorted;

  vpVelocityTwistMatrix m_cVe;
  vpMatrix m_eJe;

  double area;

  // Desired plane
  double A;
  double B;
  double C;

  vpMomentObject m_obj, m_obj_d;
  vpMomentDatabase mdb, mdb_d;
  vpMomentBasic mb_d;
  vpMomentGravityCenter mg, mg_d;
  vpMomentCentered mc, mc_d;
  vpMomentAreaNormalized man, man_d;
  vpMomentGravityCenterNormalized mgn, mgn_d;

  vpFeatureMomentGravityCenterNormalized s_mgn, s_mgn_d;
  vpFeatureMomentAreaNormalized s_man, s_man_d;
  vpFeatureVanishingPoint s_vp, s_vp_d;

  void initVS();
  vpColVector velocityToPosition( vpColVector &vel_cmd, double delta_t );
};

PLUGINLIB_EXPORT_CLASS( bebopVSNodelet, nodelet::Nodelet )

bebopVSNodelet::bebopVSNodelet()
  : bebop_has_been_setup( false )
  , m_display_setup( false )
  , m_tagSize( getTagSize( m_nh ) )   // Fetching theses values
  , m_Z_d( getDistanceToTag( m_nh ) ) // from the parameter server
  , m_cameraTilt( 0.0 )
  , m_cameraPan( 0.0 )
  , m_detector( vpDetectorAprilTag::TAG_36h11 )
  , m_vec_ip_has_been_sorted( false )
  , m_eJe( vpMatrix( 6, 4, 0 ) )
  , area( 0 )
  , A( 0.0 )
  , B( 0.0 )
  , C( 1.0 / m_Z_d )
  , m_obj( 3 )
  , m_obj_d( 3 )
  , man( 0, m_Z_d )
  , man_d( 0, m_Z_d )
  , s_mgn( mdb, A, B, C )
  , s_mgn_d( mdb_d, A, B, C )
  , s_man( mdb, A, B, C )
  , s_man_d( mdb_d, A, B, C )
{
}

bebopVSNodelet::~bebopVSNodelet() { m_task.kill(); }

void
bebopVSNodelet::onInit()
{

  m_nh = getNodeHandle();
  std::stringstream params_str;
  params_str << "TAG PARAMETERS (meters) : tagSize : " << m_tagSize << " - distanceToTag : " << m_Z_d << std::endl;
  NODELET_INFO( "%s", params_str.str().c_str() );

  initVS();

  NODELET_INFO( "Setting up publisher and subscriber..." );
  m_pubTwist = m_nh.advertise< geometry_msgs::Twist >( "cmd_moveby", 1000 );
  m_subImg   = m_nh.subscribe( "image_raw", 1000, &bebopVSNodelet::imageCallback, this );
  m_pubCam   = m_nh.advertise< geometry_msgs::Twist >( "camera_control", 1000 );
  NODELET_INFO( "Publisher and subscriber set up. Waiting for image callbacks..." );
}

void
bebopVSNodelet::imageCallback( const sensor_msgs::ImageConstPtr &msg )
{
  if ( !bebop_has_been_setup )
  {
    geometry_msgs::Twist out_cam_ctrl;
    out_cam_ctrl.linear.x  = 0;
    out_cam_ctrl.linear.y  = 0;
    out_cam_ctrl.linear.z  = 0;
    out_cam_ctrl.angular.x = 0;
    out_cam_ctrl.angular.y = m_cameraTilt;
    out_cam_ctrl.angular.z = m_cameraPan;
    m_pubCam.publish( out_cam_ctrl );

    bebop_has_been_setup = true;
    NODELET_INFO( "Setting desired camera orientation..." );
  }

  const cv::Mat frame = cv_bridge::toCvShare( msg, sensor_msgs::image_encodings::BGR8 )->image;
  vpImageConvert::convert( frame, I );

  if ( !m_display_setup )
  {
    m_display.init( I, 100, 100, "DRONE VIEW" );
    m_display_setup = true;
  }
  vpDisplay::display( I );

  std::vector< vpHomogeneousMatrix > cMo_vec;
  m_detector.detect( I, m_tagSize, m_cam, cMo_vec ); // Detect AprilTags in current image

  geometry_msgs::Twist out_cmd_pos;

  if ( m_detector.getNbObjects() == 1 )
  {
    // Update current points used to compute the moments
    std::vector< vpImagePoint > vec_ip = m_detector.getPolygon( 0 );
    std::vector< vpPoint > vec_P;
    for ( size_t i = 0; i < vec_ip.size(); i++ )
    { // size = 4
      double x = 0, y = 0;
      vpPixelMeterConversion::convertPoint( m_cam, vec_ip[i], x, y );
      vpPoint P;
      P.set_x( x );
      P.set_y( y );
      vec_P.push_back( P );
    }

    // Current moments
    m_obj.setType( vpMomentObject::DENSE_POLYGON ); // Consider the AprilTag as a polygon
    m_obj.fromVector( vec_P );                      // Initialize the object with the points coordinates

    mg.linkTo( mdb );           // Add gravity center to database
    mc.linkTo( mdb );           // Add centered moments to database
    man.linkTo( mdb );          // Add area normalized to database
    mgn.linkTo( mdb );          // Add gravity center normalized to database
    mdb.updateAll( m_obj );     // All of the moments must be updated, not just an_d
    mg.compute();               // Compute gravity center moment
    mc.compute();               // Compute centered moments AFTER gravity center
    man.setDesiredArea( area ); // Desired area was init at 0 (unknow at contruction), need to be updated here
    man.compute();              // Compute area normalized moment AFTER centered moment
    mgn.compute();              // Compute gravity center normalized moment AFTER area normalized moment

    s_mgn.update( A, B, C );
    s_mgn.compute_interaction();
    s_man.update( A, B, C );
    s_man.compute_interaction();

    /* Sort points from their height in the image, and keep original indexes.
            This is done once, in order to be independent from the orientation of the tag
            when detecting vanishing points. */
    if ( !m_vec_ip_has_been_sorted )
    {
      for ( size_t i = 0; i < vec_ip.size(); i++ )
      {
        // Add the points and their corresponding index
        std::pair< size_t, vpImagePoint > index_pair = std::pair< size_t, vpImagePoint >( i, vec_ip[i] );
        m_vec_ip_sorted.push_back( index_pair );
      }

      // Sort the points and indexes from the v value of the points
      std::sort( m_vec_ip_sorted.begin(), m_vec_ip_sorted.end(), compareImagePoint );

      m_vec_ip_has_been_sorted = true;
    }

    // Use the two highest points for the first line, and the two others for the second line.
    vpFeatureBuilder::create( s_vp, m_cam, vec_ip[m_vec_ip_sorted[0].first], vec_ip[m_vec_ip_sorted[1].first],
                              vec_ip[m_vec_ip_sorted[2].first], vec_ip[m_vec_ip_sorted[3].first],
                              vpFeatureVanishingPoint::selectAtanOneOverRho() );

    m_task.set_cVe( m_cVe );
    m_task.set_eJe( m_eJe );

    // Compute the control law. Velocities are computed in the mobile robot reference frame
    vpColVector ve = m_task.computeControlLaw();
    std::stringstream ve_str;
    ve_str << "ve: " << ve.t() << std::endl;
    NODELET_INFO( "%s", ve_str.str().c_str() );

    // Computing the corresponding displacement
    vpColVector cmd_pos = velocityToPosition( ve, 1.0 );

    // Sending the movement command to the drone

    out_cmd_pos.linear.x  = cmd_pos[0];
    out_cmd_pos.linear.y  = cmd_pos[1];
    out_cmd_pos.linear.z  = cmd_pos[2];
    out_cmd_pos.angular.x = 0;
    out_cmd_pos.angular.y = 0;
    out_cmd_pos.angular.z = cmd_pos[3];
    m_pubTwist.publish( out_cmd_pos );

    for ( size_t i = 0; i < 4; i++ )
    {
      vpDisplay::displayCross( I, vec_ip[i], 15, vpColor::red, 1 );
      std::stringstream ss;
      ss << i;
      vpDisplay::displayText( I, vec_ip[i] + vpImagePoint( 15, 15 ), ss.str(), vpColor::green );
    }

    // Display visual features
    vpDisplay::displayPolygon( I, vec_ip, vpColor::green,
                               3 ); // Current polygon used to compure an moment
    vpDisplay::displayCross( I, m_detector.getCog( 0 ), 15, vpColor::green,
                             3 ); // Current polygon used to compute a moment
    vpDisplay::displayLine( I, 0, static_cast< int >( m_cam.get_u0() ), static_cast< int >( I.getHeight() ) - 1,
                            static_cast< int >( m_cam.get_u0() ), vpColor::red,
                            3 ); // Vertical line as desired x position
    vpDisplay::displayLine( I, static_cast< int >( m_cam.get_v0() ), 0, static_cast< int >( m_cam.get_v0() ),
                            static_cast< int >( I.getWidth() ) - 1, vpColor::red,
                            3 ); // Horizontal line as desired y position

    // Display lines corresponding to the vanishing point for the horizontal lines
    vpDisplay::displayLine( I, vec_ip[m_vec_ip_sorted[0].first], vec_ip[m_vec_ip_sorted[1].first], vpColor::red, 1,
                            false );
    vpDisplay::displayLine( I, vec_ip[m_vec_ip_sorted[2].first], vec_ip[m_vec_ip_sorted[3].first], vpColor::red, 1,
                            false );

    vpDisplay::displayText( I, 10, 10, "Click to exit", vpColor::red );
    vpDisplay::flush( I );
  }
  else
  {

    std::stringstream sserr;
    sserr << "Failed to detect an Apriltag, or detected multiple ones";
    vpDisplay::displayText( I, 120, 20, sserr.str(), vpColor::red );
    vpDisplay::flush( I );

    // Stoping drone movement
    out_cmd_pos.linear.x  = 0;
    out_cmd_pos.linear.y  = 0;
    out_cmd_pos.linear.z  = 0;
    out_cmd_pos.angular.x = 0;
    out_cmd_pos.angular.y = 0;
    out_cmd_pos.angular.z = 0;
    m_pubTwist.publish( out_cmd_pos );
  }
}

void
bebopVSNodelet::initVS()
{
  NODELET_INFO( "Setting up visual servoing..." );

  m_detector.setAprilTagQuadDecimate( 4.0 );
  m_detector.setAprilTagNbThreads( 4 );
  m_detector.setDisplayTag( true );

  m_cam.initPersProjWithoutDistortion( 531.9213063, 520.8495788, 429.133986, 240.9464457 );
  m_cam.printParameters();

  // double lambda = 0.5;
  vpAdaptiveGain lambda = vpAdaptiveGain( 1.5, 0.7, 30 );
  m_task.setServo( vpServo::EYEINHAND_L_cVe_eJe );
  m_task.setInteractionMatrixType( vpServo::CURRENT );
  m_task.setLambda( lambda );

  if ( m_nh.hasParam( "/vs/cameraTilt" ) )
  {
    m_nh.getParam( "/vs/cameraTilt", m_cameraTilt );
  }
  else
  {
    ROS_WARN( "No camera tilt value found, using default value : -15 degrees." );
    m_cameraTilt = -15.0;
  }

  if ( m_nh.hasParam( "/vs/cameraPan" ) )
  {
    m_nh.getParam( "/vs/cameraPan", m_cameraPan );
  }
  else
  {
    ROS_WARN( "No camera pan value found, using default value : 0 degrees." );
    m_cameraPan = 0.0;
  }

  std::stringstream camparams_str;
  camparams_str << "CAMERA PARAMETERS (degrees) : camera Tilt : " << m_cameraTilt << " - camera pan : " << m_cameraPan
                << std::endl;
  NODELET_INFO( "%s", camparams_str.str().c_str() );

  vpRxyzVector c1_rxyz_c2( vpMath::rad( m_cameraTilt ), vpMath::rad( m_cameraPan ), 0 );

  vpRotationMatrix c1Rc2( c1_rxyz_c2 );                      // Rotation between camera 1 and 2
  vpHomogeneousMatrix c1Mc2( vpTranslationVector(), c1Rc2 ); // Homogeneous matrix between c1 and c2

  vpRotationMatrix c1Re{ 0, 1, 0, 0, 0, 1, 1, 0, 0 }; // Rotation between camera 1 and E
  vpTranslationVector c1te( 0, 0, -0.09 );            // Translation between camera 1 and E
  vpHomogeneousMatrix c1Me( c1te, c1Re );             // Homogeneous matrix between c1 and E

  vpHomogeneousMatrix c2Me = c1Mc2.inverse() * c1Me; // Homogeneous matrix between c2 and E

  // vpVelocityTwistMatrix
  m_cVe = vpVelocityTwistMatrix( c2Me );
  m_task.set_cVe( m_cVe );

  m_eJe[0][0] = 1;
  m_eJe[1][1] = 1;
  m_eJe[2][2] = 1;
  m_eJe[5][3] = 1;

  // Define the desired polygon corresponding the the AprilTag CLOCKWISE
  double X[4] = { m_tagSize / 2., m_tagSize / 2., -m_tagSize / 2., -m_tagSize / 2. };
  double Y[4] = { m_tagSize / 2., -m_tagSize / 2., -m_tagSize / 2., m_tagSize / 2. };

  std::vector< vpPoint > vec_P, vec_P_d;
  for ( int i = 0; i < 4; i++ )
  {
    vpPoint P_d( X[i], Y[i], 0 );
    vpHomogeneousMatrix cdMo( 0, 0, m_Z_d, 0, 0, 0 );
    P_d.track( cdMo ); //
    vec_P_d.push_back( P_d );
  }

  // Desired moments
  m_obj_d.setType( vpMomentObject::DENSE_POLYGON ); // Consider the AprilTag as a polygon
  m_obj_d.fromVector( vec_P_d );                    // Initialize the object with the points coordinates

  mb_d.linkTo( mdb_d );       // Add basic moments to database
  mg_d.linkTo( mdb_d );       // Add gravity center to database
  mc_d.linkTo( mdb_d );       // Add centered moments to database
  man_d.linkTo( mdb_d );      // Add area normalized to database
  mgn_d.linkTo( mdb_d );      // Add gravity center normalized to database
  mdb_d.updateAll( m_obj_d ); // All of the moments must be updated, not just an_d
  mg_d.compute();             // Compute gravity center moment
  mc_d.compute();             // Compute centered moments AFTER gravity center

  if ( m_obj_d.getType() == vpMomentObject::DISCRETE )
    area = mb_d.get( 2, 0 ) + mb_d.get( 0, 2 );
  else
    area = mb_d.get( 0, 0 );
  // Update moment with the desired area
  man_d.setDesiredArea( area );

  man_d.compute(); // Compute area normalized moment AFTER centered moments
  mgn_d.compute(); // Compute gravity center normalized moment AFTER area normalized moment

  // Add the features
  m_task.addFeature( s_mgn, s_mgn_d );
  m_task.addFeature( s_man, s_man_d );
  m_task.addFeature( s_vp, s_vp_d, vpFeatureVanishingPoint::selectAtanOneOverRho() );

  // Update desired gravity center normalized feature
  s_mgn_d.update( A, B, C );
  s_mgn_d.compute_interaction();
  // Update desired area normalized feature
  s_man_d.update( A, B, C );
  s_man_d.compute_interaction();

  // Update desired vanishing point feature for the horizontal line
  s_vp_d.setAtanOneOverRho( 0 );
  s_vp_d.setAlpha( 0 );

  NODELET_INFO( "Visual servoing settup." );
}

vpColVector
bebopVSNodelet::velocityToPosition( vpColVector &vel_cmd, double delta_t )
{
  vpColVector res;
  if ( vel_cmd.size() != 4 )
  {
    ROS_ERROR( "Can't compute velocity : dimension of the velocity vector should be equal to 4." );
    res << 0., 0., 0., 0.;
    return res;
  }

  vpColVector ve( 6 );

  ve[0] = vel_cmd[0];
  ve[1] = vel_cmd[1];
  ve[2] = vel_cmd[2];
  ve[5] = vel_cmd[3];

  vpHomogeneousMatrix M = vpExponentialMap::direct( ve, delta_t );

  double epsilon = ( std::numeric_limits< double >::epsilon() );
  if ( std::abs( M.getRotationMatrix().getThetaUVector()[0] ) >= epsilon )
  {
    ROS_ERROR( "Can't compute velocity : rotation around X axis should be 0." );
    res << 0., 0., 0., 0.;
    return res;
  }
  if ( std::abs( M.getRotationMatrix().getThetaUVector()[1] ) >= epsilon )
  {
    ROS_ERROR( "Can't compute velocity : rotation around Y axis should be 0." );
    res << 0., 0., 0., 0.;
    return res;
  }

  float dThetaZ         = static_cast< float >( M.getRotationMatrix().getThetaUVector()[2] );
  vpTranslationVector t = M.getTranslationVector();

  res << t[0], t[1], t[2], dThetaZ;
  return res;
}

#else

#endif