Fawkes API  Fawkes Development Version
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  * 2011-2014 Bahram Maleki-Fard
8  *
9  ****************************************************************************/
10 
11 /* This program is free software; you can redistribute it and/or modify
12  * it under the terms of the GNU General Public License as published by
13  * the Free Software Foundation; either version 2 of the License, or
14  * (at your option) any later version.
15  *
16  * This program is distributed in the hope that it will be useful,
17  * but WITHOUT ANY WARRANTY; without even the implied warranty of
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19  * GNU Library General Public License for more details.
20  *
21  * Read the full text in the LICENSE.GPL file in the doc directory.
22  */
23 
24 #include "goto_thread.h"
25 
26 #include "controller.h"
27 #include "exception.h"
28 
29 #include <interfaces/KatanaInterface.h>
30 
31 #include <cstdlib>
32 #include <unistd.h>
33 
34 /** @class KatanaGotoThread "goto_thread.h"
35  * Katana linear goto thread.
36  * This thread moves the arm into a specified position.
37  * @author Tim Niemueller
38  */
39 
40 /** Constructor.
41  * @param katana katana controller base class
42  * @param logger logger
43  * @param poll_interval_ms interval in ms between two checks if the
44  * final position has been reached
45  */
47  fawkes::Logger * logger,
48  unsigned int poll_interval_ms)
49 : KatanaMotionThread("KatanaGotoThread", katana, logger)
50 {
51  poll_interval_usec_ = poll_interval_ms * 1000;
52 }
53 
54 /** Set target position.
55  * @param x X coordinate relative to base
56  * @param y Y coordinate relative to base
57  * @param z Z coordinate relative to base
58  * @param phi Phi Euler angle of tool
59  * @param theta Theta Euler angle of tool
60  * @param psi Psi Euler angle of tool
61  */
62 void
63 KatanaGotoThread::set_target(float x, float y, float z, float phi, float theta, float psi)
64 {
65  x_ = x;
66  y_ = y;
67  z_ = z;
68  phi_ = phi;
69  theta_ = theta;
70  psi_ = psi;
71 }
72 
73 void
75 {
76  try {
77  // non-blocking call
78  _katana->move_to(x_, y_, z_, phi_, theta_, psi_);
80  _logger->log_warn("KatanaGotoThread",
81  "Initiating goto failed (no solution, ignoring): %s",
82  e.what());
83  _finished = true;
85  return;
86  } catch (fawkes::Exception &e) {
87  _logger->log_warn("KatanaGotoThread", "Initiating goto failed (ignoring): %s", e.what());
88  _finished = true;
90  return;
91  }
92 
93  // check if final
94  bool final = false;
95  short num_errors = 0;
96  while (!final) {
97  usleep(poll_interval_usec_);
98  try {
101  } catch (fawkes::Exception &e) {
102  if (++num_errors <= 10) {
103  _logger->log_warn("KatanaMotorControlThread", "Reading sensor/motor data failed, retrying");
104  continue;
105  } else {
106  _logger->log_warn("KatanaMotorControlThread",
107  "Receiving sensor/motor data failed too often, aborting");
109  break;
110  }
111  }
112 
113  try {
114  final = _katana->final();
116  _logger->log_warn("KatanaMotorControlTrhead", "Motor crashed: %s", e.what());
118  break;
119  }
120  }
121 
123  name(), "Position (%f,%f,%f, %f,%f,%f) reached", x_, y_, z_, phi_, theta_, psi_);
124 
125  _finished = true;
126 }
fawkes::KatanaNoSolutionException
Definition: exception.h:35
KatanaMotionThread::_logger
fawkes::Logger * _logger
Logger.
Definition: motion_thread.h:51
fawkes::KatanaInterface::ERROR_NO_SOLUTION
static const uint32_t ERROR_NO_SOLUTION
ERROR_NO_SOLUTION constant.
Definition: KatanaInterface.h:68
KatanaMotionThread
Definition: motion_thread.h:34
fawkes::RefPtr< fawkes::KatanaController >
KatanaMotionThread::_finished
bool _finished
Set to true when motion is finished, to false on reset.
Definition: motion_thread.h:49
fawkes::KatanaController::move_to
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.
KatanaMotionThread::_error_code
unsigned int _error_code
Set to the desired error code on error.
Definition: motion_thread.h:53
fawkes::KatanaController::final
virtual bool final()=0
Check if movement is final.
KatanaMotionThread::_katana
fawkes::RefPtr< fawkes::KatanaController > _katana
Katana object for interaction with the arm.
Definition: motion_thread.h:47
fawkes::KatanaMotorCrashedException
Definition: exception.h:47
fawkes::Thread::name
const char * name() const
Definition: thread.h:99
fawkes::Logger
Definition: logger.h:40
KatanaGotoThread::KatanaGotoThread
KatanaGotoThread(fawkes::RefPtr< fawkes::KatanaController > katana, fawkes::Logger *logger, unsigned int poll_interval_ms)
Constructor.
Definition: goto_thread.cpp:45
fawkes::Logger::log_warn
virtual void log_warn(const char *component, const char *format,...)=0
KatanaGotoThread::set_target
virtual void set_target(float x, float y, float z, float phi, float theta, float psi)
Set target position.
Definition: goto_thread.cpp:62
KatanaGotoThread::once
virtual void once()
Execute an action exactly once.
Definition: goto_thread.cpp:73
fawkes::Exception::what
virtual const char * what() const
Get primary string.
Definition: exception.cpp:638
fawkes::KatanaInterface::ERROR_COMMUNICATION
static const uint32_t ERROR_COMMUNICATION
ERROR_COMMUNICATION constant.
Definition: KatanaInterface.h:69
fawkes::KatanaInterface::ERROR_CMD_START_FAILED
static const uint32_t ERROR_CMD_START_FAILED
ERROR_CMD_START_FAILED constant.
Definition: KatanaInterface.h:67
fawkes::KatanaController::read_motor_data
virtual void read_motor_data()=0
Read motor data of currently active joints from device into controller libray.
fawkes::Logger::log_debug
virtual void log_debug(const char *component, const char *format,...)=0
fawkes::KatanaController::read_sensor_data
virtual void read_sensor_data()=0
Read all sensor data from device into controller libray.
fawkes::KatanaInterface::ERROR_MOTOR_CRASHED
static const uint32_t ERROR_MOTOR_CRASHED
ERROR_MOTOR_CRASHED constant.
Definition: KatanaInterface.h:70
fawkes::Exception
Definition: exception.h:39