Fawkes API Fawkes Development Version
info_thread.cpp
1
2/***************************************************************************
3 * info_thread.cpp - Kinova Jaco plugin information thread
4 *
5 * Created: Thu Jun 13 19:14:20 2013
6 * Copyright 2013 Bahram Maleki-Fard
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 "info_thread.h"
24
25#include "arm.h"
26#include "types.h"
27
28#include <interfaces/JacoInterface.h>
29
30using namespace fawkes;
31
32/** @class JacoInfoThread "info_thread.h"
33 * Jaco Arm information thread.
34 * This thread basically provides all informationen to interfaces.
35 *
36 * @author Bahram Maleki-Fard
37 */
38
39/** Constructor.
40 * @param name thread name
41 * @param arm pointer to jaco_arm_t struct, to be used in this thread
42 */
44: Thread(name, Thread::OPMODE_WAITFORWAKEUP),
45 BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_PROCESS)
46{
47 arm_ = arm;
48}
49
50/** Destructor. */
52{
53}
54
55void
57{
58}
59
60void
62{
63 arm_ = NULL;
64}
65
66void
68{
69 if (arm_ == NULL || arm_->arm == NULL || arm_->iface == NULL)
70 return;
71
72 arm_->iface->set_connected(true);
73
74 try {
75 if (arm_->iface->is_final()) {
76 arm_->arm->get_coords(cpos_);
77 arm_->iface->set_x(cpos_.at(0));
78 arm_->iface->set_y(cpos_.at(1));
79 arm_->iface->set_z(cpos_.at(2));
80 arm_->iface->set_euler1(cpos_.at(3));
81 arm_->iface->set_euler2(cpos_.at(4));
82 arm_->iface->set_euler3(cpos_.at(5));
83 }
84
85 arm_->arm->get_fingers(cpos_);
86 arm_->iface->set_finger1(std::max(0.f, std::min(60.f, cpos_.at(0))));
87 arm_->iface->set_finger2(std::max(0.f, std::min(60.f, cpos_.at(1))));
88 arm_->iface->set_finger3(std::max(0.f, std::min(60.f, cpos_.at(2))));
89
90 arm_->arm->get_joints(apos_);
91 for (unsigned int i = 0; i < apos_.size(); i++) {
92 arm_->iface->set_joints(i, apos_.at(i));
93 }
94
95 } catch (fawkes::Exception &e) {
96 logger->log_warn(name(), "Could not get position and joint values. Er: %s", e.what());
97 }
98}
virtual void init()
Initialize the thread.
Definition: info_thread.cpp:56
virtual void loop()
Code to execute in the thread.
Definition: info_thread.cpp:67
virtual void finalize()
Finalize the thread.
Definition: info_thread.cpp:61
virtual ~JacoInfoThread()
Destructor.
Definition: info_thread.cpp:51
JacoInfoThread(const char *name, fawkes::jaco_arm_t *arm)
Constructor.
Definition: info_thread.cpp:43
Thread aspect to use blocked timing.
Base class for exceptions in Fawkes.
Definition: exception.h:36
virtual const char * what() const noexcept
Get primary string.
Definition: exception.cpp:639
virtual void get_joints(std::vector< float > &to) const =0
Get the joint angles of the arm.
virtual void get_fingers(std::vector< float > &to) const =0
Get the position values of the fingers.
virtual void get_coords(std::vector< float > &to)=0
Get the cartesian coordinates of the arm.
void set_finger2(const float new_finger2)
Set finger2 value.
void set_connected(const bool new_connected)
Set connected value.
void set_euler3(const float new_euler3)
Set euler3 value.
void set_z(const float new_z)
Set z value.
void set_y(const float new_y)
Set y value.
void set_finger1(const float new_finger1)
Set finger1 value.
void set_finger3(const float new_finger3)
Set finger3 value.
void set_euler1(const float new_euler1)
Set euler1 value.
void set_x(const float new_x)
Set x value.
void set_euler2(const float new_euler2)
Set euler2 value.
bool is_final() const
Get final value.
void set_joints(unsigned int index, const float new_joints)
Set joints value at given index.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
Thread class encapsulation of pthreads.
Definition: thread.h:46
const char * name() const
Get name of thread.
Definition: thread.h:100
Fawkes library namespace.
Jaco struct containing all components required for one arm.
Definition: types.h:93
fawkes::JacoArm * arm
pointer to actual JacoArm instance, controlling this arm
Definition: types.h:95
JacoInterface * iface
pointer to JacoInterface, assigned to this arm
Definition: types.h:96