dynpick_driver.cpp
Go to the documentation of this file.
1 /*
2  * This program is based on http://github.com/tork-a/dynpick_driver and http://github.com/start-jsk/rtmros_hironx/blob/indigo-devel/hironx_ros_bridge/robot/nitta/jr3_driver.cpp
3 
4  This driver is stored in this robot-specific package for not many reasons than they are slightly customized for the robot (as of Apr 2016 it takes 2 sensor inputs in a single cpp file. It also assumes the specific device file name). So if you can separate those as a standalone, generic package that'll be appreciated (please just let us know if you will at https://github.com/start-jsk/rtmros_hironx/issues).
5 */
6 /*
7  * This program is for Dynpick F/T sensor (developed from JR3 sensor).
8  * Copyright(C) by Waseda University, Nitta Coropration. 2002.
9  *
10  * Copyright (c) 2016, TORK (Tokyo Opensource Robotics Kyokai Association)
11  * All rights reserved.
12  * # Redistribution and use in source and binary forms, with or without
13  * modification, are permitted provided that the following conditions
14  * are met:
15  * # * Redistributions of source code must retain the above copyright
16  * notice, this list of conditions and the following disclaimer.
17  * * Redistributions in binary form must reproduce the above
18  * copyright notice, this list of conditions and the following
19  * disclaimer in the documentation and/or other materials provided
20  * with the distribution.
21  * * Neither the name of TOKYO. nor the names of its contributors may be
22  * used to endorse or promote products derived from this software without
23  * specific prior written permission.
24  * # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  * POSSIBILITY OF SUCH DAMAGE.
36  */
37 
38 #include <errno.h>
39 #include <fcntl.h>
40 #include <stdio.h>
41 #include <stdlib.h>
42 #include <string.h>
43 #include <sys/iofunc.h>
44 #include <sys/dispatch.h>
45 #include <sys/mman.h>
46 #include <sys/neutrino.h>
47 #include <sys/slog.h>
48 #include <sys/time.h>
49 #include <termios.h>
50 #include <unistd.h>
51 
52 unsigned short force_sensor_data_0[6];
53 unsigned short force_sensor_data_1[6];
54 
55 /*
56  * serial stuff
57  */
58 
59 #define cfsetspeed(term, baudrate)\
60  cfsetispeed(term, baudrate);\
61  cfsetospeed(term, baudrate);
62 
63 int SetComAttr(int fdc) {
64  int res = -1;
65  if (fdc < 0) {
66  char *pmesg = strerror(errno);
67  fprintf(stderr, "failed to open : %s\n", pmesg);
68  goto over;
69  }
70 
71  struct termios term;
72  res = tcgetattr(fdc, &term);
73 
74  if (res < 0) {
75  char *pmesg = strerror(errno);
76  fprintf(stderr, "failed to tcgetattr(): %s\n", pmesg);
77  goto over;
78  }
79  cfmakeraw(&term);
80  res = cfsetspeed(&term, 921600)
81  ;
82  if (res < 0) {
83 
84  char *pmesg = strerror(errno);
85  fprintf(stderr, "failed to cfsetspeed(): %s\n", pmesg);
86  goto over;
87  }
88 
89  // settings for qnx
90  term.c_iflag |= IGNPAR; // Ignore characters with parity errors
91  term.c_cflag |= (CLOCAL | CREAD); // needed for QNX 6.3.2
92  term.c_cflag &= ~PARENB; // disable parity check
93  term.c_cflag |= CS8; // 8 data bit
94  term.c_cflag &= ~CSTOPB; // 1 stop bit
95  //term.c_lflag = IEXTEN;
96  term.c_oflag = 0;
97  term.c_lflag &= ~(ECHO | ECHOCTL | ECHONL); // disable ECHO
98 
99  //
100  term.c_iflag &= ~INPCK;
101 
102  // settings for dynpick
103  term.c_cc[VINTR] = 0; /* Ctrl-c */
104  term.c_cc[VQUIT] = 0; /* Ctrl-? */ term.c_cc[VERASE] = 0; /* del */
105  term.c_cc[VKILL] = 0; /* @ */
106  term.c_cc[VEOF] = 4; /* Ctrl-d */
107  term.c_cc[VTIME] = 0;
108  term.c_cc[VMIN] = 0;
109  //term.c_cc[VSWTC] = 0; /* '?0' */
110  term.c_cc[VSTART] = 0; /* Ctrl-q */
111  term.c_cc[VSTOP] = 0; /* Ctrl-s */
112  term.c_cc[VSUSP] = 0; /* Ctrl-z */
113  term.c_cc[VEOL] = 0; /* '?0' */
114  term.c_cc[VREPRINT] = 0; /* Ctrl-r */
115  term.c_cc[VDISCARD] = 0; /* Ctrl-u */
116  term.c_cc[VWERASE] = 0; /* Ctrl-w */
117  term.c_cc[VLNEXT] = 0; /* Ctrl-v */
118  term.c_cc[VEOL2] = 0; /* '?0' */
119 #ifdef __QNX__
120  term.c_cflag &= ~(IHFLOW | OHFLOW);
121 #endif
122 
123  // settings for qnx
124  term.c_cc[VMIN] = 100;
125  term.c_cc[VTIME] = 0;
126 
127  res = tcsetattr(fdc, TCSANOW, &term);
128  if (res < 0) {
129  char *pmesg = strerror(errno);
130  fprintf(stderr, "failed to tcsetattr(): %s\n", pmesg);
131  goto over;
132  }
133 
134  over: return (res);
135 }
136 
137 int ReadCom(int fdc, unsigned short *data) {
138  int tick;
139  char str[255];
140  int n, len;
141 
142 #define DATA_LENGTH 27
143  if (fdc < 0)
144  return DATA_LENGTH; // dummy data
145 
146  // Data Request
147  n = write(fdc, "R", 1);
148  //tcdrain(fdc); // Old legacy that is no longer needed. Besideds that, on QNX6.5.0 this is found taking unnecessarily too long (say 100msec).
149  printf("write data ret=%d, fd=%d\n", n, fdc);
150 
151  // Get Singale data
152  len = 0;
153  bzero(str, 27);
154  while (len < DATA_LENGTH) {
155  n = read(fdc, str + len, DATA_LENGTH - len);
156  printf("read data %d (%d)\n", n, len);
157  if (n > 0) {
158  len += n;
159  } else {
160  char *pmesg = strerror(errno);
161  fprintf(stderr, "failed to read data (ret=%d, fd=%d): %s (%d)\n", n,
162  fdc, pmesg, errno);
163  goto loop_exit;
164  }
165  }
166  loop_exit: {
167  int i;
168  for (i = 0; i < DATA_LENGTH; i++) {
169  fprintf(stderr, "%02x:", 0x0000ff & str[i]);
170  }
171  fprintf(stderr, "\n");
172  }
173 
174  sscanf(str, "%1d%4hx%4hx%4hx%4hx%4hx%4hx", &tick, &data[0], &data[1],
175  &data[2], &data[3], &data[4], &data[5]);
176 
177  sprintf(str, "%d,%05d,%05d,%05d,%05d,%05d,%05d\n", tick, data[0], data[1],
178  data[2], data[3], data[4], data[5]);
179 
180  return len;
181 }
182 
183 /*
184  * message stuff
185  */
186 
187 #define min(a, b) ((a) < (b) ? (a) : (b))
188 
189 int io_read(resmgr_context_t * ctp, io_read_t * msg, RESMGR_OCB_T * ocb); //
190 static char *buffer = (char *) "Hello world\n"; //
191 
192 void wait_t(void); // wait for 3 sec.
193 
194 static resmgr_connect_funcs_t ConnectFuncs; //
195 static resmgr_io_funcs_t IoFuncs; //
196 static iofunc_attr_t IoFuncAttr; //
197 
198 typedef struct {
199  uint16_t msg_no;
200  char msg_data[255];
201 } server_msg_t;
202 
203 int message_callback(message_context_t * ctp, int type, unsigned flags,
204  void *handle) {
205  server_msg_t *msg;
206  int num;
207  char msg_reply[255];
208 
209  /* cast a pointer to the message data */
210  msg = (server_msg_t *) ctp->msg;
211 
212  /* Print out some usefull information on the message */
213  //printf( "\n\nServer Got Message:\n" );
214  //printf( " type: %d\n" , type );
215  //printf( " data: %s\n\n", msg->msg_data );
216 #if 0
217  force_sensor_data *data_0;
218  force_sensor_data *data_1;
219  data_0 = (force_sensor_data *) (Jr3BaseAddress0H + (Jr3DmAddrMask << 2));
220  data_1 = (force_sensor_data *) (Jr3BaseAddress1H + (Jr3DmAddrMask << 2));
221 #endif
222  /* Build the reply message */
223  num = type - _IO_MAX;
224  switch (num) {
225  case 1: // get data
226  {
227 #if 0 // FIX ME
228  float tmp[12] = {
229  -1.0 * (float) data_0->filter0.fx / (float) data_0->full_scale.fx,
230  -1.0 * (float) data_0->filter0.fy / (float) data_0->full_scale.fy,
231  -1.0 * (float) data_0->filter0.fz / (float) data_0->full_scale.fz,
232  -1.0 * (float) data_0->filter0.mx / (float) data_0->full_scale.mx * 0.1, // Newton*meter*10
233  -1.0 * (float) data_0->filter0.my / (float) data_0->full_scale.my * 0.1,
234  -1.0 * (float) data_0->filter0.mz / (float) data_0->full_scale.mz * 0.1,
235  -1.0 * (float) data_1->filter0.fx / (float) data_1->full_scale.fx,
236  -1.0 * (float) data_1->filter0.fy / (float) data_1->full_scale.fy,
237  -1.0 * (float) data_1->filter0.fz / (float) data_1->full_scale.fz,
238  -1.0 * (float) data_1->filter0.mx / (float) data_1->full_scale.mx * 0.1,
239  -1.0 * (float) data_1->filter0.my / (float) data_1->full_scale.my * 0.1,
240  -1.0 * (float) data_1->filter0.mz / (float) data_1->full_scale.mz * 0.1
241  };
242 #endif
243  float tmp[12] = { (force_sensor_data_0[0] - 8192) / 1000.0,
244  (force_sensor_data_0[1] - 8192) / 1000.0,
245  (force_sensor_data_0[2] - 8192) / 1000.0,
246  (force_sensor_data_0[3] - 8192) / 1000.0,
247  (force_sensor_data_0[4] - 8192) / 1000.0,
248  (force_sensor_data_0[5] - 8192) / 1000.0,
249  (force_sensor_data_1[0] - 8192) / 1000.0,
250  (force_sensor_data_1[1] - 8192) / 1000.0,
251  (force_sensor_data_1[2] - 8192) / 1000.0,
252  (force_sensor_data_1[3] - 8192) / 1000.0,
253  (force_sensor_data_1[4] - 8192) / 1000.0,
254  (force_sensor_data_1[5] - 8192) / 1000.0, };
255  memcpy(msg_reply, tmp, sizeof(float) * 12);
256 
257  }
258  break;
259  case 2: // update offset
260 #if 0 // FIX ME
261  data_0->offsets.fx += data_0->filter0.fx;
262  data_0->offsets.fy += data_0->filter0.fy;
263  data_0->offsets.fz += data_0->filter0.fz;
264  data_0->offsets.mx += data_0->filter0.mx;
265  data_0->offsets.my += data_0->filter0.my;
266  data_0->offsets.mz += data_0->filter0.mz;
267  data_1->offsets.fx += data_1->filter0.fx;
268  data_1->offsets.fy += data_1->filter0.fy;
269  data_1->offsets.fz += data_1->filter0.fz;
270  data_1->offsets.mx += data_1->filter0.mx;
271  data_1->offsets.my += data_1->filter0.my;
272  data_1->offsets.mz += data_1->filter0.mz;
273 #endif
274  break;
275  case 3: // set filter
276  break;
277  case 4: // get data
278 #if 0 // FIX ME
279  get_force_sensor_info (data_0, msg_reply);
280  get_force_sensor_info (data_1, msg_reply + strlen (msg_reply));
281 #endif
282  break;
283  }
284 
285  /* Send a reply to the waiting (blocked) client */
286  MsgReply(ctp->rcvid, EOK, msg_reply, 255);
287  return 0;
288 }
289 
290 int main(int argc, char **argv) {
291  resmgr_attr_t resmgr_attr;
292  message_attr_t message_attr;
293  dispatch_t *dpp;
294  dispatch_context_t *ctp, *ctp_ret;
295  int resmgr_id, message_id;
296  int fd1, fd2;
297 
298  /* Open Serial port */
299  fd1 = open("/dev/serusb1", O_RDWR | O_NOCTTY | O_NONBLOCK);
300  if (fd1 < 0) {
301  fprintf(stderr, "could not open /dev/serusb1\n");
302  }
303  fd2 = open("/dev/serusb2", O_RDWR | O_NOCTTY | O_NONBLOCK);
304  if (fd2 < 0) {
305  fprintf(stderr, "could not open /dev/serusb2\n");
306  }
307  #ifdef __QNX__
308  slogf(0, _SLOG_INFO, "Started fd1 = %d, fd2 = %d\n", fd1, fd2);
309  #endif
310  SetComAttr(fd1);
311  SetComAttr(fd2);
312 
313  /* Create the Dispatch Interface */
314  dpp = dispatch_create();
315  if (dpp == NULL) {
316  fprintf(stderr, "dispatch_create() failed: %s\n", strerror(errno));
317  return EXIT_FAILURE;
318  }
319 
320  memset(&resmgr_attr, 0, sizeof(resmgr_attr));
321  resmgr_attr.nparts_max = 1;
322  resmgr_attr.msg_max_size = 2048;
323 
324  /* Setup the default I/O functions to handle open/read/write/... */
325  iofunc_func_init(_RESMGR_CONNECT_NFUNCS, &ConnectFuncs, _RESMGR_IO_NFUNCS,
326  &IoFuncs);
327 
328  IoFuncs.read = io_read;
329 
330  /* Setup the attribute for the entry in the filesystem */
331  iofunc_attr_init(&IoFuncAttr, S_IFNAM | 0666, 0, 0);
332  IoFuncAttr.nbytes = strlen(buffer) + 1; //
333 
334  resmgr_id = resmgr_attach(dpp, &resmgr_attr, "/dev/jr3q", _FTYPE_ANY, 0,
336  if (resmgr_id == -1) {
337  fprintf(stderr, "resmgr_attach() failed: %s\n", strerror(errno));
338  return EXIT_FAILURE;
339  }
340 
341  /* Setup Serial Port */
342 
343  /* Setup our message callback */
344  memset(&message_attr, 0, sizeof(message_attr));
345  message_attr.nparts_max = 1;
346  message_attr.msg_max_size = 4096;
347 
348  /* Attach a callback (handler) for two message types */
349  message_id = message_attach(dpp, &message_attr, _IO_MAX + 1, _IO_MAX + 10,
350  message_callback, NULL);
351  if (message_id == -1) {
352  fprintf(stderr, "message_attach() failed: %s\n", strerror(errno));
353  return EXIT_FAILURE;
354  }
355 
356  /* Setup a context for the dispatch layer to use */
357  ctp = dispatch_context_alloc(dpp);
358  if (ctp == NULL) {
359  fprintf(stderr, "dispatch_context_alloc() failed: %s\n",
360  strerror(errno));
361  return EXIT_FAILURE;
362  }
363 
364  /* The "Data Pump" - get and process messages */
365  while (1) {
366  /* do serial read */
369 
370  printf("dispatch\n");
371  /* process message */
372  ctp_ret = dispatch_block(ctp);
373  if (ctp_ret) {
374  dispatch_handler(ctp);
375  } else {
376  fprintf(stderr, "dispatch_block() failed: %s\n", strerror(errno));
377  return EXIT_FAILURE;
378  }
379  }
380 
381  return EXIT_SUCCESS;
382 }
383 
384 int io_read(resmgr_context_t * ctp, io_read_t * msg, RESMGR_OCB_T * ocb) {
385  int nleft;
386  int nbytes;
387  int nparts;
388  int status;
389 
390  if ((status = iofunc_read_verify(ctp, msg, ocb, NULL)) != EOK)
391  return (status);
392 
393  if ((msg->i.xtype & _IO_XTYPE_MASK) != _IO_XTYPE_NONE)
394  return (ENOSYS);
395 
396  /*
397  * on all reads (first and subsequent) calculate
398  * how many bytes we can return to the client,
399  * based upon the number of bytes available (nleft)
400  * and the client's buffer size
401  */
402 
403  nleft = ocb->attr->nbytes - ocb->offset;
404  nbytes = min(msg->i.nbytes, nleft);
405 
406  if (nbytes > 0) {
407  /* set up the return data IOV */
408  SETIOV(ctp->iov, buffer + ocb->offset, nbytes);
409 
410  /* set up the number of bytes (returned by client's read()) */
411  _IO_SET_READ_NBYTES(ctp, nbytes);
412 
413  /*
414  * advance the offset by the number of bytes
415  * returned to the client.
416  */
417 
418  ocb->offset += nbytes;
419  nparts = 1;
420  } else {
421  /* they've adked for zero bytes or they've already previously
422  * read everything
423  */
424 
425  _IO_SET_READ_NBYTES(ctp, 0);
426  nparts = 0;
427  }
428 
429  /* mark the access time as invalid (we just accessed it) */
430 
431  if (msg->i.nbytes > 0)
432  ocb->attr->flags |= IOFUNC_ATTR_ATIME;
433 
434  return (_RESMGR_NPARTS(nparts));
435 
436 }
437 
438 void wait_t(void) {
439  struct timeval tv, tv1;
440 
441  gettimeofday(&tv1, NULL);
442 
443  do {
444  gettimeofday(&tv, NULL);
445  } while (tv.tv_sec - tv1.tv_sec < 3); // wait for 3 sec.
446 
447  return;
448 }
unsigned short force_sensor_data_0[6]
static char * buffer
static iofunc_attr_t IoFuncAttr
int message_callback(message_context_t *ctp, int type, unsigned flags, void *handle)
#define EXIT_SUCCESS
unsigned short force_sensor_data_1[6]
png_uint_32 i
stderr
static resmgr_io_funcs_t IoFuncs
int gettimeofday(struct timeval *tv, struct timezone *tz)
status
#define cfsetspeed(term, baudrate)
int16_t fx
Definition: jr3_driver.cpp:92
#define Jr3DmAddrMask
int ReadCom(int fdc, unsigned short *data)
force_array filter0
Definition: jr3_driver.cpp:249
n
void wait_t(void)
#define min(a, b)
int SetComAttr(int fdc)
static resmgr_connect_funcs_t ConnectFuncs
int io_read(resmgr_context_t *ctp, io_read_t *msg, RESMGR_OCB_T *ocb)
std::string sprintf(char const *__restrict fmt,...)
res
str
volatile uint32_t Jr3BaseAddress0H
Definition: jr3_driver.cpp:69
#define DATA_LENGTH
int num
void get_force_sensor_info(force_sensor_data *data, char *msg)
Definition: jr3_driver.cpp:389
#define EXIT_FAILURE
int main(int argc, char **argv)
volatile uint32_t Jr3BaseAddress1H
Definition: jr3_driver.cpp:71


hironx_ros_bridge
Author(s): Kei Okada , Isaac I.Y. Saito
autogenerated on Mon Feb 28 2022 23:45:15