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 }
JacoGotoThread * goto_thread
the GotoThread of this arm.
Definition: types.h:98
Jaco struct containing all components required for one arm.
Definition: types.h:92
Base class for all messages passed through interfaces in Fawkes BlackBoard.
Definition: message.h:41
unsigned int id() const
Get message ID.
Definition: message.cpp:190
std::list< RefPtr< jaco_target_t > > jaco_target_queue_t
FIFO target queue, holding RefPtr to targets.
Definition: types.h:89
virtual void finalize()
Finalize.
Definition: act_thread.cpp:146
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1026
float finger2() const
Get finger2 value.
JacoActThread(const char *name, fawkes::jaco_arm_t *arm)
Constructor.
Definition: act_thread.cpp:46
virtual void update_openrave()
Update the openrave environment to represent the current situation.
Fawkes library namespace.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
SetPlannerParamsMessage Fawkes BlackBoard Interface Message.
fawkes::JacoArm * arm
pointer to actual JacoArm instance, controlling this arm
Definition: types.h:95
const char * id() const
Get identifier of interface.
Definition: interface.cpp:649
RefPtr< Mutex > target_mutex
mutex, used for accessing the target_queue
Definition: types.h:101
float trajec_color[4]
the color used for plotting the trajectory.
Definition: types.h:108
virtual void pos_retract()
Moves the arm to the "RETRACT" position.
virtual void loop()
Main loop.
Definition: act_thread.cpp:166
Class for simulating a dummy Kinova Jaco Arm.
Definition: arm_dummy.h:33
Thread class encapsulation of pthreads.
Definition: thread.h:45
MoveGripperMessage Fawkes BlackBoard Interface Message.
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.
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:494
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
float finger3() const
Get finger3 value.
this arm is the right one out of two.
Definition: types.h:54
JacoInterface * iface
pointer to JacoInterface, assigned to this arm
Definition: types.h:96
JacoOpenraveThread * openrave_thread
the OpenraveThread of this arm.
Definition: types.h:99
Thread aspect to use blocked timing.
virtual void pos_ready()
Moves the arm to the "READY" position.
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1179
virtual void initialize()=0
Initialize the arm.
virtual void release_joystick()=0
Simulate releasing the joystick of the Kinova Jaco arm.
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 stop()
Stops the current movement.
Base class for exceptions in Fawkes.
Definition: exception.h:35
Message * msgq_first()
Get the first message from the message queue.
Definition: interface.cpp:1164
RefPtr< jaco_target_queue_t > target_queue
queue of targets, which is processed FIFO.
Definition: types.h:105
bool started() const
Check if thread has been started.
Definition: thread.cpp:810
virtual bool final()
Check if arm is final.
Definition: goto_thread.cpp:87
CalibrateMessage Fawkes BlackBoard Interface Message.
Definition: JacoInterface.h:75
StopMessage Fawkes BlackBoard Interface Message.
const char * name() const
Get name of thread.
Definition: thread.h:100
void set_error_code(const uint32_t new_error_code)
Set error_code value.
virtual void move_gripper(float f1, float f2, float f3)
Moves only the gripper.
jaco_arm_config_t config
configuration for this arm
Definition: types.h:94
Class for commanding a Kinova Jaco Arm, using libkindrv.
Definition: arm_kindrv.h:39
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
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
void set_initialized(const bool new_initialized)
Set initialized value.
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
uint32_t button() const
Get button value.
this arm is the left one out of two.
Definition: types.h:53
RefPtr<> is a reference-counting shared smartpointer.
Definition: refptr.h:49
void set_final(const bool new_final)
Set final value.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
JoystickPushMessage Fawkes BlackBoard Interface Message.
void set_msgid(const uint32_t new_msgid)
Set msgid value.
JacoInterface Fawkes BlackBoard Interface.
Definition: JacoInterface.h:33
virtual void set_plannerparams(const std::string &params)
Set planner parameters.
AngularGotoMessage Fawkes BlackBoard Interface Message.
virtual ~JacoActThread()
Destructor.
Definition: act_thread.cpp:59
virtual void push_joystick(unsigned int button)=0
Simulate a push of a button on the joystick of the Kinova Jaco arm.
virtual void init()
Initialize.
Definition: act_thread.cpp:69
JoystickReleaseMessage Fawkes BlackBoard Interface Message.
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.
Mutex mutual exclusion lock.
Definition: mutex.h:32
RefPtr< Mutex > trajec_mutex
mutex, used for modifying trajectory of a target.
Definition: types.h:103
CartesianGotoMessage Fawkes BlackBoard Interface Message.
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
we only have one arm.
Definition: types.h:52
RetractMessage Fawkes BlackBoard Interface Message.
Definition: JacoInterface.h:95
virtual bool initialized()=0
Check if arm is initialized.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
float finger1() const
Get finger1 value.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
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...
std::string get_name() const
Get the name of the arm.
Definition: arm.h:126
virtual void close(Interface *interface)=0
Close interface.