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 #include "goto_thread.h"
25 #include "openrave_thread.h"
26 
27 #include "types.h"
28 #include "arm_kindrv.h"
29 #include "arm_dummy.h"
30 
31 #include <interfaces/JacoInterface.h>
32 #include <core/utils/refptr.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")
77  && cfg_arm.compare("dummy") )
78  throw fawkes::Exception("Bad config entry /hardware/jaco/arm '%s'", cfg_arm.c_str());
79 
80  std::string arm_name, arm_iface;
81  switch( __arm->config ) {
82  case CONFIG_SINGLE:
83  arm_name = config->get_string("/hardware/jaco/config/single/name");
84  arm_iface = config->get_string("/hardware/jaco/config/single/interface");
85  break;
86 
87  case CONFIG_LEFT:
88  arm_name = config->get_string("/hardware/jaco/config/left/name");
89  arm_iface = config->get_string("/hardware/jaco/config/left/interface");
90  break;
91 
92  case CONFIG_RIGHT:
93  arm_name = config->get_string("/hardware/jaco/config/right/name");
94  arm_iface = config->get_string("/hardware/jaco/config/right/interface");
95  break;
96 
97  default:
98  throw fawkes::Exception("Unknown arm config given");
99  break;
100  }
101 
102  // create the JacoArm object and assign correctly to config
103  try {
104  if( !cfg_arm.compare("dummy") ) {
105  __arm->arm = new JacoArmDummy("JacoDummy");
106  } else {
107  __arm->arm = new JacoArmKindrv(arm_name.c_str());
108  }
109  } catch(fawkes::Exception &e) {
110  logger->log_error(name(), "Could not connect to JacoArm. Exception follows.");
111  throw;
112  }
113 
114  // open interface for writing
115  try {
116  __arm->iface = blackboard->open_for_writing<JacoInterface>(arm_iface.c_str());
117  } catch(fawkes::Exception &e) {
118  logger->log_error(name(), "Could not open interface %s for writing. Exception follows.", arm_iface.c_str());
119  delete __arm->arm;
120  __arm = NULL;
121  throw;
122  }
123 
124  // create target/trajectory queues and mutexes
125  __arm->target_mutex = RefPtr<Mutex>(new Mutex());
126  __arm->trajec_mutex = RefPtr<Mutex>(new Mutex());
128 
129  // set trajectory colors (TODO: configurable)
130  __arm->trajec_color[0] = 0.f;
131  __arm->trajec_color[1] = 0.f;
132  __arm->trajec_color[2] = 1.f;
133  __arm->trajec_color[3] = 1.f;
134  if( __arm->config==CONFIG_RIGHT ) {
135  __arm->trajec_color[0] = 1.f;
136  __arm->trajec_color[2] = 0.f;
137  }
138 
139  // initalize arm
140  _initialize();
141 }
142 
143 /** Finalize.
144  * Close all writing interfaces and delete JacoArm instances.
145  */
146 void
148 {
149  try {
150  blackboard->close(__arm->iface);
151  } catch(fawkes::Exception& e) {
152  logger->log_warn(name(), "Could not close JacoInterface interface. Er:%s", e.what_no_backtrace());
153  }
154 
155  delete __arm->arm;
156 }
157 
158 /** Main loop.
159  * The structure is pretty obvious. We first submit changes made to the interface
160  * from threads before the ACT-hook (they might be used by other threads lateron).
161  * Then we make sure the arm is initialized, before processing incoming messages
162  * and submiting interface changes once again.
163  */
164 void
166 {
167  if( __arm==NULL || __arm->iface==NULL || __arm->openrave_thread==NULL)
168  return;
169 
170  // firts of all, submit interface updates (that other threads might have done)!
171  __arm->iface->write();
172 
173  // check if still initializing
174  if( _is_initializing() )
175  return;
176 
177 #ifdef HAVE_OPENRAVE
178  // make sure openrave-thread is ready!
179  if( !__arm->openrave_thread->started() )
180  return;
181 #endif
182 
183  // process incoming interface messages
184  _process_msgs();
185 
186  // finally, again submit interface updates
187  __arm->iface->write();
188 
189 #ifdef HAVE_OPENRAVE
191 #endif
192  __arm->iface->set_final(__arm->goto_thread->final());
193 }
194 
195 
196 
197 /* ##########################################################################
198  * private methods , referenced to by the function pointers from loop().
199  *
200  * we have one for each single_arm and dual_arm setup.
201  * ########################################################################## */
202 /** Initialize and/or calibrate single arm, if requested by config flags */
203 void
204 JacoActThread::_initialize()
205 {
206  //check if we need to initialize arm
207  if( !__arm->arm->initialized() && __cfg_auto_init ) {
208  logger->log_debug(name(), "Initializing arm, wait until finished");
209  __arm->arm->initialize();
210  __arm->iface->set_final(false);
211  //__arm.goto_thread->pos_ready();
212 
213  } else if( __arm->arm->initialized() && __cfg_auto_calib ) {
214  __arm->goto_thread->pos_ready();
215  }
216 
217  __arm->iface->set_initialized(__arm->arm->initialized());
218  __arm->iface->write();
219 }
220 
221 /** Check if arm is being initialized. */
222 bool
223 JacoActThread::_is_initializing()
224 {
225  __arm->iface->set_initialized(__arm->arm->initialized());
226 
227  if( !__arm->arm->initialized() && __cfg_auto_init ) {
228  logger->log_debug(name(), "wait for arm to initialize");
229  //__arm->initialized = __arm->iface->is_final();
230  return true;
231  }
232 
233  return false;
234 }
235 
236 /** Process interface messages. */
237 void
238 JacoActThread::_process_msgs()
239 {
240  while( ! __arm->iface->msgq_empty() ) {
241  Message *m = __arm->iface->msgq_first(m);
242  __arm->iface->set_msgid(m->id());
243  __arm->iface->set_final(false);
244  __arm->iface->set_error_code(JacoInterface::ERROR_NONE);
245  __arm->iface->write();
246 
248  JacoInterface::StopMessage *msg = __arm->iface->msgq_first(msg);
249  logger->log_debug(name(), "%s: StopMessage rcvd", __arm->iface->id());
250 
251  __arm->goto_thread->stop();
252 
253  } else if( __arm->iface->msgq_first_is<JacoInterface::CalibrateMessage>() ) {
255  logger->log_debug(name(), "%s: CalibrateMessage rcvd", __arm->iface->id());
256 
257  // Stop all (current and planned) motion. Then calibrate
258  __arm->goto_thread->stop();
259  __arm->goto_thread->pos_ready();
260 
261  } else if( __arm->iface->msgq_first_is<JacoInterface::RetractMessage>() ) {
262  JacoInterface::RetractMessage *msg = __arm->iface->msgq_first(msg);
263  logger->log_debug(name(), "%s: RetractMessage rcvd", __arm->iface->id());
264 
265  // Stop all (current and planned) motion. Then retract
266  __arm->goto_thread->stop();
267  __arm->goto_thread->pos_retract();
268 
271  logger->log_debug(name(), "%s: SetPlannerParamsMessage rcvd. params:%s", __arm->iface->id(), msg->params());
272 
273  #ifdef HAVE_OPENRAVE
274  __arm->openrave_thread->set_plannerparams(msg->params());
275  #endif
276 
279  logger->log_debug(name(), "%s: CartesianGotoMessage rcvd. x:%f y:%f z:%f e1:%f e2:%f e3:%f", __arm->iface->id(),
280  msg->x(), msg->y(), msg->z(), msg->e1(), msg->e2(), msg->e3());
281  #ifdef HAVE_OPENRAVE
282  logger->log_debug(name(), "%s: CartesianGotoMessage is being passed to openrave", __arm->iface->id());
283  // add target to OpenRAVE queue for planning
284  bool solvable = __arm->openrave_thread->add_target(msg->x(), msg->y(), msg->z(), msg->e1(), msg->e2(), msg->e3());
285  if( !solvable ) {
286  __arm->iface->set_error_code(JacoInterface::ERROR_NO_IK);
287  logger->log_warn(name(), "Failed executing CartesianGotoMessage, arm %s and/or thread %s could not find IK solution",
288  __arm->arm->get_name().c_str(), __arm->openrave_thread->name());
289  }
290  #else
291  __arm->goto_thread->set_target(msg->x(), msg->y(), msg->z(), msg->e1(), msg->e2(), msg->e3());
292  #endif
293 
294  } else if( __arm->iface->msgq_first_is<JacoInterface::AngularGotoMessage>() ) {
296 
297  logger->log_debug(name(), "%s: AngularGotoMessage rcvd. x:%f y:%f z:%f e1:%f e2:%f e3:%f", __arm->iface->id(),
298  msg->j1(), msg->j2(), msg->j3(), msg->j4(), msg->j5(), msg->j6());
299  #ifdef HAVE_OPENRAVE
300  logger->log_debug(name(), "%s: AngularGotoMessage is being passed to openrave", __arm->iface->id());
301  // add target to OpenRAVE queue for planning
302  bool joints_valid = __arm->openrave_thread->add_target_ang(msg->j1(), msg->j2(), msg->j3(), msg->j4(), msg->j5(), msg->j6());
303  if( !joints_valid ) {
304  __arm->iface->set_error_code(JacoInterface::ERROR_NO_IK);
305  logger->log_warn(name(), "Failed executing AngularGotoMessage, given target joints for arm %s are invalid or in self-collision",
306  __arm->arm->get_name().c_str());
307  }
308  #else
309  __arm->goto_thread->set_target_ang(msg->j1(), msg->j2(), msg->j3(), msg->j4(), msg->j5(), msg->j6());
310  #endif
311 
312  } else if( __arm->iface->msgq_first_is<JacoInterface::MoveGripperMessage>() ) {
314  logger->log_debug(name(), "%s: MoveGripperMessage rcvd. f1:%f f2:%f f3:%f", __arm->iface->id(),
315  msg->finger1(), msg->finger2(), msg->finger3());
316 
317  __arm->goto_thread->move_gripper(msg->finger1(), msg->finger2(), msg->finger3());
318 
319  } else if( __arm->iface->msgq_first_is<JacoInterface::JoystickPushMessage>() ) {
321  logger->log_debug(name(), "%s: JoystickPush %u rcvd", __arm->iface->id(), msg->button());
322 
323  __arm->arm->push_joystick(msg->button());
324 
327  logger->log_debug(name(), "%s: JoystickRelease rcvd", __arm->iface->id());
328 
329  __arm->arm->release_joystick();
330  __arm->iface->set_final(true);
331 
332  } else {
333  logger->log_warn(name(), "%s: Unknown message received. Skipping", __arm->iface->id());
334  }
335 
336  __arm->iface->msgq_pop();
337  }
338 }
JacoGotoThread * goto_thread
the GotoThread of this arm.
Definition: types.h:100
Jaco struct containing all components required for one arm.
Definition: types.h:95
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:197
virtual void finalize()
Finalize.
Definition: act_thread.cpp:147
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1048
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:97
const char * id() const
Get identifier of interface.
Definition: interface.cpp:661
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:165
Class for simulating a dummy Kinova Jaco Arm.
Definition: arm_dummy.h:37
Thread class encapsulation of pthreads.
Definition: thread.h:42
MoveGripperMessage Fawkes BlackBoard Interface Message.
std::list< RefPtr< jaco_target_t > > jaco_target_queue_t
FIFO target queue, holding RefPtr to targets.
Definition: types.h:92
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.
RefPtr< Mutex > trajec_mutex
mutex, used for modifying trajectory of a target.
Definition: types.h:104
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:500
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:44
float finger3() const
Get finger3 value.
this arm is the right one out of two.
Definition: types.h:57
JacoInterface * iface
pointer to JacoInterface, assigned to this arm
Definition: types.h:98
JacoOpenraveThread * openrave_thread
the OpenraveThread of this arm.
Definition: types.h:101
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:1193
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:36
Message * msgq_first()
Get the first message from the message queue.
Definition: interface.cpp:1180
RefPtr< Mutex > target_mutex
mutex, used for accessing the target_queue
Definition: types.h:103
bool started() const
Check if thread has been started.
Definition: thread.cpp:824
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:95
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:96
Class for commanding a Kinova Jaco Arm, using libkindrv.
Definition: arm_kindrv.h:40
virtual const char * what_no_backtrace() const
Get primary string (does not implicitly print the back trace).
Definition: exception.cpp:686
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.
RefPtr< jaco_target_queue_t > target_queue
queue of targets, which is processed FIFO.
Definition: types.h:106
uint32_t button() const
Get button value.
this arm is the left one out of two.
Definition: types.h:56
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.
char * params() const
Get params value.
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
CartesianGotoMessage Fawkes BlackBoard Interface Message.
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:44
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:55
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:133
virtual void close(Interface *interface)=0
Close interface.