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