Fawkes API  Fawkes Development Version
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
goto_thread.cpp
1 
2 /***************************************************************************
3  * goto_thread.cpp - Katana goto one-time thread
4  *
5  * Created: Wed Jun 10 11:45:31 2009
6  * Copyright 2006-2009 Tim Niemueller [www.niemueller.de]
7  *
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL file in the doc directory.
21  */
22 
23 #include "goto_thread.h"
24 #include "controller.h"
25 #include "exception.h"
26 
27 #include <cstdlib>
28 
29 /** @class KatanaGotoThread "goto_thread.h"
30  * Katana linear goto thread.
31  * This thread moves the arm into a specified position.
32  * @author Tim Niemueller
33  */
34 
35 /** Constructor.
36  * @param katana katana controller base class
37  * @param logger logger
38  * @param poll_interval_ms interval in ms between two checks if the
39  * final position has been reached
40  */
42  fawkes::Logger *logger,
43  unsigned int poll_interval_ms)
44  : KatanaMotionThread("KatanaGotoThread", katana, logger)
45 {
46  __poll_interval_usec = poll_interval_ms * 1000;
47 }
48 
49 
50 /** Set target position.
51  * @param x X coordinate relative to base
52  * @param y Y coordinate relative to base
53  * @param z Z coordinate relative to base
54  * @param phi Phi Euler angle of tool
55  * @param theta Theta Euler angle of tool
56  * @param psi Psi Euler angle of tool
57  */
58 void
59 KatanaGotoThread::set_target(float x, float y, float z,
60  float phi, float theta, float psi)
61 {
62  __x = x;
63  __y = y;
64  __z = z;
65  __phi = phi;
66  __theta = theta;
67  __psi = psi;
68 }
69 
70 void
72 {
73  try {
74  // non-blocking call
75  _katana->move_to(__x, __y, __z, __phi, __theta, __psi);
77  _logger->log_warn("KatanaGotoThread", "Initiating goto failed (no solution, ignoring): %s", e.what());
78  _finished = true;
80  return;
81  } catch (fawkes::Exception &e) {
82  _logger->log_warn("KatanaGotoThread", "Initiating goto failed (ignoring): %s", e.what());
83  _finished = true;
85  return;
86  }
87 
88  // check if final
89  bool final = false;
90  short num_errors = 0;
91  while ( !final ) {
92  usleep(__poll_interval_usec);
93  try {
96  } catch (fawkes::Exception &e) {
97  if (++num_errors <= 10) {
98  _logger->log_warn("KatanaMotorControlThread", "Reading sensor/motor data failed, retrying");
99  continue;
100  } else {
101  _logger->log_warn("KatanaMotorControlThread", "Receiving sensor/motor data failed too often, aborting");
103  break;
104  }
105  }
106 
107  try {
108  final = _katana->final();
110  _logger->log_warn("KatanaMotorControlTrhead", e.what());
112  break;
113  }
114  }
115 
116  _logger->log_debug(name(), "Position (%f,%f,%f, %f,%f,%f) reached",
117  __x, __y, __z, __phi, __theta, __psi);
118 
119  _finished = true;
120 }
static const uint32_t ERROR_NO_SOLUTION
ERROR_NO_SOLUTION constant.
virtual void read_motor_data()=0
Read motor data of currently active joints from device into controller libray.
fawkes::RefPtr< fawkes::KatanaController > _katana
Katana object for interaction with the arm.
Definition: motion_thread.h:50
fawkes::Logger * _logger
Logger.
Definition: motion_thread.h:54
Katana motion thread base class.
Definition: motion_thread.h:37
No joint configuration for desired target found.
Definition: exception.h:34
virtual void set_target(float x, float y, float z, float phi, float theta, float psi)
Set target position.
Definition: goto_thread.cpp:59
virtual void read_sensor_data()=0
Read all sensor data from device into controller libray.
static const uint32_t ERROR_COMMUNICATION
ERROR_COMMUNICATION constant.
virtual void once()
Execute an action exactly once.
Definition: goto_thread.cpp:71
virtual const char * what() const
Get primary string.
Definition: exception.cpp:661
Base class for exceptions in Fawkes.
Definition: exception.h:36
virtual void move_to(float x, float y, float z, float phi, float theta, float psi, bool blocking=false)=0
Move endeffctor to given coordinates.
static const uint32_t ERROR_MOTOR_CRASHED
ERROR_MOTOR_CRASHED constant.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
const char * name() const
Get name of thread.
Definition: thread.h:95
At least one motor crashed.
Definition: exception.h:45
bool _finished
Set to true when motion is finished, to false on reset.
Definition: motion_thread.h:52
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
unsigned int _error_code
Set to the desired error code on error.
Definition: motion_thread.h:56
KatanaGotoThread(fawkes::RefPtr< fawkes::KatanaController > katana, fawkes::Logger *logger, unsigned int poll_interval_ms)
Constructor.
Definition: goto_thread.cpp:41
virtual bool final()=0
Check if movement is final.
static const uint32_t ERROR_CMD_START_FAILED
ERROR_CMD_START_FAILED constant.
Interface for logging.
Definition: logger.h:34