Fawkes API Fawkes Development Version
act_thread.cpp
1
2/***************************************************************************
3 * act_thread.cpp - Kinova Jaco plugin act-thread
4 *
5 * Created: Tue Jun 04 13:13:20 2013
6 * Copyright 2013 Bahram Maleki-Fard
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 "act_thread.h"
24
25#include "arm_dummy.h"
26#include "arm_kindrv.h"
27#include "goto_thread.h"
28#include "openrave_thread.h"
29#include "types.h"
30
31#include <core/utils/refptr.h>
32#include <interfaces/JacoInterface.h>
33
34using namespace fawkes;
35
36/** @class JacoActThread "act_thread.h"
37 * Jaco Arm control thread.
38 *
39 * @author Bahram Maleki-Fard
40 */
41
42/** Constructor.
43 * @param name thread name
44 * @param arm pointer to jaco_arm_t struct, to be used in this thread
45 */
47: Thread(name, Thread::OPMODE_WAITFORWAKEUP),
49{
50 arm_ = arm;
51 arm_->arm = NULL;
52 arm_->iface = NULL;
53
54 arm_->goto_thread = NULL;
55 arm_->openrave_thread = NULL;
56}
57
58/** Destructor. */
60{
61}
62
63/** Initialize.
64 * Depending on single or dual_arm setup (defined by config flag),
65 * appropriate arms are loaded and then initialized if required to.
66 * This method also sets the correct function pointers that are used in
67 * the main loop() method. */
68void
70{
71 cfg_auto_init_ = config->get_bool("/hardware/jaco/auto_initialize");
72 cfg_auto_calib_ = config->get_bool("/hardware/jaco/auto_calibrate");
73
74 std::string cfg_arm = config->get_string("/hardware/jaco/arm");
75
76 if (cfg_arm.compare("libkindrv") && cfg_arm.compare("dummy"))
77 throw fawkes::Exception("Bad config entry /hardware/jaco/arm '%s'", cfg_arm.c_str());
78
79 std::string arm_name, arm_iface;
80 switch (arm_->config) {
81 case CONFIG_SINGLE:
82 arm_name = config->get_string("/hardware/jaco/config/single/name");
83 arm_iface = config->get_string("/hardware/jaco/config/single/interface");
84 break;
85
86 case CONFIG_LEFT:
87 arm_name = config->get_string("/hardware/jaco/config/left/name");
88 arm_iface = config->get_string("/hardware/jaco/config/left/interface");
89 break;
90
91 case CONFIG_RIGHT:
92 arm_name = config->get_string("/hardware/jaco/config/right/name");
93 arm_iface = config->get_string("/hardware/jaco/config/right/interface");
94 break;
95
96 default: throw fawkes::Exception("Unknown arm config given"); break;
97 }
98
99 // create the JacoArm object and assign correctly to config
100 try {
101 if (!cfg_arm.compare("dummy")) {
102 arm_->arm = new JacoArmDummy("JacoDummy");
103 } else {
104 arm_->arm = new JacoArmKindrv(arm_name.c_str());
105 }
106 } catch (fawkes::Exception &e) {
107 logger->log_error(name(), "Could not connect to JacoArm. Exception follows.");
108 throw;
109 }
110
111 // open interface for writing
112 try {
113 arm_->iface = blackboard->open_for_writing<JacoInterface>(arm_iface.c_str());
114 } catch (fawkes::Exception &e) {
116 "Could not open interface %s for writing. Exception follows.",
117 arm_iface.c_str());
118 delete arm_->arm;
119 arm_ = NULL;
120 throw;
121 }
122
123 // create target/trajectory queues and mutexes
124 arm_->target_mutex = RefPtr<Mutex>(new Mutex());
125 arm_->trajec_mutex = RefPtr<Mutex>(new Mutex());
127
128 // set trajectory colors (TODO: configurable)
129 arm_->trajec_color[0] = 0.f;
130 arm_->trajec_color[1] = 0.f;
131 arm_->trajec_color[2] = 1.f;
132 arm_->trajec_color[3] = 1.f;
133 if (arm_->config == CONFIG_RIGHT) {
134 arm_->trajec_color[0] = 1.f;
135 arm_->trajec_color[2] = 0.f;
136 }
137
138 // initalize arm
139 _initialize();
140}
141
142/** Finalize.
143 * Close all writing interfaces and delete JacoArm instances.
144 */
145void
147{
148 try {
149 blackboard->close(arm_->iface);
150 } catch (fawkes::Exception &e) {
152 "Could not close JacoInterface interface. Er:%s",
154 }
155
156 delete arm_->arm;
157}
158
159/** Main loop.
160 * The structure is pretty obvious. We first submit changes made to the interface
161 * from threads before the ACT-hook (they might be used by other threads lateron).
162 * Then we make sure the arm is initialized, before processing incoming messages
163 * and submiting interface changes once again.
164 */
165void
167{
168 if (arm_ == NULL || arm_->iface == NULL || arm_->openrave_thread == NULL)
169 return;
170
171 // firts of all, submit interface updates (that other threads might have done)!
172 arm_->iface->write();
173
174 // check if still initializing
175 if (_is_initializing())
176 return;
177
178#ifdef HAVE_OPENRAVE
179 // make sure openrave-thread is ready!
180 if (!arm_->openrave_thread->started())
181 return;
182#endif
183
184 // process incoming interface messages
185 _process_msgs();
186
187 // finally, again submit interface updates
188 arm_->iface->write();
189
190#ifdef HAVE_OPENRAVE
192#endif
193 arm_->iface->set_final(arm_->goto_thread->final());
194}
195
196/* ##########################################################################
197 * private methods , referenced to by the function pointers from loop().
198 *
199 * we have one for each single_arm and dual_arm setup.
200 * ########################################################################## */
201/** Initialize and/or calibrate single arm, if requested by config flags */
202void
203JacoActThread::_initialize()
204{
205 //check if we need to initialize arm
206 if (!arm_->arm->initialized() && cfg_auto_init_) {
207 logger->log_debug(name(), "Initializing arm, wait until finished");
208 arm_->arm->initialize();
209 arm_->iface->set_final(false);
210 //arm_.goto_thread->pos_ready();
211
212 } else if (arm_->arm->initialized() && cfg_auto_calib_) {
213 arm_->goto_thread->pos_ready();
214 }
215
216 arm_->iface->set_initialized(arm_->arm->initialized());
217 arm_->iface->write();
218}
219
220/** Check if arm is being initialized. */
221bool
222JacoActThread::_is_initializing()
223{
224 arm_->iface->set_initialized(arm_->arm->initialized());
225
226 if (!arm_->arm->initialized() && cfg_auto_init_) {
227 logger->log_debug(name(), "wait for arm to initialize");
228 //arm_->initialized = arm_->iface->is_final();
229 return true;
230 }
231
232 return false;
233}
234
235/** Process interface messages. */
236void
237JacoActThread::_process_msgs()
238{
239 while (!arm_->iface->msgq_empty()) {
240 Message *m = arm_->iface->msgq_first(m);
241 arm_->iface->set_msgid(m->id());
242 arm_->iface->set_final(false);
243 arm_->iface->set_error_code(JacoInterface::ERROR_NONE);
244 arm_->iface->write();
245
248 logger->log_debug(name(), "%s: StopMessage rcvd", arm_->iface->id());
249
250 arm_->goto_thread->stop();
251
254 logger->log_debug(name(), "%s: CalibrateMessage rcvd", arm_->iface->id());
255
256 // Stop all (current and planned) motion. Then calibrate
257 arm_->goto_thread->stop();
258 arm_->goto_thread->pos_ready();
259
262 logger->log_debug(name(), "%s: RetractMessage rcvd", arm_->iface->id());
263
264 // Stop all (current and planned) motion. Then retract
265 arm_->goto_thread->stop();
266 arm_->goto_thread->pos_retract();
267
271 "%s: SetPlannerParamsMessage rcvd. params:%s",
272 arm_->iface->id(),
273 msg->params());
274
275#ifdef HAVE_OPENRAVE
277#endif
278
282 "%s: CartesianGotoMessage rcvd. x:%f y:%f z:%f e1:%f e2:%f e3:%f",
283 arm_->iface->id(),
284 msg->x(),
285 msg->y(),
286 msg->z(),
287 msg->e1(),
288 msg->e2(),
289 msg->e3());
290#ifdef HAVE_OPENRAVE
292 "%s: CartesianGotoMessage is being passed to openrave",
293 arm_->iface->id());
294 // add target to OpenRAVE queue for planning
295 bool solvable = arm_->openrave_thread->add_target(
296 msg->x(), msg->y(), msg->z(), msg->e1(), msg->e2(), msg->e3());
297 if (!solvable) {
298 arm_->iface->set_error_code(JacoInterface::ERROR_NO_IK);
300 "Failed executing CartesianGotoMessage, arm %s and/or thread %s could not "
301 "find IK solution",
302 arm_->arm->get_name().c_str(),
303 arm_->openrave_thread->name());
304 }
305#else
306 arm_->goto_thread->set_target(msg->x(), msg->y(), msg->z(), msg->e1(), msg->e2(), msg->e3());
307#endif
308
311
313 "%s: AngularGotoMessage rcvd. x:%f y:%f z:%f e1:%f e2:%f e3:%f",
314 arm_->iface->id(),
315 msg->j1(),
316 msg->j2(),
317 msg->j3(),
318 msg->j4(),
319 msg->j5(),
320 msg->j6());
321#ifdef HAVE_OPENRAVE
323 "%s: AngularGotoMessage is being passed to openrave",
324 arm_->iface->id());
325 // add target to OpenRAVE queue for planning
326 bool joints_valid = arm_->openrave_thread->add_target_ang(
327 msg->j1(), msg->j2(), msg->j3(), msg->j4(), msg->j5(), msg->j6());
328 if (!joints_valid) {
329 arm_->iface->set_error_code(JacoInterface::ERROR_NO_IK);
331 "Failed executing AngularGotoMessage, given target joints for arm %s are "
332 "invalid or in self-collision",
333 arm_->arm->get_name().c_str());
334 }
335#else
337 msg->j1(), msg->j2(), msg->j3(), msg->j4(), msg->j5(), msg->j6());
338#endif
339
343 "%s: MoveGripperMessage rcvd. f1:%f f2:%f f3:%f",
344 arm_->iface->id(),
345 msg->finger1(),
346 msg->finger2(),
347 msg->finger3());
348
349 arm_->goto_thread->move_gripper(msg->finger1(), msg->finger2(), msg->finger3());
350
353 logger->log_debug(name(), "%s: JoystickPush %u rcvd", arm_->iface->id(), msg->button());
354
355 arm_->arm->push_joystick(msg->button());
356
359 logger->log_debug(name(), "%s: JoystickRelease rcvd", arm_->iface->id());
360
361 arm_->arm->release_joystick();
362 arm_->iface->set_final(true);
363
364 } else {
365 logger->log_warn(name(), "%s: Unknown message received. Skipping", arm_->iface->id());
366 }
367
368 arm_->iface->msgq_pop();
369 }
370}
virtual void init()
Initialize.
Definition: act_thread.cpp:69
virtual void loop()
Main loop.
Definition: act_thread.cpp:166
JacoActThread(const char *name, fawkes::jaco_arm_t *arm)
Constructor.
Definition: act_thread.cpp:46
virtual ~JacoActThread()
Destructor.
Definition: act_thread.cpp:59
virtual void finalize()
Finalize.
Definition: act_thread.cpp:146
virtual bool final()
Check if arm is final.
Definition: goto_thread.cpp:87
virtual void pos_retract()
Moves the arm to the "RETRACT" position.
virtual void set_target_ang(float j1, float j2, float j3, float j4, float j5, float j6, float f1=0.f, float f2=0.f, float f3=0.f)
Set new target, given joint positions This target is added to the queue, skipping trajectory planning...
virtual void set_target(float x, float y, float z, float e1, float e2, float e3, float f1=0.f, float f2=0.f, float f3=0.f)
Set new target, given cartesian coordinates.
virtual void stop()
Stops the current movement.
virtual void pos_ready()
Moves the arm to the "READY" position.
virtual void move_gripper(float f1, float f2, float f3)
Moves only the gripper.
virtual void set_plannerparams(const std::string &params)
Set planner parameters.
virtual bool add_target(float x, float y, float z, float e1, float e2, float e3, bool plan=true)
Solve IK and add target to the queue.
virtual bool add_target_ang(float j1, float j2, float j3, float j4, float j5, float j6, bool plan=true)
Add target joint values to the queue.
virtual void update_openrave()
Update the openrave environment to represent the current situation.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual void close(Interface *interface)=0
Close interface.
Thread aspect to use blocked timing.
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
Base class for exceptions in Fawkes.
Definition: exception.h:36
virtual const char * what_no_backtrace() const noexcept
Get primary string (does not implicitly print the back trace).
Definition: exception.cpp:663
bool msgq_first_is()
Check if first message has desired type.
Definition: interface.h:351
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1215
Message * msgq_first()
Get the first message from the message queue.
Definition: interface.cpp:1200
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:501
const char * id() const
Get identifier of interface.
Definition: interface.cpp:661
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1062
Class for simulating a dummy Kinova Jaco Arm.
Definition: arm_dummy.h:34
Class for commanding a Kinova Jaco Arm, using libkindrv.
Definition: arm_kindrv.h:40
virtual void release_joystick()=0
Simulate releasing the joystick of the Kinova Jaco arm.
virtual bool initialized()=0
Check if arm is initialized.
std::string get_name() const
Get the name of the arm.
Definition: arm.h:126
virtual void initialize()=0
Initialize the arm.
virtual void push_joystick(unsigned int button)=0
Simulate a push of a button on the joystick of the Kinova Jaco arm.
AngularGotoMessage Fawkes BlackBoard Interface Message.
CalibrateMessage Fawkes BlackBoard Interface Message.
Definition: JacoInterface.h:76
CartesianGotoMessage Fawkes BlackBoard Interface Message.
JoystickPushMessage Fawkes BlackBoard Interface Message.
uint32_t button() const
Get button value.
JoystickReleaseMessage Fawkes BlackBoard Interface Message.
MoveGripperMessage Fawkes BlackBoard Interface Message.
float finger1() const
Get finger1 value.
float finger3() const
Get finger3 value.
float finger2() const
Get finger2 value.
RetractMessage Fawkes BlackBoard Interface Message.
Definition: JacoInterface.h:96
SetPlannerParamsMessage Fawkes BlackBoard Interface Message.
StopMessage Fawkes BlackBoard Interface Message.
JacoInterface Fawkes BlackBoard Interface.
Definition: JacoInterface.h:34
void set_initialized(const bool new_initialized)
Set initialized value.
void set_error_code(const uint32_t new_error_code)
Set error_code value.
void set_final(const bool new_final)
Set final value.
void set_msgid(const uint32_t new_msgid)
Set msgid value.
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.
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
Base class for all messages passed through interfaces in Fawkes BlackBoard.
Definition: message.h:44
unsigned int id() const
Get message ID.
Definition: message.cpp:181
Mutex mutual exclusion lock.
Definition: mutex.h:33
RefPtr<> is a reference-counting shared smartpointer.
Definition: refptr.h:50
Thread class encapsulation of pthreads.
Definition: thread.h:46
const char * name() const
Get name of thread.
Definition: thread.h:100
bool started() const
Check if thread has been started.
Definition: thread.cpp:810
Fawkes library namespace.
@ CONFIG_SINGLE
we only have one arm.
Definition: types.h:52
@ CONFIG_LEFT
this arm is the left one out of two.
Definition: types.h:53
@ CONFIG_RIGHT
this arm is the right one out of two.
Definition: types.h:54
std::list< RefPtr< jaco_target_t > > jaco_target_queue_t
FIFO target queue, holding RefPtr to targets.
Definition: types.h:89
Jaco struct containing all components required for one arm.
Definition: types.h:93
jaco_arm_config_t config
configuration for this arm
Definition: types.h:94
float trajec_color[4]
the color used for plotting the trajectory.
Definition: types.h:108
JacoGotoThread * goto_thread
the GotoThread of this arm.
Definition: types.h:98
fawkes::JacoArm * arm
pointer to actual JacoArm instance, controlling this arm
Definition: types.h:95
JacoOpenraveThread * openrave_thread
the OpenraveThread of this arm.
Definition: types.h:99
RefPtr< Mutex > trajec_mutex
mutex, used for modifying trajectory of a target.
Definition: types.h:103
RefPtr< Mutex > target_mutex
mutex, used for accessing the target_queue
Definition: types.h:101
RefPtr< jaco_target_queue_t > target_queue
queue of targets, which is processed FIFO.
Definition: types.h:105
JacoInterface * iface
pointer to JacoInterface, assigned to this arm
Definition: types.h:96