Fawkes API  Fawkes Development Version
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
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  *
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL file in the doc directory.
21  */
22 
23 #include "act_thread.h"
24 #include "controller_kni.h"
25 #include "controller_openrave.h"
26 
27 #include <core/threading/mutex_locker.h>
28 #include <interfaces/KatanaInterface.h>
29 #include <utils/math/angle.h>
30 #include <utils/time/time.h>
31 
32 #include <algorithm>
33 #include <cstdarg>
34 
35 using namespace fawkes;
36 #ifdef HAVE_TF
37 using namespace fawkes::tf;
38 #endif
39 
40 /** @class KatanaActThread "act_thread.h"
41  * Katana act thread.
42  * This thread integrates into the Fawkes main loop at the ACT_EXEC hook and
43  * interacts with a given controller for the Katana.
44  * @author Tim Niemueller
45  */
46 
47 /** Constructor. */
49  : Thread("KatanaActThread", Thread::OPMODE_WAITFORWAKEUP),
50  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_ACT_EXEC),
51  TransformAspect(TransformAspect::BOTH, "Katana"),
52  BlackBoardInterfaceListener("KatanaActThread")
53 {
54  __last_update = new Time();
55 }
56 
57 /** Destructor. */
59 {
60  delete __last_update;
61 }
62 
63 
64 void
66 {
67  // Note: due to the use of auto_ptr and RefPtr resources are automatically
68  // freed on destruction, therefore no special handling is necessary in init()
69  // itself!
70 
71  __cfg_controller = config->get_string("/hardware/katana/controller");
72  __cfg_device = config->get_string("/hardware/katana/device");
73  __cfg_kni_conffile = config->get_string("/hardware/katana/kni_conffile");
74  __cfg_auto_calibrate = config->get_bool("/hardware/katana/auto_calibrate");
75  __cfg_defmax_speed = config->get_uint("/hardware/katana/default_max_speed");
76  __cfg_read_timeout = config->get_uint("/hardware/katana/read_timeout_msec");
77  __cfg_write_timeout = config->get_uint("/hardware/katana/write_timeout_msec");
78  __cfg_gripper_pollint = config->get_uint("/hardware/katana/gripper_pollint_msec");
79  __cfg_goto_pollint = config->get_uint("/hardware/katana/goto_pollint_msec");
80 
81  __cfg_park_x = config->get_float("/hardware/katana/park_x");
82  __cfg_park_y = config->get_float("/hardware/katana/park_y");
83  __cfg_park_z = config->get_float("/hardware/katana/park_z");
84  __cfg_park_phi = config->get_float("/hardware/katana/park_phi");
85  __cfg_park_theta = config->get_float("/hardware/katana/park_theta");
86  __cfg_park_psi = config->get_float("/hardware/katana/park_psi");
87 
88  __cfg_distance_scale = config->get_float("/hardware/katana/distance_scale");
89 
90  __cfg_update_interval = config->get_float("/hardware/katana/update_interval");
91 
92  __cfg_frame_kni = config->get_string("/plugins/static-transforms/transforms/katana_kni/child_frame");
93  __cfg_frame_openrave = config->get_string("/plugins/static-transforms/transforms/openrave/child_frame");
94 
95 #ifdef HAVE_OPENRAVE
96  __cfg_OR_enabled = config->get_bool("/hardware/katana/openrave/enabled");
97  __cfg_OR_use_viewer = config->get_bool("/hardware/katana/openrave/use_viewer");
98  __cfg_OR_auto_load_ik = config->get_bool("/hardware/katana/openrave/auto_load_ik");
99  __cfg_OR_robot_file = config->get_string("/hardware/katana/openrave/robot_file");
100  __cfg_OR_arm_model = config->get_string("/hardware/katana/openrave/arm_model");
101 #else
102  __cfg_OR_enabled = false;
103 #endif
104 
105  __last_update->set_clock(clock);
106  __last_update->set_time(0, 0);
107 
108  // Load katana controller
109  if( __cfg_controller == "kni") {
110  KatanaControllerKni* kat_ctrl = new KatanaControllerKni();
111  __katana = kat_ctrl;
112  try {
113  kat_ctrl->setup(__cfg_device, __cfg_kni_conffile,
114  __cfg_read_timeout, __cfg_write_timeout);
115  } catch(fawkes::Exception &e) {
116  logger->log_warn(name(), "Setup KatanaControllerKni failed. Ex: %s", e.what());
117  }
118  kat_ctrl = NULL;
119 
120  } else if( __cfg_controller == "openrave") {
121 #ifdef HAVE_OPENRAVE
122  if(!__cfg_OR_enabled) {
123  throw fawkes::Exception("Cannot use controller 'openrave', OpenRAVE is deactivated by config flag!");
124  }
125  __katana = new KatanaControllerOpenrave(openrave);
126 #else
127  throw fawkes::Exception("Cannot use controller 'openrave', OpenRAVE not installed!");
128 #endif
129 
130  } else {
131  throw fawkes::Exception("Invalid controller given: '%s'", __cfg_controller.c_str());
132  }
133 
134  // If you have more than one interface: catch exception and close them!
135  __katana_if = blackboard->open_for_writing<KatanaInterface>("Katana");
136 
137  // Create all other threads
138  __sensacq_thread.reset(new KatanaSensorAcquisitionThread(__katana, logger));
139  __calib_thread = new KatanaCalibrationThread(__katana, logger);
140  __gripper_thread = new KatanaGripperThread(__katana, logger,
141  __cfg_gripper_pollint);
142  __motor_control_thread = new KatanaMotorControlThread(__katana, logger, __cfg_goto_pollint);
143  __goto_thread = new KatanaGotoThread(__katana, logger, __cfg_goto_pollint);
144 #ifdef HAVE_OPENRAVE
145  __goto_openrave_thread = new KatanaGotoOpenRaveThread(__katana, logger, openrave, __cfg_goto_pollint, __cfg_OR_robot_file,
146  __cfg_OR_arm_model, __cfg_OR_auto_load_ik, __cfg_OR_use_viewer);
147  if(__cfg_OR_enabled)
148  {__goto_openrave_thread->init();}
149 #endif
150 
151  // Intialize katana controller
152  try {
153  __katana->init();
154  __katana->set_max_velocity(__cfg_defmax_speed);
155  logger->log_debug(name(), "Katana successfully initialized");
156  } catch(fawkes::Exception &e) {
157  logger->log_warn(name(), "Initializing controller failed. Ex: %s", e.what());
158  finalize();
159  throw; // need try-catch anyway?
160  }
161 
162  __sensacq_thread->start();
163 
164  bbil_add_message_interface(__katana_if);
166 
167 #ifdef USE_TIMETRACKER
168  __tt.reset(new TimeTracker());
169  __tt_count = 0;
170  __ttc_read_sensor = __tt->add_class("Read Sensor");
171 #endif
172 
173 }
174 
175 
176 void
178 {
179  if ( __actmot_thread ) {
180  __actmot_thread->cancel();
181  __actmot_thread->join();
182  __actmot_thread = NULL;
183  }
184  __sensacq_thread->cancel();
185  __sensacq_thread->join();
186  __sensacq_thread.reset();
187 
188  // Setting to NULL also deletes instance (RefPtr)
189  __calib_thread = NULL;
190  __goto_thread = NULL;
191  __gripper_thread = NULL;
192  __motor_control_thread = NULL;
193 #ifdef HAVE_OPENRAVE
194  if(__cfg_OR_enabled)
195  {__goto_openrave_thread->finalize();}
196  __goto_openrave_thread = NULL;
197 #endif
198 
199  try {
200  __katana->stop();
201  } catch(fawkes::Exception &e) {
202  logger->log_warn(name(), "failed stopping katana. Ex:%s", e.what());
203  }
204  __katana = NULL;
205 
207  blackboard->close(__katana_if);
208 }
209 
210 
211 void
213 {
214  if ( __cfg_auto_calibrate ) {
215  start_motion(__calib_thread, 0, "Auto calibration enabled, calibrating");
216  __katana_if->set_enabled(true);
217  __katana_if->write();
218  }
219 }
220 
221 
222 /** Update position data in BB interface.
223  * @param refresh recv new data from arm
224  */
225 void
226 KatanaActThread::update_position(bool refresh)
227 {
228  try {
229  __katana->read_coordinates(refresh);
230  if( __cfg_controller == "kni") {
231  __katana_if->set_x(__cfg_distance_scale * __katana->x());
232  __katana_if->set_y(__cfg_distance_scale * __katana->y());
233  __katana_if->set_z(__cfg_distance_scale * __katana->z());
234  } else if( __cfg_controller == "openrave") {
235 
236  if( !tf_listener->frame_exists(__cfg_frame_openrave) ) {
237  logger->log_warn(name(), "tf frames not existing: '%s'. Skipping transform to kni coordinates.",
238  __cfg_frame_openrave.c_str() );
239  } else {
240  Stamped<Point> target;
241  Stamped<Point> target_local(Point(__katana->x(), __katana->y(), __katana->z()),
242  fawkes::Time(0,0), __cfg_frame_openrave);
243 
244  tf_listener->transform_point(__cfg_frame_kni, target_local, target);
245 
246  __katana_if->set_x(target.getX());
247  __katana_if->set_y(target.getY());
248  __katana_if->set_z(target.getZ());
249  }
250  }
251  __katana_if->set_phi(__katana->phi());
252  __katana_if->set_theta(__katana->theta());
253  __katana_if->set_psi(__katana->psi());
254  } catch (fawkes::Exception &e) {
255  logger->log_warn(name(), "Updating position values failed: %s", e.what());
256  }
257 
258  float *a = __katana_if->angles();
259  fawkes::Time now(clock);
260 
261  static const float p90 = deg2rad(90);
262  static const float p180 = deg2rad(180);
263 
264  Transform bs_j1(Quaternion(a[0], 0, 0), Vector3(0, 0, 0.141));
265  Transform j1_j2(Quaternion(0, a[1] - p90, 0), Vector3(0, 0, 0.064));
266  Transform j2_j3(Quaternion(0, a[2] + p180, 0), Vector3(0, 0, 0.190));
267  Transform j3_j4(Quaternion(0, -a[3] - p180, 0), Vector3(0, 0, 0.139));
268  Transform j4_j5(Quaternion(-a[4], 0, 0), Vector3(0, 0, 0.120));
269  Transform j5_gr(Quaternion(0, -p90, 0), Vector3(0, 0, 0.065));
270 
271  tf_publisher->send_transform(bs_j1, now, "/katana/base", "/katana/j1");
272  tf_publisher->send_transform(j1_j2, now, "/katana/j1", "/katana/j2");
273  tf_publisher->send_transform(j2_j3, now, "/katana/j2", "/katana/j3");
274  tf_publisher->send_transform(j3_j4, now, "/katana/j3", "/katana/j4");
275  tf_publisher->send_transform(j4_j5, now, "/katana/j4", "/katana/j5");
276  tf_publisher->send_transform(j5_gr, now, "/katana/j5", "/katana/gripper"); //remember to adjust name in message-processing on change
277 }
278 
279 
280 /** Update sensor values as necessary.
281  * To be called only from KatanaSensorThread. Makes the local decision whether
282  * sensor can be written (calibration is not running) and whether the data
283  * needs to be refreshed (no active motion).
284  */
285 void
287 {
288  MutexLocker lock(loop_mutex);
289  if ( __actmot_thread != __calib_thread ) {
290  update_sensors(! __actmot_thread);
291  }
292 }
293 
294 
295 /** Update sensor value in BB interface.
296  * @param refresh recv new data from arm
297  */
298 void
299 KatanaActThread::update_sensors(bool refresh)
300 {
301  try {
302  std::vector<int> sensors;
303  __katana->get_sensors(sensors, false);
304 
305  const int num_sensors = std::min(sensors.size(), __katana_if->maxlenof_sensor_value());
306  for (int i = 0; i < num_sensors; ++i) {
307  if (sensors.at(i) <= 0) {
308  __katana_if->set_sensor_value(i, 0);
309  } else if (sensors.at(i) >= 255) {
310  __katana_if->set_sensor_value(i, 255);
311  } else {
312  __katana_if->set_sensor_value(i, sensors.at(i));
313  }
314  }
315  } catch (fawkes::Exception &e) {
316  logger->log_warn(name(), "Updating sensor values failed: %s", e.what());
317  }
318 
319  if (refresh) __sensacq_thread->wakeup();
320 }
321 
322 
323 /** Update motor encoder and angle data in BB interface.
324  * @param refresh recv new data from arm
325  */
326 void
327 KatanaActThread::update_motors(bool refresh)
328 {
329  try {
330  if( __katana->joint_encoders()) {
331  std::vector<int> encoders;
332  __katana->get_encoders(encoders, refresh);
333  for(unsigned int i=0; i<encoders.size(); i++) {
334  __katana_if->set_encoders(i, encoders.at(i));
335  }
336  }
337 
338  if( __katana->joint_angles()) {
339  std::vector<float> angles;
340  __katana->get_angles(angles, false);
341  for(unsigned int i=0; i<angles.size(); i++) {
342  __katana_if->set_angles(i, angles.at(i));
343  }
344  }
345  } catch (fawkes::Exception &e) {
346  logger->log_warn(name(), "Updating motor values failed. Ex:%s", e.what());
347  }
348 }
349 
350 
351 /** Start a motion.
352  * @param motion_thread motion thread to start
353  * @param msgid BB message ID of message that caused the motion
354  * @param logmsg message to print, format for following arguments
355  */
356 void
357 KatanaActThread::start_motion(RefPtr<KatanaMotionThread> motion_thread,
358  unsigned int msgid, const char *logmsg, ...)
359 {
360  va_list arg;
361  va_start(arg, logmsg);
362  logger->vlog_debug(name(), logmsg, arg);
363  __sensacq_thread->set_enabled(false);
364  __actmot_thread = motion_thread;
365  __actmot_thread->start(/* wait */ false);
366  __katana_if->set_msgid(msgid);
367  __katana_if->set_final(false);
368  va_end(arg);
369 }
370 
371 
372 /** Stop any running motion. */
373 void
374 KatanaActThread::stop_motion()
375 {
376  logger->log_info(name(), "Stopping arm movement");
377  loop_mutex->lock();
378  if (__actmot_thread) {
379  __actmot_thread->cancel();
380  __actmot_thread->join();
381  __actmot_thread = NULL;
382  }
383  try {
384  __katana->stop();
385  } catch (fawkes::Exception &e) {
386  logger->log_warn(name(), "Failed to freeze robot on stop: %s", e.what());
387  }
388  loop_mutex->unlock();
389 }
390 
391 
392 void
394 {
395  if ( __actmot_thread ) {
396  update_motors(/* refresh */ false);
397  update_position(/* refresh */ false);
398  __katana_if->write();
399  if (! __actmot_thread->finished()) {
400  return;
401  } else {
402  logger->log_debug(name(), "Motion thread %s finished, collecting", __actmot_thread->name());
403  __actmot_thread->join();
404  __katana_if->set_final(true);
405  __katana_if->set_error_code(__actmot_thread->error_code());
406  if (__actmot_thread == __calib_thread) {
407  __katana_if->set_calibrated(true);
408  }
409  __actmot_thread->reset();
410  __actmot_thread = NULL;
411  logger->log_debug(name(), "Motion thread collected");
412  __sensacq_thread->set_enabled(true);
413 
414  update_motors(/* refresh */ true);
415  update_position(/* refresh */ true);
416 
417 #ifdef HAVE_OPENRAVE
418  if(__cfg_OR_enabled) { __goto_openrave_thread->update_openrave_data(); }
419 #endif
420  }
421  } else if (!__katana_if->is_enabled()) {
422  update_position(/* refresh */ true);
423  update_motors(/* refresh */ true);
424 
425  } else {
426  // update every once in a while to keep transforms updated
427  fawkes::Time now(clock);
428  if ((now - __last_update) >= __cfg_update_interval) {
429  __last_update->stamp();
430  update_position(/* refresh */ false);
431  update_motors(/* refresh */ false);
432  }
433  }
434 
435  while (! __katana_if->msgq_empty() && ! __actmot_thread ) {
436  if (__katana_if->msgq_first_is<KatanaInterface::CalibrateMessage>()) {
437  KatanaInterface::CalibrateMessage *msg = __katana_if->msgq_first(msg);
438  start_motion(__calib_thread, msg->id(), "Calibrating arm");
439 
440  } else if (__katana_if->msgq_first_is<KatanaInterface::LinearGotoMessage>()) {
441  KatanaInterface::LinearGotoMessage *msg = __katana_if->msgq_first(msg);
442 
443  bool trans_frame_exists = tf_listener->frame_exists(msg->trans_frame());
444  bool rot_frame_exists = tf_listener->frame_exists(msg->rot_frame());
445  if( !trans_frame_exists || !rot_frame_exists ) {
446  logger->log_warn(name(), "tf frames not existing: '%s%s%s'. Skipping message.",
447  trans_frame_exists ? "" : msg->trans_frame(),
448  trans_frame_exists ? "" : "', '",
449  rot_frame_exists ? "" : msg->rot_frame() );
450  } else {
451  Stamped<Point> target;
452  Stamped<Point> target_local(Point(msg->x(), msg->y(), msg->z()),
453  fawkes::Time(0,0), msg->trans_frame());
454  if( __cfg_OR_enabled ) {
455 #ifdef HAVE_OPENRAVE
456  tf_listener->transform_point(__cfg_frame_openrave, target_local, target);
457  if( msg->offset_xy() != 0.f ) {
458  Vector3 offset(target.getX(), target.getY(), 0.f);
459  offset = (offset/offset.length()) * msg->offset_xy(); // Vector3 does not support a set_length method
460  target += offset;
461  }
462  // TODO: how to transform euler rotation to quaternion, to be used for tf??
463  if( strcmp(msg->trans_frame(), "/katana/gripper")==0 ) {
464  __goto_openrave_thread->set_target(msg->x(), msg->y(), msg->z(),
465  msg->phi(), msg->theta(), msg->psi());
466  __goto_openrave_thread->set_arm_extension(true);
467  } else {
468  __goto_openrave_thread->set_target(target.getX(), target.getY(), target.getZ(),
469  msg->phi(), msg->theta(), msg->psi());
470  }
471  __goto_openrave_thread->set_theta_error(msg->theta_error());
472  __goto_openrave_thread->set_move_straight(msg->is_straight());
473  #ifdef EARLY_PLANNING
474  __goto_openrave_thread->plan_target();
475  #endif
476  start_motion(__goto_openrave_thread, msg->id(),
477  "Linear movement to (%f,%f,%f, %f,%f,%f), frame '%s', theta_error:%f, straight:%u",
478  target.getX(), target.getY(), target.getZ(),
479  msg->phi(), msg->theta(), msg->psi(), __cfg_frame_openrave.c_str(), msg->theta_error(), msg->is_straight());
480 #endif
481  } else {
482  tf_listener->transform_point(__cfg_frame_kni, target_local, target);
483  if( msg->offset_xy() != 0.f ) {
484  Vector3 offset(target.getX(), target.getY(), 0.f);
485  offset = (offset/offset.length()) * msg->offset_xy(); // Vector3 does not support a set_length method
486  target += offset;
487  }
488  __goto_thread->set_target(target.getX() / __cfg_distance_scale,
489  target.getY() / __cfg_distance_scale,
490  target.getZ() / __cfg_distance_scale,
491  msg->phi(), msg->theta(), msg->psi());
492  start_motion(__goto_thread, msg->id(),
493  "Linear movement to (%f,%f,%f, %f,%f,%f), frame '%s'",
494  target.getX(), target.getY(), target.getZ(),
495  msg->phi(), msg->theta(), msg->psi(), __cfg_frame_kni.c_str());
496  }
497  }
498 
499  } else if (__katana_if->msgq_first_is<KatanaInterface::LinearGotoKniMessage>()) {
500  KatanaInterface::LinearGotoKniMessage *msg = __katana_if->msgq_first(msg);
501 
502  float x = msg->x() * __cfg_distance_scale;
503  float y = msg->y() * __cfg_distance_scale;
504  float z = msg->z() * __cfg_distance_scale;
505 
506  tf::Stamped<Point> target;
507  tf::Stamped<Point> target_local(tf::Point(x, y, z),
508  fawkes::Time(0,0), __cfg_frame_kni);
509 
510  if( __cfg_OR_enabled ) {
511 #ifdef HAVE_OPENRAVE
512  tf_listener->transform_point(__cfg_frame_openrave, target_local, target);
513  __goto_openrave_thread->set_target(target.getX(), target.getY(), target.getZ(),
514  msg->phi(), msg->theta(), msg->psi());
515  #ifdef EARLY_PLANNING
516  __goto_openrave_thread->plan_target();
517  #endif
518  start_motion(__goto_openrave_thread, msg->id(),
519  "Linear movement to (%f,%f,%f, %f,%f,%f), frame '%s'",
520  target.getX(), target.getY(), target.getZ(),
521  msg->phi(), msg->theta(), msg->psi(), __cfg_frame_openrave.c_str());
522 #endif
523  } else {
524  __goto_thread->set_target(msg->x(), msg->y(), msg->z(),
525  msg->phi(), msg->theta(), msg->psi());
526 
527  start_motion(__goto_thread, msg->id(),
528  "Linear movement to (%f,%f,%f, %f,%f,%f), frame '%s'",
529  x, y, z,
530  msg->phi(), msg->theta(), msg->psi(), __cfg_frame_kni.c_str());
531  }
532 
533 #ifdef HAVE_OPENRAVE
534  } else if (__katana_if->msgq_first_is<KatanaInterface::ObjectGotoMessage>() && __cfg_OR_enabled) {
535  KatanaInterface::ObjectGotoMessage *msg = __katana_if->msgq_first(msg);
536 
537  float rot_x = 0.f;
538  if( msg->rot_x() )
539  { rot_x = msg->rot_x(); }
540 
541  __goto_openrave_thread->set_target(msg->object(), rot_x);
542  #ifdef EARLY_PLANNING
543  __goto_openrave_thread->plan_target();
544  #endif
545  start_motion(__goto_openrave_thread, msg->id(),
546  "Linear movement to object (%s, %f)", msg->object(), msg->rot_x());
547 #endif
548 
549  } else if (__katana_if->msgq_first_is<KatanaInterface::ParkMessage>()) {
550  KatanaInterface::ParkMessage *msg = __katana_if->msgq_first(msg);
551 
552  if(__cfg_OR_enabled) {
553 #ifdef HAVE_OPENRAVE
554  tf::Stamped<Point> target;
555  tf::Stamped<Point> target_local(tf::Point(__cfg_park_x * __cfg_distance_scale,
556  __cfg_park_y * __cfg_distance_scale,
557  __cfg_park_z * __cfg_distance_scale),
558  fawkes::Time(0,0), __cfg_frame_kni);
559  tf_listener->transform_point(__cfg_frame_openrave, target_local, target);
560 
561  __goto_openrave_thread->set_target(target.getX(), target.getY(), target.getZ(),
562  __cfg_park_phi, __cfg_park_theta, __cfg_park_psi);
563  #ifdef EARLY_PLANNING
564  __goto_openrave_thread->plan_target();
565  #endif
566  start_motion(__goto_openrave_thread, msg->id(), "Parking arm");
567 #endif
568  } else {
569  __goto_thread->set_target(__cfg_park_x, __cfg_park_y, __cfg_park_z,
570  __cfg_park_phi, __cfg_park_theta, __cfg_park_psi);
571  start_motion(__goto_thread, msg->id(), "Parking arm");
572  }
573 
574  } else if (__katana_if->msgq_first_is<KatanaInterface::OpenGripperMessage>()) {
575  KatanaInterface::OpenGripperMessage *msg = __katana_if->msgq_first(msg);
576  __gripper_thread->set_mode(KatanaGripperThread::OPEN_GRIPPER);
577  start_motion(__gripper_thread, msg->id(), "Opening gripper");
578 
579  } else if (__katana_if->msgq_first_is<KatanaInterface::CloseGripperMessage>()) {
580  KatanaInterface::CloseGripperMessage *msg = __katana_if->msgq_first(msg);
581  __gripper_thread->set_mode(KatanaGripperThread::CLOSE_GRIPPER);
582  start_motion(__gripper_thread, msg->id(), "Closing gripper");
583 
584  } else if (__katana_if->msgq_first_is<KatanaInterface::SetEnabledMessage>()) {
585  KatanaInterface::SetEnabledMessage *msg = __katana_if->msgq_first(msg);
586 
587  try {
588  if (msg->is_enabled()) {
589  logger->log_debug(name(), "Turning ON the arm");
590  __katana->turn_on();
591  update_position(/* refresh */ true);
592  update_motors(/* refresh */ true);
593 #ifdef HAVE_OPENRAVE
594  if(__cfg_OR_enabled)
595  {__goto_openrave_thread->update_openrave_data();}
596 #endif
597  } else {
598  logger->log_debug(name(), "Turning OFF the arm");
599  __katana->turn_off();
600  }
601  __katana_if->set_enabled(msg->is_enabled());
602  } catch (/*KNI*/::Exception &e) {
603  logger->log_warn(name(), "Failed enable/disable arm: %s", e.what());
604  }
605 
606  } else if (__katana_if->msgq_first_is<KatanaInterface::SetMaxVelocityMessage>()) {
607  KatanaInterface::SetMaxVelocityMessage *msg = __katana_if->msgq_first(msg);
608 
609  unsigned int max_vel = msg->max_velocity();
610  if ( max_vel == 0 ) max_vel = __cfg_defmax_speed;
611 
612  try {
613  __katana->set_max_velocity(max_vel);
614  } catch (fawkes::Exception &e) {
615  logger->log_warn(name(), "Failed setting max velocity. Ex:%s", e.what());
616  }
617  __katana_if->set_max_velocity(max_vel);
618 
619  } else if (__katana_if->msgq_first_is<KatanaInterface::SetPlannerParamsMessage>()) {
620  KatanaInterface::SetPlannerParamsMessage *msg = __katana_if->msgq_first(msg);
621 
622  if( __cfg_OR_enabled ) {
623 #ifdef HAVE_OPENRAVE
624  __goto_openrave_thread->set_plannerparams(msg->plannerparams());
625 #endif
626  }
627 
628  } else if (__katana_if->msgq_first_is<KatanaInterface::SetMotorEncoderMessage>()) {
629  KatanaInterface::SetMotorEncoderMessage *msg = __katana_if->msgq_first(msg);
630 
631  __motor_control_thread->set_encoder(msg->nr(), msg->enc(), false);
632  start_motion(__motor_control_thread, msg->id(), "Moving motor");
633 
634  } else if (__katana_if->msgq_first_is<KatanaInterface::MoveMotorEncoderMessage>()) {
635  KatanaInterface::MoveMotorEncoderMessage *msg = __katana_if->msgq_first(msg);
636 
637  __motor_control_thread->set_encoder(msg->nr(), msg->enc(), true);
638  start_motion(__motor_control_thread, msg->id(), "Moving motor");
639 
640  } else if (__katana_if->msgq_first_is<KatanaInterface::SetMotorAngleMessage>()) {
641  KatanaInterface::SetMotorAngleMessage *msg = __katana_if->msgq_first(msg);
642 
643  __motor_control_thread->set_angle(msg->nr(), msg->angle(), false);
644  start_motion(__motor_control_thread, msg->id(), "Moving motor");
645 
646  } else if (__katana_if->msgq_first_is<KatanaInterface::MoveMotorAngleMessage>()) {
647  KatanaInterface::MoveMotorAngleMessage *msg = __katana_if->msgq_first(msg);
648 
649  __motor_control_thread->set_angle(msg->nr(), msg->angle(), true);
650  start_motion(__motor_control_thread, msg->id(), "Moving motor");
651 
652  } else {
653  logger->log_warn(name(), "Unknown message received");
654  }
655 
656  __katana_if->msgq_pop();
657  }
658 
659  __katana_if->write();
660 
661 #ifdef USE_TIMETRACKER
662  if (++__tt_count > 100) {
663  __tt_count = 0;
664  __tt->print_to_stdout();
665  }
666 #endif
667 }
668 
669 
670 bool
672  Message *message) throw()
673 {
674  if (message->is_of_type<KatanaInterface::StopMessage>()) {
675  stop_motion();
676  return false; // do not enqueue StopMessage
677  } else if (message->is_of_type<KatanaInterface::FlushMessage>()) {
678  stop_motion();
679  logger->log_info(name(), "Flushing message queue");
680  __katana_if->msgq_flush();
681  return false;
682  } else {
683  logger->log_info(name(), "Received message of type %s, enqueueing", message->type());
684  return true;
685  }
686 }