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 */
62void
63KatanaGotoThread::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
73void
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}
KatanaGotoThread(fawkes::RefPtr< fawkes::KatanaController > katana, fawkes::Logger *logger, unsigned int poll_interval_ms)
Constructor.
Definition: goto_thread.cpp:46
virtual void set_target(float x, float y, float z, float phi, float theta, float psi)
Set target position.
Definition: goto_thread.cpp:63
virtual void once()
Execute an action exactly once.
Definition: goto_thread.cpp:74
Katana motion thread base class.
Definition: motion_thread.h:36
fawkes::Logger * _logger
Logger.
Definition: motion_thread.h:52
fawkes::RefPtr< fawkes::KatanaController > _katana
Katana object for interaction with the arm.
Definition: motion_thread.h:48
bool _finished
Set to true when motion is finished, to false on reset.
Definition: motion_thread.h:50
unsigned int _error_code
Set to the desired error code on error.
Definition: motion_thread.h:54
Base class for exceptions in Fawkes.
Definition: exception.h:36
virtual const char * what() const noexcept
Get primary string.
Definition: exception.cpp:639
virtual void read_motor_data()=0
Read motor data of currently active joints from device into controller libray.
virtual void read_sensor_data()=0
Read all sensor data from device into controller libray.
virtual bool final()=0
Check if movement is final.
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_NO_SOLUTION
ERROR_NO_SOLUTION constant.
static const uint32_t ERROR_COMMUNICATION
ERROR_COMMUNICATION constant.
static const uint32_t ERROR_MOTOR_CRASHED
ERROR_MOTOR_CRASHED constant.
static const uint32_t ERROR_CMD_START_FAILED
ERROR_CMD_START_FAILED constant.
At least one motor crashed.
Definition: exception.h:44
No joint configuration for desired target found.
Definition: exception.h:32
Interface for logging.
Definition: logger.h:42
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
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:100