Main MRPT website > C++ reference for MRPT 1.9.9
CRovio.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "hwdrivers-precomp.h" // Precompiled headers
11 
12 #include <mrpt/hwdrivers/CRovio.h>
14 #include <mrpt/comms/net_utils.h>
15 #include <mrpt/io/CMemoryStream.h>
17 
18 #include <thread>
19 
20 using namespace mrpt::obs;
21 using namespace mrpt::hwdrivers;
22 using namespace mrpt::io;
23 using namespace mrpt::img;
24 using namespace mrpt::comms::net;
25 using namespace std;
26 
27 CRovio::TOptions::TOptions()
28  : IP("150.214.109.134"), user("admin"), password("investigacion")
29 // We could fill the camera matrices here instead of in initialization
30 {
31  double cam_params[] = {1650.740234375,
32  0,
33  805.33190917968750,
34  0,
35  1640.6497802734375,
36  675.1627197265625,
37  0,
38  0,
39  1};
42  0.12792350351810455, -0.4786585867404937, 0.011172077618539333,
43  -0.0037264013662934303);
44  // cameraParams.focalLengthMeters =
45 }
46 
47 /*-------------------------------------------------
48  INITIALIZE
49  -----------------------------------------------*/
50 void CRovio::initialize() // string &errormsg, string url_out, string user_out,
51 // string password_out)
52 {
53  string response, errormsg;
54  http_get(
55  format("http://%s/rev.cgi?Cmd=nav&action=1", options.IP.c_str()),
56  response, errormsg, 80, options.user, options.password);
57 
58  if (!response.empty())
59  cout << "[CRovio::Initialize] Response:\n" << response << endl;
60 
61  if (!errormsg.empty())
62  THROW_EXCEPTION_FMT("Error initializing Rovio: %s", errormsg.c_str());
63 }
64 
65 /*-------------------------------------------------
66  SEND MOVEMENT COMMAND (Manual Drive)
67  -----------------------------------------------*/
68 bool CRovio::send_cmd_action(int direction, int speed)
69 {
70  string response, errormsg;
71  string command = format(
72  "http://%s/rev.cgi?Cmd=nav&action=18&drive=%i&speed=%i",
73  options.IP.c_str(), direction, speed);
74  http_get(command, response, errormsg, 80, options.user, options.password);
75  return errormsg.empty();
76 }
77 
78 /*-------------------------------------------------
79  PATH MANAGEMENT
80  -----------------------------------------------*/
82 {
83  string response, errormsg;
84  string command;
85  command =
86  format("http://%s/rev.cgi?Cmd=nav&action=%i", options.IP.c_str(), act);
87  http_get(command, response, errormsg, 80, options.user, options.password);
88  return errormsg.empty();
89 }
90 
91 bool CRovio::path_management(int act, const string& path_name)
92 {
93  string response, errormsg;
94  string command;
95  command = format(
96  "http://%s/rev.cgi?Cmd=nav&action=%i&name=%s", options.IP.c_str(), act,
97  path_name.c_str());
98  http_get(command, response, errormsg, 80, options.user, options.password);
99  return errormsg.empty();
100 }
101 
102 /*-------------------------------------------------
103  GENERAL COMMAND
104  -----------------------------------------------*/
105 bool CRovio::general_command(int act, string& response, string& errormsg)
106 {
107  string command;
108  command =
109  format("http://%s/rev.cgi?Cmd=nav&action=%i", options.IP.c_str(), act);
110  http_get(command, response, errormsg, 80, options.user, options.password);
111  return errormsg.empty();
112 }
113 
114 /*-------------------------------------------------
115  MOVE ROBOT
116  -----------------------------------------------*/
117 bool CRovio::move(char direction, int speed)
118 {
119  switch (direction)
120  {
121  case 'f': // Forward
122  return send_cmd_action(1, speed);
123  case 'b': // Backward
124  return send_cmd_action(2, speed);
125  case 'l': // Left
126  return send_cmd_action(3, speed);
127  case 'r': // Right
128  return send_cmd_action(4, speed);
129  default:
130  cout << "Error in parameter of move()";
131  return false;
132  }
133 }
134 
135 bool CRovio::rotate(char direction, int speed)
136 {
137  switch (direction)
138  {
139  case 'l': // Left
140  return send_cmd_action(5, speed);
141  case 'r': // Right
142  return send_cmd_action(6, speed);
143  default:
144  cout << "Error in parameter of rotate()";
145  return false;
146  }
147 }
148 /*-------------------------------------------------
149  MOVE HEAD
150  -----------------------------------------------*/
151 bool CRovio::takeHeadUp() { return send_cmd_action(11, 5); }
152 bool CRovio::takeHeadMiddle() { return send_cmd_action(13, 5); }
153 bool CRovio::takeHeadDown() { return send_cmd_action(12, 5); }
154 /*-------------------------------------------------
155  PATH COMMAND
156  -----------------------------------------------*/
157 bool CRovio::pathRecord() { return path_management(2); }
159 bool CRovio::pathRecordSave(const string& path_name)
160 {
161  return path_management(4, path_name);
162 }
163 bool CRovio::pathDelete(const string& path_name)
164 {
165  return path_management(5, path_name);
166 }
167 bool CRovio::pathGetList(string& path_list)
168 {
169  string error;
170  general_command(6, path_list, error);
171  return error.empty();
172 }
176 bool CRovio::pathRunPause() { return path_management(10); }
177 bool CRovio::pathRename(const string& current_name, const string& new_name)
178 {
179  string response, errormsg;
180  string command = format(
181  "http://%s/rev.cgi?Cmd=nav&action=11&name=%s&newname=%s",
182  options.IP.c_str(), current_name.c_str(), new_name.c_str());
183  http_get(command, response, errormsg, 80, options.user, options.password);
184  return errormsg.empty();
185 }
186 
187 /*-------------------------------------------------
188  GO HOME
189  -----------------------------------------------*/
190 bool CRovio::goHome(bool dock, int speed)
191 {
192  if (dock)
193  return send_cmd_action(13, speed);
194  else
195  return send_cmd_action(12, speed);
196 }
197 
198 /*-------------------------------------------------
199  CAMERA FUNCTIONS
200  -----------------------------------------------*/
202  const mrpt::config::CConfigFileBase& configSource,
203  const std::string& section)
204 {
205  options.cameraParams.loadFromConfigFile(section, configSource);
206 
207  // Any other params??
208 }
209 /*-------------------------------------------------
210  VIDEO STREAMING
211  -----------------------------------------------*/
212 void CRovio::thread_video() // This function takes a frame and waits until
213 // getLastImage() ask for it, and so on.
214 {
215  try
216  {
217  // obj -> this
218  CFFMPEG_InputStream in_video;
219  string video_url = format("rtsp://%s/webcam", options.IP.c_str());
220 
221  const bool open_ok = in_video.openURL(
222  video_url, false /*grayscale*/, false /* verbose */);
223 
226 
228  {
229  std::cerr << "[CRovio] Error opening video stream: " << video_url
230  << std::endl;
231  return; // Error!
232  }
233 
234  while (!m_videothread_must_exit)
235  {
237  mrpt::make_aligned_shared<CObservationImage>();
238 
239  if (in_video.retrieveFrame(obs->image))
240  {
241  obs->cameraParams = options.cameraParams;
242 
243  // Critical section
244  {
245  std::lock_guard<std::mutex> cs(this->buffer_img_cs);
246  this->buffer_img = obs;
247  // cout<<"[CRovio::threadVideo] Image grabbed\n";
248  }
249  }
250  else
251  {
252  // obs.clear(); //If no image was copied, destroy the thisect.
253  cout << "[CRovio::thread_video] Warning: the program doesn't "
254  "receive any image\n";
255  }
256  std::this_thread::sleep_for(10ms);
257  } // end while
258 
259  in_video.close();
260 
261  m_videothread_finished = true;
262  }
263  catch (std::exception& e) // que hace eactamente esto?
264  {
265  m_videothread_initialized_done = true; // Just in case...
266  m_videothread_finished = true;
267  cout << "Error in thread_video thread";
268  cerr << e.what() << endl;
269  }
270  catch (...)
271  {
272  m_videothread_initialized_done = true; // Just in case...
273  m_videothread_finished = true;
274  cout << "Error in thread_video thread";
275  }
276 }
277 
279 {
280  if (m_videoThread.get_id() == std::thread::id())
281  {
284  m_videothread_must_exit = false;
285  m_videothread_finished = false;
286 
287  m_videoThread =
288  std::thread(&mrpt::hwdrivers::CRovio::thread_video, this);
289 
291  {
292  std::this_thread::sleep_for(10ms);
293  }
294 
295  // Ok or error?
297  {
298  return false;
299  }
300  else
301  return true; // Grabbing video
302  }
303  else
304  return true;
305 }
306 
308 {
309  return (m_videoThread.joinable() && !m_videothread_finished);
310 }
311 
313 {
314  bool was_already_stop = true;
316  if (isVideoStreamming())
317  {
318  m_videoThread.join();
319  was_already_stop = false;
320  }
321 
322  return !was_already_stop;
323 }
324 
327  lastImage) // This function grabbes the images taken by thread_video
328 {
329  if (!isVideoStreamming()) return false;
330 
331  {
332  std::lock_guard<std::mutex> cs(buffer_img_cs);
333  if (!buffer_img) return false;
334 
335  lastImage = buffer_img;
336  }
337 
338  return true;
339 }
340 
341 /*-------------------------------------------------
342  CAPTURE PICTURE
343  -----------------------------------------------*/
344 bool CRovio::captureImageAsync(CImage& picture, bool rectified)
345 {
346  try
347  {
348  std::vector<uint8_t> resp;
349  string errormsg;
350  string MF =
351  format("http://%s/Jpeg/CamImg[0000].jpg", options.IP.c_str());
352  http_get(MF, resp, errormsg, 80, options.user, options.password);
353 
354  CMemoryStream stream(&resp[0], resp.size());
355  picture.loadFromStreamAsJPEG(stream);
356  if (rectified) // Comprobar que las matrices existen y son
357  // correctas********************
359  // picture.saveToFile("0000.jpg");
360  // cout<<"Response:\n"<<response<<endl;
361  return true;
362  }
363  catch (std::exception& e)
364  {
365  cerr << e.what() << endl;
366  return false;
367  }
368 }
369 
370 /*-------------------------------------------------
371  STATE
372  -----------------------------------------------*/
374 {
376  size_t x_pos, /*y_pos, theta_pos,*/ lenght;
377  string x_value, response, errormsg;
378  mrpt::math::TPose2D pose;
379  general_command(1, response, errormsg); // Get report from Rovio to
380  // response
381 
382  // Getting x value
383  x_pos = response.find("x=");
384  x_value = response.substr((x_pos + 2), 8);
385  lenght = x_value.find('|');
386  x_value = x_value.substr(0, lenght);
387  char* x_char = new char[lenght];
388  strcpy(x_char, x_value.c_str());
389  pose.x = atof(x_char);
390 
391  string error;
392  string state;
393  general_command(1, state, error);
394 
395  return error.empty();
396 }
397 
398 /*-------------------------------------------------
399  GET ENCODERS
400  -----------------------------------------------*/
401 long convertToLong(char* sLong)
402 {
403  char* result = strpbrk(sLong, "-0123456789ABCDEF");
404  char* stop;
405  return strtol(result, &stop, 16);
406 }
407 
409 {
411  string resp, error, field;
412  // string field_name[12]={"Packet length","Not Used","Left Wheel:Dir
413  // Rotation","Left Wheel:Ticks","Right Wheel:Dir Rotation","Right
414  // Wheel:Ticks","Rear Wheel:Dir Rotation","Rear Wheel:Ticks","Not
415  // used","Head Position","Batery","Config Status"};
416  size_t length;
417  int* a_enc = new int[11]; // 12 encoder's fields
418  long l_value;
419 
420  general_command(20, resp, error); // get Encoders string to resp
421  if (error.empty())
422  {
423  size_t pos = (resp.find("responses =") + 12);
424  for (int i = 0; i <= 11; i++)
425  {
426  if ((i == 3) || (i == 5) || (i == 7))
427  length = 4;
428  else
429  length = 2;
430 
431  field = resp.substr(pos, length);
432  pos += length;
433 
434  /*---------- String to binary conv------------------*/
435  char* cstr = new char[field.size() + 1];
436  strcpy(cstr, field.c_str());
437  l_value = convertToLong(cstr);
438 
439  if ((i == 2) || (i == 4) ||
440  (i == 6)) // just interested in bit(2)-> "0000X0"
441  l_value = ((l_value >> 1) & 0x1); // This extracts the last but
442  // one bit of l_value which
443  // sustitutes l_value
444  a_enc[i] = l_value;
445  }
446  // Upload the encoders value
447  if (a_enc[2])
448  this->encoders.left += a_enc[3]; // Esta esto bien asi? o deberia
449  // usar encoders como parametro de
450  // entrada
451  else
452  this->encoders.left -= a_enc[3];
453  if (a_enc[4])
454  this->encoders.left += a_enc[5];
455  else
456  this->encoders.left -= a_enc[5];
457  if (a_enc[6])
458  this->encoders.left += a_enc[7];
459  else
460  this->encoders.left -= a_enc[7];
461 
462  return true;
463  }
464  else
465  {
466  cout << "\n---------------------------------------------------" << endl;
467  cout << "ERROR->" << error << endl;
468 
469  return false;
470  }
471 }
472 
473 /*-------------------------------------------------
474  GET POSITION WITH NORTHSTAR SYSTEM
475  -----------------------------------------------*/
477 {
478  size_t x_pos, y_pos, theta_pos, lenght;
479  string x_value, y_value, theta_value, response, errormsg;
480  general_command(1, response, errormsg); // Get report from Rovio to
481  // response
482 
483  // Getting x value
484  x_pos = response.find("x=");
485  x_value = response.substr((x_pos + 2), 8);
486  lenght = x_value.find('|');
487  x_value = x_value.substr(0, lenght);
488  char* x_char = new char[lenght];
489  strcpy(x_char, x_value.c_str());
490  pose.x = atof(x_char);
491 
492  // Getting y value
493  y_pos = response.find("y=");
494  y_value = response.substr((y_pos + 2), 8);
495  lenght = y_value.find('|');
496  y_value = y_value.substr(0, lenght);
497  char* y_char = new char[lenght];
498  strcpy(y_char, y_value.c_str());
499  pose.y = atof(y_char);
500 
501  // Getting theta value
502  theta_pos = response.find("theta=");
503  theta_value = response.substr((theta_pos + 6), 8);
504  lenght = theta_value.find('|');
505  theta_value = theta_value.substr(0, lenght);
506  char* theta_char = new char[lenght];
507  strcpy(theta_char, theta_value.c_str());
508  pose.phi = atof(theta_char);
509 
510  return errormsg.empty();
511 }
512 
515 {
516  if (isVideoStreamming()) stop_video();
517 }
bool retrieveFrame(mrpt::img::CImage &out_img)
Get the next frame from the video stream.
std::thread m_videoThread
Definition: CRovio.h:31
bool getNextImageSync(mrpt::obs::CObservationImage::Ptr &lastImage)
Returns the next frame from Rovio&#39;s live video stream, after starting the live streaming with retriev...
Definition: CRovio.cpp:325
void rectifyImageInPlace(const mrpt::img::TCamera &cameraParams)
Rectify (un-distort) the image according to a certain camera matrix and vector of distortion coeffici...
Definition: CImage.cpp:2050
bool getEncoders(TEncoders &encoders)
Returns a TEncoders with information of Rovio encoders (since last read, it seems Rovio is continuous...
Definition: CRovio.cpp:408
double x
X,Y coordinates.
bool send_cmd_action(int act, int speed)
Definition: CRovio.cpp:68
bool path_management(int act)
Definition: CRovio.cpp:81
void setDistortionParamsFromValues(double k1, double k2, double p1, double p2, double k3=0)
Set the vector of distortion params of the camera from the individual values of the distortion coeffi...
Definition: TCamera.h:150
bool rotate(char direction, int speed=5)
rotate send Rovio the command to rotate in the specified direcction &#39;r&#39;->right, &#39;l&#39;->left ...
Definition: CRovio.cpp:135
bool pathDelete(const std::string &path_name)
Definition: CRovio.cpp:163
bool m_videothread_initialized_error
Definition: CRovio.h:34
bool m_videothread_initialized_done
Definition: CRovio.h:33
Contains classes for various device interfaces.
mrpt::img::TCamera cameraParams
Definition: CRovio.h:60
A generic class which process a video file or other kind of input stream (http, rtsp) and allows the ...
bool getPosition(mrpt::math::TPose2D &out_pose)
Returns the Rovio&#39;s pose.
Definition: CRovio.cpp:476
bool pathGetList(std::string &path_list)
Get list of saved paths.
Definition: CRovio.cpp:167
STL namespace.
bool m_videothread_finished
Definition: CRovio.h:35
ERRORCODE_HTTP http_get(const string &url, std::vector< uint8_t > &out_content, string &out_errormsg, int port=80, const string &auth_user=string(), const string &auth_pass=string(), int *out_http_responsecode=nullptr, mrpt::system::TParameters< string > *extra_headers=nullptr, mrpt::system::TParameters< string > *out_headers=nullptr, int timeout_ms=1000)
Perform an HTTP GET operation (version for retrieving the data as a std::vector<uint8_t>) ...
Definition: net_utils.cpp:386
void loadFromConfigFile(const std::string &section, const mrpt::config::CConfigFileBase &cfg)
Load all the params from a config source, in the format used in saveToConfigFile(), that is:
Definition: TCamera.cpp:135
mrpt::math::CMatrixDouble33 intrinsicParams
Matrix of intrinsic parameters (containing the focal length and principal point coordinates) ...
Definition: TCamera.h:44
This class allows loading and storing values and vectors of different types from a configuration text...
bool move(char direction, int speed=5)
move send Rovio the command to move in the specified direcction
Definition: CRovio.cpp:117
bool captureImageAsync(mrpt::img::CImage &out_img, bool recttified)
Returns a snapshot from Rovio, if rectified is set true, the returned image is rectified with the par...
Definition: CRovio.cpp:344
This CStream derived class allow using a memory buffer as a CStream.
void initialize()
Establish Connection with Rovio and log in its system: Important, fill out "options" members BEFORE c...
Definition: CRovio.cpp:50
This namespace contains representation of robot actions and observations.
void thread_video()
This function takes a frame and waits until getLastImage ask for it, and so on.
Definition: CRovio.cpp:212
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
bool isVideoStreamming() const
Return true if video is streaming correctly.
Definition: CRovio.cpp:307
bool openURL(const std::string &url, bool grab_as_grayscale=false, bool verbose=false)
Open a video file or a video stream (rtsp://) This can be used to open local video files (eg...
void loadFromStreamAsJPEG(mrpt::io::CStream &in)
Reads the image from a binary stream containing a binary jpeg file.
bool retrieve_video()
This function launchs a thread with the function "thread_video()" which gets frames into a buffer...
Definition: CRovio.cpp:278
GLsizei const GLchar ** string
Definition: glext.h:4101
long convertToLong(char *sLong)
Definition: CRovio.cpp:401
struct mrpt::hwdrivers::CRovio::TEncoders encoders
bool stop_video()
This function stops and joins the thread launched by "retrieve_video()".
Definition: CRovio.cpp:312
bool getRovioState(TRovioState &state)
Returns a TRovioState with internal information of Rovio (State, Navigation Signal Strength...
Definition: CRovio.cpp:373
bool m_videothread_must_exit
Definition: CRovio.h:32
GLuint id
Definition: glext.h:3909
GLuint GLsizei GLsizei * length
Definition: glext.h:4064
bool takeHeadUp()
Head positions.
Definition: CRovio.cpp:151
Lightweight 2D pose.
bool pathRecordSave(const std::string &path_name)
Definition: CRovio.cpp:159
A set of useful routines for networking.
Definition: net_utils.h:23
bool goHome(bool dock, int speed=5)
goHome(bool dock) drives Rovio in front of charging station if the paremeter dock is set to false...
Definition: CRovio.cpp:190
void close()
Close the video stream (this is called automatically at destruction).
struct mrpt::hwdrivers::CRovio::TOptions options
std::mutex buffer_img_cs
Definition: CRovio.h:38
char * strcpy(char *dest, size_t destSize, const char *source) noexcept
An OS-independent version of strcpy.
Definition: os.cpp:297
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:43
double phi
Orientation (rads)
bool general_command(int act, std::string &response, std::string &errormsg)
Definition: CRovio.cpp:105
void loadConfig(const mrpt::config::CConfigFileBase &configSource, const std::string &section)
Loads the rovio camera calibration parameters (of leave the default ones if not found) (See CGenericS...
Definition: CRovio.cpp:201
bool pathRename(const std::string &old_name, const std::string &new_name)
Definition: CRovio.cpp:177
A class for storing images as grayscale or RGB bitmaps.
Definition: img/CImage.h:130
mrpt::obs::CObservationImage::Ptr buffer_img
Definition: CRovio.h:37
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019