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