source: level2/competencies/RobotHouse/MapUpdater - Copy (2).h @ 534

Revision 534, 10.5 KB checked in by KDucasse, 9 years ago (diff)
Line 
1
2/*
3Map builder, just updates a map, thats all
4is a good example of a port interupt;
5*/
6
7
8/* we dont need to explisitly stop the robot, it'll always time out after a secound in the virtual robot */
9
10#define DEBUGmap 1     
11#define RobotRad 0.45
12#define RobotStopDistance 0.4 // the distance it should stop at if something is on laser, its nearly the robots rad
13#define HowCloseToPointUntillStop 0.2 // the distance it should accept its in the right posistion
14#define HowLongToWaitWithoutPath 30 // how long the robot should wait if it cant plan a path
15
16/* we could use a 3d map, but the focus isn't navigation, no one will see it and it'll just take more cpu */
17/* the idea is we have two locations, wanted and present, if they dont equal then we goto where we are ment to go */
18
19//#include "SamClass.h"
20#include <mrpt/slam.h>
21#include <mrpt/gui.h>
22#include <mrpt/base.h>
23#include <mrpt/utils/CTicTac.h>
24#include <math.h>
25
26using namespace mrpt;
27using namespace mrpt::hwdrivers;
28using namespace mrpt::slam;
29using namespace mrpt::gui;
30using namespace mrpt::utils;
31using namespace std;
32
33
34string WantedLocation =" ";
35string CurrentLocation = " ";
36bool CurrentlyHavePath = false;
37bool IHavePermissionToMove = false;
38
39
40 CMetricMapBuilderICP                           *mapBuilder;           
41 TSetOfMetricMapInitializers            metricMapsOpts;
42 CConfigFile                                            iniFile("MapSetup.ini");
43 CICP::TConfigParams                            icpOptions;
44 COccupancyGridMap2D Gridmap;
45 CPose3D CurRobotPose;
46 mrpt::gui::CDisplayWindow                      Pathwin("Computed path");
47 CImage         img;
48 double LowestLaserReadings;
49 double iterTimer =0;
50 CPathPlanningCircularRobot     pathPlanning;
51 bool ThereIsNoPath;
52 CPose2D StartPose,FinnishPose;
53 std::deque<poses::TPoint2D>            Path;
54 bool ignorenext;
55//CObservation2DRangeScanPtr scanptr;
56 bool laserin = false;
57 bool odoin = false;
58
59static CObservationOdometryPtr odoptr;
60//static CObservation2DRangeScanPtr scanptr;
61/*
62 class LaserPort : public BufferedPort<BinPortable<CObservation2DRangeScan>>
63{
64    virtual void onRead(BinPortable<CObservation2DRangeScan> &b)
65         {
66                 laserin = true;
67                 if(odoin==true){puts("interupted odo");}
68                b.content().maxRange=1;
69                LowestLaserReadings = 100000; // thats very big in mtrs
70                for(int x = 0;x<b.content().scan.size();x++)
71                        {
72                                b.content().scan[x]=0;
73                        if(b.content().scan[x]<LowestLaserReadings&&b.content().scan[x]!=0)
74                                {
75                                LowestLaserReadings=b.content().scan[x];
76                                }
77                        }
78                CObservation2DRangeScan scan = b.content();
79                CObservation2DRangeScanPtr scanptr(scan.duplicateGetSmartPtr());
80                scanptr->maxRange=5;
81                 mapBuilder->processObservation(scanptr);
82                 laserin = false;
83     }
84};
85*/
86/*
87 class OdoPort : public BufferedPort<Bottle>
88{
89    virtual void onRead(Bottle &b)
90         {
91                 odoin = true;
92                 if(laserin==true){puts("interupted laser");}
93                 try
94                 {
95                 odoptr = CObservationOdometry::Create();
96                 CPose2D odo(b.get(0).asDouble(),b.get(1).asDouble(),b.get(2).asDouble());
97                 odoptr->odometry = odo;
98                 mapBuilder->processObservation(odoptr);
99                 }
100                 catch(exception& e){cout << "except:" << e.what()<<endl;}
101                odoin = false;
102        }
103};
104*/
105 class DestinPort : public BufferedPort<Bottle>
106{
107    virtual void onRead(Bottle &b)
108         {
109                IHavePermissionToMove =true;
110                FinnishPose.x(b.get(0).asDouble());
111                FinnishPose.y(b.get(1).asDouble());
112                FinnishPose.phi(b.get(2).asDouble());
113     }
114};
115
116
117
118class MapUpdater: public SamClass
119{
120private:
121Network yarp;
122//LaserPort LaserIn; // its gonna be process local connection, so i can send the whole class :)
123//OdoPort OdoIn;
124BufferedPort <BinPortable<CObservation2DRangeScan>> LaserIn;
125BufferedPort<Bottle> OdoIn;
126
127DestinPort Destination;
128BufferedPort <Bottle> MoveOut;
129BufferedPort <Bottle> Reached;
130
131bool WhereWeThereLastTime;
132double TimeToStop;
133
134double MTimeNeedid;
135double RobotSpeed;
136public:
137
138        void CheckLaser(void)
139        {
140                BinPortable<CObservation2DRangeScan> *LaserData = LaserIn.read(false);
141                if(LaserData!=NULL)
142                {
143                        for(int x = 0;x<LaserData->content().scan.size();x++)
144                        {
145                                LaserData->content().scan[x] = 0;
146                        }
147                        CObservation2DRangeScan scan(LaserData->content());
148                        CObservation2DRangeScanPtr scanptr(scan.duplicateGetSmartPtr());
149                        mapBuilder->processObservation(scanptr);
150                        printf("laser time %u \n",scanptr->timestamp);
151                }
152
153        }
154        void CheckOdo(void)
155        {
156                Bottle *CC = OdoIn.read(false);
157                if(CC!=NULL)
158                {
159                odoptr = CObservationOdometry::Create();
160                CPose2D odo(CC->get(0).asDouble(),CC->get(1).asDouble(),CC->get(2).asDouble());
161                odoptr->odometry = odo;
162                mapBuilder->processObservation(odoptr);
163                }
164        }
165
166       
167        void SamInit(void)
168        {
169        RecognisePort("LaserIn");
170        RecognisePort("OdoIn");
171        RecognisePort("MoveOut");
172        RecognisePort("TargetIn");
173        RecognisePort("CMion");
174        Destination.open("/MAP_TargetIn");
175        LaserIn.open("/MAP_LaserIn"); //myPortStatus
176        OdoIn.open("/MAP_OdoIn");
177        MoveOut.open("/MAP_MoveOut");
178        Reached.open("/MAP_CMion");
179
180        LaserIn.useCallback();
181        OdoIn.useCallback();
182        Destination.useCallback();
183
184        LaserIn.setReporter(myPortStatus);
185        OdoIn.setReporter(myPortStatus);
186        MoveOut.setReporter(myPortStatus);
187        Destination.setReporter(myPortStatus);
188        Reached.setReporter(myPortStatus);
189
190        StartModule("/MAP");
191
192        pathPlanning.robotRadius = RobotRad;// + RobotRadError;
193        pathPlanning.minStepInReturnedPath=0.6;
194        pathPlanning.occupancyThreshold=0.01;
195        ThereIsNoPath=true;
196
197        //LaserIn
198        metricMapsOpts.loadFromConfigFile(iniFile, "MappingApplication");
199        icpOptions.loadFromConfigFile(iniFile, "ICP");
200        mapBuilder = new CMetricMapBuilderICP(&metricMapsOpts,0.001,DEG2RAD(0.001),&icpOptions);
201        mapBuilder->options.verbose=false;
202        mapBuilder->enableMapUpdating(true);// start off with localisation only;
203        mapBuilder->setCurrentMapFile("Robothousemap.simplemap");
204
205        mapBuilder->ICP_options.matchAgainstTheGrid = true;
206//      mapBuilder->options.verbose                                     = false;
207//      mapBuilder->options.enableMapUpdating           = true;
208        mapBuilder->options.debugForceInsertion         = false;
209        mapBuilder->options.insertImagesAlways          = true;
210        /////////////////////////////////////////////////////////////// REALLY IMPORTANT!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
211        // this line is the threshold for 1 updating the posistion, and 2 adding to the map.
212        // the higher the better map becouse incorrect readings wont be put in the map
213        // but if its too high it can get lost and never work at all.
214        //mapBuilder->ICP_options.minICPgoodnessToAccept=0.9;
215
216        MTimeNeedid =0;
217        WhereWeThereLastTime=false;
218        TimeToStop=0;
219        }
220       
221       
222        void SamIter(void)
223        {
224                CheckOdo();
225                CheckLaser();
226               
227                if(!mapBuilder->getCurrentlyBuiltMetricMap()->isEmpty())
228                {
229
230                Gridmap = *mapBuilder->getCurrentlyBuiltMetricMap()->m_gridMaps[0].pointer();
231
232                StartPose.x(mapBuilder->getCurrentPoseEstimation()->getEstimatedPose().x());
233                StartPose.y(mapBuilder->getCurrentPoseEstimation()->getEstimatedPose().y());
234                StartPose.phi(mapBuilder->getCurrentPoseEstimation()->getEstimatedPose().yaw());
235                showmap();
236               
237
238                // if we are not in the right location then every secound replan route
239                if(!AreWeThere() && yarp::os::Time::now()>MTimeNeedid && IHavePermissionToMove)// if in wrong location and we havn't got a path
240                        {
241                        PlanPath();
242                        if(!Path.empty())
243                                {
244                                MTimeNeedid = yarp::os::Time::now() + 1;       
245                                Bottle &bb = MoveOut.prepare();
246                                bb.clear();
247                                bb.addInt(5);//priority
248                                bb.addDouble(1);//time
249                                bb.addDouble(RobotSpeed);// liner vel
250
251                                double Angle =RAD2DEG(atan2(Path.front().y - StartPose.y(),Path.front().x - StartPose.x()));
252                                bb.addDouble(RAD2DEG(Angle)); //rot vel
253                                MoveOut.write();
254                                }
255
256                        }
257                       
258                }
259        }
260
261        bool AreWeThere(void)
262        {
263               
264                double distanceTofinalObjective = StartPose.distance2DTo(FinnishPose.x(),FinnishPose.y());
265                if(distanceTofinalObjective<LowestLaserReadings/2){RobotSpeed=distanceTofinalObjective*1000;} // needs to put in mm
266                else                                                                                      {RobotSpeed=(LowestLaserReadings/2)*1000;}            // needs to put in mm
267                       
268                if(distanceTofinalObjective < HowCloseToPointUntillStop)
269                {
270                        IHavePermissionToMove=false;
271                        Path.clear();
272                        if(WhereWeThereLastTime==false)// so we only just got into pos
273                        {
274                                Bottle &BBB2 = Reached.prepare();
275                                BBB2.clear();
276                                BBB2.addDouble(StartPose.x());
277                                BBB2.addDouble(StartPose.y());
278                                Reached.write();
279                        }
280
281                        WhereWeThereLastTime=true;
282                        return true;
283                }
284                WhereWeThereLastTime=false;
285                return false;
286
287        }
288
289        void PlanPath(void)
290        {
291
292        //      puts("planning path");
293                        try{pathPlanning.computePath(Gridmap,StartPose,FinnishPose,Path,ThereIsNoPath,-1.0f);}
294                                catch(...){}
295                                CPose2D TempPose;
296                                if(ThereIsNoPath) // try imagining the robot is behind itself
297                                {
298                                TempPose = StartPose + CPose2D(-RobotRad,0,0);
299                                try{pathPlanning.computePath(Gridmap,TempPose,FinnishPose,Path,ThereIsNoPath,-1.0f);}
300                                catch(...){}
301                                }
302                                if(ThereIsNoPath)// try imagining the robot to right right of itself
303                                {
304                                TempPose = StartPose + CPose2D(0,RobotRad,0);
305                                try{pathPlanning.computePath(Gridmap,TempPose,FinnishPose,Path,ThereIsNoPath,-1.0f);}
306                                catch(...){}
307                                }
308                                if(ThereIsNoPath)// try imagining the robot to right right of itself
309                                {
310                                TempPose = StartPose + CPose2D(0,-RobotRad,0);
311                                try{pathPlanning.computePath(Gridmap,TempPose,FinnishPose,Path,ThereIsNoPath,-1.0f);}
312                                catch(...){}
313                                }
314
315
316                                if(!ThereIsNoPath){TimeToStop=0;}
317                                else                      {TimeToStop++;}
318
319                        if(TimeToStop>HowLongToWaitWithoutPath)// so we only just got into pos
320                        {
321                                Bottle &BBB3 = Reached.prepare();
322                                BBB3.clear();
323                                BBB3.addDouble(StartPose.x());
324                                BBB3.addDouble(StartPose.y());
325                                Reached.write();
326                                IHavePermissionToMove=false;
327                                TimeToStop=0;
328                        }
329
330                        //      puts("stoped planning path");
331
332                                // need to do something so if it cant find it for 10 secs then stop
333        }
334
335
336        void showmap(void)
337        {
338                Gridmap.getAsImage(img,false);
339                CPose3DPDFPtr posePDF = mapBuilder->getCurrentPoseEstimation();
340                CPose2D RobotLoc(mapBuilder->getCurrentPoseEstimation()->getEstimatedPose());
341                CPose2D Nose = RobotLoc + CPose2D(RobotRad,0,0);
342                img.drawCircle(Gridmap.x2idx(RobotLoc.x()),Gridmap.getSizeY()-1-Gridmap.y2idx(RobotLoc.y()),RobotRad/ Gridmap.getResolution(),TColor(50,100,50));
343                img.drawCircle(Gridmap.x2idx(Nose.x()),Gridmap.getSizeY()-1-Gridmap.y2idx(Nose.y()),(RobotRad/2)/ Gridmap.getResolution(),TColor(50,100,50));
344               
345               
346                if(!Path.empty())
347                {
348                        for(int uu =0;uu<Path.size();uu++)
349                        {
350                                img.drawCircle(Gridmap.x2idx(Path[uu].x),Gridmap.getSizeY()-1-Gridmap.y2idx(Path[uu].y),RobotRad/ Gridmap.getResolution(),TColor(50,50,150));
351                        }
352                }
353       
354               
355                Pathwin.showImage(img);
356        }
357};
358
Note: See TracBrowser for help on using the repository browser.