dsaboost.cpp
Go to the documentation of this file.
1 //======================================================================
31 //======================================================================
32 
33 #include <iostream>
34 #include <fstream>
35 #include <vector>
36 
37 
38 // Include the cSDH interface
39 #include "sdh/sdh.h"
40 #include "sdh/util.h"
42 #include "sdh/basisdef.h"
43 #include "sdh/dsa.h"
44 #include "dsaboost.h"
45 
46 #include <boost/bind.hpp>
47 #include <boost/mem_fn.hpp>
48 
50 
51 extern cDBG cdbg;
52 
53 //-----------------------------------------------------------------
54 
55 
56 
57 
68 {
69  assert( ts != NULL );
70 
71  int error_level = 0;
72 
73  cdbg << "cDSAUpdater::Updater(): thread started\n";
74  while( true )
75  {
76  try
77  {
78  //cdbg << "_Updater: updating\n";
79 
80  //semaphore.acquire()
81  ts->UpdateFrame();
82  //semaphore.release()
83 
84 
85  if ( error_level > 0 )
86  error_level--;
87 
88  //
89  boost::this_thread::sleep(boost::posix_time::milliseconds(1000/40));
90 
91  }
92  catch (cDSAException* e)
93  {
94  error_level++;
95 
96  // ignore errors like checksum errors and retry next time
97  cdbg << "cDSAUpdater::Updater(): ignoring " << e->what() << "\n";
98 
99  delete e;
100 
101  }
102 
103  if ( error_level > error_threshold )
104  {
105  std::cerr << "cDSAUpdater::Updater(): Too many errors from Tactile sensors of SDH. Reconnecting to DSACON32m!\n";
106 
107  try
108  {
109  ts->Close();
110  boost::this_thread::sleep(boost::posix_time::milliseconds(1000));
111  }
112  catch (cDSAException* e)
113  {
114  // ignore errors while trying to reconnect
115  cdbg << "cDSAUpdater::Updater(): ignoring " << e->what() << " while reconnecting step 1\n";
116  delete e;
117  }
118 
119  try
120  {
121  ts->Open();
122  }
123  catch (cDSAException* e)
124  {
125  // ignore errors while trying to reconnect
126  cdbg << "cDSAUpdater::Updater(): ignoring " << e->what() << " while reconnecting step 2\n";
127  delete e;
128  }
129 
130  try
131  {
132  ts->SetFramerate( 30, true );
133 
134  error_level = 0; // reset error_level after successfull reconnection
135  std::cerr << "cDSAUpdater::Updater() Succesfully reconnected to DSACON32m of SDH\n";
136  }
137  catch (cDSAException* e)
138  {
139  // ignore errors while trying to reconnect
140  cdbg << "cDSAUpdater::Updater(): ignoring " << e->what() << " while reconnecting step 3\n";
141  delete e;
142  }
143  }
144  }
145 }
146 //-----------------------------------------------------------------
147 
148 
149 cDSAUpdater::cDSAUpdater( cDSA* _ts, int _error_threshold ) :
150  error_threshold(_error_threshold),
151  ts(_ts)
152 {
153  assert( ts != NULL );
154 
155  ts->SetFramerate( 30, true );
156 
157  // start thread that reads the responses
158  cdbg << "cDSAUpdater::cDSAUpdater(): Starting new updater thread...";
159  updater_thread = boost::thread( boost::bind(&cDSAUpdater::Updater, this ) );
160  cdbg << "OK\n";
161 }
162 //-----------------------------------------------------------------
163 
164 
166 {
169 }
170 
171 
172 void cIsGraspedByArea::SetCondition( double eaf0p, double eaf0d, double eaf1p, double eaf1d, double eaf2p, double eaf2d )
173 {
174  expected_area[0] = eaf0p * FullArea( 0 );
175  expected_area[1] = eaf0d * FullArea( 1 );
176  expected_area[2] = eaf1p * FullArea( 2 );
177  expected_area[3] = eaf1d * FullArea( 3 );
178  expected_area[4] = eaf2p * FullArea( 4 );
179  expected_area[5] = eaf2d * FullArea( 5 );
180 }
181 
183 void cIsGraspedByArea::SetCondition( double* eafx )
184 {
185  for ( int i=0; i<6; i++ )
186  expected_area[i] = eafx[i] * FullArea( i );
187 }
188 
191 // call base class constructors:
192 : cIsGraspedBase( _ts )
193 {
194  SetCondition( 0.5, 0.5, 0.5, 0.5, 0.5, 0.5 );
195 }
196 
203 {
204  cDSA::sContactInfo contact_info;
205  bool is_grasped = true;
206  for ( int i=0; i<6; i++ )
207  {
208  contact_info = ts->GetContactInfo(i);
209 
210  if (contact_info.area < expected_area[ i ])
211  {
212  is_grasped = false;
213  break;
214  }
215  }
216  return is_grasped;
217 }
218 
219 //======================================================================
220 /*
221  Here are some settings for the emacs/xemacs editor (and can be safely ignored)
222  (e.g. to explicitely set C++ mode for *.h header files)
223 
224  Local Variables:
225  mode:C++
226  mode:ELSE
227  End:
228 */
229 //======================================================================]
Structure to hold info about the contact of one sensor patch.
Definition: dsa.h:249
SDH::cDSA * ts
ptr to the cDSA tactile sensor object to use
Definition: dsaboost.h:104
helper stuff for the DSA using boost
This file contains the interface to class #SDH::cSDH, the end user class to access the SDH from a PC...
int error_threshold
Definition: dsaboost.h:66
A class to print colored debug messages.
Definition: dbg.h:113
UInt16 cells_x
Definition: dsa.h:186
void Open(void)
(Re-)open connection to DSACON32m controller, this is called by the constructor automatically, but is still usefull to call after a call to Close()
Definition: dsa.cpp:671
UInt16 cells_y
Definition: dsa.h:187
#define NULL
Definition: getopt1.c:56
Derived exception class for low-level DSA related exceptions.
Definition: dsa.h:88
virtual const char * what() const
void Close(void)
Set the framerate of the remote DSACON32m controller to 0 and close connection to it...
Definition: dsa.cpp:739
cIsGraspedByArea(SDH::cDSA *_ts)
default constructor which initializes the internal date
Definition: dsaboost.cpp:190
double FullArea(int m)
helper function, return full area of sensor patch m in mm*mm
Definition: dsaboost.cpp:165
void SetFramerate(UInt16 framerate, bool do_RLE=true, bool do_data_acquisition=true)
Definition: dsa.cpp:766
cDSAUpdater(cDSA *_ts, int _error_threshold=DEFAULT_ERROR_THRESHOLD)
Definition: dsaboost.cpp:149
SDH::cDSA is the end user interface class to access the DSACON32m, the tactile sensor controller of t...
Definition: dsa.h:111
float texel_width
Definition: dsa.h:184
abstract base class for calculation of IsGrasped condition using tactile sensor information ...
Definition: dsaboost.h:101
Interface of auxilliary utility functions for SDHLibrary-CPP.
This file contains interface to #SDH::cDSA, a class to communicate with the tactile sensors of the SD...
void SetCondition(double eaf0p, double eaf0d, double eaf1p, double eaf1d, double eaf2p, double eaf2d)
Definition: dsaboost.cpp:172
boost::thread updater_thread
Definition: dsaboost.h:64
#define USING_NAMESPACE_SDH
void Updater()
the actual run function of the updater thread
Definition: dsaboost.cpp:67
float texel_height
Definition: dsa.h:185
This file contains settings to make the SDHLibrary compile on differen systems:
This file contains some basic definitions (defines, macros, datatypes)
double expected_area[6]
array of expected contact area of sensor patch in mm*mm
Definition: dsaboost.h:125
cDSA * ts
Definition: dsaboost.h:67
virtual bool IsGrasped(void)
Definition: dsaboost.cpp:202
USING_NAMESPACE_SDH cDBG cdbg
double area
area of contact in mm*mm.
Definition: dsa.h:252
sTactileSensorFrame const & UpdateFrame()
read the tactile sensor frame from remote DSACON32m and return a reference to it. A command to query ...
Definition: dsa.h:500
sMatrixInfo const & GetMatrixInfo(int m) const
Return a reference to the sMatrixInfo of matrix m read from the remote DSACON32m controller.
Definition: dsa.h:485


sdhlibrary_cpp
Author(s): Dirk Osswald
autogenerated on Mon Feb 28 2022 23:41:51