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 }
fawkes::JacoInterface::JoystickPushMessage
Definition: JacoInterface.h:295
fawkes::JacoInterface::AngularGotoMessage
Definition: JacoInterface.h:192
fawkes::jaco_arm_struct::goto_thread
JacoGotoThread * goto_thread
the GotoThread of this arm.
Definition: types.h:98
fawkes::Interface::msgq_first_is
bool msgq_first_is()
Check if first message has desired type.
Definition: interface.h:314
JacoGotoThread::final
virtual bool final()
Check if arm is final.
Definition: goto_thread.cpp:87
fawkes::Interface::msgq_pop
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1184
fawkes::Interface::msgq_empty
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1031
JacoGotoThread::move_gripper
virtual void move_gripper(float f1, float f2, float f3)
Moves only the gripper.
Definition: goto_thread.cpp:243
JacoGotoThread::set_target_ang
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...
Definition: goto_thread.cpp:177
fawkes::JacoArm::push_joystick
virtual void push_joystick(unsigned int button)=0
Simulate a push of a button on the joystick of the Kinova Jaco arm.
fawkes::JacoInterface::StopMessage
Definition: JacoInterface.h:127
fawkes::Mutex
Definition: mutex.h:38
JacoGotoThread::stop
virtual void stop()
Stops the current movement.
Definition: goto_thread.cpp:261
fawkes::Message
Definition: message.h:41
fawkes::RefPtr
RefPtr<> is a reference-counting shared smartpointer.
Definition: refptr.h:57
fawkes::JacoArm::initialized
virtual bool initialized()=0
Check if arm is initialized.
fawkes::JacoArm::release_joystick
virtual void release_joystick()=0
Simulate releasing the joystick of the Kinova Jaco arm.
fawkes::JacoInterface::CartesianGotoMessage::e1
float e1() const
Get e1 value.
Definition: JacoInterface.cpp:956
fawkes::Configuration::get_bool
virtual bool get_bool(const char *path)=0
JacoOpenraveThread::add_target
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.
Definition: openrave_thread.cpp:375
fawkes::JacoInterface::AngularGotoMessage::j4
float j4() const
Get j4 value.
Definition: JacoInterface.cpp:1216
JacoOpenraveThread::add_target_ang
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.
Definition: openrave_thread.cpp:467
fawkes::JacoArm::get_name
std::string get_name() const
Get the name of the arm.
Definition: arm.h:132
fawkes::CONFIG_SINGLE
@ CONFIG_SINGLE
we only have one arm.
Definition: types.h:52
JacoActThread::init
virtual void init()
Initialize.
Definition: act_thread.cpp:69
fawkes::JacoArm::initialize
virtual void initialize()=0
Initialize the arm.
fawkes::JacoInterface::RetractMessage
Definition: JacoInterface.h:107
fawkes::BlockedTimingAspect
Definition: blocked_timing.h:56
fawkes::JacoInterface::CartesianGotoMessage::z
float z() const
Get z value.
Definition: JacoInterface.cpp:926
fawkes::JacoInterface::AngularGotoMessage::j1
float j1() const
Get j1 value.
Definition: JacoInterface.cpp:1126
fawkes::Thread::name
const char * name() const
Definition: thread.h:100
fawkes::JacoInterface::AngularGotoMessage::j5
float j5() const
Get j5 value.
Definition: JacoInterface.cpp:1246
JacoGotoThread::pos_ready
virtual void pos_ready()
Moves the arm to the "READY" position.
Definition: goto_thread.cpp:214
fawkes::Interface::id
const char * id() const
Get identifier of interface.
Definition: interface.cpp:654
fawkes::jaco_arm_struct::trajec_color
float trajec_color[4]
the color used for plotting the trajectory.
Definition: types.h:108
fawkes::CONFIG_RIGHT
@ CONFIG_RIGHT
this arm is the right one out of two.
Definition: types.h:54
fawkes::JacoArmDummy
Definition: arm_dummy.h:39
fawkes::jaco_arm_struct::iface
JacoInterface * iface
pointer to JacoInterface, assigned to this arm
Definition: types.h:96
fawkes::JacoInterface::AngularGotoMessage::j3
float j3() const
Get j3 value.
Definition: JacoInterface.cpp:1186
JacoActThread::loop
virtual void loop()
Main loop.
Definition: act_thread.cpp:166
fawkes::jaco_arm_struct::openrave_thread
JacoOpenraveThread * openrave_thread
the OpenraveThread of this arm.
Definition: types.h:99
fawkes::JacoInterface::MoveGripperMessage::finger1
float finger1() const
Get finger1 value.
Definition: JacoInterface.cpp:1374
fawkes::LoggingAspect::logger
Logger * logger
Definition: logging.h:53
JacoGotoThread::set_target
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.
Definition: goto_thread.cpp:130
fawkes::BlackBoard::close
virtual void close(Interface *interface)=0
fawkes::Logger::log_error
virtual void log_error(const char *component, const char *format,...)=0
fawkes::JacoInterface::MoveGripperMessage
Definition: JacoInterface.h:237
fawkes::CONFIG_LEFT
@ CONFIG_LEFT
this arm is the left one out of two.
Definition: types.h:53
JacoActThread::finalize
virtual void finalize()
Finalize.
Definition: act_thread.cpp:146
fawkes::JacoInterface::CartesianGotoMessage::e2
float e2() const
Get e2 value.
Definition: JacoInterface.cpp:986
fawkes
fawkes::jaco_arm_struct::arm
fawkes::JacoArm * arm
pointer to actual JacoArm instance, controlling this arm
Definition: types.h:95
fawkes::JacoInterface::CartesianGotoMessage
Definition: JacoInterface.h:147
fawkes::JacoInterface::CartesianGotoMessage::y
float y() const
Get y value.
Definition: JacoInterface.cpp:896
fawkes::jaco_arm_struct::target_mutex
RefPtr< Mutex > target_mutex
mutex, used for accessing the target_queue
Definition: types.h:101
JacoActThread::~JacoActThread
virtual ~JacoActThread()
Destructor.
Definition: act_thread.cpp:59
fawkes::Logger::log_warn
virtual void log_warn(const char *component, const char *format,...)=0
fawkes::JacoInterface::JoystickPushMessage::button
uint32_t button() const
Get button value.
Definition: JacoInterface.cpp:1616
JacoOpenraveThread::update_openrave
virtual void update_openrave()
Update the openrave environment to represent the current situation.
Definition: openrave_thread.cpp:290
fawkes::JacoInterface::MoveGripperMessage::finger3
float finger3() const
Get finger3 value.
Definition: JacoInterface.cpp:1434
fawkes::ConfigurableAspect::config
Configuration * config
Definition: configurable.h:53
fawkes::JacoInterface::AngularGotoMessage::j6
float j6() const
Get j6 value.
Definition: JacoInterface.cpp:1276
fawkes::jaco_arm_struct::target_queue
RefPtr< jaco_target_queue_t > target_queue
queue of targets, which is processed FIFO.
Definition: types.h:105
JacoActThread::JacoActThread
JacoActThread(const char *name, fawkes::jaco_arm_t *arm)
Constructor.
Definition: act_thread.cpp:46
JacoGotoThread::pos_retract
virtual void pos_retract()
Moves the arm to the "RETRACT" position.
Definition: goto_thread.cpp:228
fawkes::JacoArmKindrv
Definition: arm_kindrv.h:39
fawkes::Exception::what_no_backtrace
virtual const char * what_no_backtrace() const
Get primary string (does not implicitly print the back trace).
Definition: exception.cpp:663
fawkes::Thread::started
bool started() const
Check if thread has been started.
Definition: thread.cpp:816
fawkes::JacoInterface::set_error_code
void set_error_code(const uint32_t new_error_code)
Set error_code value.
Definition: JacoInterface.cpp:597
fawkes::Thread
Definition: thread.h:45
fawkes::BlackBoardAspect::blackboard
BlackBoard * blackboard
Definition: blackboard.h:49
fawkes::jaco_arm_struct
Jaco struct containing all components required for one arm.
Definition: types.h:92
JacoOpenraveBaseThread::set_plannerparams
virtual void set_plannerparams(const std::string &params)
Set planner parameters.
Definition: openrave_base_thread.cpp:130
fawkes::Configuration::get_string
virtual std::string get_string(const char *path)=0
fawkes::Message::id
unsigned int id() const
Get message ID.
Definition: message.cpp:186
fawkes::JacoInterface::SetPlannerParamsMessage::params
char * params() const
Get params value.
Definition: JacoInterface.cpp:1525
fawkes::JacoInterface::MoveGripperMessage::finger2
float finger2() const
Get finger2 value.
Definition: JacoInterface.cpp:1404
fawkes::JacoInterface::AngularGotoMessage::j2
float j2() const
Get j2 value.
Definition: JacoInterface.cpp:1156
fawkes::Interface::msgq_first
Message * msgq_first()
Get the first message from the message queue.
Definition: interface.cpp:1169
fawkes::jaco_arm_struct::config
jaco_arm_config_t config
configuration for this arm
Definition: types.h:94
fawkes::JacoInterface::CartesianGotoMessage::e3
float e3() const
Get e3 value.
Definition: JacoInterface.cpp:1016
fawkes::JacoInterface::CalibrateMessage
Definition: JacoInterface.h:87
fawkes::JacoInterface::set_initialized
void set_initialized(const bool new_initialized)
Set initialized value.
Definition: JacoInterface.cpp:157
fawkes::Logger::log_debug
virtual void log_debug(const char *component, const char *format,...)=0
fawkes::Interface::write
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:499
fawkes::JacoInterface
Definition: JacoInterface.h:39
fawkes::jaco_target_queue_t
std::list< RefPtr< jaco_target_t > > jaco_target_queue_t
FIFO target queue, holding RefPtr to targets.
Definition: types.h:89
fawkes::JacoInterface::set_final
void set_final(const bool new_final)
Set final value.
Definition: JacoInterface.cpp:562
fawkes::jaco_arm_struct::trajec_mutex
RefPtr< Mutex > trajec_mutex
mutex, used for modifying trajectory of a target.
Definition: types.h:103
fawkes::JacoInterface::SetPlannerParamsMessage
Definition: JacoInterface.h:270
fawkes::BlackBoard::open_for_writing
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
fawkes::JacoInterface::set_msgid
void set_msgid(const uint32_t new_msgid)
Set msgid value.
Definition: JacoInterface.cpp:529
fawkes::JacoInterface::JoystickReleaseMessage
Definition: JacoInterface.h:320
fawkes::JacoInterface::CartesianGotoMessage::x
float x() const
Get x value.
Definition: JacoInterface.cpp:866
fawkes::Exception
Definition: exception.h:41