MRPT  2.0.4
CRovio.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "hwdrivers-precomp.h" // Precompiled headers
11 
12 #include <mrpt/comms/net_utils.h>
14 #include <mrpt/hwdrivers/CRovio.h>
15 #include <mrpt/io/CMemoryStream.h>
16 #include <mrpt/math/TPose2D.h>
18 
19 #include <thread>
20 
21 using namespace mrpt::obs;
22 using namespace mrpt::hwdrivers;
23 using namespace mrpt::io;
24 using namespace mrpt::img;
25 using namespace mrpt::comms::net;
26 using namespace std;
27 
28 CRovio::TOptions::TOptions()
29  : IP("150.214.109.134"), user("admin"), password("investigacion")
30 // We could fill the camera matrices here instead of in initialization
31 {
32  double cam_params[] = {1650.740234375,
33  0,
34  805.33190917968750,
35  0,
36  1640.6497802734375,
37  675.1627197265625,
38  0,
39  0,
40  1};
43  0.12792350351810455, -0.4786585867404937, 0.011172077618539333,
44  -0.0037264013662934303);
45  // cameraParams.focalLengthMeters =
46 }
47 
48 /*-------------------------------------------------
49  INITIALIZE
50  -----------------------------------------------*/
51 void CRovio::initialize() // string &errormsg, string url_out, string user_out,
52 // string password_out)
53 {
54  string response, errormsg;
55  http_get(
56  format("http://%s/rev.cgi?Cmd=nav&action=1", options.IP.c_str()),
57  response, errormsg, 80, options.user, options.password);
58 
59  if (!response.empty())
60  cout << "[CRovio::Initialize] Response:\n" << response << endl;
61 
62  if (!errormsg.empty())
63  THROW_EXCEPTION_FMT("Error initializing Rovio: %s", errormsg.c_str());
64 }
65 
66 /*-------------------------------------------------
67  SEND MOVEMENT COMMAND (Manual Drive)
68  -----------------------------------------------*/
69 bool CRovio::send_cmd_action(int direction, int speed)
70 {
71  string response, errormsg;
72  string command = format(
73  "http://%s/rev.cgi?Cmd=nav&action=18&drive=%i&speed=%i",
74  options.IP.c_str(), direction, speed);
75  http_get(command, response, errormsg, 80, options.user, options.password);
76  return errormsg.empty();
77 }
78 
79 /*-------------------------------------------------
80  PATH MANAGEMENT
81  -----------------------------------------------*/
83 {
84  string response, errormsg;
85  string command;
86  command =
87  format("http://%s/rev.cgi?Cmd=nav&action=%i", options.IP.c_str(), act);
88  http_get(command, response, errormsg, 80, options.user, options.password);
89  return errormsg.empty();
90 }
91 
92 bool CRovio::path_management(int act, const string& path_name)
93 {
94  string response, errormsg;
95  string command;
96  command = format(
97  "http://%s/rev.cgi?Cmd=nav&action=%i&name=%s", options.IP.c_str(), act,
98  path_name.c_str());
99  http_get(command, response, errormsg, 80, options.user, options.password);
100  return errormsg.empty();
101 }
102 
103 /*-------------------------------------------------
104  GENERAL COMMAND
105  -----------------------------------------------*/
106 bool CRovio::general_command(int act, string& response, string& errormsg)
107 {
108  string command;
109  command =
110  format("http://%s/rev.cgi?Cmd=nav&action=%i", options.IP.c_str(), act);
111  http_get(command, response, errormsg, 80, options.user, options.password);
112  return errormsg.empty();
113 }
114 
115 /*-------------------------------------------------
116  MOVE ROBOT
117  -----------------------------------------------*/
118 bool CRovio::move(char direction, int speed)
119 {
120  switch (direction)
121  {
122  case 'f': // Forward
123  return send_cmd_action(1, speed);
124  case 'b': // Backward
125  return send_cmd_action(2, speed);
126  case 'l': // Left
127  return send_cmd_action(3, speed);
128  case 'r': // Right
129  return send_cmd_action(4, speed);
130  default:
131  cout << "Error in parameter of move()";
132  return false;
133  }
134 }
135 
136 bool CRovio::rotate(char direction, int speed)
137 {
138  switch (direction)
139  {
140  case 'l': // Left
141  return send_cmd_action(5, speed);
142  case 'r': // Right
143  return send_cmd_action(6, speed);
144  default:
145  cout << "Error in parameter of rotate()";
146  return false;
147  }
148 }
149 /*-------------------------------------------------
150  MOVE HEAD
151  -----------------------------------------------*/
152 bool CRovio::takeHeadUp() { return send_cmd_action(11, 5); }
153 bool CRovio::takeHeadMiddle() { return send_cmd_action(13, 5); }
154 bool CRovio::takeHeadDown() { return send_cmd_action(12, 5); }
155 /*-------------------------------------------------
156  PATH COMMAND
157  -----------------------------------------------*/
158 bool CRovio::pathRecord() { return path_management(2); }
160 bool CRovio::pathRecordSave(const string& path_name)
161 {
162  return path_management(4, path_name);
163 }
164 bool CRovio::pathDelete(const string& path_name)
165 {
166  return path_management(5, path_name);
167 }
168 bool CRovio::pathGetList(string& path_list)
169 {
170  string error;
171  general_command(6, path_list, error);
172  return error.empty();
173 }
177 bool CRovio::pathRunPause() { return path_management(10); }
178 bool CRovio::pathRename(const string& current_name, const string& new_name)
179 {
180  string response, errormsg;
181  string command = format(
182  "http://%s/rev.cgi?Cmd=nav&action=11&name=%s&newname=%s",
183  options.IP.c_str(), current_name.c_str(), new_name.c_str());
184  http_get(command, response, errormsg, 80, options.user, options.password);
185  return errormsg.empty();
186 }
187 
188 /*-------------------------------------------------
189  GO HOME
190  -----------------------------------------------*/
191 bool CRovio::goHome(bool dock, int speed)
192 {
193  if (dock)
194  return send_cmd_action(13, speed);
195  else
196  return send_cmd_action(12, speed);
197 }
198 
199 /*-------------------------------------------------
200  CAMERA FUNCTIONS
201  -----------------------------------------------*/
203  const mrpt::config::CConfigFileBase& configSource,
204  const std::string& section)
205 {
206  options.cameraParams.loadFromConfigFile(section, configSource);
207 
208  // Any other params??
209 }
210 /*-------------------------------------------------
211  VIDEO STREAMING
212  -----------------------------------------------*/
213 void CRovio::thread_video() // This function takes a frame and waits until
214 // getLastImage() ask for it, and so on.
215 {
216  try
217  {
218  // obj -> this
219  CFFMPEG_InputStream in_video;
220  string video_url = format("rtsp://%s/webcam", options.IP.c_str());
221 
222  const bool open_ok = in_video.openURL(
223  video_url, false /*grayscale*/, false /* verbose */);
224 
227 
229  {
230  std::cerr << "[CRovio] Error opening video stream: " << video_url
231  << std::endl;
232  return; // Error!
233  }
234 
235  while (!m_videothread_must_exit)
236  {
237  CObservationImage::Ptr obs = std::make_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 (const 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) picture.undistort(picture, options.cameraParams);
357  return true;
358  }
359  catch (const std::exception& e)
360  {
361  cerr << e.what() << endl;
362  return false;
363  }
364 }
365 
366 /*-------------------------------------------------
367  STATE
368  -----------------------------------------------*/
369 bool CRovio::getRovioState([[maybe_unused]] CRovio::TRovioState& st)
370 {
371  size_t x_pos, /*y_pos, theta_pos,*/ lenght;
372  string x_value, response, errormsg;
373  mrpt::math::TPose2D pose;
374  general_command(1, response, errormsg); // Get report from Rovio to
375  // response
376 
377  // Getting x value
378  x_pos = response.find("x=");
379  x_value = response.substr((x_pos + 2), 8);
380  lenght = x_value.find('|');
381  x_value = x_value.substr(0, lenght);
382  char* x_char = new char[lenght];
383  strcpy(x_char, x_value.c_str());
384  pose.x = atof(x_char);
385 
386  string error;
387  string state;
388  general_command(1, state, error);
389 
390  return error.empty();
391 }
392 
393 /*-------------------------------------------------
394  GET ENCODERS
395  -----------------------------------------------*/
396 long convertToLong(char* sLong)
397 {
398  char* result = strpbrk(sLong, "-0123456789ABCDEF");
399  char* stop;
400  return strtol(result, &stop, 16);
401 }
402 
403 bool CRovio::getEncoders([[maybe_unused]] CRovio::TEncoders& encs)
404 {
405  string resp, error, field;
406  // string field_name[12]={"Packet length","Not Used","Left Wheel:Dir
407  // Rotation","Left Wheel:Ticks","Right Wheel:Dir Rotation","Right
408  // Wheel:Ticks","Rear Wheel:Dir Rotation","Rear Wheel:Ticks","Not
409  // used","Head Position","Batery","Config Status"};
410  size_t length;
411  int* a_enc = new int[11]; // 12 encoder's fields
412  long l_value;
413 
414  general_command(20, resp, error); // get Encoders string to resp
415  if (error.empty())
416  {
417  size_t pos = (resp.find("responses =") + 12);
418  for (int i = 0; i <= 11; i++)
419  {
420  if ((i == 3) || (i == 5) || (i == 7))
421  length = 4;
422  else
423  length = 2;
424 
425  field = resp.substr(pos, length);
426  pos += length;
427 
428  /*---------- String to binary conv------------------*/
429  char* cstr = new char[field.size() + 1];
430  strcpy(cstr, field.c_str());
431  l_value = convertToLong(cstr);
432 
433  if ((i == 2) || (i == 4) ||
434  (i == 6)) // just interested in bit(2)-> "0000X0"
435  l_value = ((l_value >> 1) & 0x1); // This extracts the last but
436  // one bit of l_value which
437  // sustitutes l_value
438  a_enc[i] = l_value;
439  }
440  // Upload the encoders value
441  if (a_enc[2])
442  this->encoders.left += a_enc[3]; // Esta esto bien asi? o deberia
443  // usar encoders como parametro de
444  // entrada
445  else
446  this->encoders.left -= a_enc[3];
447  if (a_enc[4])
448  this->encoders.left += a_enc[5];
449  else
450  this->encoders.left -= a_enc[5];
451  if (a_enc[6])
452  this->encoders.left += a_enc[7];
453  else
454  this->encoders.left -= a_enc[7];
455 
456  encs = this->encoders;
457  return true;
458  }
459  else
460  {
461  cout << "\n---------------------------------------------------" << endl;
462  cout << "ERROR->" << error << endl;
463 
464  return false;
465  }
466 }
467 
468 /*-------------------------------------------------
469  GET POSITION WITH NORTHSTAR SYSTEM
470  -----------------------------------------------*/
472 {
473  size_t x_pos, y_pos, theta_pos, lenght;
474  string x_value, y_value, theta_value, response, errormsg;
475  general_command(1, response, errormsg); // Get report from Rovio to
476  // response
477 
478  // Getting x value
479  x_pos = response.find("x=");
480  x_value = response.substr((x_pos + 2), 8);
481  lenght = x_value.find('|');
482  x_value = x_value.substr(0, lenght);
483  char* x_char = new char[lenght];
484  strcpy(x_char, x_value.c_str());
485  pose.x = atof(x_char);
486 
487  // Getting y value
488  y_pos = response.find("y=");
489  y_value = response.substr((y_pos + 2), 8);
490  lenght = y_value.find('|');
491  y_value = y_value.substr(0, lenght);
492  char* y_char = new char[lenght];
493  strcpy(y_char, y_value.c_str());
494  pose.y = atof(y_char);
495 
496  // Getting theta value
497  theta_pos = response.find("theta=");
498  theta_value = response.substr((theta_pos + 6), 8);
499  lenght = theta_value.find('|');
500  theta_value = theta_value.substr(0, lenght);
501  char* theta_char = new char[lenght];
502  strcpy(theta_char, theta_value.c_str());
503  pose.phi = atof(theta_char);
504 
505  return errormsg.empty();
506 }
507 
508 CRovio::CRovio() = default;
510 {
511  if (isVideoStreamming()) stop_video();
512 }
bool retrieveFrame(mrpt::img::CImage &out_img)
Get the next frame from the video stream.
std::thread m_videoThread
Definition: CRovio.h:28
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
bool getEncoders(TEncoders &encoders)
Returns a TEncoders with information of Rovio encoders (since last read, it seems Rovio is continuous...
Definition: CRovio.cpp:403
double x
X,Y coordinates.
Definition: TPose2D.h:30
bool send_cmd_action(int act, int speed)
Definition: CRovio.cpp:69
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
bool path_management(int act)
Definition: CRovio.cpp:82
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:160
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:136
bool pathDelete(const std::string &path_name)
Definition: CRovio.cpp:164
bool m_videothread_initialized_error
Definition: CRovio.h:31
bool m_videothread_initialized_done
Definition: CRovio.h:30
Contains classes for various device interfaces.
mrpt::img::TCamera cameraParams
Definition: CRovio.h:57
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:471
bool pathGetList(std::string &path_list)
Get list of saved paths.
Definition: CRovio.cpp:168
STL namespace.
bool m_videothread_finished
Definition: CRovio.h:32
char * strcpy(char *dest, size_t destSize, const char *source) noexcept
An OS-independent version of strcpy.
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:165
mrpt::math::CMatrixDouble33 intrinsicParams
Matrix of intrinsic parameters (containing the focal length and principal point coordinates): ...
Definition: TCamera.h:50
void loadFromArray(const VECTOR &vals)
Definition: CMatrixFixed.h:171
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:118
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:51
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:213
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
long convertToLong(char *sLong)
Definition: CRovio.cpp:396
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:369
bool m_videothread_must_exit
Definition: CRovio.h:29
bool takeHeadUp()
Head positions.
Definition: CRovio.cpp:152
Lightweight 2D pose.
Definition: TPose2D.h:22
bool pathRecordSave(const std::string &path_name)
Definition: CRovio.cpp:160
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:191
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:35
void undistort(CImage &out_img, const mrpt::img::TCamera &cameraParams) const
Undistort the image according to some camera parameters, and returns an output undistorted image...
Definition: CImage.cpp:1691
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:69
double phi
Orientation (rads)
Definition: TPose2D.h:32
bool general_command(int act, std::string &response, std::string &errormsg)
Definition: CRovio.cpp:106
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:202
bool pathRename(const std::string &old_name, const std::string &new_name)
Definition: CRovio.cpp:178
A class for storing images as grayscale or RGB bitmaps.
Definition: img/CImage.h:148
mrpt::obs::CObservationImage::Ptr buffer_img
Definition: CRovio.h:34



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020