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


visp_ros
Author(s): Francois Pasteau, Fabien Spindler, Gatien Gaumerais, Alexander Oliva
autogenerated on Wed Mar 2 2022 01:13:33