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 
30 using 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 
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 }
fawkes::JacoInterface::set_finger1
void set_finger1(const float new_finger1)
Set finger1 value.
Definition: JacoInterface.cpp:434
fawkes::JacoInterface::is_final
bool is_final() const
Get final value.
Definition: JacoInterface.cpp:541
fawkes::JacoInterface::set_x
void set_x(const float new_x)
Set x value.
Definition: JacoInterface.cpp:188
fawkes::JacoInterface::set_connected
void set_connected(const bool new_connected)
Set connected value.
Definition: JacoInterface.cpp:126
fawkes::JacoInterface::set_y
void set_y(const float new_y)
Set y value.
Definition: JacoInterface.cpp:219
fawkes::BlockedTimingAspect
Definition: blocked_timing.h:56
fawkes::Thread::name
const char * name() const
Definition: thread.h:100
fawkes::jaco_arm_struct::iface
JacoInterface * iface
pointer to JacoInterface, assigned to this arm
Definition: types.h:96
fawkes::JacoArm::get_coords
virtual void get_coords(std::vector< float > &to)=0
Get the cartesian coordinates of the arm.
fawkes::LoggingAspect::logger
Logger * logger
Definition: logging.h:53
fawkes::JacoInterface::set_euler3
void set_euler3(const float new_euler3)
Set euler3 value.
Definition: JacoInterface.cpp:343
fawkes
fawkes::jaco_arm_struct::arm
fawkes::JacoArm * arm
pointer to actual JacoArm instance, controlling this arm
Definition: types.h:95
fawkes::JacoInterface::set_joints
void set_joints(unsigned int index, const float new_joints)
Set joints value at given index.
Definition: JacoInterface.cpp:401
JacoInfoThread::~JacoInfoThread
virtual ~JacoInfoThread()
Destructor.
Definition: info_thread.cpp:51
fawkes::Logger::log_warn
virtual void log_warn(const char *component, const char *format,...)=0
fawkes::JacoInterface::set_z
void set_z(const float new_z)
Set z value.
Definition: JacoInterface.cpp:250
fawkes::JacoInterface::set_euler1
void set_euler1(const float new_euler1)
Set euler1 value.
Definition: JacoInterface.cpp:281
JacoInfoThread::finalize
virtual void finalize()
Finalize the thread.
Definition: info_thread.cpp:61
fawkes::JacoInterface::set_finger2
void set_finger2(const float new_finger2)
Set finger2 value.
Definition: JacoInterface.cpp:465
fawkes::JacoArm::get_joints
virtual void get_joints(std::vector< float > &to) const =0
Get the joint angles of the arm.
JacoInfoThread::JacoInfoThread
JacoInfoThread(const char *name, fawkes::jaco_arm_t *arm)
Constructor.
Definition: info_thread.cpp:43
fawkes::Exception::what
virtual const char * what() const
Get primary string.
Definition: exception.cpp:639
JacoInfoThread::init
virtual void init()
Initialize the thread.
Definition: info_thread.cpp:56
fawkes::Thread
Definition: thread.h:45
JacoInfoThread::loop
virtual void loop()
Code to execute in the thread.
Definition: info_thread.cpp:67
fawkes::jaco_arm_struct
Jaco struct containing all components required for one arm.
Definition: types.h:92
fawkes::JacoInterface::set_euler2
void set_euler2(const float new_euler2)
Set euler2 value.
Definition: JacoInterface.cpp:312
fawkes::JacoInterface::set_finger3
void set_finger3(const float new_finger3)
Set finger3 value.
Definition: JacoInterface.cpp:496
fawkes::JacoArm::get_fingers
virtual void get_fingers(std::vector< float > &to) const =0
Get the position values of the fingers.
fawkes::Exception
Definition: exception.h:41