Fawkes API  Fawkes Development Version
act_thread.h
1 
2 /***************************************************************************
3  * act_thread.h - Katana plugin act thread
4  *
5  * Created: Mon Jun 08 17:59:57 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 #ifndef _PLUGINS_KATANA_ACT_THREAD_H_
25 #define _PLUGINS_KATANA_ACT_THREAD_H_
26 
27 #include <aspect/blackboard.h>
28 #include <aspect/blocked_timing.h>
29 #include <aspect/clock.h>
30 #include <aspect/configurable.h>
31 #include <aspect/logging.h>
32 #include <aspect/tf.h>
33 #include <core/threading/thread.h>
34 #ifdef HAVE_OPENRAVE
35 # include <plugins/openrave/aspect/openrave.h>
36 #endif
37 #include <blackboard/interface_listener.h>
38 #include <core/utils/refptr.h>
39 #ifdef USE_TIMETRACKER
40 # include <utils/time/tracker.h>
41 #endif
42 #include <memory>
43 #include <string>
44 #include <vector>
45 
46 namespace fawkes {
47 class KatanaInterface;
48 class JointInterface;
49 class Time;
50 class KatanaController;
51 } // namespace fawkes
52 
54 class KatanaMotionThread;
56 class KatanaGotoThread;
60 
61 class KatanaActThread : public fawkes::Thread,
62  public fawkes::ClockAspect,
64  public fawkes::LoggingAspect,
68 #ifdef HAVE_OPENRAVE
70 #endif
72 {
73 public:
76 
77  virtual void init();
78  virtual void finalize();
79  virtual void once();
80  virtual void loop();
81 
82  // For BlackBoardInterfaceListener
83  virtual bool bb_interface_message_received(fawkes::Interface *interface,
84  fawkes::Message * message) throw();
85 
86  void update_sensor_values();
87 
88  /** Stub to see name in backtrace for easier debugging. @see Thread::run() */
89 protected:
90  virtual void
91  run()
92  {
93  Thread::run();
94  }
95 
96 private:
97  void stop_motion();
98  void update_position(bool refresh);
99  void update_sensors(bool refresh);
100  void update_motors(bool refresh);
101  void start_motion(fawkes::RefPtr<KatanaMotionThread> motion_thread,
102  unsigned int msgid,
103  const char * logmsg,
104  ...);
105 
106 private:
107  fawkes::KatanaInterface * katana_if_;
108  std::vector<fawkes::JointInterface *> *joint_ifs_;
109 
110  std::string cfg_controller_;
111  std::string cfg_device_;
112  std::string cfg_kni_conffile_;
113  bool cfg_auto_calibrate_;
114  unsigned int cfg_defmax_speed_;
115  unsigned int cfg_read_timeout_;
116  unsigned int cfg_write_timeout_;
117  unsigned int cfg_gripper_pollint_;
118  unsigned int cfg_goto_pollint_;
119  float cfg_park_x_;
120  float cfg_park_y_;
121  float cfg_park_z_;
122  float cfg_park_phi_;
123  float cfg_park_theta_;
124  float cfg_park_psi_;
125 
126  float cfg_distance_scale_;
127 
128  float cfg_update_interval_;
129 
130  std::string cfg_frame_kni_;
131  std::string cfg_frame_gripper_;
132  std::string cfg_frame_openrave_;
133 
134  bool cfg_OR_enabled_;
135 #ifdef HAVE_OPENRAVE
136  bool cfg_OR_use_viewer_;
137  bool cfg_OR_auto_load_ik_;
138  std::string cfg_OR_robot_file_;
139  std::string cfg_OR_arm_model_;
140 #endif
141 
143  fawkes::RefPtr<KatanaMotionThread> actmot_thread_;
146  fawkes::RefPtr<KatanaGripperThread> gripper_thread_;
147  fawkes::RefPtr<KatanaMotorControlThread> motor_control_thread_;
148 #ifdef HAVE_OPENRAVE
149  fawkes::RefPtr<KatanaGotoOpenRaveThread> goto_openrave_thread_;
150 #endif
151 
153 
154  fawkes::Time *last_update_;
155 
156 #ifdef USE_TIMETRACKER
158  unsigned int tt_count_;
159  unsigned int ttc_read_sensor_;
160 #endif
161 };
162 
163 #endif
KatanaActThread::update_sensor_values
void update_sensor_values()
Update sensor values as necessary.
Definition: act_thread.cpp:372
KatanaActThread::init
virtual void init()
Initialize the thread.
Definition: act_thread.cpp:80
KatanaGotoOpenRaveThread
class KatanaGotoOpenRaveThread
Definition: goto_openrave_thread.h:40
KatanaGripperThread
Definition: gripper_thread.h:27
KatanaActThread::run
virtual void run()
Stub to see name in backtrace for easier debugging.
Definition: act_thread.h:90
KatanaMotionThread
Definition: motion_thread.h:34
KatanaActThread::loop
virtual void loop()
Code to execute in the thread.
Definition: act_thread.cpp:477
fawkes::Message
Definition: message.h:40
fawkes::RefPtr< KatanaMotionThread >
KatanaActThread
Definition: act_thread.h:60
fawkes::BlackBoardInterfaceListener
Definition: interface_listener.h:45
KatanaActThread::finalize
virtual void finalize()
Finalize the thread.
Definition: act_thread.cpp:233
fawkes::BlockedTimingAspect
Definition: blocked_timing.h:54
KatanaMotorControlThread
Definition: motor_control_thread.h:27
KatanaGotoThread
Definition: goto_thread.h:29
KatanaActThread::KatanaActThread
KatanaActThread()
Constructor.
Definition: act_thread.cpp:64
fawkes::BlackBoardAspect
Definition: blackboard.h:36
fawkes
fawkes::LoggingAspect
Definition: logging.h:36
fawkes::KatanaInterface
Definition: KatanaInterface.h:37
fawkes::TransformAspect
Definition: tf.h:41
fawkes::Interface
Definition: interface.h:77
fawkes::Time
Definition: time.h:96
KatanaSensorAcquisitionThread
Definition: sensacq_thread.h:33
KatanaActThread::bb_interface_message_received
virtual bool bb_interface_message_received(fawkes::Interface *interface, fawkes::Message *message)
BlackBoard message received notification.
Definition: act_thread.cpp:797
fawkes::Thread
Definition: thread.h:44
KatanaCalibrationThread
Definition: calib_thread.h:27
fawkes::ConfigurableAspect
Definition: configurable.h:36
KatanaActThread::once
virtual void once()
Execute an action exactly once.
Definition: act_thread.cpp:286
fawkes::ClockAspect
Definition: clock.h:38
fawkes::OpenRaveAspect
Definition: openrave.h:38
KatanaActThread::~KatanaActThread
~KatanaActThread()
Destructor.
Definition: act_thread.cpp:74