add missing using namespace std
[rock-drivers:orogen-laserscanner_sick.git] / tasks / Task.cpp
1 /* Generated from orogen/lib/orogen/templates/tasks/Task.cpp */
2
3 #include "Task.hpp"
4
5 using namespace laserscanner_sick;
6 using namespace std;
7
8 Task::Task(std::string const& name)
9     : TaskBase(name)
10     , sick(0)
11     , activity(0)
12 {
13     status = 1;
14     num_measurements = 0; 
15     resolution = 0.0;
16     start_angle = 0.0;
17     timestamp_estimator = 0;
18 }
19
20 Task::Task(std::string const& name, RTT::ExecutionEngine* engine)
21     : TaskBase(name, engine)
22     , sick(0)
23     , activity(0)
24 {
25     status = 1;
26     num_measurements = 0;
27     resolution = 0.0;
28     start_angle = 0.0;
29     timestamp_estimator = 0;
30 }
31
32 Task::~Task()
33 {
34     delete timestamp_estimator;
35 }
36
37 /// The following lines are template definitions for the various state machine
38 // hooks defined by Orocos::RTT. See Task.hpp for more detailed
39 // documentation about them.
40
41 bool Task::configureHook()
42 {
43     if (! TaskBase::configureHook())
44         return false;
45     sick = new SickToolbox::SickLMS1xx(_hostname.get(), _port.get());
46     timestamp_estimator = new aggregator::TimestampEstimator(base::Time::fromSeconds(2),base::Time::fromSeconds(0.02),INT_MAX);
47
48     try{
49         sick->Initialize();
50     }
51     catch(...){
52         cerr << cerr << "Initialize failed! Are you using the correct IP address?" << endl;
53         return false;
54     }
55
56     try{
57         sick->SetSickScanDataFormat(SickToolbox::SickLMS1xx::SICK_LMS_1XX_SCAN_FORMAT_DIST_DOUBLE_PULSE_REFLECT_16BIT);
58     }
59     catch(SickToolbox::SickConfigException sick_exception){
60         std::cout << sick_exception.what() << std::endl;
61         return false;
62     }
63
64     if(sick->IntToSickScanFreq(_frequency.get()) == SickToolbox::SickLMS1xx::SICK_LMS_1XX_SCAN_FREQ_UNKNOWN)
65         return false;
66     
67     if(sick->DoubleToSickScanRes(_resolution.get()) == SickToolbox::SickLMS1xx::SICK_LMS_1XX_SCAN_RES_UNKNOWN)
68         return false;
69
70     sick->SetSickScanFreqAndRes(sick->IntToSickScanFreq(_frequency.get()), sick->DoubleToSickScanRes(_resolution.get()));
71
72     try{
73         resolution = (sick->SickScanResToDouble(sick->GetSickScanRes()))/180.0*M_PI;
74         //printf("Resolution: %f\n", sick->SickScanResToDouble(sick->GetSickScanRes()));
75         start_angle = -(M_PI*0.5)+((sick->GetSickStartAngle())/180.0*M_PI);
76         //printf("Frequency: %i\n", sick->SickScanFreqToInt(sick->GetSickScanFreq()));
77     }
78     catch(SickToolbox::SickIOException sick_exception) {
79         std::cout << sick_exception.what() << std::endl;
80         return false;
81     }
82     activity = getActivity<RTT::extras::FileDescriptorActivity>();
83
84     return true;
85 }
86
87 bool Task::startHook()
88 {
89     if (! TaskBase::startHook() || !activity)
90         return false;
91     activity->watch(sick->getReadFD());
92     return true;
93 }
94
95 void Task::updateHook()
96 {
97     TaskBase::updateHook();
98     base::samples::LaserScan scan;
99     _timestamp_estimator_status.write(timestamp_estimator->getStatus());
100
101     try{
102         sick->GetSickMeasurements(range_1_vals,range_2_vals,reflect_1_vals,reflect_2_vals,num_measurements,&status);
103         for(unsigned int i=0; i<num_measurements; i++){
104             scan.ranges.push_back(range_1_vals[i]);
105             //std::cout << range_1_vals[i] << std::endl;
106         }
107         for(unsigned int i=0; i<num_measurements; i++){
108             scan.remission.push_back(reflect_1_vals[i]);
109         }
110     }
111     catch(SickToolbox::SickIOException sick_exception) {
112         std::cout << sick_exception.what() << std::endl;
113         exception();
114         return;
115     }
116     catch(SickToolbox::SickTimeoutException sick_exception) {
117         std::cout << sick_exception.what() << std::endl;
118         exception();
119         return;
120     }
121     catch(...) {
122         cerr << "An Error Occurred!" << endl;
123         exception();
124         return;
125     }
126     scan.time = timestamp_estimator->update(base::Time::now());
127     scan.start_angle = start_angle;
128     scan.angular_resolution = resolution;
129     scan.speed = 50.0*(2.0*M_PI);
130     scan.minRange = 500; 
131     scan.maxRange = 20000;
132
133     _io_status.write(sick->getBufferMonitorStatus());
134     _scan.write(scan);
135 }
136
137 // void Task::errorHook()
138 // {
139 //     TaskBase::errorHook();
140 // }
141 // void Task::stopHook()
142 // {
143 //     TaskBase::stopHook();
144 // }
145
146 void Task::cleanupHook()
147 {
148     TaskBase::cleanupHook();
149     if(sick)
150         delete sick;
151     sick = 0;
152 }
153