source: level2/competencies/RobotHouse/MapUpdater - Copy (3).h @ 535

Revision 535, 2.2 KB checked in by KDucasse, 10 years ago (diff)
Line 
1
2#define validrangesize 682
3//#include "SamClass.h"
4#include <mrpt/slam.h>
5#include <mrpt/gui.h>
6#include <mrpt/base.h>
7#include <mrpt/utils/CTicTac.h>
8#include <math.h>
9
10using namespace mrpt;
11using namespace mrpt::hwdrivers;
12using namespace mrpt::slam;
13using namespace mrpt::gui;
14using namespace mrpt::utils;
15using namespace std;
16
17
18
19CICP    icp;
20CSimplePointsMap    M,T;
21CDisplayWindowPlots             win("Laser scans");
22CDisplayWindowPlots             win2("Main map");
23CPose2D            RobotPose(0,0,0);
24
25class MapUpdater: public SamClass
26{
27private:
28Network yarp;
29BufferedPort <Bottle> LaserIn;
30BufferedPort <Bottle> OdoIn;
31
32public:
33        void CheckOdo(void)
34        {
35       
36        }
37
38        void CheckLaser(void)
39        {
40                Bottle *Bscan = LaserIn.read(false);
41                if(Bscan!=NULL)
42                {
43                CObservation2DRangeScan scan;
44                scan.validRange.resize(validrangesize);
45                scan.validRange.assign(validrangesize,'1');//19
46                scan.aperture=4.1887902;// 240 degree scan range in radians
47                scan.maxRange=4;//(m)
48                scan.scan.resize(682);
49                CPose3D  SensorPos(0,0,0,0,0,0);//(0.175,0.0,0,0,0);//21 // used this for most the demo
50                scan.setSensorPose(SensorPos); 
51                for(int x=0;x<Bscan->size();x++)
52                        {
53                                scan.scan[x]=Bscan->get(x).asDouble();
54                        }
55                T.clear();
56                T.insertObservation(&scan);
57
58                if(M.isEmpty()&&!T.isEmpty()){M.insertObservation(&scan);}
59
60                CPosePDFPtr pdf =  icp.Align(&M,&T,CPose2D(0,0,0));
61                RobotPose = pdf->getEstimatedPose();
62
63                printf("robots loc %s \n",RobotPose.asString().c_str());
64
65                CPose3D RobotPose3D(RobotPose);
66                M.insertObservation(&scan,&RobotPose3D);
67
68               
69
70                vector_float    xs,ys,zs,xa,ya,za;
71                T.getAllPoints(xs,ys,zs);
72                M.getAllPoints(xa,ya,za);
73
74                win.plot(xs,ys,".b3");
75                win.axis_equal();
76
77                win2.plot(xa,ya,".b3");
78                win2.axis_equal();
79
80                }
81
82
83        }
84
85
86       
87        void SamInit(void)
88        {
89        RecognisePort("LaserIn");
90        RecognisePort("OdoIn");
91        LaserIn.open("/MAP_LaserIn"); //myPortStatus
92        OdoIn.open("/MAP_OdoIn");
93        LaserIn.setReporter(myPortStatus);
94        OdoIn.setReporter(myPortStatus);
95        StartModule("/MAP");
96
97       
98        icp.options.maxIterations = 250;
99
100//      ICPer.options.maxIterations = 250;
101        }
102       
103       
104        void SamIter(void)
105        {
106                CheckOdo();
107                CheckLaser();
108
109
110
111
112
113       
114        }
115
116
117
118
119       
120};
121
Note: See TracBrowser for help on using the repository browser.