Fawkes API Fawkes Development Version
gripper_thread.cpp
1
2/***************************************************************************
3 * gripper_thread.cpp - Katana gripper one-time thread
4 *
5 * Created: Thu Jun 11 11:59:38 2009
6 * Copyright 2006-2009 Tim Niemueller [www.niemueller.de]
7 * 2012-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 "gripper_thread.h"
25
26#include "controller.h"
27#include "exception.h"
28
29#include <interfaces/KatanaInterface.h>
30
31#include <unistd.h>
32
33/** @class KatanaGripperThread "gripper_thread.h"
34 * Katana gripper thread.
35 * This thread opens or closes the gripper when started.
36 * @author Tim Niemueller
37 */
38
39/** Constructor.
40 * @param katana katana controller base class
41 * @param logger logger
42 * @param poll_interval_ms interval in ms between two checks if the
43 * final position has been reached
44 */
46 fawkes::Logger * logger,
47 unsigned int poll_interval_ms)
48: KatanaMotionThread("KatanaGripperThread", katana, logger)
49{
50 mode_ = OPEN_GRIPPER;
51 poll_interval_usec_ = poll_interval_ms * 1000;
52}
53
54/** Set mode.
55 * @param mode open, either open or close
56 */
57void
59{
60 mode_ = mode;
61}
62
63void
65{
66 try {
67 // non-blocking call
68 if (mode_ == CLOSE_GRIPPER) {
69 _katana->gripper_close(/* wait */ false);
70 } else {
71 _katana->gripper_open(/* wait */ false);
72 }
73
74 } catch (fawkes::Exception &e) {
75 _logger->log_warn("KatanaGripperThread",
76 "Starting gripper motion failed (ignoring): %s",
77 e.what());
78 _finished = true;
80 return;
81 }
82
83 // check if final
84 bool final = false;
85 short num_errors = 0;
86 while (!final) {
87 usleep(poll_interval_usec_);
88 try {
91 } catch (fawkes::Exception &e) {
92 if (++num_errors <= 10) {
93 _logger->log_warn("KatanaMotorControlThread", "Reading sensor/motor data failed, retrying");
94 continue;
95 } else {
96 _logger->log_warn("KatanaMotorControlThread",
97 "Receiving sensor/motor data failed too often, aborting");
99 break;
100 }
101 }
102
103 try {
104 final = _katana->final();
106 _logger->log_warn("KatanaMotorControlTrhead", "Motor crashed: %s", e.what());
108 break;
109 }
110 }
111
112 _logger->log_debug("KatanaGripperThread", "Gripper motion finished");
113
114 _finished = true;
115}
void set_mode(gripper_mode_t mode)
Set mode.
gripper_mode_t
Gripper execution mode.
@ CLOSE_GRIPPER
Close gripper.
@ OPEN_GRIPPER
Open gripper.
virtual void once()
Execute an action exactly once.
KatanaGripperThread(fawkes::RefPtr< fawkes::KatanaController > katana, fawkes::Logger *logger, unsigned int poll_interval_ms)
Constructor.
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 gripper_open(bool blocking=false)=0
Open Gripper.
virtual void read_motor_data()=0
Read motor data of currently active joints from device into controller libray.
virtual void gripper_close(bool blocking=false)=0
Close Gripper.
virtual void read_sensor_data()=0
Read all sensor data from device into controller libray.
virtual bool final()=0
Check if movement is final.
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
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.