Fawkes API  Fawkes Development Version
bimanual_goto_thread.h
1 
2 /***************************************************************************
3  * bimaual_goto_thread.h - Jaco plugin movement thread for coordinated bimanual manipulation
4  *
5  * Created: Mon Sep 29 23:17:12 2014
6  * Copyright 2014 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 #ifndef __PLUGINS_JACO_BIMANUAL_GOTO_THREAD_H_
24 #define __PLUGINS_JACO_BIMANUAL_GOTO_THREAD_H_
25 
26 #include "types.h"
27 
28 #include <core/threading/thread.h>
29 #include <aspect/logging.h>
30 #include <aspect/configurable.h>
31 #include <aspect/blackboard.h>
32 
33 namespace fawkes {
34  class Mutex;
35 }
37 : public fawkes::Thread,
38  public fawkes::LoggingAspect,
41 {
42  public:
44  virtual ~JacoBimanualGotoThread();
45 
46  virtual void init();
47  virtual void finalize();
48  virtual void loop();
49 
50  virtual bool final();
51 
52  virtual void stop();
53  virtual void move_gripper(float l_f1, float l_f2, float l_f3, float r_f1, float r_f2, float r_f3);
54 
55  /** Stub to see name in backtrace for easier debugging. @see Thread::run() */
56  protected: virtual void run() { Thread::run(); }
57 
58  private:
59  void _lock_queues() const;
60  void _unlock_queues() const;
61  void _enqueue_targets(fawkes::RefPtr<fawkes::jaco_target_t> l,
63 
64  void _move_grippers();
65  void _exec_trajecs();
66 
67  void _check_final();
68 
69  typedef struct arm_struct {
70  fawkes::jaco_arm_t *arm;
72  float finger_last[4]; // 3 positions + 1 counter
73  } arm_struct_t;
74 
75  struct {
76  arm_struct_t l;
77  arm_struct_t r;
78  } __arms;
79 
80  arm_struct_t* __v_arms[2]; // just a helper, to be able to iterate over both arms
81 
82  fawkes::jaco_dual_arm_t *__dual_arms; // have redundancy now, but keep this just to be sure
83 
84  fawkes::Mutex* __final_mutex;
85  bool __final;
86 };
87 
88 
89 #endif
Jaco struct containing all components required for one arm.
Definition: types.h:95
Thread aspect to access to BlackBoard.
Definition: blackboard.h:34
Jaco Arm movement thread.
Fawkes library namespace.
virtual void run()
Code to execute in the thread.
Definition: thread.cpp:939
Thread class encapsulation of pthreads.
Definition: thread.h:42
virtual void run()
Stub to see name in backtrace for easier debugging.
Thread aspect to log output.
Definition: logging.h:35
Thread aspect to access configuration data.
Definition: configurable.h:35
RefPtr<> is a reference-counting shared smartpointer.
Definition: refptr.h:49
Jaco struct containing all components required for a dual-arm setup.
Definition: types.h:112
Mutex mutual exclusion lock.
Definition: mutex.h:32