24 #include "act_thread.h"
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"
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>
43 # include <plugins/openrave/manipulator.h>
44 # include <plugins/openrave/robot.h>
53 using namespace fawkes::tf;
65 :
Thread(
"KatanaActThread",
Thread::OPMODE_WAITFORWAKEUP),
70 last_update_ =
new Time();
86 std::string cfg_prefix =
"/hardware/katana/";
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());
101 cfg_park_theta_ =
config->
get_float((cfg_prefix +
"park_theta").c_str());
104 cfg_distance_scale_ =
config->
get_float((cfg_prefix +
"distance_scale").c_str());
106 cfg_update_interval_ =
config->
get_float((cfg_prefix +
"update_interval").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());
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());
119 cfg_OR_enabled_ =
false;
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());
137 if (cfg_controller_ ==
"kni") {
141 kat_ctrl->
setup(cfg_device_, cfg_kni_conffile_, cfg_read_timeout_, cfg_write_timeout_);
147 }
else if (cfg_controller_ ==
"openrave") {
149 if (!cfg_OR_enabled_) {
151 "Cannot use controller 'openrave', OpenRAVE is deactivated by config flag!");
155 throw fawkes::Exception(
"Cannot use controller 'openrave', OpenRAVE not installed!");
159 throw fawkes::Exception(
"Invalid controller given: '%s'", cfg_controller_.c_str());
165 joint_ifs_ =
new std::vector<JointInterface *>();
167 for (
long long i = 0; i < 5; ++i) {
168 joint_name =
config->
get_string((cfg_prefix +
"joints/" + std::to_string(i)).c_str());
170 joint_ifs_->push_back(joint_if);
176 joint_ifs_->push_back(joint_if);
180 joint_ifs_->push_back(joint_if);
202 cfg_OR_auto_load_ik_,
204 if (cfg_OR_enabled_) {
205 goto_openrave_thread_->init();
220 sensacq_thread_->
start();
225 #ifdef USE_TIMETRACKER
228 ttc_read_sensor_ = tt_->add_class(
"Read Sensor");
236 if (actmot_thread_) {
238 actmot_thread_->
join();
239 actmot_thread_ = NULL;
246 sensacq_thread_->
cancel();
247 sensacq_thread_->
join();
248 sensacq_thread_.
reset();
254 calib_thread_ = NULL;
256 gripper_thread_ = NULL;
257 motor_control_thread_ = NULL;
259 if (cfg_OR_enabled_ && goto_openrave_thread_)
261 goto_openrave_thread_ = NULL;
279 for (std::vector<JointInterface *>::iterator it = joint_ifs_->begin(); it != joint_ifs_->end();
288 if (cfg_auto_calibrate_) {
289 start_motion(calib_thread_, 0,
"Auto calibration enabled, calibrating");
299 KatanaActThread::update_position(
bool 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") {
310 "tf frames not existing: '%s'. Skipping transform to kni coordinates.",
311 cfg_frame_openrave_.c_str());
316 cfg_frame_openrave_);
320 katana_if_->
set_x(target.getX());
321 katana_if_->
set_y(target.getY());
322 katana_if_->
set_z(target.getZ());
332 float *a = katana_if_->
angles();
334 joint_ifs_->at(0)->set_position(a[0] + M_PI);
335 joint_ifs_->at(1)->set_position(a[1]);
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();
375 if (actmot_thread_ != calib_thread_) {
376 update_sensors(!actmot_thread_);
384 KatanaActThread::update_sensors(
bool refresh)
387 std::vector<int> sensors;
391 for (
int i = 0; i < num_sensors; ++i) {
392 if (sensors.at(i) <= 0) {
394 }
else if (sensors.at(i) >= 255) {
405 sensacq_thread_->
wakeup();
412 KatanaActThread::update_motors(
bool refresh)
416 std::vector<int> encoders;
418 for (
unsigned int i = 0; i < encoders.size(); i++) {
424 std::vector<float> angles;
426 for (
unsigned int i = 0; i < angles.size(); i++) {
447 va_start(arg, logmsg);
450 actmot_thread_ = motion_thread;
451 actmot_thread_->
start(
false);
459 KatanaActThread::stop_motion()
463 if (actmot_thread_) {
465 actmot_thread_->
join();
466 actmot_thread_ = NULL;
479 if (actmot_thread_) {
480 update_motors(
false);
481 update_position(
false);
487 actmot_thread_->
join();
490 if (actmot_thread_ == calib_thread_) {
493 actmot_thread_->
reset();
494 actmot_thread_ = NULL;
498 update_motors(
true);
499 update_position(
true);
502 if (cfg_OR_enabled_) {
503 goto_openrave_thread_->update_openrave_data();
508 update_position(
true);
509 update_motors(
true);
514 if ((now - last_update_) >= cfg_update_interval_) {
515 last_update_->
stamp();
516 update_position(
false);
517 update_motors(
false);
521 while (!katana_if_->
msgq_empty() && !actmot_thread_) {
524 start_motion(calib_thread_, msg->
id(),
"Calibrating arm");
531 if (!trans_frame_exists || !rot_frame_exists) {
533 "tf frames not existing: '%s%s%s'. Skipping message.",
535 trans_frame_exists ?
"" :
"', '",
536 rot_frame_exists ?
"" : msg->
rot_frame());
542 if (cfg_OR_enabled_) {
546 Vector3 offset(target.getX(), target.getY(), 0.f);
547 offset = (offset / offset.length())
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);
557 goto_openrave_thread_->set_target(
558 target.getX(), target.getY(), target.getZ(), msg->
phi(), msg->
theta(), msg->
psi());
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();
566 goto_openrave_thread_,
568 "Linear movement to (%f,%f,%f, %f,%f,%f), frame '%s', theta_error:%f, straight:%u",
575 cfg_frame_openrave_.c_str(),
582 Vector3 offset(target.getX(), target.getY(), 0.f);
583 offset = (offset / offset.length())
587 goto_thread_->
set_target(target.getX() / cfg_distance_scale_,
588 target.getY() / cfg_distance_scale_,
589 target.getZ() / cfg_distance_scale_,
593 start_motion(goto_thread_,
595 "Linear movement to (%f,%f,%f, %f,%f,%f), frame '%s'",
602 cfg_frame_kni_.c_str());
609 float x = msg->
x() * cfg_distance_scale_;
610 float y = msg->
y() * cfg_distance_scale_;
611 float z = msg->
z() * cfg_distance_scale_;
616 if (cfg_OR_enabled_) {
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();
624 start_motion(goto_openrave_thread_,
626 "Linear movement to (%f,%f,%f, %f,%f,%f), frame '%s'",
633 cfg_frame_openrave_.c_str());
637 msg->
x(), msg->
y(), msg->
z(), msg->
phi(), msg->
theta(), msg->
psi());
639 start_motion(goto_thread_,
641 "Linear movement to (%f,%f,%f, %f,%f,%f), frame '%s'",
648 cfg_frame_kni_.c_str());
657 rot_x = msg->
rot_x();
660 goto_openrave_thread_->set_target(msg->
object(), rot_x);
661 # ifdef EARLY_PLANNING
662 goto_openrave_thread_->plan_target();
664 start_motion(goto_openrave_thread_,
666 "Linear movement to object (%s, %f)",
674 if (cfg_OR_enabled_) {
678 cfg_park_y_ * cfg_distance_scale_,
679 cfg_park_z_ * cfg_distance_scale_),
683 goto_openrave_thread_->set_target(target.getX(),
689 # ifdef EARLY_PLANNING
690 goto_openrave_thread_->plan_target();
692 start_motion(goto_openrave_thread_, msg->
id(),
"Parking arm");
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");
703 start_motion(gripper_thread_, msg->
id(),
"Opening gripper");
708 start_motion(gripper_thread_, msg->
id(),
"Closing gripper");
717 update_position(
true);
718 update_motors(
true);
721 goto_openrave_thread_->update_openrave_data();
737 max_vel = cfg_defmax_speed_;
749 if (cfg_OR_enabled_) {
751 goto_openrave_thread_->set_plannerparams(msg->
plannerparams());
759 start_motion(motor_control_thread_, msg->
id(),
"Moving motor");
765 start_motion(motor_control_thread_, msg->
id(),
"Moving motor");
771 start_motion(motor_control_thread_, msg->
id(),
"Moving motor");
777 start_motion(motor_control_thread_, msg->
id(),
"Moving motor");
788 #ifdef USE_TIMETRACKER
789 if (++tt_count_ > 100) {
791 tt_->print_to_stdout();
804 logger->
log_info(name(),
"Flushing message queue");
805 katana_if_->msgq_flush();
808 logger->
log_info(name(),
"Received message of type %s, enqueueing", message->type());
virtual void once()
Execute an action exactly once.
virtual void init()
Initialize the thread.
virtual void finalize()
Finalize the thread.
~KatanaActThread()
Destructor.
void update_sensor_values()
Update sensor values as necessary.
virtual bool bb_interface_message_received(fawkes::Interface *interface, fawkes::Message *message) noexcept
BlackBoard message received notification.
KatanaActThread()
Constructor.
virtual void loop()
Code to execute in the thread.
Katana calibration thread.
class KatanaGotoOpenRaveThread
Katana linear goto thread.
virtual void set_target(float x, float y, float z, float phi, float theta, float psi)
Set target position.
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.
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.
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.
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.
Configuration * config
This is the Configuration member used to access the configuration.
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.
virtual const char * what() const noexcept
Get primary string.
Base class for all Fawkes BlackBoard interfaces.
bool msgq_first_is()
Check if first message has desired type.
void msgq_pop()
Erase first message from queue.
Message * msgq_first()
Get the first message from the message queue.
void write()
Write from local copy into BlackBoard memory.
bool msgq_empty()
Check if queue is empty.
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.
float y() const
Get y value.
float z() const
Get z value.
float psi() const
Get psi value.
float x() const
Get x value.
float theta() const
Get theta value.
float phi() const
Get phi value.
LinearGotoMessage Fawkes BlackBoard Interface Message.
float z() const
Get z value.
char * trans_frame() const
Get trans_frame value.
float psi() const
Get psi value.
float phi() const
Get phi 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.
float theta() const
Get theta value.
float y() const
Get y value.
float x() const
Get x value.
bool is_straight() const
Get straight value.
MoveMotorAngleMessage Fawkes BlackBoard Interface Message.
uint32_t nr() const
Get nr value.
float angle() const
Get angle value.
MoveMotorEncoderMessage Fawkes BlackBoard Interface Message.
uint32_t nr() const
Get nr value.
uint32_t enc() const
Get enc value.
ObjectGotoMessage Fawkes BlackBoard Interface Message.
char * object() const
Get object value.
float rot_x() const
Get rot_x 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.
uint32_t nr() const
Get nr value.
float angle() const
Get angle value.
SetMotorEncoderMessage Fawkes BlackBoard Interface Message.
uint32_t enc() const
Get enc value.
uint32_t nr() const
Get nr value.
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.
Base class for all messages passed through interfaces in Fawkes BlackBoard.
unsigned int id() const
Get message ID.
virtual void log_info(const char *component, const char *format,...)
Log informational message.
void lock()
Lock this mutex.
void unlock()
Unlock the mutex.
void reset()
Reset pointer.
Thread class encapsulation of pthreads.
Mutex * loop_mutex
Mutex that is used to protect a call to loop().
const char * name() const
Get name of thread.
void start(bool wait=true)
Call this method to start the thread.
void join()
Join the thread.
void wakeup()
Wake up thread.
void cancel()
Cancel a thread.
virtual void finalize()
Finalize the thread.
A class for handling time.
void set_clock(Clock *clock)
Set clock for this instance.
Time & stamp()
Set this time to the current time.
void set_time(const timeval *tv)
Sets the time.
Wrapper class to add time stamp and frame ID to base types.
Fawkes library namespace.