source: level2/competencies/SamgarV1Modules/Navi-Robot-uh/MapHelper.h @ 468

Revision 468, 3.8 KB checked in by KDucasse, 9 years ago (diff)
Line 
1#include <mrpt/core.h>
2#include <iostream>
3#include <fstream>
4#include <conio.h>
5#include <windows.h>
6
7#include "SamgarMainClass.h"
8
9using namespace mrpt;
10using namespace mrpt::slam;
11using namespace mrpt::opengl;
12using namespace mrpt::gui;
13using namespace mrpt::system;
14using namespace mrpt::math;
15using namespace mrpt::utils;
16using namespace std;
17using namespace yarp;
18
19using namespace mrpt::synch;
20using namespace mrpt::random;
21
22/* We are using multhreading
23
24These delays are the only thing stopping the threads from trying to take over a core, if you set them to zero you will get good speeds
25but they will try to occupy three/or less cores continuasly
26
27*/
28
29#define MainWait 0.05//0.25 worked well but want laser quicker // this is the main map update and localisation, you want this pretty fast
30#define GuiWait  0.35 // this is ONLY for the user to see whats going on, helpfull but not important
31#define PlanWait 0.05 // this is how often when it has a target to replan, quicker better but it will slow down near obsticles so this being high should cause crashes
32
33#define validrangesize 682 // number of laser readings in one scan
34
35#ifdef CreateVars
36#define EXTERN
37#else
38#define EXTERN extern
39#endif
40
41
42
43EXTERN CMetricMapBuilderICP                     *mapBuilder;
44EXTERN TSetOfMetricMapInitializers              metricMapsOpts;
45EXTERN CICP::TConfigParams                              icpOptions;
46
47#ifdef CreateVars
48CConfigFile                                             iniFile( "icp-slam_demo.ini"); // open the file
49mrpt::gui::CDisplayWindow               Pathwin("Computed path");
50CImage          img;
51#else
52extern CConfigFile                                              iniFile;
53#endif
54
55EXTERN COccupancyGridMap2D GuiGridmap;
56EXTERN CPose3D GuicurRobotPose;
57
58EXTERN COccupancyGridMap2D PathGridmap;
59EXTERN CPose3D PathcurRobotPose;
60
61EXTERN float distoobjective;
62
63
64EXTERN CActionRobotMovement2D ::TMotionModelOptions MyOptions;
65EXTERN CActionRobotMovement2D   MyRobotAction;
66EXTERN CActionCollectionPtr     action;
67
68EXTERN static CSensoryFramePtr             observations;
69EXTERN static CObservation2DRangeScanPtr scan ;
70EXTERN static CObservationOdometryPtr GlobalOdo;
71
72EXTERN static CSimpleMapPtr MyMap;
73//SamgarModule *Mapper;
74
75EXTERN CCriticalSection  csCounter;
76
77EXTERN bool HasGotInitialPosistion = false;//true;//false;// should be false to wait for odo
78
79EXTERN bool obsupdate;
80EXTERN bool odoupdate;
81
82EXTERN bool OnEXIT;
83
84EXTERN double Speed,ErrorBox;
85
86EXTERN bool    HaveTarget,JustReachedTarget;
87EXTERN CPose2D PrimayTarget;
88EXTERN CPose2D PrimayTarget2;
89
90#define RobotRad 0.45
91#define RobotRadError 0.20
92#define ShadowPathLength 20
93#define Iterationsbeforegiveuppath 50
94
95EXTERN CPathPlanningCircularRobot       pathPlanning;
96
97//EXTERN        CThreadSafeVariable thereisnopath =     CThreadSafeVariable();
98
99EXTERN  CThreadSafeVariable<bool>   Thereisnopath;
100
101
102
103
104EXTERN TColor Blue;
105EXTERN TColor Red;
106EXTERN TColor Green;
107EXTERN TColor OffSetColor;
108
109EXTERN Bottle BotVelAngError;
110
111EXTERN std::deque<poses::TPoint2D>              PathWhereImGoing;
112EXTERN std::deque<poses::TPoint2D>              PathWhereImGoing2;
113EXTERN std::deque<poses::TPoint2D>              PathWhereImGoingMap;
114
115EXTERN std::deque<poses::TPoint2D>              PathWhereIveBeen;
116
117void LoadMapBuilder(void);
118void LoadMovementParams(void);
119void LoadLaserParams(void);
120CPose2D GetDiffOdo(CPose2D);
121void GetLatestOdoData(SamgarModule *tempmodule);
122void GetLatestLaserData(SamgarModule *tempmodule);
123void GetTarget(SamgarModule *tempmodule);
124
125void ShowMap(void);
126void ClearProcessedData(void);
127void LoadColors(void);
128void DrawPointy(CPose2D left,CPose2D right,CPose2D top);
129void DrawMyLine(double x1,double y1,double x2,double y2,TColor color);
130void DrawMyTraveledPath(CPose3D mycurrentpos);
131void LoadPathPlanner(void);
132void PlanPath(void);
133void CalcTraveledPath(CPose3D MCP);
134void DrawMyFuturePath(CPose3D mycurrentpos);
135void EXIT(void);
136bool HaveReachedDestination(CPose3D RobotCurrent);
137void GetAngleAndSetSpeed (CPose3D RobotCurrent);
Note: See TracBrowser for help on using the repository browser.