Fawkes API  Fawkes Development Version
openrobotino_com_thread.h
1 /***************************************************************************
2  * com_thread.h - Robotino com thread
3  *
4  * Created: Thu Sep 11 11:43:42 2014
5  * Copyright 2011-2014 Tim Niemueller [www.niemueller.de]
6  ****************************************************************************/
7 
8 /* This program is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation; either version 2 of the License, or
11  * (at your option) any later version.
12  *
13  * This program is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU Library General Public License for more details.
17  *
18  * Read the full text in the LICENSE.GPL file in the doc directory.
19  */
20 
21 #ifndef _PLUGINS_ROBOTINO_OPENROBOTINO_COM_THREAD_H_
22 #define _PLUGINS_ROBOTINO_OPENROBOTINO_COM_THREAD_H_
23 
24 #include "com_thread.h"
25 
26 #include <aspect/blackboard.h>
27 #include <aspect/configurable.h>
28 #include <core/threading/thread.h>
29 #include <utils/time/time.h>
30 
31 #ifdef HAVE_OPENROBOTINO_API_1
32 # include <rec/robotino/com/Com.h>
33 namespace rec {
34 namespace sharedmemory {
35 template <typename SharedType>
36 class SharedMemory;
37 }
38 namespace iocontrol {
39 namespace robotstate {
40 class State;
41 }
42 namespace remotestate {
43 class SetState;
44 }
45 } // namespace iocontrol
46 } // namespace rec
47 #else
48 namespace rec {
49 namespace robotino {
50 namespace api2 {
51 class Com;
52 class AnalogInputArray;
53 class Bumper;
54 class DigitalInputArray;
55 class DistanceSensorArray;
56 class ElectricalGripper;
57 class Gyroscope;
58 class MotorArray;
59 class Odometry;
60 class PowerManagement;
61 } // namespace api2
62 } // namespace robotino
63 } // namespace rec
64 #endif
65 
66 namespace fawkes {
67 class Mutex;
68 class Clock;
69 class TimeWait;
70 } // namespace fawkes
71 
73 #ifdef HAVE_OPENROBOTINO_API_1
74  public rec::robotino::com::Com,
75 #endif
77 {
78 public:
80  virtual ~OpenRobotinoComThread();
81 
82  virtual void init();
83  virtual void once();
84  virtual void loop();
85  virtual void finalize();
86 
87  virtual bool is_connected();
88 
89  virtual void set_gripper(bool opened);
90  virtual bool is_gripper_open();
91  virtual void set_speed_points(float s1, float s2, float s3);
92  virtual void
93  get_act_velocity(float &a1, float &a2, float &a3, unsigned int &seq, fawkes::Time &t);
94  virtual void get_odometry(double &x, double &y, double &phi);
95 
96  virtual void reset_odometry();
97  virtual void set_bumper_estop_enabled(bool enabled);
98  virtual void set_motor_accel_limits(float min_accel, float max_accel);
99  virtual void set_digital_output(unsigned int digital_out, bool enable);
100 
101  /** Stub to see name in backtrace for easier debugging. @see Thread::run() */
102 protected:
103  virtual void
104  run()
105  {
106  Thread::run();
107  }
108 
109 private:
110 #ifdef HAVE_OPENROBOTINO_API_1
111  using rec::robotino::com::Com::sensorState;
112  virtual void updateEvent();
113 #endif
114  void process_api_v1();
115  void process_api_v2();
116 
117 private:
118  std::string cfg_hostname_;
119  bool cfg_quit_on_disconnect_;
120  unsigned int cfg_sensor_update_cycle_time_;
121  bool cfg_gripper_enabled_;
122  bool cfg_enable_gyro_;
123 
124  fawkes::TimeWait *time_wait_;
125  unsigned int last_seqnum_;
126 
127 #ifdef HAVE_OPENROBOTINO_API_1
128  rec::robotino::com::Com * com_;
129  fawkes::Mutex * state_mutex_;
130  unsigned int active_state_;
131  rec::iocontrol::remotestate::SensorState sensor_states_[2];
132  fawkes::Time times_[2];
133 
134  rec::sharedmemory::SharedMemory<rec::iocontrol::robotstate::State> *statemem_;
135  rec::iocontrol::robotstate::State * state_;
136 
137  rec::iocontrol::remotestate::SetState *set_state_;
138 
139 #else
140  rec::robotino::api2::Com * com_;
141  rec::robotino::api2::AnalogInputArray * analog_inputs_com_;
142  rec::robotino::api2::Bumper * bumper_com_;
143  rec::robotino::api2::DigitalInputArray * digital_inputs_com_;
144  rec::robotino::api2::DistanceSensorArray *distances_com_;
145  rec::robotino::api2::ElectricalGripper * gripper_com_;
146  rec::robotino::api2::Gyroscope * gyroscope_com_;
147  rec::robotino::api2::MotorArray * motors_com_;
148  rec::robotino::api2::Odometry * odom_com_;
149  rec::robotino::api2::PowerManagement * power_com_;
150 #endif
151 };
152 
153 #endif
OpenRobotinoComThread::set_speed_points
virtual void set_speed_points(float s1, float s2, float s3)
Set speed points for wheels.
Definition: openrobotino_com_thread.cpp:496
fawkes::Mutex
Definition: mutex.h:38
OpenRobotinoComThread::OpenRobotinoComThread
OpenRobotinoComThread()
Constructor.
Definition: openrobotino_com_thread.cpp:57
OpenRobotinoComThread::get_odometry
virtual void get_odometry(double &x, double &y, double &phi)
Get current odometry.
Definition: openrobotino_com_thread.cpp:452
OpenRobotinoComThread
Definition: openrobotino_com_thread.h:72
OpenRobotinoComThread::once
virtual void once()
Execute an action exactly once.
Definition: openrobotino_com_thread.cpp:169
OpenRobotinoComThread::is_gripper_open
virtual bool is_gripper_open()
Check if gripper is open.
Definition: openrobotino_com_thread.cpp:475
OpenRobotinoComThread::finalize
virtual void finalize()
Finalize the thread.
Definition: openrobotino_com_thread.cpp:138
OpenRobotinoComThread::set_motor_accel_limits
virtual void set_motor_accel_limits(float min_accel, float max_accel)
Definition: openrobotino_com_thread.cpp:383
OpenRobotinoComThread::set_digital_output
virtual void set_digital_output(unsigned int digital_out, bool enable)
Definition: openrobotino_com_thread.cpp:390
OpenRobotinoComThread::get_act_velocity
virtual void get_act_velocity(float &a1, float &a2, float &a3, unsigned int &seq, fawkes::Time &t)
Get actual velocity.
Definition: openrobotino_com_thread.cpp:412
OpenRobotinoComThread::loop
virtual void loop()
Code to execute in the thread.
Definition: openrobotino_com_thread.cpp:175
fawkes
RobotinoComThread
Definition: com_thread.h:38
OpenRobotinoComThread::reset_odometry
virtual void reset_odometry()
Reset odometry to zero.
Definition: openrobotino_com_thread.cpp:368
OpenRobotinoComThread::set_bumper_estop_enabled
virtual void set_bumper_estop_enabled(bool enabled)
Definition: openrobotino_com_thread.cpp:529
OpenRobotinoComThread::run
virtual void run()
Stub to see name in backtrace for easier debugging.
Definition: openrobotino_com_thread.h:104
fawkes::Time
Definition: time.h:98
OpenRobotinoComThread::~OpenRobotinoComThread
virtual ~OpenRobotinoComThread()
Destructor.
Definition: openrobotino_com_thread.cpp:65
OpenRobotinoComThread::init
virtual void init()
Initialize the thread.
Definition: openrobotino_com_thread.cpp:70
OpenRobotinoComThread::is_connected
virtual bool is_connected()
Check if we are connected to OpenRobotino.
Definition: openrobotino_com_thread.cpp:399
fawkes::TimeWait
Definition: wait.h:38
fawkes::ConfigurableAspect
Definition: configurable.h:38
OpenRobotinoComThread::set_gripper
virtual void set_gripper(bool opened)
Open or close gripper.
Definition: openrobotino_com_thread.cpp:514