gloveServer_impl.cc
Go to the documentation of this file.
1 
18 /* system includes */
19 #include <stdio.h>
20 #include <sys/time.h>
21 #include <math.h>
22 #include <time.h>
23 #include <vector>
24 #include <algorithm>
25 
26 /* ros */
27 #include <ros/ros.h>
28 
29 #include <sstream>
30 
31 /* my includes */
32 #include "gloveConstants.h"
33 #include "gloveServer_impl.h"
34 
35 extern unsigned int debugLevel;
36 
38 
39  printf("Assuming %lu sensors.\n", sizeof(GLOVERAW_MAP2_GLOVEINVENTOR) / sizeof(unsigned int));
40  for (unsigned int i = 0; i < sizeof(GLOVERAW_MAP2_GLOVEINVENTOR) / sizeof(unsigned int); i++)
42 
43  myID = id;
44  myName = name;
45  threadRunning = false;
46 }
47 //
49  delete gloveDev;
50  threadRunning = false;
51 }
52 
53 bool gloveServer_impl::init(std::string serialPort) {
54  /* initial ros node */
55  std::string pubName = (myID == rightGlove) ? "rightGloveData" : "leftGloveData";
56  gloveDataPub = rosNode.advertise<asr_msgs::AsrGlove>(pubName, 1000);
57  gloveDataPub_radian = rosNode.advertise<asr_msgs::AsrGlove>(pubName+"_radian", 1000);
58 
59  /* start gloveDevice */
60  gloveDev = new gloveDevice;
61  if (!gloveDev->init(serialPort.c_str())) {
62  printf("Could not start GloveDevice... gloveDev->init(%s) failed. Bailing out\n", serialPort.c_str());
63  return false;
64  }
65  gloveDev->showCurrentStatus();
66 
67  return true;
68 }
69 
70 bool gloveServer_impl::loadCalibFile(const char * srcFileName) {
71  printf("Loading calibration file \"%s\"\n", srcFileName);
72 
73  FILE *f;
74 
75  // open file
76  if ((f = fopen(srcFileName, "r")) == NULL) {
77  printf("ERROR loading calibration file - open file error!\n");
78 
79  std::string tmp(std::string(DEFAULT_CALIBRATION_PATH) + srcFileName);
80  printf("Trying to load calibration file \"%s\"\n", tmp.c_str());
81  if ((f = fopen(tmp.c_str(), "r")) == NULL) {
82  printf("Could not open file.\n");
83  return false;
84  }
85  }
86  printf("Successfully opened file.\n");
87 
89  bool threadState = threadRunning;
90 
91  if (threadState) {
92  printf("Stopping server thread.\n");
93  this->stop();
94  usleep(65000);
95  }
96 
98  FulcRaw1.clear();
99  FulcRad1.clear();
100  FulcRaw2.clear();
101  FulcRad2.clear();
102 
103  double tmpraw1, tmprad1, tmpraw2, tmprad2;
104  // read calibration data from file
105  for (int i = 0; i < MAX_SENSORS; i++) {
106  if (!fscanf(f, "%lf:%lf %lf:%lf\n", &tmprad1, &tmpraw1, &tmprad2, &tmpraw2)) {
107  printf("load calibration file - error: read from file !\n");
108  fclose(f);
109  return false;
110  }
112  if (fabs(tmpraw2 - tmpraw1) < MIN_JOINT_RESOLUTION) {
113 
114  printf("ERROR in calibration data!\nJoint %s does not achieve minimum resolution.\n", JOINT_NAME[i]);
115  printf("Joint resolution: %d Minimum resolution: %d\n\n", (int) fabs(tmpraw2 - tmpraw1),
117 
118  if (fabs(tmpraw2 - tmpraw1) < 2) {
119 
120  printf("Bailing out...");
121  fclose(f);
122  exit(-1);
123  }
124  }
125 
126  FulcRaw1.push_back(tmpraw1);
127  FulcRad1.push_back(tmprad1);
128  FulcRaw2.push_back(tmpraw2);
129  FulcRad2.push_back(tmprad2);
130  }
131  fclose(f);
132 
133  printf("load calibration file successful !\n");
134 
135  if (threadState) {
136  printf("Starting server thread again.\n");
137  this->start();
138  }
139 
140  return true;
141 }
142 
144  threadRunning = true;
145 }
146 
149  threadRunning = false;
150 }
151 
153  boostWorkerThread->join();
154 }
155 
156 #define MAX_SETS 3
157 #define PAUSE_SET 10000
158 #define PAUSE_WRITE 10000
159 
160 unsigned char getMedian(unsigned char (&vAcc)[MAX_SETS][MAX_SENSORS], unsigned int index) {
161  if (MAX_SETS == 1)
162  return vAcc[0][index];
163 
164  std::vector<unsigned int> data;
165  for (unsigned int i = 0; i < MAX_SETS; i++)
166  data.push_back((unsigned int) vAcc[i][index]);
167  std::sort(data.begin(), data.end());
168 
169  if (MAX_SETS % 2 == 0)
170  return (unsigned char) ((data[MAX_SETS / 2 - 1] + data[MAX_SETS / 2]) / 2);
171  else
172  return (unsigned char) data[(MAX_SETS - 1) / 2];
173 }
174 
176 
177  unsigned char v[MAX_SENSORS], n, v_perm[MAX_SENSORS];
178  unsigned char vAcc[MAX_SETS][MAX_SENSORS];
179  statusQuery_t queryStatus;
181 
182  asr_msgs::AsrGlove gloveData;
183  asr_msgs::AsrGlove gloveData_radian;
184  gloveData.id = (myID == leftGlove) ? "L" : "R";
185  gloveData_radian.id = (myID == leftGlove) ? "L" : "R";
186 
187  while (threadRunning & ros::ok()) {
188 
190  for (unsigned int i = 0; i < MAX_SETS; i++) {
191  gloveDev->cmdGetSensorValues(vAcc[i], &n, &queryStatus, &gloveStatus);
192 // gloveDev->cmdGetSensorDummyValues(vAcc[i], &n, &queryStatus, &gloveStatus);
193 
194  if (debugLevel > 2) {
195  printf("vAcc: ");
196  for (unsigned int j = 0; j < MAX_SENSORS; j++)
197  printf("%3d ", vAcc[i][j]);
198  printf("\n");
199  }
200 
201  if (i + 1 < MAX_SETS)
202  usleep(PAUSE_SET);
203  else if (debugLevel > 2)
204  printf("\n");
205  }
206 
207  for (unsigned int i = 0; i < MAX_SENSORS; i++) {
208  v[i] = getMedian(vAcc, i);
209  }
210 
211  if (debugLevel > 2) {
212  printf("v: ");
213  for (unsigned int j = 0; j < MAX_SENSORS; j++)
214  printf("%3d ", v[j]);
215 
216  printf("\n");
217  }
218 
220  if (!permuteValues(v, v_perm)) {
221 
222  printf("Could not permute values. Bailing out.\n");
223  }
224 
225  time_t now;
226  time(&now);
227  if (debugLevel > 2)
228  printf("%s", ctime(&now));
229 
230  if (debugLevel > 0) {
231  printf("v_perm %s:", myID == leftGlove ? "L" : "R");
232  for (unsigned int i = 0; i < n; i++)
233  printf("%3d ", (int) v_perm[i]);
234  printf("\n");
235  }
236 
237  double *temp = new double[n];
238  for (unsigned int i = 0 ; i < n ; i++) temp[i] = (double) v_perm[i];
239  if (!sendNotificationDataToRos(temp, n, gloveData, gloveDataPub)) {
240  printf("Couldn't write Notification to NotificationManager. Error!\n");
241  exit(-1);
242  }
243 
244  if (!sendNotificationDataToRos(getRadianArray(v_perm, n), n, gloveData_radian, gloveDataPub_radian)) {
245  printf("Couldn't write Notification to NotificationManager. Error!\n");
246  exit(-1);
247  }
248 
249  usleep(PAUSE_WRITE); //500000);//30000);
250  }
251  threadRunning = false;
252 }
253 
254 bool gloveServer_impl::permuteValues(unsigned char* valIn, unsigned char* valOut) {
255 
256  if (sizeof(valIn) > sizeof(valOut)) {
257  printf("ERROR in permuteValues(). Array sizes do not fit for permutation!\n");
258  return false;
259  }
260 
261  for (unsigned int i = 0; i < MAX_SENSORS; i++) {
262  // printf( "valOut[%d] = valIn[%d] = %c\n", i, GLOVERAW_MAP2_GLOVEINVENTOR[i], valIn[GLOVERAW_MAP2_GLOVEINVENTOR[i]]);
263  valOut[i] = valIn[GLOVERAW_MAP2_GLOVEINVENTOR[i]];
264  }
265  return true;
266 }
267 
268 double gloveServer_impl::getRadian(int index, unsigned char val) {
269  /*
270  unsigned char zero = calibVals[0][index];
271  unsigned char min = calibVals[1][index];
272  unsigned char max = calibVals[2][index];
273  */
274  double alfa;
275 
276  if (RADSIGN[index] == 1)
277  alfa = -alfa;
278 
279  alfa =
280  ((FulcRad2[index] - FulcRad1[index]) * ((double) val - FulcRaw1[index])
281  / (FulcRaw2[index] - FulcRaw1[index])) + FulcRad1[index];
282  // printf( "rad2 %f rad1 %f raw2 %f raw1 %f val %f alfa %f\n", FulcRad2[index], FulcRad1[index], FulcRaw2[index], FulcRaw1[index], (double)val, alfa);
283  return alfa;
284 }
285 
286 double *gloveServer_impl::getRadianArray(unsigned char* data, unsigned int size){
287  double * radianArray = new double[size];
288  for ( unsigned int i = 0 ; i < size; i++){
289  radianArray[i] = getRadian(i, data[i]);
290  }
291  return radianArray;
292 }
293 
294 bool gloveServer_impl::sendNotificationDataToRos(double* data, unsigned int size, asr_msgs::AsrGlove gloveData, ros::Publisher publisher) {
295  for (unsigned int i = 0; i < size; i++) {
296  gloveData.data[i] = data[i];
297  }
298 
299  std::stringstream output;
300  output << "Glove Data Message published: " << myID << " ";
301  for (unsigned int i = 0; i < size; i++) {
302  output << gloveData.data[i] << " ";
303  }
304  ROS_INFO("%s", output.str().c_str());
305  publisher.publish(gloveData);
306  ros::spinOnce();
307 
308  return true;
309 }
310 
311 #if gloveServer_impl_test
312 int main(int argc, char **argv)
313 {
314 
315  // Create a new instance of the implementation
317 
318  while (1);
319 
320  delete impl;
321 
322  return retval;
323 }
324 #endif /* gloveServer_impl_test */
325 
unsigned int GLOVERAW_MAP2_GLOVEINVENTOR[22]
double getRadian(int index, unsigned char val)
boost::thread * boostWorkerThread
Thread for delivering tracker data to nCenter.
gloveDevice * gloveDev
double * getRadianArray(unsigned char *data, unsigned int size)
void publish(const boost::shared_ptr< M > &message) const
std::vector< double > FulcRaw2
f
const int RADSIGN[22]
Sign of angle. Notation in inventor model order, use after permutation!!!
const int MIN_JOINT_RESOLUTION
bool loadCalibFile(const char *srcFileName)
std::vector< double > FulcRad2
bool permuteValues(unsigned char *valIn, unsigned char *valOut)
permute sensor values according to GLOVERAW_MAP2_GLOVEINVENTOR mapping
unsigned char getMedian(unsigned char(&vAcc)[MAX_SETS][MAX_SENSORS], unsigned int index)
ros::Publisher gloveDataPub_radian
bool init(std::string serialPort)
void workerThread()
this method is called by static method s_workerthread
std::vector< double > FulcRaw1
bool cmdGetSensorValues(vector_t values, uchar_t *n, statusQuery_t *query, gloveStatusByte_t *gloveStatusByte)
Definition: gloveDevice.cc:390
#define PAUSE_WRITE
std::string myName
ros::Publisher gloveDataPub
bool threadRunning
flag switching on/off thread
int main(int argc, char **argv)
#define ROS_INFO(...)
unsigned int debugLevel
ROSCPP_DECL bool ok()
#define MAX_SETS
gloveServer_impl(gloveID id, std::string name)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void showCurrentStatus()
Definition: gloveDevice.cc:788
ros::NodeHandle rosNode
const unsigned int GLOVERAW_MAP2_GLOVEINVENTOR_DEFAULT[22]
#define DEFAULT_CALIBRATION_PATH
ROSCPP_DECL void spinOnce()
const char * JOINT_NAME[]
for debug output
bool sendNotificationDataToRos(double *data, unsigned int size, asr_msgs::AsrGlove gloveData, ros::Publisher publisher)
Send sensor data to Notification Center.
#define MAX_SENSORS
Definition: gloveDevice.h:38
#define PAUSE_SET
gloveID
std::vector< double > FulcRad1


asr_cyberglove_lib
Author(s): Heller Florian, Mei├čner Pascal, Nguyen Trung, Yi Xie
autogenerated on Mon Jun 10 2019 12:40:38