Switch to direct way to work with I2C.
[veter:vehicle.git] / src / RemoteVehicleI.cpp
1 /* Copyright (c) 2010 Andrey Nechypurenko
2    See the file LICENSE for copying permission. 
3 */
4
5 #include "RemoteVehicleI.h"
6 #include <unistd.h>
7 #include "SensorActuatorIDs.h"
8
9
10 RemoteVehicleI::RemoteVehicleI(Ice::ObjectAdapterPtr adm_adapter,
11                                Ice::ObjectAdapterPtr ctl_adapter,
12                                const Ice::PropertiesPtr &properties,
13                                int ac, char **av)
14   : admin_adapter(adm_adapter), 
15     control_adapter(ctl_adapter), 
16     motor_control(properties),
17     servo_control(properties, "cameraservo")
18 {
19   this->nav_sensor_thread = new NavSensorThread();
20
21   std::string camera_devs = properties->getProperty("Camera.devices");
22   if(camera_devs.empty())
23     camera_devs = "/dev/video%i";
24   VideoCameraSensorIPtr camera0 = 
25     new VideoCameraSensorI(properties, adm_adapter, ac, av, camera_devs.c_str());
26   this->cameras.push_back(CameraList::value_type(camera0, vehicleadmin::SensorPrx(NULL)));
27   
28   // Add I2C compass and sonar
29   GenericSensorList::value_type s;
30
31   s.first = new GenericSensorI(properties, 
32                                adm_adapter, 
33                                this->nav_sensor_thread, 
34                                "compass");
35   s.second = vehicleadmin::SensorPrx::uncheckedCast(ctl_adapter->addWithUUID(s.first));
36   this->generic_sensors.push_back(s);
37
38   s.first = new GenericSensorI(properties, 
39                                adm_adapter, 
40                                this->nav_sensor_thread, 
41                                "sonar1");
42   s.second = vehicleadmin::SensorPrx::uncheckedCast(ctl_adapter->addWithUUID(s.first));
43   this->generic_sensors.push_back(s);
44
45   std::string prop;
46   ActuatorList::value_type a;
47   vehicleadmin::ActuatorDescription descr;
48
49   // first motor
50   descr.id = MOTOR0;
51   prop = properties->getProperty("Actuators.motorl.description");
52   descr.description = prop.empty() ? "Left motor" : prop;
53   prop = properties->getProperty("Actuators.motorl.type");
54   descr.type = prop.empty() ? "brush, 7.2V max" : prop;
55   a.first = new ActuatorI(descr, adm_adapter, descr.id);
56   a.second = vehicleadmin::ActuatorPrx::uncheckedCast(ctl_adapter->addWithUUID(a.first));
57   this->actuators.push_back(a);
58
59   // second motor
60   descr.id = MOTOR1;
61   prop = properties->getProperty("Actuators.motorr.description");
62   descr.description = prop.empty() ? "Right motor" : prop;
63   prop = properties->getProperty("Actuators.motorr.type");
64   descr.type = prop.empty() ? "brush, 7.2V max" : prop;
65   a.first = new ActuatorI(descr, adm_adapter, descr.id);
66   a.second = vehicleadmin::ActuatorPrx::uncheckedCast(ctl_adapter->addWithUUID(a.first));
67   this->actuators.push_back(a);
68
69   // servo motor to rotate the camera
70   descr.id = CAMERA_SERVO;
71   prop = properties->getProperty("Actuators.cameraservo.description");
72   descr.description = prop.empty() ? "Camera rotation" : prop;
73   prop = properties->getProperty("Actuators.cameraservo.type");
74   descr.type = prop.empty() ? "Futaba servo" : prop;
75   a.first = new ActuatorI(descr, adm_adapter, descr.id);
76   a.second = vehicleadmin::ActuatorPrx::uncheckedCast(ctl_adapter->addWithUUID(a.first));
77   this->actuators.push_back(a);
78 }
79
80
81 RemoteVehicleI::~RemoteVehicleI()
82 {
83   if(this->nav_sensor_thread)
84   {
85     this->nav_sensor_thread->requestShutdown();
86     fprintf(stdout, "Waiting for nav sensor sender thread to shutdown\n");
87     this->nst_ctl.join();
88     
89     // Not a leak. Smart pointer will decrease the ref count 
90     // and delete the object.
91     this->nav_sensor_thread = NULL;
92   }
93 }
94
95
96 std::string 
97 RemoteVehicleI::getVehicleDescription(const Ice::Current &current)
98 {
99   std::string descr("V-");
100   char hostname[1024];
101   int res = gethostname(hostname, sizeof(hostname)/sizeof(hostname[0]));
102   if(res != 0)
103     {// error obtaining host name
104       descr += "unknown";
105     }
106   else
107     {
108       // just for the case if hostname is longer then our buffer
109       hostname[sizeof(hostname)/sizeof(hostname[0]) - 1] = '\0';
110       descr += hostname;
111     }
112   return descr;
113 }
114
115
116 vehicleadmin::SensorList 
117 RemoteVehicleI::getSensorList(const Ice::Current &current)
118 {
119   vehicleadmin::SensorList lst;
120
121   for(CameraList::iterator camera = this->cameras.begin();
122       camera != this->cameras.end(); ++camera)
123   {
124     if(camera->first->isPresent())
125     {
126       if(camera->second == NULL)
127         camera->second = vehicleadmin::SensorPrx::uncheckedCast(this->control_adapter->addWithUUID(camera->first));
128       lst.push_back(camera->second);
129     }
130     else
131       camera->second = NULL;
132   }
133
134   for(GenericSensorList::const_iterator sensor = this->generic_sensors.begin();
135       sensor != this->generic_sensors.end(); ++sensor)
136   lst.push_back(sensor->second);
137
138   return lst;
139 }
140
141
142 vehicleadmin::ActuatorList 
143 RemoteVehicleI::getActuatorList(const Ice::Current &current)
144 {
145   vehicleadmin::ActuatorList lst;
146   for(ActuatorList::const_iterator actuator = this->actuators.begin();
147       actuator != this->actuators.end(); ++actuator)
148     lst.push_back(actuator->second);
149   return lst;
150 }
151
152
153 void 
154 RemoteVehicleI::setActuators(const vehicle::ActuatorFrame &duties, 
155                              const ::Ice::Current &current)
156 {
157   vehicle::ActuatorFrame d;
158   for(vehicle::ActuatorFrame::const_iterator i = duties.begin();
159       i != duties.end(); ++i)
160     {
161       if(i->id == 2)
162         this->servo_control.setDuties(*i);
163       else
164         d.push_back(*i);
165     }
166
167   if(!d.empty())
168     this->motor_control.setDuties(d);
169 }
170
171
172 void 
173 RemoteVehicleI::setSensorReceiver(const vehicle::SensorFrameReceiverPrx &callback, 
174                                   const ::Ice::Current &current)
175 {
176   for(CameraList::const_iterator camera = this->cameras.begin();
177       camera != this->cameras.end(); ++camera)
178   {
179     if(camera->first->isPresent())
180       camera->first->setSensorReceiver(callback);
181   }
182
183   if(!this->nav_sensor_thread->isAlive())
184   {
185     printf("Starting nav sensor thread\n");
186     this->nst_ctl = this->nav_sensor_thread->start();
187   }
188
189   this->nav_sensor_thread->setCallback(callback);
190 }
191
192
193 void 
194 RemoteVehicleI::cleanSensorReceiver(const ::Ice::Current &current)
195 {
196   for(CameraList::const_iterator camera = this->cameras.begin();
197       camera != this->cameras.end(); ++camera)
198   {
199     if(camera->first->isPresent())
200       camera->first->setSensorReceiver(NULL);
201   }
202
203   this->nav_sensor_thread->setCallback(NULL);
204   if(this->nav_sensor_thread->isAlive())
205   {
206     printf("Stoping nav sensor thread...\n");
207     this->nav_sensor_thread->requestShutdown();
208     this->nst_ctl.join();
209     printf("Nav sensor thread stoped.\n");
210   }
211 }
212