visual_servo_nodelet.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2019 by INRIA. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * ("GPL") version 2 as published by the Free Software Foundation.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact INRIA about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See http://visp.inria.fr/ for more information.
17  *
18  * This software was developed at:
19  * INRIA Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  * https://team.inria.fr/rainbow/fr/
24  *
25  * If you have questions regarding the use of this file, please contact
26  * INRIA at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Visual servoing nodelet to control Parrot Bebop 2 drone.
33  *
34  * Authors:
35  * Gatien Gaumerais
36  * Fabien Spindler
37  *
38  *****************************************************************************/
39 
40 #include <ros/ros.h>
41 
42 #include <nodelet/nodelet.h>
44 
45 #include <geometry_msgs/Pose.h>
46 #include <geometry_msgs/Twist.h>
47 #include <geometry_msgs/PoseArray.h>
48 
49 #include <cv_bridge/cv_bridge.h>
50 #include <image_transport/image_transport.h>
52 
53 #include <visp3/core/vpConfig.h>
54 #include <visp3/core/vpExponentialMap.h>
55 #include <visp3/core/vpImageConvert.h>
56 #include <visp3/core/vpMomentAreaNormalized.h>
57 #include <visp3/core/vpMomentBasic.h>
58 #include <visp3/core/vpMomentCentered.h>
59 #include <visp3/core/vpMomentDatabase.h>
60 #include <visp3/core/vpMomentGravityCenter.h>
61 #include <visp3/core/vpMomentGravityCenterNormalized.h>
62 #include <visp3/core/vpMomentObject.h>
63 #include <visp3/core/vpPixelMeterConversion.h>
64 #include <visp3/core/vpPoint.h>
65 #include <visp3/core/vpTime.h>
66 #include <visp3/detection/vpDetectorAprilTag.h>
67 #include <visp3/gui/vpDisplayX.h>
68 #include <visp3/visual_features/vpFeatureBuilder.h>
69 #include <visp3/visual_features/vpFeatureMomentAreaNormalized.h>
70 #include <visp3/visual_features/vpFeatureMomentGravityCenterNormalized.h>
71 #include <visp3/visual_features/vpFeatureVanishingPoint.h>
72 #include <visp3/vs/vpServo.h>
73 #include <visp3/vs/vpServoDisplay.h>
74 
75 #include <iostream>
76 #include <string>
77 
78 #if VISP_VERSION_INT > VP_VERSION_INT(3,2,0) // ViSP >= 3.0.0
79 
80 static double getTagSize(ros::NodeHandle nh)
81 {
82  double res = 0;
83 
84  if (nh.hasParam("/vs/tagSize")) {
85  nh.getParam("/vs/tagSize", res);
86  return res;
87  } else {
88  ROS_WARN("No tag size parameter has been found, using default value : 0.14 meters.");
89  return 0.14;
90  }
91 }
92 
93 static double getDistanceToTag(ros::NodeHandle nh)
94 {
95  double res = 0;
96  if (nh.hasParam("/vs/distanceToTag")) {
97  nh.getParam("/vs/distanceToTag", res);
98  return res;
99  } else {
100  ROS_WARN("No distance to tag parameter has been found, using default value : 1.5 meters.");
101  return 1.5;
102  }
103 }
104 
105 bool compareImagePoint(std::pair<size_t, vpImagePoint> p1, std::pair<size_t, vpImagePoint> p2)
106 {
107  return (p1.second.get_v() < p2.second.get_v());
108 }
109 
110 class bebopVSNodelet : public nodelet::Nodelet
111 {
112 public:
113  bebopVSNodelet();
114  virtual ~bebopVSNodelet();
115 
116  virtual void onInit();
117 
118  void imageCallback(const sensor_msgs::ImageConstPtr &msg);
119 
120 private:
121  ros::NodeHandle m_nh;
122  ros::Publisher m_pubTwist;
123  ros::Subscriber m_subImg;
124  ros::Publisher m_pubCam;
125 
126  bool bebop_has_been_setup;
127 
128  vpDisplayX m_display;
129  bool m_display_setup;
130  vpImage<unsigned char> I;
131 
132  double m_tagSize;
133  double m_Z_d;
134 
135  double m_cameraTilt;
136  double m_cameraPan;
137 
138  vpServo m_task;
139  vpDetectorAprilTag m_detector;
140  vpCameraParameters m_cam;
141 
142  bool m_vec_ip_has_been_sorted;
143  std::vector<std::pair<size_t, vpImagePoint>> m_vec_ip_sorted;
144 
145  vpVelocityTwistMatrix m_cVe;
146  vpMatrix m_eJe;
147 
148  double area;
149 
150  // Desired plane
151  double A;
152  double B;
153  double C;
154 
155  vpMomentObject m_obj, m_obj_d;
156  vpMomentDatabase mdb, mdb_d;
157  vpMomentBasic mb_d;
158  vpMomentGravityCenter mg, mg_d;
159  vpMomentCentered mc, mc_d;
160  vpMomentAreaNormalized man, man_d;
161  vpMomentGravityCenterNormalized mgn, mgn_d;
162 
163  vpFeatureMomentGravityCenterNormalized s_mgn, s_mgn_d;
164  vpFeatureMomentAreaNormalized s_man, s_man_d;
165  vpFeatureVanishingPoint s_vp, s_vp_d;
166 
167  void initVS();
168  vpColVector velocityToPosition(vpColVector &vel_cmd, double delta_t);
169 };
170 
172 
173 bebopVSNodelet::bebopVSNodelet()
174  : bebop_has_been_setup(false)
175  , m_display_setup(false)
176  , m_tagSize(getTagSize(m_nh)) // Fetching theses values
177  , m_Z_d(getDistanceToTag(m_nh)) // from the parameter server
178  , m_cameraTilt(0.0)
179  , m_cameraPan(0.0)
180  , m_detector(vpDetectorAprilTag::TAG_36h11)
181  , m_vec_ip_has_been_sorted(false)
182  , m_eJe(vpMatrix(6, 4, 0))
183  , area(0)
184  , A(0.0)
185  , B(0.0)
186  , C(1.0 / m_Z_d)
187  , m_obj(3)
188  , m_obj_d(3)
189  , man(0, m_Z_d)
190  , man_d(0, m_Z_d)
191  , s_mgn(mdb, A, B, C)
192  , s_mgn_d(mdb_d, A, B, C)
193  , s_man(mdb, A, B, C)
194  , s_man_d(mdb_d, A, B, C)
195 {
196 
197 }
198 
199 bebopVSNodelet::~bebopVSNodelet()
200 {
201  m_task.kill();
202 }
203 
204 void bebopVSNodelet::onInit(){
205 
206  m_nh = getNodeHandle();
207  std::stringstream params_str;
208  params_str << "TAG PARAMETERS (meters) : tagSize : " << m_tagSize
209  << " - distanceToTag : " << m_Z_d << std::endl;
210  NODELET_INFO("%s", params_str.str().c_str());
211 
212  initVS();
213 
214  NODELET_INFO("Setting up publisher and subscriber...");
215  m_pubTwist = m_nh.advertise<geometry_msgs::Twist>("cmd_moveby", 1000);
216  m_subImg = m_nh.subscribe("image_raw", 1000, &bebopVSNodelet::imageCallback, this);
217  m_pubCam = m_nh.advertise<geometry_msgs::Twist>("camera_control", 1000);
218  NODELET_INFO("Publisher and subscriber set up. Waiting for image callbacks...");
219 }
220 
221 void bebopVSNodelet::imageCallback(const sensor_msgs::ImageConstPtr &msg)
222 {
223  if (!bebop_has_been_setup) {
224  geometry_msgs::Twist out_cam_ctrl;
225  out_cam_ctrl.linear.x = 0;
226  out_cam_ctrl.linear.y = 0;
227  out_cam_ctrl.linear.z = 0;
228  out_cam_ctrl.angular.x = 0;
229  out_cam_ctrl.angular.y = m_cameraTilt;
230  out_cam_ctrl.angular.z = m_cameraPan;
231  m_pubCam.publish(out_cam_ctrl);
232 
233  bebop_has_been_setup = true;
234  NODELET_INFO("Setting desired camera orientation...");
235  }
236 
237  const cv::Mat frame = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::BGR8)->image;
238  vpImageConvert::convert(frame, I);
239 
240  if (!m_display_setup) {
241  m_display.init(I, 100, 100, "DRONE VIEW");
242  m_display_setup = true;
243  }
244  vpDisplay::display(I);
245 
246  std::vector<vpHomogeneousMatrix> cMo_vec;
247  m_detector.detect(I, m_tagSize, m_cam, cMo_vec); // Detect AprilTags in current image
248 
249  geometry_msgs::Twist out_cmd_pos;
250 
251  if (m_detector.getNbObjects() == 1) {
252  // Update current points used to compute the moments
253  std::vector<vpImagePoint> vec_ip = m_detector.getPolygon(0);
254  std::vector<vpPoint> vec_P;
255  for (size_t i = 0; i < vec_ip.size(); i++) { // size = 4
256  double x = 0, y = 0;
257  vpPixelMeterConversion::convertPoint(m_cam, vec_ip[i], x, y);
258  vpPoint P;
259  P.set_x(x);
260  P.set_y(y);
261  vec_P.push_back(P);
262  }
263 
264  // Current moments
265  m_obj.setType(vpMomentObject::DENSE_POLYGON); // Consider the AprilTag as a polygon
266  m_obj.fromVector(vec_P); // Initialize the object with the points coordinates
267 
268  mg.linkTo(mdb); // Add gravity center to database
269  mc.linkTo(mdb); // Add centered moments to database
270  man.linkTo(mdb); // Add area normalized to database
271  mgn.linkTo(mdb); // Add gravity center normalized to database
272  mdb.updateAll(m_obj); // All of the moments must be updated, not just an_d
273  mg.compute(); // Compute gravity center moment
274  mc.compute(); // Compute centered moments AFTER gravity center
275  man.setDesiredArea(
276  area); // Desired area was init at 0 (unknow at contruction), need to be updated here
277  man.compute(); // Compute area normalized moment AFTER centered moment
278  mgn.compute(); // Compute gravity center normalized moment AFTER area normalized moment
279 
280  s_mgn.update(A, B, C);
281  s_mgn.compute_interaction();
282  s_man.update(A, B, C);
283  s_man.compute_interaction();
284 
285  /* Sort points from their height in the image, and keep original indexes.
286  This is done once, in order to be independent from the orientation of the tag
287  when detecting vanishing points. */
288  if (!m_vec_ip_has_been_sorted) {
289  for (size_t i = 0; i < vec_ip.size(); i++) {
290  // Add the points and their corresponding index
291  std::pair<size_t, vpImagePoint> index_pair
292  = std::pair<size_t, vpImagePoint>(i, vec_ip[i]);
293  m_vec_ip_sorted.push_back(index_pair);
294  }
295 
296  // Sort the points and indexes from the v value of the points
297  std::sort(m_vec_ip_sorted.begin(), m_vec_ip_sorted.end(), compareImagePoint);
298 
299  m_vec_ip_has_been_sorted = true;
300  }
301 
302  // Use the two highest points for the first line, and the two others for the second line.
303  vpFeatureBuilder::create(s_vp,
304  m_cam,
305  vec_ip[m_vec_ip_sorted[0].first],
306  vec_ip[m_vec_ip_sorted[1].first],
307  vec_ip[m_vec_ip_sorted[2].first],
308  vec_ip[m_vec_ip_sorted[3].first],
309  vpFeatureVanishingPoint::selectAtanOneOverRho());
310 
311  m_task.set_cVe(m_cVe);
312  m_task.set_eJe(m_eJe);
313 
314  // Compute the control law. Velocities are computed in the mobile robot reference frame
315  vpColVector ve = m_task.computeControlLaw();
316  std::stringstream ve_str;
317  ve_str << "ve: " << ve.t() << std::endl;
318  NODELET_INFO("%s", ve_str.str().c_str());
319 
320  // Computing the corresponding displacement
321  vpColVector cmd_pos = velocityToPosition(ve, 1.0);
322 
323  //Sending the movement command to the drone
324 
325  out_cmd_pos.linear.x = cmd_pos[0];
326  out_cmd_pos.linear.y = cmd_pos[1];
327  out_cmd_pos.linear.z = cmd_pos[2];
328  out_cmd_pos.angular.x = 0;
329  out_cmd_pos.angular.y = 0;
330  out_cmd_pos.angular.z = cmd_pos[3];
331  m_pubTwist.publish(out_cmd_pos);
332 
333  for (size_t i = 0; i < 4; i++) {
334  vpDisplay::displayCross(I, vec_ip[i], 15, vpColor::red, 1);
335  std::stringstream ss;
336  ss << i;
337  vpDisplay::displayText(I, vec_ip[i] + vpImagePoint(15, 15), ss.str(), vpColor::green);
338  }
339 
340  // Display visual features
341  vpDisplay::displayPolygon(I,
342  vec_ip,
343  vpColor::green,
344  3); // Current polygon used to compure an moment
345  vpDisplay::displayCross(I,
346  m_detector.getCog(0),
347  15,
348  vpColor::green,
349  3); // Current polygon used to compute a moment
350  vpDisplay::displayLine(I,
351  0,
352  static_cast<int>(m_cam.get_u0()),
353  static_cast<int>(I.getHeight()) - 1,
354  static_cast<int>(m_cam.get_u0()),
355  vpColor::red,
356  3); // Vertical line as desired x position
357  vpDisplay::displayLine(I,
358  static_cast<int>(m_cam.get_v0()),
359  0,
360  static_cast<int>(m_cam.get_v0()),
361  static_cast<int>(I.getWidth()) - 1,
362  vpColor::red,
363  3); // Horizontal line as desired y position
364 
365  // Display lines corresponding to the vanishing point for the horizontal lines
366  vpDisplay::displayLine(I,
367  vec_ip[m_vec_ip_sorted[0].first],
368  vec_ip[m_vec_ip_sorted[1].first],
369  vpColor::red,
370  1,
371  false);
372  vpDisplay::displayLine(I,
373  vec_ip[m_vec_ip_sorted[2].first],
374  vec_ip[m_vec_ip_sorted[3].first],
375  vpColor::red,
376  1,
377  false);
378 
379  vpDisplay::displayText(I, 10, 10, "Click to exit", vpColor::red);
380  vpDisplay::flush(I);
381 
382  } else {
383 
384  std::stringstream sserr;
385  sserr << "Failed to detect an Apriltag, or detected multiple ones";
386  vpDisplay::displayText(I, 120, 20, sserr.str(), vpColor::red);
387  vpDisplay::flush(I);
388 
389  //Stoping drone movement
390  out_cmd_pos.linear.x = 0;
391  out_cmd_pos.linear.y = 0;
392  out_cmd_pos.linear.z = 0;
393  out_cmd_pos.angular.x = 0;
394  out_cmd_pos.angular.y = 0;
395  out_cmd_pos.angular.z = 0;
396  m_pubTwist.publish(out_cmd_pos);
397  }
398 }
399 
400 void bebopVSNodelet::initVS()
401 {
402  NODELET_INFO("Setting up visual servoing...");
403 
404  m_detector.setAprilTagQuadDecimate(4.0);
405  m_detector.setAprilTagNbThreads(4);
406  m_detector.setDisplayTag(true);
407 
408  m_cam.initPersProjWithoutDistortion(531.9213063, 520.8495788, 429.133986, 240.9464457);
409  m_cam.printParameters();
410 
411  //double lambda = 0.5;
412  vpAdaptiveGain lambda = vpAdaptiveGain(1.5, 0.7, 30);
413  m_task.setServo(vpServo::EYEINHAND_L_cVe_eJe);
414  m_task.setInteractionMatrixType(vpServo::CURRENT);
415  m_task.setLambda(lambda);
416 
417  if (m_nh.hasParam("/vs/cameraTilt")) {
418  m_nh.getParam("/vs/cameraTilt", m_cameraTilt);
419  } else {
420  ROS_WARN("No camera tilt value found, using default value : -15 degrees.");
421  m_cameraTilt = -15.0;
422  }
423 
424  if (m_nh.hasParam("/vs/cameraPan")) {
425  m_nh.getParam("/vs/cameraPan", m_cameraPan);
426  } else {
427  ROS_WARN("No camera pan value found, using default value : 0 degrees.");
428  m_cameraPan = 0.0;
429  }
430 
431  std::stringstream camparams_str;
432  camparams_str << "CAMERA PARAMETERS (degrees) : camera Tilt : " << m_cameraTilt
433  << " - camera pan : " << m_cameraPan << std::endl;
434  NODELET_INFO("%s", camparams_str.str().c_str());
435 
436  vpRxyzVector c1_rxyz_c2(vpMath::rad(m_cameraTilt), vpMath::rad(m_cameraPan), 0);
437 
438  vpRotationMatrix c1Rc2(c1_rxyz_c2); // Rotation between camera 1 and 2
439  vpHomogeneousMatrix c1Mc2(vpTranslationVector(), c1Rc2); // Homogeneous matrix between c1 and c2
440 
441  vpRotationMatrix c1Re{0, 1, 0, 0, 0, 1, 1, 0, 0}; // Rotation between camera 1 and E
442  vpTranslationVector c1te(0, 0, -0.09); // Translation between camera 1 and E
443  vpHomogeneousMatrix c1Me(c1te, c1Re); // Homogeneous matrix between c1 and E
444 
445  vpHomogeneousMatrix c2Me = c1Mc2.inverse() * c1Me; // Homogeneous matrix between c2 and E
446 
447  //vpVelocityTwistMatrix
448  m_cVe = vpVelocityTwistMatrix(c2Me);
449  m_task.set_cVe(m_cVe);
450 
451  m_eJe[0][0] = 1;
452  m_eJe[1][1] = 1;
453  m_eJe[2][2] = 1;
454  m_eJe[5][3] = 1;
455 
456  // Define the desired polygon corresponding the the AprilTag CLOCKWISE
457  double X[4] = {m_tagSize / 2., m_tagSize / 2., -m_tagSize / 2., -m_tagSize / 2.};
458  double Y[4] = {m_tagSize / 2., -m_tagSize / 2., -m_tagSize / 2., m_tagSize / 2.};
459 
460  std::vector<vpPoint> vec_P, vec_P_d;
461  for (int i = 0; i < 4; i++) {
462  vpPoint P_d(X[i], Y[i], 0);
463  vpHomogeneousMatrix cdMo(0, 0, m_Z_d, 0, 0, 0);
464  P_d.track(cdMo); //
465  vec_P_d.push_back(P_d);
466  }
467 
468  // Desired moments
469  m_obj_d.setType(vpMomentObject::DENSE_POLYGON); // Consider the AprilTag as a polygon
470  m_obj_d.fromVector(vec_P_d); // Initialize the object with the points coordinates
471 
472  mb_d.linkTo(mdb_d); // Add basic moments to database
473  mg_d.linkTo(mdb_d); // Add gravity center to database
474  mc_d.linkTo(mdb_d); // Add centered moments to database
475  man_d.linkTo(mdb_d); // Add area normalized to database
476  mgn_d.linkTo(mdb_d); // Add gravity center normalized to database
477  mdb_d.updateAll(m_obj_d); // All of the moments must be updated, not just an_d
478  mg_d.compute(); // Compute gravity center moment
479  mc_d.compute(); // Compute centered moments AFTER gravity center
480 
481  if (m_obj_d.getType() == vpMomentObject::DISCRETE)
482  area = mb_d.get(2, 0) + mb_d.get(0, 2);
483  else
484  area = mb_d.get(0, 0);
485  // Update moment with the desired area
486  man_d.setDesiredArea(area);
487 
488  man_d.compute(); // Compute area normalized moment AFTER centered moments
489  mgn_d.compute(); // Compute gravity center normalized moment AFTER area normalized moment
490 
491  // Add the features
492  m_task.addFeature(s_mgn, s_mgn_d);
493  m_task.addFeature(s_man, s_man_d);
494  m_task.addFeature(s_vp, s_vp_d, vpFeatureVanishingPoint::selectAtanOneOverRho());
495 
496  // Update desired gravity center normalized feature
497  s_mgn_d.update(A, B, C);
498  s_mgn_d.compute_interaction();
499  // Update desired area normalized feature
500  s_man_d.update(A, B, C);
501  s_man_d.compute_interaction();
502 
503  // Update desired vanishing point feature for the horizontal line
504  s_vp_d.setAtanOneOverRho(0);
505  s_vp_d.setAlpha(0);
506 
507  NODELET_INFO("Visual servoing settup.");
508 }
509 
510 vpColVector bebopVSNodelet::velocityToPosition(vpColVector &vel_cmd, double delta_t)
511 {
512  vpColVector res;
513  if (vel_cmd.size() != 4) {
514  ROS_ERROR(
515  "Can't compute velocity : dimension of the velocity vector should be equal to 4.");
516  res << 0., 0., 0., 0.;
517  return res;
518  }
519 
520  vpColVector ve(6);
521 
522  ve[0] = vel_cmd[0];
523  ve[1] = vel_cmd[1];
524  ve[2] = vel_cmd[2];
525  ve[5] = vel_cmd[3];
526 
527  vpHomogeneousMatrix M = vpExponentialMap::direct(ve, delta_t);
528 
529  double epsilon = (std::numeric_limits<double>::epsilon());
530  if (std::abs(M.getRotationMatrix().getThetaUVector()[0]) >= epsilon) {
531  ROS_ERROR("Can't compute velocity : rotation around X axis should be 0.");
532  res << 0., 0., 0., 0.;
533  return res;
534  }
535  if (std::abs(M.getRotationMatrix().getThetaUVector()[1]) >= epsilon) {
536  ROS_ERROR("Can't compute velocity : rotation around Y axis should be 0.");
537  res << 0., 0., 0., 0.;
538  return res;
539  }
540 
541  float dThetaZ = static_cast<float>(M.getRotationMatrix().getThetaUVector()[2]);
542  vpTranslationVector t = M.getTranslationVector();
543 
544  res << t[0], t[1], t[2], dThetaZ;
545  return res;
546 }
547 
548 #else
549 
550 #endif
551 
CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
double epsilon
TFSIMD_FORCE_INLINE const tfScalar & y() const
#define ROS_WARN(...)
TFSIMD_FORCE_INLINE const tfScalar & x() const
ros::NodeHandle & getNodeHandle() const
#define NODELET_INFO(...)
bool getParam(const std::string &key, std::string &s) const
virtual void onInit()=0
bool hasParam(const std::string &key) const
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
#define ROS_ERROR(...)


visp_ros
Author(s): Francois Pasteau, Fabien Spindler, Gatien Gaumerais
autogenerated on Fri Jul 3 2020 03:41:43