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
39namespace fawkes {
40class Mutex;
41class TimeWait;
42
43class MotorInterface;
44class Laser360Interface;
45class NavigatorInterface;
46
47class LaserOccupancyGrid;
48class Search;
49
50class SelectDriveMode;
51class BaseMotorInstruct;
52} // namespace fawkes
53
54class ColliVisualizationThread;
55
62{
63public:
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
79private:
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
Thread that performs the navigation and collision avoidance algorithms.
Definition: colli_thread.h:62
ColliThread()
Constructor.
virtual void init()
Initialize the thread.
void colli_relgoto(float x, float y, float ori, fawkes::NavigatorInterface *iface)
Sends a goto-command, using relative coordinates.
void colli_stop()
Sends a stop-command.
virtual void finalize()
Finalize the thread.
void colli_goto(float x, float y, float ori, fawkes::NavigatorInterface *iface)
Sends a goto-command, using global coordinates.
virtual ~ColliThread()
Destructor.
virtual void loop()
Code to execute in the thread.
bool is_final() const
Checks if the colli is final.
virtual void set_vis_thread(ColliVisualizationThread *vis_thread)
Set the visualization thread.
The Basic of a Motorinstructor.
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:34
Thread aspect to access configuration data.
Definition: configurable.h:33
Laser360Interface Fawkes BlackBoard Interface.
This OccGrid is derived by the Occupancy Grid originally from Andreas Strack, but modified for speed ...
Definition: og_laser.h:47
Thread aspect to log output.
Definition: logging.h:33
MotorInterface Fawkes BlackBoard Interface.
Mutex mutual exclusion lock.
Definition: mutex.h:33
NavigatorInterface Fawkes BlackBoard Interface.
This is the plan class.
Definition: astar_search.h:45
This class selects the correct drive mode and calls the appopriate drive component.
Thread class encapsulation of pthreads.
Definition: thread.h:46
Time wait utility.
Definition: wait.h:33
Thread aspect to access the transform system.
Definition: tf.h:39
Fawkes library namespace.
colli_motor_instruct_mode_t
Colli motor_instuct modes.
Definition: types.h:70
colli_escape_mode_t
Colli Escape modes.
Definition: types.h:67
colli_state_t
Colli States.
Definition: types.h:33
Cartesian coordinates (2D).
Definition: types.h:65
Colli data, refering to current movement.
Definition: types.h:42
Storing Translation and rotation.
Definition: types.h:60
Point with cartesian coordinates as signed integers.
Definition: types.h:42