source: level2/competencies/RobotHouse/Vlaser.h @ 528

Revision 528, 1.8 KB checked in by KDucasse, 10 years ago (diff)
Line 
1
2/*
3Virtual laser
4Sends a Bottle over the network, could add laser posistion in a header so we could use multiple lasers without much fuss
5*/
6/********************* REMEBER TO TWEAK EXCLUSION ANGLES (ANGLES WHICH HIT THE ROBOT ETC) *******************/
7#define DEBUGlaser 1
8#define mindistance 0.30 // in m, we know the laser hits the robot so get rid of those readings
9#define LaserWait       0
10
11
12#include "SamClass.h"
13#include <mrpt/slam.h>
14#include <mrpt/hwdrivers/CHokuyoURG.h>
15#include <mrpt/hwdrivers/CSerialPort.h>
16#include <mrpt/gui.h>
17
18using namespace mrpt;
19using namespace mrpt::hwdrivers;
20using namespace mrpt::slam;
21using namespace mrpt::gui;
22using namespace mrpt::utils;
23using namespace std;
24
25
26class Vlaser: public SamClass
27{
28private:
29BufferedPort<Bottle> myfirst; // its gonna be process local connection, so i can send the whole class :)
30
31
32Network yarp;
33public:
34CHokuyoURG      laser;
35
36
37        void SamInit(void)
38        {
39        puts("in laser");
40        laser.setSerialPort("COM10");
41        puts("set serial \n Starting laser, this may take a minute");
42
43        bool TurnedOn = false;
44
45        while(!TurnedOn) // sometimes the coms are busy, wait untill we get what we want
46        {
47                try{TurnedOn = laser.turnOn();}
48                catch (...){}
49        }
50        puts("laser turned on");
51        RecognisePort("Out");
52        StartModule("/Laser");
53        myfirst.open("/Laser_Out"); //myPortStatus
54        myfirst.setReporter(myPortStatus);
55       
56
57        }
58       
59        void SamIter(void)
60        {
61        bool thereIsObservation,hardError;     
62        CObservation2DRangeScan myscan;
63       
64       
65    laser.doProcessSimple( thereIsObservation, myscan, hardError );
66        Bottle &Send = myfirst.prepare();
67        Send.clear();
68
69        if(thereIsObservation )
70                {
71                        for(int x=0;x<myscan.scan.size();x++)
72                        {
73                                if(myscan.scan[x]<mindistance){myscan.scan[x]=0;}
74                                Send.addDouble(myscan.scan[x]);
75                        }
76                myfirst.write();
77                }
78        }
79};
Note: See TracBrowser for help on using the repository browser.