Main MRPT website > C++ reference for MRPT 1.5.6
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-2017, 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/utils/net_utils.h>
17 
18 using namespace mrpt::utils;
19 using namespace mrpt::obs;
20 using namespace mrpt::hwdrivers;
21 using namespace mrpt::utils::net;
22 using namespace std;
23 
24 
25 CRovio::TOptions::TOptions() :
26  IP("150.214.109.134"),
27  user("admin"),
28  password("investigacion")
29  //We could fill the camera matrices here instead of in initialization
30 {
31  double cam_params[] = {
32  1650.740234375, 0, 805.33190917968750,
33  0, 1640.6497802734375, 675.1627197265625,
34  0, 0, 1 };
36  cameraParams.setDistortionParamsFromValues(0.12792350351810455, -0.4786585867404937, 0.011172077618539333, -0.0037264013662934303);
37  //cameraParams.focalLengthMeters =
38 }
39 
40 /*-------------------------------------------------
41  INITIALIZE
42  -----------------------------------------------*/
43 void CRovio::initialize() //string &errormsg, string url_out, string user_out, string password_out)
44 {
45  string response, errormsg;
46  http_get (format("http://%s/rev.cgi?Cmd=nav&action=1",options.IP.c_str()), response, errormsg, 80, options.user, options.password);
47 
48  if (!response.empty())
49  cout<<"[CRovio::Initialize] Response:\n"<<response<<endl;
50 
51  if (!errormsg.empty())
52  THROW_EXCEPTION_FMT("Error initializing Rovio: %s",errormsg.c_str() );
53 }
54 
55 /*-------------------------------------------------
56  SEND MOVEMENT COMMAND (Manual Drive)
57  -----------------------------------------------*/
58 bool CRovio::send_cmd_action(int direction, int speed)
59 {
60  string response, errormsg;
61  string command = format("http://%s/rev.cgi?Cmd=nav&action=18&drive=%i&speed=%i", options.IP.c_str(), direction, speed);
62  http_get (command, response, errormsg, 80, options.user, options.password);
63  return errormsg.empty();
64 }
65 
66 /*-------------------------------------------------
67  PATH MANAGEMENT
68  -----------------------------------------------*/
70 {
71  string response, errormsg;
72  string command;
73  command = format("http://%s/rev.cgi?Cmd=nav&action=%i", options.IP.c_str(), act);
74  http_get (command, response, errormsg, 80, options.user, options.password);
75  return errormsg.empty();
76 }
77 
78 bool CRovio::path_management(int act, const string &path_name)
79 {
80  string response, errormsg;
81  string command;
82  command = format("http://%s/rev.cgi?Cmd=nav&action=%i&name=%s", options.IP.c_str(), act, path_name.c_str());
83  http_get (command, response, errormsg, 80, options.user, options.password);
84  return errormsg.empty();
85 }
86 
87 /*-------------------------------------------------
88  GENERAL COMMAND
89  -----------------------------------------------*/
90 bool CRovio::general_command(int act, string &response, string &errormsg)
91 {
92  string command;
93  command = format("http://%s/rev.cgi?Cmd=nav&action=%i", options.IP.c_str(), act);
94  http_get (command, response, errormsg, 80, options.user, options.password);
95  return errormsg.empty();
96 }
97 
98 
99 /*-------------------------------------------------
100  MOVE ROBOT
101  -----------------------------------------------*/
102 bool CRovio::move( char direction, int speed )
103 {
104  switch(direction)
105  {
106  case 'f': //Forward
107  return send_cmd_action(1,speed);
108  case 'b': //Backward
109  return send_cmd_action(2,speed);
110  case 'l': //Left
111  return send_cmd_action(3,speed);
112  case 'r': //Right
113  return send_cmd_action(4,speed);
114  default:
115  cout << "Error in parameter of move()";
116  return false;
117  }
118 }
119 
120 bool CRovio::rotate( char direction, int speed )
121 {
122  switch(direction)
123  {
124  case 'l': //Left
125  return send_cmd_action(5,speed);
126  case 'r': //Right
127  return send_cmd_action(6,speed);
128  default:
129  cout << "Error in parameter of rotate()";
130  return false;
131  }
132 
133 }
134 /*-------------------------------------------------
135  MOVE HEAD
136  -----------------------------------------------*/
138 {
139  return send_cmd_action(11,5);
140 }
142 {
143  return send_cmd_action(13,5);
144 }
146 {
147  return send_cmd_action(12,5);
148 }
149 
150 /*-------------------------------------------------
151  PATH COMMAND
152  -----------------------------------------------*/
154 {
155  return path_management(2);
156 }
158 {
159  return path_management(3);
160 }
161 bool CRovio::pathRecordSave(const string &path_name)
162 {
163  return path_management(4,path_name);
164 }
165 bool CRovio::pathDelete(const string &path_name)
166 {
167  return path_management(5,path_name);
168 }
169 bool CRovio::pathGetList(string &path_list)
170 {
171  string error;
172  general_command(6, path_list, error);
173  return error.empty();
174 }
176 {
177  return path_management(7);
178 }
180 {
181  return path_management(8);
182 }
184 {
185  return path_management(9);
186 }
188 {
189  return path_management(10);
190 }
191 bool CRovio::pathRename(const string &current_name, const string &new_name)
192 {
193  string response, errormsg;
194  string command = format("http://%s/rev.cgi?Cmd=nav&action=11&name=%s&newname=%s", options.IP.c_str(), current_name.c_str(), new_name.c_str());
195  http_get (command, response, errormsg, 80, options.user, options.password);
196  return errormsg.empty();
197 }
198 
199 /*-------------------------------------------------
200  GO HOME
201  -----------------------------------------------*/
202 bool CRovio::goHome(bool dock,int speed )
203 {
204  if(dock)
205  return send_cmd_action(13, speed );
206  else
207  return send_cmd_action(12, speed );
208 }
209 
210 /*-------------------------------------------------
211  CAMERA FUNCTIONS
212  -----------------------------------------------*/
214  const mrpt::utils::CConfigFileBase &configSource,
215  const std::string &section )
216 {
217  options.cameraParams.loadFromConfigFile(section,configSource);
218 
219  // Any other params??
220 }
221 /*-------------------------------------------------
222  VIDEO STREAMING
223  -----------------------------------------------*/
224 void CRovio::thread_video() //This function takes a frame and waits until getLastImage() ask for it, and so on.
225 {
226  try
227  {
228  // obj -> this
229  CFFMPEG_InputStream in_video;
230  string video_url=format("rtsp://%s/webcam", options.IP.c_str() );
231 
232  const bool open_ok = in_video.openURL(video_url, false /*grayscale*/, false /* verbose */ );
233 
236 
238  {
239  std::cerr << "[CRovio] Error opening video stream: " << video_url << std::endl;
240  return; // Error!
241  }
242 
244  {
245  CObservationImagePtr obs = CObservationImage::Create();
246 
247  if (in_video.retrieveFrame(obs->image) )
248  {
249  obs->cameraParams = options.cameraParams;
250 
251  //Critical section
252  {
254  this->buffer_img = obs;
255  //cout<<"[CRovio::threadVideo] Image grabbed\n";
256  }
257  }
258  else
259  {
260  //obs.clear(); //If no image was copied, destroy the thisect.
261  cout<<"[CRovio::thread_video] Warning: the program doesn't receive any image\n";
262  }
264  }//end while
265 
266  in_video.close();
267 
268  m_videothread_finished = true;
269  }
270  catch(std::exception &e)//que hace eactamente esto?
271  {
272  m_videothread_initialized_done = true; // Just in case...
273  m_videothread_finished = true;
274  cout<<"Error in thread_video thread";
275  cerr << e.what() << endl;
276  }
277  catch(...)
278  {
279  m_videothread_initialized_done = true; // Just in case...
280  m_videothread_finished = true;
281  cout<<"Error in thread_video thread";
282  }
283 }
284 
286 {
287  if(m_videoThread.isClear())
288  {
291  m_videothread_must_exit = false;
292  m_videothread_finished = false;
293 
295 
298  }
299 
300  // Ok or error?
302  {
303  m_videoThread.clear();
304  return false;
305  }
306  else return true; // Grabbing video
307  }
308  else
309  return true;
310 }
311 
313 {
315 }
316 
318 {
319  bool was_already_stop = true;
321  if (isVideoStreamming())
322  {
324  was_already_stop = false;
325  }
326  m_videoThread.clear();
327 
328  return !was_already_stop;
329 }
330 
331 bool CRovio::getNextImageSync(CObservationImagePtr& lastImage ) //This function grabbes the images taken by thread_video
332 {
333  if (!isVideoStreamming())
334  return false;
335 
336  {
338  if(!buffer_img)
339  return false;
340 
341  lastImage = buffer_img;
342  }
343 
344  return true;
345 }
346 
347 /*-------------------------------------------------
348  CAPTURE PICTURE
349  -----------------------------------------------*/
350 bool CRovio::captureImageAsync( CImage & picture, bool rectified)
351 {
352  try
353  {
354  vector_byte resp;
355  string errormsg;
356  string MF=format("http://%s/Jpeg/CamImg[0000].jpg",options.IP.c_str());
357  http_get (MF, resp, errormsg, 80, options.user, options.password);
358 
359  CMemoryStream stream( &resp[0], resp.size() );
360  picture.loadFromStreamAsJPEG(stream);
361  if( rectified )//Comprobar que las matrices existen y son correctas********************
363  //picture.saveToFile("0000.jpg");
364  //cout<<"Response:\n"<<response<<endl;
365  return true;
366  }
367  catch(std::exception &e)
368  {
369  cerr << e.what() << endl;
370  return false;
371  }
372 }
373 
374 /*-------------------------------------------------
375  STATE
376  -----------------------------------------------*/
378 {
379  MRPT_UNUSED_PARAM(status);
380  size_t x_pos, /*y_pos, theta_pos,*/ lenght;
381  string x_value, response, errormsg;
382  mrpt::math::TPose2D pose;
383  general_command(1, response, errormsg); //Get report from Rovio to response
384 
385  //Getting x value
386  x_pos=response.find("x=");
387  x_value=response.substr((x_pos+2),8);
388  lenght=x_value.find('|');
389  x_value=x_value.substr(0,lenght);
390  char* x_char=new char [lenght];
391  strcpy(x_char, x_value.c_str());
392  pose.x=atof(x_char);
393 
394  string error;
395  string state;
396  general_command(1, state, error);
397 
398  return error.empty();
399 }
400 
401 /*-------------------------------------------------
402  GET ENCODERS
403  -----------------------------------------------*/
404 long convertToLong(char *sLong)
405 {
406  char * result=strpbrk(sLong,"-0123456789ABCDEF");
407  char * stop;
408  return strtol(result,&stop,16);
409 }
410 
412 {
413  MRPT_UNUSED_PARAM(encoders);
414  string resp, error, field;
415  //string field_name[12]={"Packet length","Not Used","Left Wheel:Dir Rotation","Left Wheel:Ticks","Right Wheel:Dir Rotation","Right Wheel:Ticks","Rear Wheel:Dir Rotation","Rear Wheel:Ticks","Not 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)||(i==6) ) //just interested in bit(2)-> "0000X0"
440  l_value=( (l_value>>1) & 0x1); //This extracts the last but one bit of l_value which sustitutes l_value
441  a_enc[i] = l_value;
442  }
443  //Upload the encoders value
444  if(a_enc[2])
445  this->encoders.left += a_enc[3]; //Esta esto bien asi? o deberia usar encoders como parametro de entrada
446  else
447  this->encoders.left -= a_enc[3];
448  if(a_enc[4])
449  this->encoders.left += a_enc[5];
450  else
451  this->encoders.left -= a_enc[5];
452  if(a_enc[6])
453  this->encoders.left += a_enc[7];
454  else
455  this->encoders.left -= a_enc[7];
456 
457  return true;
458 
459  }else{
460  cout <<"\n---------------------------------------------------" << endl;
461  cout << "ERROR->" <<error <<endl;
462 
463  return false;
464  }
465 }
466 
467 /*-------------------------------------------------
468  GET POSITION WITH NORTHSTAR SYSTEM
469  -----------------------------------------------*/
471 {
472  size_t x_pos, y_pos, theta_pos, lenght;
473  string x_value, y_value, theta_value, response, errormsg;
474  general_command(1, response, errormsg); //Get report from Rovio to response
475 
476  //Getting x value
477  x_pos=response.find("x=");
478  x_value=response.substr((x_pos+2),8);
479  lenght=x_value.find('|');
480  x_value=x_value.substr(0,lenght);
481  char* x_char=new char [lenght];
482  strcpy(x_char, x_value.c_str());
483  pose.x=atof(x_char);
484 
485  //Getting y value
486  y_pos=response.find("y=");
487  y_value=response.substr((y_pos+2),8);
488  lenght=y_value.find('|');
489  y_value=y_value.substr(0,lenght);
490  char* y_char=new char [lenght];
491  strcpy(y_char, y_value.c_str());
492  pose.y=atof(y_char);
493 
494  //Getting theta value
495  theta_pos=response.find("theta=");
496  theta_value=response.substr((theta_pos+6),8);
497  lenght=theta_value.find('|');
498  theta_value=theta_value.substr(0,lenght);
499  char* theta_char=new char [lenght];
500  strcpy(theta_char, theta_value.c_str());
501  pose.phi=atof(theta_char);
502 
503  return errormsg.empty();
504 }
505 
507 {
508 
509 }
510 
512 {
513  if (isVideoStreamming())
514  stop_video();
515 }
GLuint GLsizei GLsizei * length
Definition: glew.h:1732
A class acquiring a CCriticalSection at its constructor, and releasing it at destructor.
std::vector< uint8_t > vector_byte
Definition: types_simple.h:26
bool getEncoders(TEncoders &encoders)
Returns a TEncoders with information of Rovio encoders (since last read, it seems Rovio is continuous...
Definition: CRovio.cpp:411
bool send_cmd_action(int act, int speed)
Definition: CRovio.cpp:58
bool path_management(int act)
Definition: CRovio.cpp:69
A class for storing images as grayscale or RGB bitmaps.
Definition: CImage.h:101
void rectifyImageInPlace(const mrpt::utils::TCamera &cameraParams)
Rectify (un-distort) the image according to a certain camera matrix and vector of distortion coeffici...
Definition: CImage.cpp:2050
bool rotate(char direction, int speed=5)
rotate send Rovio the command to rotate in the specified direcction 'r'->right, 'l'->left ...
Definition: CRovio.cpp:120
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
bool pathDelete(const std::string &path_name)
Definition: CRovio.cpp:165
struct mrpt::hwdrivers::CRovio::TEncoders encoders
bool m_videothread_initialized_error
Definition: CRovio.h:36
bool m_videothread_initialized_done
Definition: CRovio.h:35
bool retrieveFrame(mrpt::utils::CImage &out_img)
Get the next frame from the video stream.
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's pose.
Definition: CRovio.cpp:470
bool pathGetList(std::string &path_list)
Get list of saved paths.
Definition: CRovio.cpp:169
char BASE_IMPEXP * strcpy(char *dest, size_t destSize, const char *source) MRPT_NO_THROWS
An OS-independent version of strcpy.
Definition: os.cpp:296
bool m_videothread_finished
Definition: CRovio.h:37
This class allows loading and storing values and vectors of different types from a configuration text...
bool captureImageAsync(mrpt::utils::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:350
ERRORCODE_HTTP BASE_IMPEXP http_get(const string &url, vector_byte &out_content, string &out_errormsg, int port=80, const string &auth_user=string(), const string &auth_pass=string(), int *out_http_responsecode=NULL, mrpt::utils::TParameters< string > *extra_headers=NULL, mrpt::utils::TParameters< string > *out_headers=NULL, int timeout_ms=1000)
Perform an HTTP GET operation (version for retrieving the data as a vector_byte)
Definition: net_utils.cpp:388
bool move(char direction, int speed=5)
move send Rovio the command to move in the specified direcction
Definition: CRovio.cpp:102
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
This CStream derived class allow using a memory buffer as a CStream.
Definition: CMemoryStream.h:26
void BASE_IMPEXP sleep(int time_ms) MRPT_NO_THROWS
An OS-independent method for sending the current thread to "sleep" for a given period of time...
Definition: threads.cpp:57
mrpt::utils::TCamera cameraParams
Definition: CRovio.h:63
void initialize()
Establish Connection with Rovio and log in its system: Important, fill out "options" members BEFORE c...
Definition: CRovio.cpp:43
void thread_video()
This function takes a frame and waits until getLastImage ask for it, and so on.
Definition: CRovio.cpp:224
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:21
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...
mrpt::obs::CObservationImagePtr buffer_img
Definition: CRovio.h:39
bool isVideoStreamming() const
Return true if video is streaming correctly.
Definition: CRovio.cpp:312
bool retrieve_video()
This function launchs a thread with the function "thread_video()" which gets frames into a buffer...
Definition: CRovio.cpp:285
mrpt::synch::CCriticalSection buffer_img_cs
Definition: CRovio.h:40
mrpt::math::CMatrixDouble33 intrinsicParams
Matrix of intrinsic parameters (containing the focal length and principal point coordinates) ...
Definition: TCamera.h:54
long convertToLong(char *sLong)
Definition: CRovio.cpp:404
TThreadHandle createThreadFromObjectMethod(CLASS *obj, void(CLASS::*func)(PARAM), PARAM param)
Creates a new thread running a non-static method (so it will have access to "this") from another meth...
Definition: threads.h:216
double y
X,Y coordinates.
mrpt::system::TThreadHandle m_videoThread
Definition: CRovio.h:33
bool stop_video()
This function stops and joins the thread launched by "retrieve_video()".
Definition: CRovio.cpp:317
GLsizei const GLcharARB ** string
Definition: glew.h:3293
bool getRovioState(TRovioState &state)
Returns a TRovioState with internal information of Rovio (State, Navigation Signal Strength...
Definition: CRovio.cpp:377
bool m_videothread_must_exit
Definition: CRovio.h:34
bool takeHeadUp()
Head positions.
Definition: CRovio.cpp:137
Lightweight 2D pose.
bool pathRecordSave(const std::string &path_name)
Definition: CRovio.cpp:161
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:144
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:202
void close()
Close the video stream (this is called automatically at destruction).
struct mrpt::hwdrivers::CRovio::TOptions options
bool getNextImageSync(mrpt::obs::CObservationImagePtr &lastImage)
Returns the next frame from Rovio's live video stream, after starting the live streaming with retriev...
Definition: CRovio.cpp:331
double phi
Orientation (rads)
bool general_command(int act, std::string &response, std::string &errormsg)
Definition: CRovio.cpp:90
void loadFromConfigFile(const std::string &section, const mrpt::utils::CConfigFileBase &cfg)
Load all the params from a config source, in the format used in saveToConfigFile(), that is:
Definition: TCamera.cpp:130
bool pathRename(const std::string &old_name, const std::string &new_name)
Definition: CRovio.cpp:191
bool isClear() const
Returns true if the handle is uninitialized.
Definition: threads.h:64
void loadConfig(const mrpt::utils::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:213
void BASE_IMPEXP joinThread(const TThreadHandle &threadHandle)
Waits until the given thread ends.
Definition: threads.cpp:190
void loadFromStreamAsJPEG(CStream &in)
Reads the image from a binary stream containing a binary jpeg file.



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018