Fawkes API  Fawkes Development Version
act_thread.cpp
1 
2 /***************************************************************************
3  * act_thread.cpp - Katana plugin act thread
4  *
5  * Created: Mon Jun 08 18:00:56 2009
6  * Copyright 2006-2009 Tim Niemueller [www.niemueller.de]
7  * 2010-2014 Bahram Maleki-Fard
8  *
9  ****************************************************************************/
10 
11 /* This program is free software; you can redistribute it and/or modify
12  * it under the terms of the GNU General Public License as published by
13  * the Free Software Foundation; either version 2 of the License, or
14  * (at your option) any later version.
15  *
16  * This program is distributed in the hope that it will be useful,
17  * but WITHOUT ANY WARRANTY; without even the implied warranty of
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19  * GNU Library General Public License for more details.
20  *
21  * Read the full text in the LICENSE.GPL file in the doc directory.
22  */
23 
24 #include "act_thread.h"
25 #include "motion_thread.h"
26 #include "calib_thread.h"
27 #include "goto_thread.h"
28 #include "goto_openrave_thread.h"
29 #include "gripper_thread.h"
30 #include "sensacq_thread.h"
31 #include "motor_control_thread.h"
32 #include "controller_kni.h"
33 #include "controller_openrave.h"
34 
35 #include <core/threading/mutex_locker.h>
36 #include <interfaces/KatanaInterface.h>
37 #include <interfaces/JointInterface.h>
38 #include <utils/math/angle.h>
39 #include <utils/time/time.h>
40 
41 #ifdef HAVE_OPENRAVE
42  #include <plugins/openrave/robot.h>
43  #include <plugins/openrave/manipulator.h>
44 #endif
45 
46 #include <algorithm>
47 #include <cstdarg>
48 
49 using namespace fawkes;
50 #ifdef HAVE_TF
51 using namespace fawkes::tf;
52 #endif
53 
54 /** @class KatanaActThread "act_thread.h"
55  * Katana act thread.
56  * This thread integrates into the Fawkes main loop at the ACT_EXEC hook and
57  * interacts with a given controller for the Katana.
58  * @author Tim Niemueller
59  */
60 
61 /** Constructor. */
63  : Thread("KatanaActThread", Thread::OPMODE_WAITFORWAKEUP),
64  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_ACT_EXEC),
65  TransformAspect(TransformAspect::BOTH, "Katana"),
66  BlackBoardInterfaceListener("KatanaActThread")
67 {
68  __last_update = new Time();
69 }
70 
71 /** Destructor. */
73 {
74  delete __last_update;
75 }
76 
77 
78 void
80 {
81  // Note: due to the use of auto_ptr and RefPtr resources are automatically
82  // freed on destruction, therefore no special handling is necessary in init()
83  // itself!
84 
85  std::string cfg_prefix = "/hardware/katana/";
86  __cfg_controller = config->get_string((cfg_prefix + "controller").c_str());
87  __cfg_device = config->get_string((cfg_prefix + "device").c_str());
88  __cfg_kni_conffile = config->get_string((cfg_prefix + "kni_conffile").c_str());
89  __cfg_auto_calibrate = config->get_bool((cfg_prefix + "auto_calibrate").c_str());
90  __cfg_defmax_speed = config->get_uint((cfg_prefix + "default_max_speed").c_str());
91  __cfg_read_timeout = config->get_uint((cfg_prefix + "read_timeout_msec").c_str());
92  __cfg_write_timeout = config->get_uint((cfg_prefix + "write_timeout_msec").c_str());
93  __cfg_gripper_pollint = config->get_uint((cfg_prefix + "gripper_pollint_msec").c_str());
94  __cfg_goto_pollint = config->get_uint((cfg_prefix + "goto_pollint_msec").c_str());
95 
96  __cfg_park_x = config->get_float((cfg_prefix + "park_x").c_str());
97  __cfg_park_y = config->get_float((cfg_prefix + "park_y").c_str());
98  __cfg_park_z = config->get_float((cfg_prefix + "park_z").c_str());
99  __cfg_park_phi = config->get_float((cfg_prefix + "park_phi").c_str());
100  __cfg_park_theta = config->get_float((cfg_prefix + "park_theta").c_str());
101  __cfg_park_psi = config->get_float((cfg_prefix + "park_psi").c_str());
102 
103  __cfg_distance_scale = config->get_float((cfg_prefix + "distance_scale").c_str());
104 
105  __cfg_update_interval = config->get_float((cfg_prefix + "update_interval").c_str());
106 
107  __cfg_frame_kni = config->get_string((cfg_prefix + "frame/kni").c_str());
108  __cfg_frame_gripper = config->get_string((cfg_prefix + "frame/gripper").c_str());
109  __cfg_frame_openrave = config->get_string((cfg_prefix + "frame/openrave").c_str());
110 
111 #ifdef HAVE_OPENRAVE
112  __cfg_OR_enabled = config->get_bool((cfg_prefix + "openrave/enabled").c_str());
113  __cfg_OR_use_viewer = config->get_bool((cfg_prefix + "openrave/use_viewer").c_str());
114  __cfg_OR_auto_load_ik = config->get_bool((cfg_prefix + "openrave/auto_load_ik").c_str());
115  __cfg_OR_robot_file = config->get_string((cfg_prefix + "openrave/robot_file").c_str());
116  __cfg_OR_arm_model = config->get_string((cfg_prefix + "openrave/arm_model").c_str());
117 #else
118  __cfg_OR_enabled = false;
119 #endif
120 
121  // see if config entries for joints exist
122  std::string joint_name;
123  for( long long i = 0; i < 5; ++i) {
124  joint_name = config->get_string((cfg_prefix + "joints/" + std::to_string(i)).c_str() );
125  joint_name.clear();
126  }
127  joint_name = config->get_string((cfg_prefix + "joints/finger_l").c_str());
128  joint_name.clear();
129  joint_name = config->get_string((cfg_prefix + "joints/finger_r").c_str());
130  joint_name.clear();
131 
132  __last_update->set_clock(clock);
133  __last_update->set_time(0, 0);
134 
135  // Load katana controller
136  if( __cfg_controller == "kni") {
137  KatanaControllerKni* kat_ctrl = new KatanaControllerKni();
138  __katana = kat_ctrl;
139  try {
140  kat_ctrl->setup(__cfg_device, __cfg_kni_conffile,
141  __cfg_read_timeout, __cfg_write_timeout);
142  } catch(fawkes::Exception &e) {
143  logger->log_warn(name(), "Setup KatanaControllerKni failed. Ex: %s", e.what());
144  }
145  kat_ctrl = NULL;
146 
147  } else if( __cfg_controller == "openrave") {
148 #ifdef HAVE_OPENRAVE
149  if(!__cfg_OR_enabled) {
150  throw fawkes::Exception("Cannot use controller 'openrave', OpenRAVE is deactivated by config flag!");
151  }
152  __katana = new KatanaControllerOpenrave(openrave);
153 #else
154  throw fawkes::Exception("Cannot use controller 'openrave', OpenRAVE not installed!");
155 #endif
156 
157  } else {
158  throw fawkes::Exception("Invalid controller given: '%s'", __cfg_controller.c_str());
159  }
160 
161  // If you have more than one interface: catch exception and close them!
162  try {
163  __katana_if = blackboard->open_for_writing<KatanaInterface>("Katana");
164  __joint_ifs = new std::vector<JointInterface*>();
165  JointInterface* joint_if = NULL;
166  for( long long i = 0; i < 5; ++i) {
167  joint_name = config->get_string((cfg_prefix + "joints/" + std::to_string(i)).c_str() );
168  joint_if = blackboard->open_for_writing<JointInterface>(joint_name.c_str());
169  __joint_ifs->push_back( joint_if );
170  joint_name.clear();
171  }
172 
173  joint_name = config->get_string((cfg_prefix + "joints/finger_l").c_str());
174  joint_if = blackboard->open_for_writing<JointInterface>(joint_name.c_str());
175  __joint_ifs->push_back( joint_if );
176  joint_name.clear();
177  joint_name = config->get_string((cfg_prefix + "joints/finger_r").c_str());
178  joint_if = blackboard->open_for_writing<JointInterface>(joint_name.c_str());
179  __joint_ifs->push_back( joint_if );
180  joint_name.clear();
181 
182  joint_if = NULL;
183  } catch(Exception &e) {
184  finalize();
185  throw;
186  }
187 
188  // Create all other threads
189  __sensacq_thread = new KatanaSensorAcquisitionThread(__katana, logger);
190  __calib_thread = new KatanaCalibrationThread(__katana, logger);
191  __gripper_thread = new KatanaGripperThread(__katana, logger, __cfg_gripper_pollint);
192  __motor_control_thread = new KatanaMotorControlThread(__katana, logger, __cfg_goto_pollint);
193  __goto_thread = new KatanaGotoThread(__katana, logger, __cfg_goto_pollint);
194 #ifdef HAVE_OPENRAVE
195  __goto_openrave_thread = new KatanaGotoOpenRaveThread(__katana, logger, openrave, __cfg_goto_pollint, __cfg_OR_robot_file,
196  __cfg_OR_arm_model, __cfg_OR_auto_load_ik, __cfg_OR_use_viewer);
197  if(__cfg_OR_enabled)
198  {__goto_openrave_thread->init();}
199 #endif
200 
201  // Intialize katana controller
202  try {
203  __katana->init();
204  __katana->set_max_velocity(__cfg_defmax_speed);
205  logger->log_debug(name(), "Katana successfully initialized");
206  } catch(fawkes::Exception &e) {
207  logger->log_warn(name(), "Initializing controller failed. Ex: %s", e.what());
208  finalize();
209  throw;
210  }
211 
212  __sensacq_thread->start();
213 
214  bbil_add_message_interface(__katana_if);
216 
217 #ifdef USE_TIMETRACKER
218  __tt.reset(new TimeTracker());
219  __tt_count = 0;
220  __ttc_read_sensor = __tt->add_class("Read Sensor");
221 #endif
222 
223 }
224 
225 
226 void
228 {
229  try {
230  if( __actmot_thread ) {
231  __actmot_thread->cancel();
232  __actmot_thread->join();
233  __actmot_thread = NULL;
234  }
235  } catch(fawkes::Exception &e) {
236  logger->log_warn(name(), "failed finalizing motion thread. Ex:%s", e.what());
237  }
238 
239  try {
240  __sensacq_thread->cancel();
241  __sensacq_thread->join();
242  __sensacq_thread.reset();
243  } catch(fawkes::Exception &e) {
244  logger->log_warn(name(), "failed finalizing sensor_aquisition thread. Ex:%s", e.what());
245  }
246 
247  // Setting to NULL also deletes instance (RefPtr)
248  __calib_thread = NULL;
249  __goto_thread = NULL;
250  __gripper_thread = NULL;
251  __motor_control_thread = NULL;
252 #ifdef HAVE_OPENRAVE
253  if( __cfg_OR_enabled && __goto_openrave_thread )
254  __goto_openrave_thread->finalize();
255  __goto_openrave_thread = NULL;
256 #endif
257 
258  try {
259  __katana->stop();
260  } catch(fawkes::Exception &e) {
261  logger->log_warn(name(), "failed stopping katana. Ex:%s", e.what());
262  }
263  __katana = NULL;
264 
265  try {
267  } catch(fawkes::Exception &e) {
268  logger->log_warn(name(), "failed unregistering blackboard listener. Ex:%s", e.what());
269  }
270 
271  if(__katana_if)
272  blackboard->close(__katana_if);
273  for (std::vector<JointInterface*>::iterator it = __joint_ifs->begin(); it != __joint_ifs->end(); ++it) {
274  blackboard->close(*it);
275  }
276 }
277 
278 
279 void
281 {
282  if ( __cfg_auto_calibrate ) {
283  start_motion(__calib_thread, 0, "Auto calibration enabled, calibrating");
284  __katana_if->set_enabled(true);
285  __katana_if->write();
286  }
287 }
288 
289 
290 /** Update position data in BB interface.
291  * @param refresh recv new data from arm
292  */
293 void
294 KatanaActThread::update_position(bool refresh)
295 {
296  try {
297  __katana->read_coordinates(refresh);
298  if( __cfg_controller == "kni") {
299  __katana_if->set_x(__cfg_distance_scale * __katana->x());
300  __katana_if->set_y(__cfg_distance_scale * __katana->y());
301  __katana_if->set_z(__cfg_distance_scale * __katana->z());
302  } else if( __cfg_controller == "openrave") {
303 
304  if( !tf_listener->frame_exists(__cfg_frame_openrave) ) {
305  logger->log_warn(name(), "tf frames not existing: '%s'. Skipping transform to kni coordinates.",
306  __cfg_frame_openrave.c_str() );
307  } else {
308  Stamped<Point> target;
309  Stamped<Point> target_local(Point(__katana->x(), __katana->y(), __katana->z()),
310  fawkes::Time(0,0), __cfg_frame_openrave);
311 
312  tf_listener->transform_point(__cfg_frame_kni, target_local, target);
313 
314  __katana_if->set_x(target.getX());
315  __katana_if->set_y(target.getY());
316  __katana_if->set_z(target.getZ());
317  }
318  }
319  __katana_if->set_phi(__katana->phi());
320  __katana_if->set_theta(__katana->theta());
321  __katana_if->set_psi(__katana->psi());
322  } catch (fawkes::Exception &e) {
323  logger->log_warn(name(), "Updating position values failed: %s", e.what());
324  }
325 
326  float *a = __katana_if->angles();
327 
328  __joint_ifs->at(0)->set_position( a[0] + M_PI);
329  __joint_ifs->at(1)->set_position( a[1]);// + M_PI/2);
330  __joint_ifs->at(2)->set_position( a[2] + M_PI);
331  __joint_ifs->at(3)->set_position( a[3] - M_PI);
332  __joint_ifs->at(4)->set_position( a[4]);
333  __joint_ifs->at(5)->set_position( -a[5]/2.f - 0.5f);
334  __joint_ifs->at(6)->set_position( -a[5]/2.f - 0.5f);
335  for( unsigned int i=0; i<__joint_ifs->size(); ++i) {
336  __joint_ifs->at(i)->write();
337  }
338 /*
339  fawkes::Time now(clock);
340 
341  static const float p90 = deg2rad(90);
342  static const float p180 = deg2rad(180);
343 
344  Transform bs_j1(Quaternion(a[0], 0, 0), Vector3(0, 0, 0.141));
345  Transform j1_j2(Quaternion(0, a[1] - p90, 0), Vector3(0, 0, 0.064));
346  Transform j2_j3(Quaternion(0, a[2] + p180, 0), Vector3(0, 0, 0.190));
347  Transform j3_j4(Quaternion(0, -a[3] - p180, 0), Vector3(0, 0, 0.139));
348  Transform j4_j5(Quaternion(-a[4], 0, 0), Vector3(0, 0, 0.120));
349  Transform j5_gr(Quaternion(0, -p90, 0), Vector3(0, 0, 0.065));
350 
351  tf_publisher->send_transform(bs_j1, now, "/katana/base", "/katana/j1");
352  tf_publisher->send_transform(j1_j2, now, "/katana/j1", "/katana/j2");
353  tf_publisher->send_transform(j2_j3, now, "/katana/j2", "/katana/j3");
354  tf_publisher->send_transform(j3_j4, now, "/katana/j3", "/katana/j4");
355  tf_publisher->send_transform(j4_j5, now, "/katana/j4", "/katana/j5");
356  tf_publisher->send_transform(j5_gr, now, "/katana/j5", "/katana/gripper"); //remember to adjust name in message-processing on change
357 */
358 }
359 
360 
361 /** Update sensor values as necessary.
362  * To be called only from KatanaSensorThread. Makes the local decision whether
363  * sensor can be written (calibration is not running) and whether the data
364  * needs to be refreshed (no active motion).
365  */
366 void
368 {
369  MutexLocker lock(loop_mutex);
370  if ( __actmot_thread != __calib_thread ) {
371  update_sensors(! __actmot_thread);
372  }
373 }
374 
375 
376 /** Update sensor value in BB interface.
377  * @param refresh recv new data from arm
378  */
379 void
380 KatanaActThread::update_sensors(bool refresh)
381 {
382  try {
383  std::vector<int> sensors;
384  __katana->get_sensors(sensors, false);
385 
386  const int num_sensors = std::min(sensors.size(), __katana_if->maxlenof_sensor_value());
387  for (int i = 0; i < num_sensors; ++i) {
388  if (sensors.at(i) <= 0) {
389  __katana_if->set_sensor_value(i, 0);
390  } else if (sensors.at(i) >= 255) {
391  __katana_if->set_sensor_value(i, 255);
392  } else {
393  __katana_if->set_sensor_value(i, sensors.at(i));
394  }
395  }
396  } catch (fawkes::Exception &e) {
397  logger->log_warn(name(), "Updating sensor values failed: %s", e.what());
398  }
399 
400  if (refresh) __sensacq_thread->wakeup();
401 }
402 
403 
404 /** Update motor encoder and angle data in BB interface.
405  * @param refresh recv new data from arm
406  */
407 void
408 KatanaActThread::update_motors(bool refresh)
409 {
410  try {
411  if( __katana->joint_encoders()) {
412  std::vector<int> encoders;
413  __katana->get_encoders(encoders, refresh);
414  for(unsigned int i=0; i<encoders.size(); i++) {
415  __katana_if->set_encoders(i, encoders.at(i));
416  }
417  }
418 
419  if( __katana->joint_angles()) {
420  std::vector<float> angles;
421  __katana->get_angles(angles, false);
422  for(unsigned int i=0; i<angles.size(); i++) {
423  __katana_if->set_angles(i, angles.at(i));
424  }
425  }
426  } catch (fawkes::Exception &e) {
427  logger->log_warn(name(), "Updating motor values failed. Ex:%s", e.what());
428  }
429 }
430 
431 
432 /** Start a motion.
433  * @param motion_thread motion thread to start
434  * @param msgid BB message ID of message that caused the motion
435  * @param logmsg message to print, format for following arguments
436  */
437 void
438 KatanaActThread::start_motion(RefPtr<KatanaMotionThread> motion_thread,
439  unsigned int msgid, const char *logmsg, ...)
440 {
441  va_list arg;
442  va_start(arg, logmsg);
443  logger->vlog_debug(name(), logmsg, arg);
444  __sensacq_thread->set_enabled(false);
445  __actmot_thread = motion_thread;
446  __actmot_thread->start(/* wait */ false);
447  __katana_if->set_msgid(msgid);
448  __katana_if->set_final(false);
449  va_end(arg);
450 }
451 
452 
453 /** Stop any running motion. */
454 void
455 KatanaActThread::stop_motion()
456 {
457  logger->log_info(name(), "Stopping arm movement");
458  loop_mutex->lock();
459  if (__actmot_thread) {
460  __actmot_thread->cancel();
461  __actmot_thread->join();
462  __actmot_thread = NULL;
463  }
464  try {
465  __katana->stop();
466  } catch (fawkes::Exception &e) {
467  logger->log_warn(name(), "Failed to freeze robot on stop: %s", e.what());
468  }
469  loop_mutex->unlock();
470 }
471 
472 
473 void
475 {
476  if ( __actmot_thread ) {
477  update_motors(/* refresh */ false);
478  update_position(/* refresh */ false);
479  __katana_if->write();
480  if (! __actmot_thread->finished()) {
481  return;
482  } else {
483  logger->log_debug(name(), "Motion thread %s finished, collecting", __actmot_thread->name());
484  __actmot_thread->join();
485  __katana_if->set_final(true);
486  __katana_if->set_error_code(__actmot_thread->error_code());
487  if (__actmot_thread == __calib_thread) {
488  __katana_if->set_calibrated(true);
489  }
490  __actmot_thread->reset();
491  __actmot_thread = NULL;
492  logger->log_debug(name(), "Motion thread collected");
493  __sensacq_thread->set_enabled(true);
494 
495  update_motors(/* refresh */ true);
496  update_position(/* refresh */ true);
497 
498 #ifdef HAVE_OPENRAVE
499  if(__cfg_OR_enabled) { __goto_openrave_thread->update_openrave_data(); }
500 #endif
501  }
502  } else if (!__katana_if->is_enabled()) {
503  update_position(/* refresh */ true);
504  update_motors(/* refresh */ true);
505 
506  } else {
507  // update every once in a while to keep transforms updated
508  fawkes::Time now(clock);
509  if ((now - __last_update) >= __cfg_update_interval) {
510  __last_update->stamp();
511  update_position(/* refresh */ false);
512  update_motors(/* refresh */ false);
513  }
514  }
515 
516  while (! __katana_if->msgq_empty() && ! __actmot_thread ) {
517  if (__katana_if->msgq_first_is<KatanaInterface::CalibrateMessage>()) {
518  KatanaInterface::CalibrateMessage *msg = __katana_if->msgq_first(msg);
519  start_motion(__calib_thread, msg->id(), "Calibrating arm");
520 
521  } else if (__katana_if->msgq_first_is<KatanaInterface::LinearGotoMessage>()) {
522  KatanaInterface::LinearGotoMessage *msg = __katana_if->msgq_first(msg);
523 
524  bool trans_frame_exists = tf_listener->frame_exists(msg->trans_frame());
525  bool rot_frame_exists = tf_listener->frame_exists(msg->rot_frame());
526  if( !trans_frame_exists || !rot_frame_exists ) {
527  logger->log_warn(name(), "tf frames not existing: '%s%s%s'. Skipping message.",
528  trans_frame_exists ? "" : msg->trans_frame(),
529  trans_frame_exists ? "" : "', '",
530  rot_frame_exists ? "" : msg->rot_frame() );
531  } else {
532  Stamped<Point> target;
533  Stamped<Point> target_local(Point(msg->x(), msg->y(), msg->z()),
534  fawkes::Time(0,0), msg->trans_frame());
535  if( __cfg_OR_enabled ) {
536 #ifdef HAVE_OPENRAVE
537  tf_listener->transform_point(__cfg_frame_openrave, target_local, target);
538  if( msg->offset_xy() != 0.f ) {
539  Vector3 offset(target.getX(), target.getY(), 0.f);
540  offset = (offset/offset.length()) * msg->offset_xy(); // Vector3 does not support a set_length method
541  target += offset;
542  }
543  // TODO: how to transform euler rotation to quaternion, to be used for tf??
544  if( strcmp(msg->trans_frame(), __cfg_frame_gripper.c_str())==0 ) {
545  __goto_openrave_thread->set_target(msg->x(), msg->y(), msg->z(),
546  msg->phi(), msg->theta(), msg->psi());
547  __goto_openrave_thread->set_arm_extension(true);
548  } else {
549  __goto_openrave_thread->set_target(target.getX(), target.getY(), target.getZ(),
550  msg->phi(), msg->theta(), msg->psi());
551  }
552  __goto_openrave_thread->set_theta_error(msg->theta_error());
553  __goto_openrave_thread->set_move_straight(msg->is_straight());
554  #ifdef EARLY_PLANNING
555  __goto_openrave_thread->plan_target();
556  #endif
557  start_motion(__goto_openrave_thread, msg->id(),
558  "Linear movement to (%f,%f,%f, %f,%f,%f), frame '%s', theta_error:%f, straight:%u",
559  target.getX(), target.getY(), target.getZ(),
560  msg->phi(), msg->theta(), msg->psi(), __cfg_frame_openrave.c_str(), msg->theta_error(), msg->is_straight());
561 #endif
562  } else {
563  tf_listener->transform_point(__cfg_frame_kni, target_local, target);
564  if( msg->offset_xy() != 0.f ) {
565  Vector3 offset(target.getX(), target.getY(), 0.f);
566  offset = (offset/offset.length()) * msg->offset_xy(); // Vector3 does not support a set_length method
567  target += offset;
568  }
569  __goto_thread->set_target(target.getX() / __cfg_distance_scale,
570  target.getY() / __cfg_distance_scale,
571  target.getZ() / __cfg_distance_scale,
572  msg->phi(), msg->theta(), msg->psi());
573  start_motion(__goto_thread, msg->id(),
574  "Linear movement to (%f,%f,%f, %f,%f,%f), frame '%s'",
575  target.getX(), target.getY(), target.getZ(),
576  msg->phi(), msg->theta(), msg->psi(), __cfg_frame_kni.c_str());
577  }
578  }
579 
580  } else if (__katana_if->msgq_first_is<KatanaInterface::LinearGotoKniMessage>()) {
581  KatanaInterface::LinearGotoKniMessage *msg = __katana_if->msgq_first(msg);
582 
583  float x = msg->x() * __cfg_distance_scale;
584  float y = msg->y() * __cfg_distance_scale;
585  float z = msg->z() * __cfg_distance_scale;
586 
587  tf::Stamped<Point> target;
588  tf::Stamped<Point> target_local(tf::Point(x, y, z),
589  fawkes::Time(0,0), __cfg_frame_kni);
590 
591  if( __cfg_OR_enabled ) {
592 #ifdef HAVE_OPENRAVE
593  tf_listener->transform_point(__cfg_frame_openrave, target_local, target);
594  __goto_openrave_thread->set_target(target.getX(), target.getY(), target.getZ(),
595  msg->phi(), msg->theta(), msg->psi());
596  #ifdef EARLY_PLANNING
597  __goto_openrave_thread->plan_target();
598  #endif
599  start_motion(__goto_openrave_thread, msg->id(),
600  "Linear movement to (%f,%f,%f, %f,%f,%f), frame '%s'",
601  target.getX(), target.getY(), target.getZ(),
602  msg->phi(), msg->theta(), msg->psi(), __cfg_frame_openrave.c_str());
603 #endif
604  } else {
605  __goto_thread->set_target(msg->x(), msg->y(), msg->z(),
606  msg->phi(), msg->theta(), msg->psi());
607 
608  start_motion(__goto_thread, msg->id(),
609  "Linear movement to (%f,%f,%f, %f,%f,%f), frame '%s'",
610  x, y, z,
611  msg->phi(), msg->theta(), msg->psi(), __cfg_frame_kni.c_str());
612  }
613 
614 #ifdef HAVE_OPENRAVE
615  } else if (__katana_if->msgq_first_is<KatanaInterface::ObjectGotoMessage>() && __cfg_OR_enabled) {
616  KatanaInterface::ObjectGotoMessage *msg = __katana_if->msgq_first(msg);
617 
618  float rot_x = 0.f;
619  if( msg->rot_x() )
620  { rot_x = msg->rot_x(); }
621 
622  __goto_openrave_thread->set_target(msg->object(), rot_x);
623  #ifdef EARLY_PLANNING
624  __goto_openrave_thread->plan_target();
625  #endif
626  start_motion(__goto_openrave_thread, msg->id(),
627  "Linear movement to object (%s, %f)", msg->object(), msg->rot_x());
628 #endif
629 
630  } else if (__katana_if->msgq_first_is<KatanaInterface::ParkMessage>()) {
631  KatanaInterface::ParkMessage *msg = __katana_if->msgq_first(msg);
632 
633  if(__cfg_OR_enabled) {
634 #ifdef HAVE_OPENRAVE
635  tf::Stamped<Point> target;
636  tf::Stamped<Point> target_local(tf::Point(__cfg_park_x * __cfg_distance_scale,
637  __cfg_park_y * __cfg_distance_scale,
638  __cfg_park_z * __cfg_distance_scale),
639  fawkes::Time(0,0), __cfg_frame_kni);
640  tf_listener->transform_point(__cfg_frame_openrave, target_local, target);
641  __goto_openrave_thread->set_target(target.getX(), target.getY(), target.getZ(),
642  __cfg_park_phi, __cfg_park_theta, __cfg_park_psi);
643  #ifdef EARLY_PLANNING
644  __goto_openrave_thread->plan_target();
645  #endif
646  start_motion(__goto_openrave_thread, msg->id(), "Parking arm");
647 #endif
648  } else {
649  __goto_thread->set_target(__cfg_park_x, __cfg_park_y, __cfg_park_z,
650  __cfg_park_phi, __cfg_park_theta, __cfg_park_psi);
651  start_motion(__goto_thread, msg->id(), "Parking arm");
652  }
653 
654  } else if (__katana_if->msgq_first_is<KatanaInterface::OpenGripperMessage>()) {
655  KatanaInterface::OpenGripperMessage *msg = __katana_if->msgq_first(msg);
656  __gripper_thread->set_mode(KatanaGripperThread::OPEN_GRIPPER);
657  start_motion(__gripper_thread, msg->id(), "Opening gripper");
658 
659  } else if (__katana_if->msgq_first_is<KatanaInterface::CloseGripperMessage>()) {
660  KatanaInterface::CloseGripperMessage *msg = __katana_if->msgq_first(msg);
661  __gripper_thread->set_mode(KatanaGripperThread::CLOSE_GRIPPER);
662  start_motion(__gripper_thread, msg->id(), "Closing gripper");
663 
664  } else if (__katana_if->msgq_first_is<KatanaInterface::SetEnabledMessage>()) {
665  KatanaInterface::SetEnabledMessage *msg = __katana_if->msgq_first(msg);
666 
667  try {
668  if (msg->is_enabled()) {
669  logger->log_debug(name(), "Turning ON the arm");
670  __katana->turn_on();
671  update_position(/* refresh */ true);
672  update_motors(/* refresh */ true);
673 #ifdef HAVE_OPENRAVE
674  if(__cfg_OR_enabled)
675  __goto_openrave_thread->update_openrave_data();
676 #endif
677  } else {
678  logger->log_debug(name(), "Turning OFF the arm");
679  __katana->turn_off();
680  }
681  __katana_if->set_enabled(msg->is_enabled());
682  } catch (/*KNI*/::Exception &e) {
683  logger->log_warn(name(), "Failed enable/disable arm: %s", e.what());
684  }
685 
686  } else if (__katana_if->msgq_first_is<KatanaInterface::SetMaxVelocityMessage>()) {
687  KatanaInterface::SetMaxVelocityMessage *msg = __katana_if->msgq_first(msg);
688 
689  unsigned int max_vel = msg->max_velocity();
690  if ( max_vel == 0 ) max_vel = __cfg_defmax_speed;
691 
692  try {
693  __katana->set_max_velocity(max_vel);
694  } catch (fawkes::Exception &e) {
695  logger->log_warn(name(), "Failed setting max velocity. Ex:%s", e.what());
696  }
697  __katana_if->set_max_velocity(max_vel);
698 
699  } else if (__katana_if->msgq_first_is<KatanaInterface::SetPlannerParamsMessage>()) {
700  KatanaInterface::SetPlannerParamsMessage *msg = __katana_if->msgq_first(msg);
701 
702  if( __cfg_OR_enabled ) {
703 #ifdef HAVE_OPENRAVE
704  __goto_openrave_thread->set_plannerparams(msg->plannerparams());
705 #endif
706  }
707 
708  } else if (__katana_if->msgq_first_is<KatanaInterface::SetMotorEncoderMessage>()) {
709  KatanaInterface::SetMotorEncoderMessage *msg = __katana_if->msgq_first(msg);
710 
711  __motor_control_thread->set_encoder(msg->nr(), msg->enc(), false);
712  start_motion(__motor_control_thread, msg->id(), "Moving motor");
713 
714  } else if (__katana_if->msgq_first_is<KatanaInterface::MoveMotorEncoderMessage>()) {
715  KatanaInterface::MoveMotorEncoderMessage *msg = __katana_if->msgq_first(msg);
716 
717  __motor_control_thread->set_encoder(msg->nr(), msg->enc(), true);
718  start_motion(__motor_control_thread, msg->id(), "Moving motor");
719 
720  } else if (__katana_if->msgq_first_is<KatanaInterface::SetMotorAngleMessage>()) {
721  KatanaInterface::SetMotorAngleMessage *msg = __katana_if->msgq_first(msg);
722 
723  __motor_control_thread->set_angle(msg->nr(), msg->angle(), false);
724  start_motion(__motor_control_thread, msg->id(), "Moving motor");
725 
726  } else if (__katana_if->msgq_first_is<KatanaInterface::MoveMotorAngleMessage>()) {
727  KatanaInterface::MoveMotorAngleMessage *msg = __katana_if->msgq_first(msg);
728 
729  __motor_control_thread->set_angle(msg->nr(), msg->angle(), true);
730  start_motion(__motor_control_thread, msg->id(), "Moving motor");
731 
732  } else {
733  logger->log_warn(name(), "Unknown message received");
734  }
735 
736  __katana_if->msgq_pop();
737  }
738 
739  __katana_if->write();
740 
741 #ifdef USE_TIMETRACKER
742  if (++__tt_count > 100) {
743  __tt_count = 0;
744  __tt->print_to_stdout();
745  }
746 #endif
747 }
748 
749 
750 bool
752 {
753  if (message->is_of_type<KatanaInterface::StopMessage>()) {
754  stop_motion();
755  return false; // do not enqueue StopMessage
756  } else if (message->is_of_type<KatanaInterface::FlushMessage>()) {
757  stop_motion();
758  logger->log_info(name(), "Flushing message queue");
759  __katana_if->msgq_flush();
760  return false;
761  } else {
762  logger->log_info(name(), "Received message of type %s, enqueueing", message->type());
763  return true;
764  }
765 }
virtual void init()=0
Initialize controller.
void set_enabled(const bool new_enabled)
Set enabled value.
void set_sensor_value(unsigned int index, const uint8_t new_sensor_value)
Set sensor_value value at given index.
virtual void setup(std::string &device, std::string &kni_conffile, unsigned int read_timeout, unsigned int write_timeout)
Setup parameters needed to initialize Katana arm with libkni.
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
void set_enabled(bool enabled)
Set whether data acquisition is enabled or not.
virtual double y()=0
Get y-coordinate of latest endeffector position.
virtual void loop()
Code to execute in the thread.
Definition: act_thread.cpp:474
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1048
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
virtual void vlog_debug(const char *component, const char *format, va_list va)=0
Log debug message.
void transform_point(const std::string &target_frame, const Stamped< Point > &stamped_in, Stamped< Point > &stamped_out) const
Transform a stamped point into the target frame.
void set_z(const float new_z)
Set z value.
uint8_t max_velocity() const
Get max_velocity value.
LinearGotoKniMessage Fawkes BlackBoard Interface Message.
float theta_error() const
Get theta_error value.
virtual void get_angles(std::vector< float > &to, bool refresh=false)=0
Get angle values of joints/motors.
void set_y(const float new_y)
Set y value.
Fawkes library namespace.
Katana motor control thread.
CalibrateMessage Fawkes BlackBoard Interface Message.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
void unlock()
Unlock the mutex.
Definition: mutex.cpp:135
virtual double theta()=0
Get theta-rotation of latest endeffector orientation.
Mutex locking helper.
Definition: mutex_locker.h:33
bool is_enabled() const
Get enabled value.
void set_final(const bool new_final)
Set final value.
SetMotorEncoderMessage Fawkes BlackBoard Interface Message.
void set_calibrated(const bool new_calibrated)
Set calibrated value.
A class for handling time.
Definition: time.h:91
Controller class for a Neuronics Katana, using libkni to interact with the real Katana arm...
virtual const char * what() const
Get primary string.
Definition: exception.cpp:661
virtual void set_target(float x, float y, float z, float phi, float theta, float psi)
Set target position.
Definition: goto_thread.cpp:63
virtual void unregister_listener(BlackBoardInterfaceListener *listener)
Unregister BB interface listener.
Definition: blackboard.cpp:218
Thread class encapsulation of pthreads.
Definition: thread.h:42
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:500
SetMotorAngleMessage Fawkes BlackBoard Interface Message.
virtual double z()=0
Get z-coordinate of latest endeffector position.
void set_phi(const float new_phi)
Set phi value.
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:79
void set_psi(const float new_psi)
Set psi value.
Mutex * loop_mutex
Mutex that is used to protect a call to loop().
Definition: thread.h:139
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:44
void set_theta(const float new_theta)
Set theta value.
virtual void read_coordinates(bool refresh=false)=0
Store current coordinates of endeeffctor.
Thread aspect to access the transform system.
Definition: tf.h:42
virtual void turn_on()=0
Turn on arm/motors.
char * object() const
Get object value.
Clock * clock
By means of this member access to the clock is given.
Definition: clock.h:45
void set_msgid(const uint32_t new_msgid)
Set msgid value.
virtual void finalize()
Finalize the thread.
Definition: act_thread.cpp:227
ObjectGotoMessage Fawkes BlackBoard Interface Message.
virtual void once()
Execute an action exactly once.
Definition: act_thread.cpp:280
void reset()
Reset pointer.
Definition: refptr.h:464
Thread aspect to use blocked timing.
float * angles() const
Get angles value.
virtual void register_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Register BB event listener.
Definition: blackboard.cpp:190
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1193
bool is_enabled() const
Get enabled value.
bool finished() const
Did the motion finish already?
virtual void init()
Initialize the thread.
Definition: act_thread.cpp:79
OpenGripperMessage Fawkes BlackBoard Interface Message.
virtual void reset()
Reset for next execution.
void wakeup()
Wake up thread.
Definition: thread.cpp:1000
KatanaInterface Fawkes BlackBoard Interface.
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
void set_clock(Clock *clock)
Set clock for this instance.
Definition: time.cpp:329
LinearGotoMessage Fawkes BlackBoard Interface Message.
virtual bool joint_angles()=0
Check if controller provides joint angle values.
virtual void finalize()
Finalize the thread.
Definition: thread.cpp:473
SetPlannerParamsMessage Fawkes BlackBoard Interface Message.
unsigned int error_code() const
Error code.
Controller class for a Neuronics Katana, using libkni to interact with the real Katana arm...
SetMaxVelocityMessage Fawkes BlackBoard Interface Message.
Time tracking utility.
Definition: tracker.h:38
Katana gripper thread.
void set_error_code(const uint32_t new_error_code)
Set error_code value.
virtual double x()=0
Get x-coordinate of latest endeffector position.
const char * name() const
Get name of thread.
Definition: thread.h:95
virtual void get_sensors(std::vector< int > &to, bool refresh=false)=0
Get sensor values.
Katana sensor acquisition thread.
void set_angles(unsigned int index, const float new_angles)
Set angles value at given index.
char * rot_frame() const
Get rot_frame value.
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.
class KatanaGotoOpenRaveThread
CloseGripperMessage Fawkes BlackBoard Interface Message.
virtual bool bb_interface_message_received(fawkes::Interface *interface, fawkes::Message *message)
BlackBoard message received notification.
Definition: act_thread.cpp:751
void set_mode(gripper_mode_t mode)
Set mode.
void cancel()
Cancel a thread.
Definition: thread.cpp:651
Wrapper class to add time stamp and frame ID to base types.
Definition: types.h:133
virtual void turn_off()=0
Turn off arm/motors.
void set_x(const float new_x)
Set x value.
StopMessage Fawkes BlackBoard Interface Message.
size_t maxlenof_sensor_value() const
Get maximum length of sensor_value value.
float offset_xy() const
Get offset_xy value.
MoveMotorAngleMessage Fawkes BlackBoard Interface Message.
char * plannerparams() const
Get plannerparams value.
void set_encoders(unsigned int index, const int32_t new_encoders)
Set encoders value at given index.
void update_sensor_values()
Update sensor values as necessary.
Definition: act_thread.cpp:367
void set_time(const timeval *tv)
Sets the time.
Definition: time.cpp:262
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
Katana linear goto thread.
Definition: goto_thread.h:30
KatanaActThread()
Constructor.
Definition: act_thread.cpp:62
void msgq_flush()
Flush all messages.
Definition: interface.cpp:1064
void join()
Join the thread.
Definition: thread.cpp:610
MoveMotorEncoderMessage Fawkes BlackBoard Interface Message.
virtual void set_angle(unsigned int nr, float value, bool inc=false)
Set target angle value.
JointInterface Fawkes BlackBoard Interface.
virtual double psi()=0
Get psi-rotation of latest endeffector orientation.
void lock()
Lock this mutex.
Definition: mutex.cpp:89
Time & stamp()
Set this time to the current time.
Definition: time.cpp:783
virtual bool joint_encoders()=0
Check if controller provides joint encoder values.
ParkMessage Fawkes BlackBoard Interface Message.
virtual unsigned int get_uint(const char *path)=0
Get value from configuration which is of type unsigned int.
virtual void set_max_velocity(unsigned int vel)=0
Set maximum velocity.
char * trans_frame() const
Get trans_frame value.
tf::Transformer * tf_listener
This is the transform listener which saves transforms published by other threads in the system...
Definition: tf.h:70
void set_max_velocity(const uint8_t new_max_velocity)
Set max_velocity value.
virtual double phi()=0
Get x-coordinate of latest endeffector position.
virtual void set_encoder(unsigned int nr, int value, bool inc=false)
Set target encoder value.
~KatanaActThread()
Destructor.
Definition: act_thread.cpp:72
FlushMessage Fawkes BlackBoard Interface Message.
Katana calibration thread.
Definition: calib_thread.h:28
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:44
void bbil_add_message_interface(Interface *interface)
Add an interface to the message received watch list.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
virtual void stop()=0
Stop movement immediately.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
BlackBoard interface listener.
bool frame_exists(const std::string &frame_id_str) const
Check if frame exists.
void start(bool wait=true)
Call this method to start the thread.
Definition: thread.cpp:511
SetEnabledMessage Fawkes BlackBoard Interface Message.
bool is_straight() const
Get straight value.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
virtual void get_encoders(std::vector< int > &to, bool refresh=false)=0
Get encoder values of joints/motors.
virtual void close(Interface *interface)=0
Close interface.