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
51using namespace fawkes;
52#ifdef HAVE_TF
53using 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
79void
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") {
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
232void
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
285void
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 */
298void
299KatanaActThread::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_)) {
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 */
371void
373{
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 */
383void
384KatanaActThread::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 */
411void
412KatanaActThread::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 */
440void
441KatanaActThread::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. */
458void
459KatanaActThread::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 }
474}
475
476void
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) {
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>()) {
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>()) {
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
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>()) {
757
758 motor_control_thread_->set_encoder(msg->nr(), msg->enc(), false);
759 start_motion(motor_control_thread_, msg->id(), "Moving motor");
760
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>()) {
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>()) {
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
796bool
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}
virtual void once()
Execute an action exactly once.
Definition: act_thread.cpp:286
virtual void init()
Initialize the thread.
Definition: act_thread.cpp:80
virtual void finalize()
Finalize the thread.
Definition: act_thread.cpp:233
~KatanaActThread()
Destructor.
Definition: act_thread.cpp:74
void update_sensor_values()
Update sensor values as necessary.
Definition: act_thread.cpp:372
virtual bool bb_interface_message_received(fawkes::Interface *interface, fawkes::Message *message) noexcept
BlackBoard message received notification.
Definition: act_thread.cpp:797
KatanaActThread()
Constructor.
Definition: act_thread.cpp:64
virtual void loop()
Code to execute in the thread.
Definition: act_thread.cpp:477
Katana calibration thread.
Definition: calib_thread.h:29
class KatanaGotoOpenRaveThread
Katana linear goto thread.
Definition: goto_thread.h:31
virtual void set_target(float x, float y, float z, float phi, float theta, float psi)
Set target position.
Definition: goto_thread.cpp:63
Katana gripper thread.
void set_mode(gripper_mode_t mode)
Set mode.
@ CLOSE_GRIPPER
Close gripper.
@ OPEN_GRIPPER
Open gripper.
virtual void reset()
Reset for next execution.
unsigned int error_code() const
Error code.
bool finished() const
Did the motion finish already?
Katana motor control thread.
virtual void set_encoder(unsigned int nr, int value, bool inc=false)
Set target encoder value.
virtual void set_angle(unsigned int nr, float value, bool inc=false)
Set target angle value.
Katana sensor acquisition thread.
void set_enabled(bool enabled)
Set whether data acquisition is enabled or not.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
BlackBoard interface listener.
void bbil_add_message_interface(Interface *interface)
Add an interface to the message received watch list.
virtual void unregister_listener(BlackBoardInterfaceListener *listener)
Unregister BB interface listener.
Definition: blackboard.cpp:212
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual void register_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Register BB event listener.
Definition: blackboard.cpp:185
virtual void close(Interface *interface)=0
Close interface.
Thread aspect to use blocked timing.
Clock * clock
By means of this member access to the clock is given.
Definition: clock.h:42
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
virtual unsigned int get_uint(const char *path)=0
Get value from configuration which is of type unsigned int.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
Base class for exceptions in Fawkes.
Definition: exception.h:36
virtual const char * what() const noexcept
Get primary string.
Definition: exception.cpp:639
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:80
bool msgq_first_is()
Check if first message has desired type.
Definition: interface.h:351
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1215
Message * msgq_first()
Get the first message from the message queue.
Definition: interface.cpp:1200
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:501
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1062
JointInterface Fawkes BlackBoard Interface.
Controller class for a Neuronics Katana, using libkni to interact with the real Katana arm.
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.
Controller class for a Neuronics Katana, using libkni to interact with the real Katana arm.
virtual double phi()=0
Get x-coordinate of latest endeffector position.
virtual bool joint_angles()=0
Check if controller provides joint angle values.
virtual void set_max_velocity(unsigned int vel)=0
Set maximum velocity.
virtual double theta()=0
Get theta-rotation of latest endeffector orientation.
virtual double psi()=0
Get psi-rotation of latest endeffector orientation.
virtual double y()=0
Get y-coordinate of latest endeffector position.
virtual void init()=0
Initialize controller.
virtual double z()=0
Get z-coordinate of latest endeffector position.
virtual bool joint_encoders()=0
Check if controller provides joint encoder values.
virtual double x()=0
Get x-coordinate of latest endeffector position.
virtual void turn_on()=0
Turn on arm/motors.
virtual void get_sensors(std::vector< int > &to, bool refresh=false)=0
Get sensor values.
virtual void get_encoders(std::vector< int > &to, bool refresh=false)=0
Get encoder values of joints/motors.
virtual void stop()=0
Stop movement immediately.
virtual void get_angles(std::vector< float > &to, bool refresh=false)=0
Get angle values of joints/motors.
virtual void read_coordinates(bool refresh=false)=0
Store current coordinates of endeeffctor.
virtual void turn_off()=0
Turn off arm/motors.
CalibrateMessage Fawkes BlackBoard Interface Message.
CloseGripperMessage Fawkes BlackBoard Interface Message.
FlushMessage Fawkes BlackBoard Interface Message.
LinearGotoKniMessage Fawkes BlackBoard Interface Message.
LinearGotoMessage Fawkes BlackBoard Interface Message.
char * trans_frame() const
Get trans_frame value.
float offset_xy() const
Get offset_xy value.
float theta_error() const
Get theta_error value.
char * rot_frame() const
Get rot_frame value.
bool is_straight() const
Get straight value.
MoveMotorAngleMessage Fawkes BlackBoard Interface Message.
MoveMotorEncoderMessage Fawkes BlackBoard Interface Message.
ObjectGotoMessage Fawkes BlackBoard Interface Message.
char * object() const
Get object value.
OpenGripperMessage Fawkes BlackBoard Interface Message.
ParkMessage Fawkes BlackBoard Interface Message.
SetEnabledMessage Fawkes BlackBoard Interface Message.
bool is_enabled() const
Get enabled value.
SetMaxVelocityMessage Fawkes BlackBoard Interface Message.
uint8_t max_velocity() const
Get max_velocity value.
SetMotorAngleMessage Fawkes BlackBoard Interface Message.
SetMotorEncoderMessage Fawkes BlackBoard Interface Message.
SetPlannerParamsMessage Fawkes BlackBoard Interface Message.
char * plannerparams() const
Get plannerparams value.
StopMessage Fawkes BlackBoard Interface Message.
KatanaInterface Fawkes BlackBoard Interface.
void set_error_code(const uint32_t new_error_code)
Set error_code value.
bool is_enabled() const
Get enabled value.
void set_calibrated(const bool new_calibrated)
Set calibrated value.
void set_sensor_value(unsigned int index, const uint8_t new_sensor_value)
Set sensor_value value at given index.
void set_final(const bool new_final)
Set final value.
void set_phi(const float new_phi)
Set phi value.
void set_encoders(unsigned int index, const int32_t new_encoders)
Set encoders value at given index.
float * angles() const
Get angles value.
void set_msgid(const uint32_t new_msgid)
Set msgid value.
void set_max_velocity(const uint8_t new_max_velocity)
Set max_velocity value.
void set_theta(const float new_theta)
Set theta value.
void set_y(const float new_y)
Set y value.
void set_z(const float new_z)
Set z value.
size_t maxlenof_sensor_value() const
Get maximum length of sensor_value value.
void set_enabled(const bool new_enabled)
Set enabled value.
void set_x(const float new_x)
Set x value.
void set_psi(const float new_psi)
Set psi value.
void set_angles(unsigned int index, const float new_angles)
Set angles value at given index.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void vlog_debug(const char *component, const char *format, va_list va)=0
Log debug message.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
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:181
virtual void log_info(const char *component, const char *format,...)
Log informational message.
Definition: multi.cpp:195
Mutex locking helper.
Definition: mutex_locker.h:34
void lock()
Lock this mutex.
Definition: mutex.cpp:87
void unlock()
Unlock the mutex.
Definition: mutex.cpp:131
void reset()
Reset pointer.
Definition: refptr.h:455
Thread class encapsulation of pthreads.
Definition: thread.h:46
Mutex * loop_mutex
Mutex that is used to protect a call to loop().
Definition: thread.h:152
const char * name() const
Get name of thread.
Definition: thread.h:100
void start(bool wait=true)
Call this method to start the thread.
Definition: thread.cpp:499
void join()
Join the thread.
Definition: thread.cpp:597
void wakeup()
Wake up thread.
Definition: thread.cpp:995
void cancel()
Cancel a thread.
Definition: thread.cpp:646
virtual void finalize()
Finalize the thread.
Definition: thread.cpp:463
Time tracking utility.
Definition: tracker.h:37
A class for handling time.
Definition: time.h:93
void set_clock(Clock *clock)
Set clock for this instance.
Definition: time.cpp:308
Time & stamp()
Set this time to the current time.
Definition: time.cpp:704
void set_time(const timeval *tv)
Sets the time.
Definition: time.cpp:246
Thread aspect to access the transform system.
Definition: tf.h:39
tf::Transformer * tf_listener
This is the transform listener which saves transforms published by other threads in the system.
Definition: tf.h:67
Wrapper class to add time stamp and frame ID to base types.
Definition: types.h:130
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.
bool frame_exists(const std::string &frame_id_str) const
Check if frame exists.
Fawkes library namespace.