pbvs_blob.cpp
Go to the documentation of this file.
1 #include <ros/ros.h>
2 #include <ros/console.h>
3 #include <std_msgs/Int8.h>
4 #include <geometry_msgs/TwistStamped.h>
5 #include <geometry_msgs/PoseStamped.h>
6 
7 #include <cv_bridge/cv_bridge.h>
8 
9 #include <visp_bridge/3dpose.h>
10 #include <visp_bridge/camera.h>
11 
12 #include <visp_ros/BlobTracker.h>
13 
14 #include <visp3/core/vpCameraParameters.h>
15 #include <visp3/core/vpHomogeneousMatrix.h>
16 #include <visp3/core/vpImageConvert.h>
17 #include <visp3/gui/vpDisplayX.h>
18 #include <visp3/gui/vpDisplayGDI.h>
19 #include <visp3/gui/vpDisplayOpenCV.h>
20 #include <visp3/visual_features/vpFeatureTranslation.h>
21 #include <visp3/visual_features/vpFeatureThetaU.h>
22 #include <visp3/vs/vpAdaptiveGain.h>
23 #include <visp3/vs/vpServo.h>
24 
25 class VS
26 {
27 private:
30  ros::Subscriber m_subData; // pose_stamped
31  ros::Subscriber m_subStatus; // tracker status (1 OK, 0 NOK)
32 
33  vpServo m_task;
34  vpFeatureTranslation m_s_t, m_s_t_d;
35  vpFeatureThetaU m_s_tu, m_s_tu_d;
36 
37  double m_lambda;
38  vpAdaptiveGain m_lambda_adapt;
39 
40  vpColVector m_v;
41 
42  vpHomogeneousMatrix m_cMo, m_cdMc, m_cdMo;
43  bool m_init;
45 
46  double m_t_x_d, m_t_y_d, m_t_z_d; // Desired translation as a vpTranslationVector in [m]
47  double m_tu_x_d, m_tu_y_d, m_tu_z_d; // Desired rotation as a vpThetaUVector in [deg]
48 
49  double m_square_size;
50 
51  double m_cam_px;
52  double m_cam_py;
53  double m_cam_u0;
54  double m_cam_v0;
55  double m_cam_kud;
56  double m_cam_kdu;
57 
58  vpCameraParameters m_cam;
59  vpImage<unsigned char> m_I_grayscale;
60 #ifdef VISP_HAVE_X11
61  vpDisplayX *m_display;
62 #elif defined(VISP_HAVE_GDI)
63  vpDisplayGDI *m_display;
64 #elif defined(VISP_HAVE_OPENCV)
65  vpDisplayOpenCV *m_display;
66 #endif
67 
68  unsigned int m_thickness;
69  std::vector< std::vector<vpImagePoint> > ips_trajectory;
70 
71  bool m_quit;
72 
73 public:
74  void init_vs();
75  void init_display();
76  void display(const visp_ros::BlobTracker::ConstPtr& msg);
77  void data_callback(const visp_ros::BlobTracker::ConstPtr& msg);
78  void status_callback(const std_msgs::Int8ConstPtr& msg);
79  void spin();
80  VS(int argc, char**argv);
81  virtual ~VS() {
82  m_task.kill();
83  };
84 };
85 
86 VS::VS(int argc, char**argv)
88  m_lambda(0.5), m_v(6, 0), m_cMo(), m_cdMc(), m_cdMo(), m_init(false), m_enable_servo(0),
89  m_t_x_d(0), m_t_y_d(0), m_t_z_d(0), m_tu_x_d(0), m_tu_y_d(0), m_tu_z_d(0),
90  m_square_size(0.12), m_cam_px(-1), m_cam_py(-1), m_cam_u0(-1), m_cam_v0(-1), m_cam_kud(-1), m_cam_kdu(-1), m_cam(),
92 {
93  m_subData = m_nh.subscribe("/blob_tracker/data", 1000, &VS::data_callback, this);
94  m_subStatus = m_nh.subscribe("/blob_tracker/status", 1000, &VS::status_callback, this);
95  m_pubTwist = m_nh.advertise<geometry_msgs::TwistStamped>("pbvs/cmd_camvel", 1000);
96 
97  // Load desired pose from parameters
98  m_nh.getParam("tx_d", m_t_x_d);
99  m_nh.getParam("ty_d", m_t_y_d);
100  m_nh.getParam("tz_d", m_t_z_d);
101  m_nh.getParam("tux_d", m_tu_x_d);
102  m_nh.getParam("tuy_d", m_tu_y_d);
103  m_nh.getParam("tuy_d", m_tu_z_d);
104 
105  // Load object size
106  m_nh.getParam("square_size", m_square_size);
107 
108  // Load camera parameters
109  m_nh.getParam("cam_px", m_cam_px);
110  m_nh.getParam("cam_py", m_cam_py);
111  m_nh.getParam("cam_u0", m_cam_u0);
112  m_nh.getParam("cam_v0", m_cam_v0);
113  m_nh.getParam("cam_kud", m_cam_kud);
114  m_nh.getParam("cam_kdu", m_cam_kdu);
115 
116  if (m_cam_px < 0 || m_cam_py < 0 || m_cam_u0 < 0 || m_cam_v0 < 0) {
117  ROS_ERROR("Camera parameters are not set");
118  }
119 
120  if (m_cam_kud == -1 || m_cam_kdu == -1) {
121  m_cam.initPersProjWithoutDistortion(m_cam_px, m_cam_py, m_cam_u0, m_cam_v0);
122  }
123  else {
124  m_cam.initPersProjWithDistortion(m_cam_px, m_cam_py, m_cam_u0, m_cam_v0, m_cam_kud, m_cam_kdu);
125  }
126 
127  m_cdMo.buildFrom(m_t_x_d, m_t_y_d, m_t_z_d, vpMath::rad(m_tu_x_d), vpMath::rad(m_tu_y_d), vpMath::rad(m_tu_z_d));
128  std::cout << "Desired pose: " << m_cdMo << std::endl;
129 }
130 
132 {
133  m_lambda_adapt.initStandard(3, 0.2, 30);
134 
135  m_task.setServo(vpServo::EYEINHAND_CAMERA);
136  m_task.setInteractionMatrixType(vpServo::CURRENT);
137  m_task.setLambda(m_lambda);
138 // m_task.setLambda(m_lambda_adapt);
139 
140  m_task.addFeature(m_s_t, m_s_t_d);
141  m_task.addFeature(m_s_tu, m_s_tu_d);
142 }
143 
145 {
146 #ifdef VISP_HAVE_X11
147  m_display = new vpDisplayX;
148 #elif VISP_HAVE_GDI
149  m_display = new vpDisplayGDI;
150 #elif VISP_HAVE_OPENCV
151  m_display = new vpDisplayOpenCV;
152 #endif
153  if (m_display) {
154  std::cout << "Image size: " << m_I_grayscale.getWidth() << " x " << m_I_grayscale.getHeight() << std::endl;
155  std::cout << "Camera parameters:\n" << m_cam << std::endl;
156  m_display->init(m_I_grayscale, m_I_grayscale.getWidth()+120, m_I_grayscale.getHeight()+20, "PBVS controller");
157  }
158 }
159 
160 void VS::display(const visp_ros::BlobTracker::ConstPtr& msg)
161 {
162  try {
163  cv_bridge::CvImagePtr cv_image_ptr = cv_bridge::toCvCopy(msg->image);
164  cv::Mat cv_frame = cv_image_ptr->image;
165  vpImageConvert::convert(cv_frame, m_I_grayscale);
166 
167  if (m_display == NULL) {
168  init_display();
169  }
170 
171  vpDisplay::display(m_I_grayscale);
172 
173  std::vector<vpImagePoint> ips;
174  for (unsigned int i = 0; i < msg->blob_cogs.size(); i++) {
175  vpImagePoint ip(msg->blob_cogs[i].i, msg->blob_cogs[i].j);
176  ips.push_back(ip);
177  std::stringstream ss;
178  ss << i;
179  vpDisplay::displayText(m_I_grayscale, ip + vpImagePoint(-20, 20), ss.str(), vpColor::red);
180  vpDisplay::displayCross(m_I_grayscale, ip, 15, vpColor::green, m_thickness);
181  }
182  for (unsigned int i = 1; i < ips_trajectory.size(); i++) {
183  for (unsigned int j = 0; j < ips_trajectory[i].size(); j++) {
184  vpDisplay::displayLine(m_I_grayscale, ips_trajectory[i][j], ips_trajectory[i-1][j], vpColor::green, m_thickness);
185  }
186  }
187 
188  ips_trajectory.push_back(ips);
189 
190  vpDisplay::displayFrame(m_I_grayscale, m_cMo, m_cam, m_square_size * 2. / 3. , vpColor::none, m_thickness);
191  vpDisplay::displayFrame(m_I_grayscale, m_cdMo, m_cam, m_square_size / 2., vpColor::none, m_thickness);
192 
193  vpDisplay::flush(m_I_grayscale);
194 
195  } catch (cv_bridge::Exception& e) {
196  ROS_ERROR("cv_bridge exception: %s", e.what());
197  return;
198  }
199 }
200 
201 void VS::data_callback(const visp_ros::BlobTracker::ConstPtr& msg)
202 {
203  geometry_msgs::TwistStamped camvel_msg;
204  try {
205  if (! m_init) {
206  init_vs();
207  m_init = true;
208  }
209  std::ostringstream strs;
210  strs << "Receive a new pose" << std::endl;
211  std::string str;
212  str = strs.str();
213  ROS_DEBUG("%s", str.c_str());
214 
215  m_cMo = visp_bridge::toVispHomogeneousMatrix(msg->pose_stamped.pose);
216 
217  std::cout << "pbvs cMo:\n" << m_cMo << std::endl;
218  std::cout << "pbvs cdMo:\n" << m_cdMo << std::endl;
219  // Update visual features
220  m_cdMc = m_cdMo * m_cMo.inverse();
221  std::cout << "m_cdMc:\n" << m_cdMc << std::endl;
222  m_s_t.buildFrom(m_cdMc);
223  m_s_tu.buildFrom(m_cdMc);
224 
225  m_v = m_task.computeControlLaw();
226  std::cout << "v: " << m_v.t() << std::endl;
227 
228  display(msg);
229 
230  camvel_msg.header.stamp = ros::Time::now();
231  camvel_msg.twist.linear.x = m_v[0];
232  camvel_msg.twist.linear.y = m_v[1];
233  camvel_msg.twist.linear.z = m_v[2];
234  camvel_msg.twist.angular.x = m_v[3];
235  camvel_msg.twist.angular.y = m_v[4];
236  camvel_msg.twist.angular.z = m_v[5];
237 
238  m_pubTwist.publish(camvel_msg);
239  }
240  catch(...) {
241  ROS_INFO("Catch an exception: set vel to 0");
242  camvel_msg.header.stamp = ros::Time::now();
243  camvel_msg.twist.linear.x = 0;
244  camvel_msg.twist.linear.y = 0;
245  camvel_msg.twist.linear.z = 0;
246  camvel_msg.twist.angular.x = 0;
247  camvel_msg.twist.angular.y = 0;
248  camvel_msg.twist.angular.z = 0;
249  m_pubTwist.publish(camvel_msg);
250  }
251 }
252 
253 void VS::status_callback(const std_msgs::Int8ConstPtr& msg)
254 {
255  if (msg->data == 0) {
256  m_enable_servo = false;
257 
258  // Stop the robot
259  geometry_msgs::TwistStamped camvel_msg;
260  camvel_msg.header.stamp = ros::Time::now();
261  camvel_msg.twist.linear.x = 0;
262  camvel_msg.twist.linear.y = 0;
263  camvel_msg.twist.linear.z = 0;
264  camvel_msg.twist.angular.x = 0;
265  camvel_msg.twist.angular.y = 0;
266  camvel_msg.twist.angular.z = 0;
267  m_pubTwist.publish(camvel_msg);
268  }
269  else if (msg->data == 1) {
270  m_enable_servo = true;
271  }
272  else {
273  m_quit = true;
274  }
275 }
276 
277 void VS::spin()
278 {
279  ros::Rate loop_rate(60);
280  while (m_nh.ok() && ! m_quit) {
281  ros::spinOnce();
282  loop_rate.sleep();
283  }
284  if (m_display) {
285  delete m_display;
286  m_display = NULL;
287  }
288  std::cout << "PBVS controller stopped..." << std::endl;
289 }
290 
291 int main(int argc, char **argv)
292 {
293  ros::init(argc, argv, "PBVS");
294 
295  VS vs(argc, argv);
296 
297  vs.spin();
298 }
299 
300 
void status_callback(const std_msgs::Int8ConstPtr &msg)
Definition: pbvs_blob.cpp:253
std::vector< std::vector< vpImagePoint > > ips_trajectory
Definition: pbvs_blob.cpp:69
double m_cam_u0
Definition: pbvs_blob.cpp:53
double m_square_size
Definition: pbvs_blob.cpp:49
ros::Subscriber m_subData
Definition: pbvs_blob.cpp:30
double m_t_x_d
Definition: pbvs_blob.cpp:46
double m_t_z_d
Definition: pbvs_blob.cpp:46
void data_callback(const visp_ros::BlobTracker::ConstPtr &msg)
Definition: pbvs_blob.cpp:201
void publish(const boost::shared_ptr< M > &message) const
VS(int argc, char **argv)
Definition: pbvs_blob.cpp:86
vpFeatureTranslation m_s_t
Definition: pbvs_blob.cpp:34
vpHomogeneousMatrix toVispHomogeneousMatrix(const geometry_msgs::Transform &trans)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
vpColVector m_v
Definition: pbvs_blob.cpp:40
vpAdaptiveGain m_lambda_adapt
Definition: pbvs_blob.cpp:38
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
Definition: pbvs_blob.cpp:25
double m_tu_y_d
Definition: pbvs_blob.cpp:47
vpFeatureThetaU m_s_tu_d
Definition: pbvs_blob.cpp:35
void init_vs()
Definition: pbvs_blob.cpp:131
void display(const visp_ros::BlobTracker::ConstPtr &msg)
Definition: pbvs_blob.cpp:160
double m_tu_z_d
Definition: pbvs_blob.cpp:47
vpServo m_task
Definition: pbvs_blob.cpp:33
void init_display()
Definition: pbvs_blob.cpp:144
double m_cam_py
Definition: pbvs_blob.cpp:52
bool m_quit
Definition: pbvs_blob.cpp:71
vpHomogeneousMatrix m_cdMc
Definition: pbvs_blob.cpp:42
bool m_init
Definition: pbvs_blob.cpp:43
#define ROS_INFO(...)
vpFeatureThetaU m_s_tu
Definition: pbvs_blob.cpp:35
double m_cam_px
Definition: pbvs_blob.cpp:51
CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
ros::Publisher m_pubTwist
Definition: pbvs_blob.cpp:29
double m_t_y_d
Definition: pbvs_blob.cpp:46
double m_cam_kud
Definition: pbvs_blob.cpp:55
double m_cam_kdu
Definition: pbvs_blob.cpp:56
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
virtual ~VS()
Definition: pbvs_blob.cpp:81
bool sleep()
vpHomogeneousMatrix m_cMo
Definition: pbvs_blob.cpp:42
bool m_enable_servo
Definition: pbvs_blob.cpp:44
unsigned int m_thickness
Definition: pbvs_blob.cpp:68
vpImage< unsigned char > m_I_grayscale
Definition: pbvs_blob.cpp:59
bool getParam(const std::string &key, std::string &s) const
vpFeatureTranslation m_s_t_d
Definition: pbvs_blob.cpp:34
vpCameraParameters m_cam
Definition: pbvs_blob.cpp:58
static Time now()
void spin()
Definition: pbvs_blob.cpp:277
vpDisplayX * m_display
Definition: pbvs_blob.cpp:61
ros::Subscriber m_subStatus
Definition: pbvs_blob.cpp:31
int main(int argc, char **argv)
Definition: pbvs_blob.cpp:291
bool ok() const
double m_tu_x_d
Definition: pbvs_blob.cpp:47
double m_lambda
Definition: pbvs_blob.cpp:37
ROSCPP_DECL void spinOnce()
#define ROS_ERROR(...)
vpHomogeneousMatrix m_cdMo
Definition: pbvs_blob.cpp:42
double m_cam_v0
Definition: pbvs_blob.cpp:54
ros::NodeHandle m_nh
Definition: pbvs_blob.cpp:28
#define ROS_DEBUG(...)


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