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 #include "types.h"
25 #include "arm.h"
26 
27 #include <interfaces/JacoInterface.h>
28 
29 using namespace fawkes;
30 
31 /** @class JacoInfoThread "info_thread.h"
32  * Jaco Arm information thread.
33  * This thread basically provides all informationen to interfaces.
34  *
35  * @author Bahram Maleki-Fard
36  */
37 
38 /** Constructor.
39  * @param name thread name
40  * @param arm pointer to jaco_arm_t struct, to be used in this thread
41  */
43  : Thread(name, Thread::OPMODE_WAITFORWAKEUP),
44  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_PROCESS)
45 {
46  __arm = arm;
47 }
48 
49 
50 /** Destructor. */
52 {
53 }
54 
55 void
57 {
58 }
59 
60 void
62 {
63  __arm = NULL;
64 }
65 
66 void
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 }
Jaco struct containing all components required for one arm.
Definition: types.h:95
void set_y(const float new_y)
Set y value.
virtual ~JacoInfoThread()
Destructor.
Definition: info_thread.cpp:51
Fawkes library namespace.
fawkes::JacoArm * arm
pointer to actual JacoArm instance, controlling this arm
Definition: types.h:97
void set_finger1(const float new_finger1)
Set finger1 value.
virtual const char * what() const
Get primary string.
Definition: exception.cpp:661
Thread class encapsulation of pthreads.
Definition: thread.h:42
virtual void get_coords(std::vector< float > &to)=0
Get the cartesian coordinates of the arm.
bool is_final() const
Get final value.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:44
void set_connected(const bool new_connected)
Set connected value.
JacoInterface * iface
pointer to JacoInterface, assigned to this arm
Definition: types.h:98
Thread aspect to use blocked timing.
JacoInfoThread(const char *name, fawkes::jaco_arm_t *arm)
Constructor.
Definition: info_thread.cpp:42
Base class for exceptions in Fawkes.
Definition: exception.h:36
virtual void get_joints(std::vector< float > &to) const =0
Get the joint angles of the arm.
const char * name() const
Get name of thread.
Definition: thread.h:95
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void finalize()
Finalize the thread.
Definition: info_thread.cpp:61
void set_joints(unsigned int index, const float new_joints)
Set joints value at given index.
void set_finger3(const float new_finger3)
Set finger3 value.
void set_euler3(const float new_euler3)
Set euler3 value.
void set_z(const float new_z)
Set z value.
void set_euler1(const float new_euler1)
Set euler1 value.
virtual void init()
Initialize the thread.
Definition: info_thread.cpp:56
virtual void get_fingers(std::vector< float > &to) const =0
Get the position values of the fingers.
void set_finger2(const float new_finger2)
Set finger2 value.
void set_x(const float new_x)
Set x value.
virtual void loop()
Code to execute in the thread.
Definition: info_thread.cpp:67
void set_euler2(const float new_euler2)
Set euler2 value.