Fawkes API  Fawkes Development Version
colli_thread.h
1 
2 /***************************************************************************
3  * colli_thread.h - Fawkes Colli Thread
4  *
5  * Created: Wed Oct 16 18:00:00 2013
6  * Copyright 2013-2014 Bahram Maleki-Fard
7  * 2014 Tobias Neumann
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_COLLI_COLLI_THREAD_H_
25 #define __PLUGINS_COLLI_COLLI_THREAD_H_
26 
27 #include "common/types.h"
28 
29 #include <core/threading/thread.h>
30 #include <aspect/clock.h>
31 #include <aspect/configurable.h>
32 #include <aspect/logging.h>
33 #include <aspect/blackboard.h>
34 #include <aspect/tf.h>
35 
36 #include <utils/time/clock.h>
37 #include <utils/math/types.h>
38 #include <utils/math/angle.h>
39 
40 namespace fawkes
41 {
42  class Mutex;
43  class TimeWait;
44 
45  class MotorInterface;
46  class Laser360Interface;
47  class NavigatorInterface;
48 
49  class LaserOccupancyGrid;
50  class Search;
51 
52  class SelectDriveMode;
53  class BaseMotorInstruct;
54 }
55 
56 class ColliVisualizationThread;
57 
59 : public fawkes::Thread,
60  public fawkes::ClockAspect,
61  public fawkes::LoggingAspect,
65 {
66  public:
67  ColliThread();
68  virtual ~ColliThread();
69 
70  virtual void init();
71  virtual void loop();
72  virtual void finalize();
73 
74  virtual void set_vis_thread(ColliVisualizationThread* vis_thread);
75 
76  bool is_final() const;
77 
78  void colli_goto(float x, float y, float ori, fawkes::NavigatorInterface* iface);
79  void colli_relgoto(float x, float y, float ori, fawkes::NavigatorInterface* iface);
80  void colli_stop();
81 
82  private:
83  fawkes::Mutex* mutex_;
84  fawkes::TimeWait* timer_;
85 
86  /* ************************************************************************ */
87  /* PRIVATE OBJECT VARIABLES */
88  /* ************************************************************************ */
89  /*
90  * This is a short list of types that have been transformed from RCSoftX->fawkes:
91  *
92  * Mopo_Client -> MotorInterface (motor data)
93  * Laser_Client -> Laser360Interface (laser data)
94  * Colli_Target_Client -> NavigatorInterface (colli target)
95  * Colli_Data_Server -> colli_data_t (colli data)
96  *
97  * Point -> cart_coord_2d_t (point with 2 floats)
98  */
99  fawkes::MotorInterface* if_motor_; // MotorObject
100  fawkes::Laser360Interface* if_laser_; // LaserScannerObject
101  fawkes::NavigatorInterface* if_colli_target_; // TargetObject
102  fawkes::colli_data_t colli_data_; // Colli Data Object
103 
104  fawkes::LaserOccupancyGrid* occ_grid_; // the grid to drive on
105  fawkes::Search* search_; // our plan module which calculates the info
106 
107  fawkes::SelectDriveMode* select_drive_mode_; // the drive mode selection module
108  fawkes::BaseMotorInstruct* motor_instruct_; // the motor instructor module
109  fawkes::BaseMotorInstruct* emergency_motor_instruct_; // the emergency motor instructor module
110 
111  ColliVisualizationThread* vis_thread_; // the VisualizationThread
112 
113  /* ************************************************************************ */
114  /* PRIVATE VARIABLES THAT HAVE TO BE HANDLED ALL OVER THE MODULE */
115  /* ************************************************************************ */
116  fawkes::point_t robo_grid_pos_; // the robots position in the grid
117  fawkes::point_t laser_grid_pos_; // the laser its position in the grid ( not equal to robopos!!! )
118  fawkes::point_t target_grid_pos_; // the targets position in the grid
119 
120  fawkes::point_t local_grid_target_; // the local target in grid
121  fawkes::point_t local_grid_trajec_; // the local trajec in grid
122 
123  fawkes::cart_coord_2d_t local_target_; // the local target (relative)
124  fawkes::cart_coord_2d_t local_trajec_; // the local trajec (relative)
125 
126  fawkes::colli_trans_rot_t proposed_; // the proposed trans-rot that should be realized in MotorInstruct
127 
128  bool cfg_write_spam_debug_;
129  bool cfg_emergency_stop_enabled_; // true if emergency stop is used
130  float cfg_emergency_threshold_distance_; // threshold distance if emergency stop triggers
131  float cfg_emergency_threshold_velocity_; // threshold velocity if emergency stop triggers
132  float cfg_emergency_velocity_max_; // if emergency stop triggers, this is the max velocity
133 
134  fawkes::colli_state_t colli_state_; // representing current colli status
135  bool target_new_;
136 
137  fawkes::cart_coord_2d_t target_point_; // for update
138 
139  int escape_count_; // count escaping behaviour
140 
141  // Config file constants that are read at the beginning
142  int frequency_; // frequency of the colli
143  float occ_grid_height_, occ_grid_width_; // occgrid field sizes
144  int occ_grid_cell_height_, occ_grid_cell_width_; // occgrid cell sizes
145  float max_robo_inc_; // maximum increasement of the robots size
146  bool cfg_obstacle_inc_; // indicator if obstacles should be increased or not
147 
148  bool cfg_visualize_idle_; /**< Defines if visualization should run when robot is idle without a target. */
149 
150  float cfg_min_rot_; /**< The minimum rotation speed. */
151  float cfg_min_drive_dist_; /**< The minimum distance to drive straight to target */
152  float cfg_min_long_dist_drive_; /**< The minimum distance to drive straight to target in a long distance */
153  float cfg_min_long_dist_prepos_;/**< The minimum distance to drive to a pre-positino of a target in long distance */
154  float cfg_min_rot_dist_; /**< The minimum rotation distance to rotate, when at target */
155  float cfg_target_pre_pos_; /**< Distance to target pre-position (only if colli_state_t == DriveToOrientPoint) */
156  fawkes::colli_escape_mode_t cfg_escape_mode_;
157  fawkes::colli_motor_instruct_mode_t cfg_motor_instruct_mode_;
158 
159  float cfg_max_velocity_; /**< The maximum allowd velocity */
160 
161  std::string cfg_frame_base_; /**< The frame of the robot's base */
162  std::string cfg_frame_laser_; /**< The frame of the laser */
163 
164  std::string cfg_iface_motor_; /**< The ID of the MotorInterface */
165  std::string cfg_iface_laser_; /**< The ID of the LaserInterface */
166  std::string cfg_iface_colli_; /**< The ID of the NavigatorInterface for colli target*/
167  float cfg_iface_read_timeout_; /**< Maximum age (in seconds) of required data from reading interfaces*/
168 
169  fawkes::cart_coord_2d_t laser_to_base_; /**< The distance from laser to base */
170  bool laser_to_base_valid_; /**< Do we have a valid distance from laser to base? */
171 
172  float distance_to_next_target_; /**< the distance to the next target in drive direction*/
173 
174 
175  /* ************************************************************************ */
176  /* PRIVATE METHODS */
177  /* ************************************************************************ */
178  /// Run the actual Colli procedure
179  void colli_execute_();
180 
181  /// Handle an incoming goto command
182  void colli_goto_(float x, float y, float ori, fawkes::NavigatorInterface* iface);
183 
184  /// Register all BB-Interfaces at the Blackboard.
185  void open_interfaces();
186 
187  /// Initialize all modules used by the Colli
188  void initialize_modules();
189 
190  /// update interface values
191  void interfaces_read();
192 
193  /// Check if the interface data is valid, i.e. not outdated
194  bool interface_data_valid();
195 
196  /// Check, in what state the colli is, and what to do
197  void update_colli_state();
198 
199  /// Calculate all information out of the updated blackboard data
200  void update_modules();
201 
202  /// Check, if we have to do escape mode, or if we have to drive the ordinary way ;-)
203  bool check_escape();
204 
205 };
206 
207 #endif
208 
Laser360Interface Fawkes BlackBoard Interface.
Thread aspect to access to BlackBoard.
Definition: blackboard.h:34
Thread aspect that allows to obtain the current time from the clock.
Definition: clock.h:36
Cartesian coordinates (2D).
Definition: types.h:59
Fawkes library namespace.
colli_motor_instruct_mode_t
Colli motor_instuct modes.
Definition: types.h:73
colli_state_t
Colli States.
Definition: types.h:36
Thread class encapsulation of pthreads.
Definition: thread.h:42
Storing Translation and rotation.
Definition: types.h:60
Thread aspect to access the transform system.
Definition: tf.h:42
Thread that performs the navigation and collision avoidance algorithms.
Definition: colli_thread.h:58
This OccGrid is derived by the Occupancy Grid originally from Andreas Strack, but modified for speed ...
Definition: og_laser.h:49
Thread aspect to log output.
Definition: logging.h:35
This is the plan class.
Definition: astar_search.h:47
This class selects the correct drive mode and calls the appopriate drive component.
Thread aspect to access configuration data.
Definition: configurable.h:35
Point with cartesian coordinates as signed integers.
Definition: types.h:40
colli_escape_mode_t
Colli Escape modes.
Definition: types.h:67
The Basic of a Motorinstructor.
MotorInterface Fawkes BlackBoard Interface.
Mutex mutual exclusion lock.
Definition: mutex.h:32
Time wait utility.
Definition: wait.h:32
Colli data, refering to current movement.
Definition: types.h:44
NavigatorInterface Fawkes BlackBoard Interface.