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 
34 using 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. */
68 void
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) {
115  logger->log_error(name(),
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  */
145 void
147 {
148  try {
149  blackboard->close(arm_->iface);
150  } catch (fawkes::Exception &e) {
151  logger->log_warn(name(),
152  "Could not close JacoInterface interface. Er:%s",
153  e.what_no_backtrace());
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  */
165 void
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 */
202 void
203 JacoActThread::_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. */
221 bool
222 JacoActThread::_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. */
236 void
237 JacoActThread::_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 
247  JacoInterface::StopMessage *msg = arm_->iface->msgq_first(msg);
248  logger->log_debug(name(), "%s: StopMessage rcvd", arm_->iface->id());
249 
250  arm_->goto_thread->stop();
251 
252  } else if (arm_->iface->msgq_first_is<JacoInterface::CalibrateMessage>()) {
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 
260  } else if (arm_->iface->msgq_first_is<JacoInterface::RetractMessage>()) {
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 
270  logger->log_debug(name(),
271  "%s: SetPlannerParamsMessage rcvd. params:%s",
272  arm_->iface->id(),
273  msg->params());
274 
275 #ifdef HAVE_OPENRAVE
277 #endif
278 
281  logger->log_debug(name(),
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
291  logger->log_debug(name(),
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);
299  logger->log_warn(name(),
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 
312  logger->log_debug(name(),
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
322  logger->log_debug(name(),
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);
330  logger->log_warn(name(),
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 
342  logger->log_debug(name(),
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
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:314
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1182
Message * msgq_first()
Get the first message from the message queue.
Definition: interface.cpp:1167
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:494
const char * id() const
Get identifier of interface.
Definition: interface.cpp:652
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1029
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:45
unsigned int id() const
Get message ID.
Definition: message.cpp:180
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