WPILibC++  trunk
AxisCameraParams.cpp
1 /*----------------------------------------------------------------------------*/
2 /* Copyright (c) FIRST 2008. All Rights Reserved. */
3 /* Open Source Software - may be modified and shared by FRC teams. The code */
4 /* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
5 /*----------------------------------------------------------------------------*/
6 
7 #include "Vision/AxisCameraParams.h"
8 
9 #include "Vision/AxisCamera.h"
10 #include <inetLib.h>
11 #include "pcre.h"
12 #include <sockLib.h>
13 #include <string.h>
14 #include "Synchronized.h"
15 #include "Timer.h"
16 #include "Utility.h"
17 #include "WPIErrors.h"
18 
19 #if JAVA_CAMERA_LIB != 1
20 #include "DriverStation.h"
21 #endif
22 
23 static const char *const kRotationChoices[] = {"0", "180"};
24 static const char *const kResolutionChoices[] = {"640x480", "640x360", "320x240", "160x120"};
25 static const char *const kExposureControlChoices[] = { "automatic", "hold", "flickerfree50", "flickerfree60" };
26 static const char *const kWhiteBalanceChoices[] = { "auto", "holdwb", "fixed_outdoor1",
27  "fixed_outdoor2", "fixed_indoor", "fixed_fluor1", "fixed_fluor2" };
28 
32 AxisCameraParams::AxisCameraParams(const char* ipAddress)
33  : m_paramTask("paramTask", (FUNCPTR) s_ParamTaskFunction)
34  , m_paramChangedSem (NULL)
35  , m_socketPossessionSem (NULL)
36  , m_brightnessParam (NULL)
37  , m_compressionParam (NULL)
38  , m_exposurePriorityParam (NULL)
39  , m_colorLevelParam (NULL)
40  , m_maxFPSParam (NULL)
41  , m_rotationParam (NULL)
42  , m_resolutionParam (NULL)
43  , m_exposureControlParam (NULL)
44  , m_whiteBalanceParam (NULL)
45 {
46  if (ipAddress == NULL || strlen(ipAddress) == 0)
47  {
48 #if JAVA_CAMERA_LIB == 1
49  wpi_setWPIErrorWithContext(ParameterOutOfRange, "IP Address must be specified");
50  return;
51 #else
53  ds->WaitForData();
54  uint16_t teamNumber = ds->GetTeamNumber();
55  char cameraIP[16];
56  snprintf(cameraIP, 16, "10.%d.%d.11", teamNumber / 100, teamNumber % 100);
57  m_ipAddress = inet_addr(cameraIP);
58 #endif
59  }
60  else
61  {
62  m_ipAddress = inet_addr((char*)ipAddress);
63  }
64 
65  if (m_ipAddress == (u_long)ERROR)
66  {
67  wpi_setErrnoError();
68  return;
69  }
70 
71  m_brightnessParam = new IntCameraParameter("ImageSource.I0.Sensor.Brightness=%i",
72  "root.ImageSource.I0.Sensor.Brightness=(.*)", false);
73  m_parameters.push_back(m_brightnessParam);
74  m_colorLevelParam = new IntCameraParameter("ImageSource.I0.Sensor.ColorLevel=%i",
75  "root.ImageSource.I0.Sensor.ColorLevel=(.*)", false);
76  m_parameters.push_back(m_colorLevelParam);
77  m_exposurePriorityParam = new IntCameraParameter("ImageSource.I0.Sensor.exposurePriority=%i",
78  "root.ImageSource.I0.Sensor.ExposurePriority=(.*)", false);
79  m_parameters.push_back(m_exposurePriorityParam);
80  m_compressionParam = new IntCameraParameter("Image.I0.Appearance.Compression=%i",
81  "root.Image.I0.Appearance.Compression=(.*)", true);
82  m_parameters.push_back(m_compressionParam);
83  m_maxFPSParam = new IntCameraParameter("Image.I0.Stream.FPS=%i",
84  "root.Image.I0.Stream.FPS=(.*)", false);
85  m_parameters.push_back(m_maxFPSParam);
86  m_rotationParam = new EnumCameraParameter("Image.I0.Appearance.Rotation=%s",
87  "root.Image.I0.Appearance.Rotation=(.*)", true, kRotationChoices, sizeof(kRotationChoices)/sizeof(kRotationChoices[0]));
88  m_parameters.push_back(m_rotationParam);
89  m_resolutionParam = new EnumCameraParameter("Image.I0.Appearance.Resolution=%s",
90  "root.Image.I0.Appearance.Resolution=(.*)", true, kResolutionChoices, sizeof(kResolutionChoices)/sizeof(kResolutionChoices[0]));
91  m_parameters.push_back(m_resolutionParam);
92  m_exposureControlParam = new EnumCameraParameter("ImageSource.I0.Sensor.Exposure=%s",
93  "root.ImageSource.I0.Sensor.Exposure=(.*)", false, kExposureControlChoices, sizeof(kExposureControlChoices)/sizeof(kExposureControlChoices[0]));
94  m_parameters.push_back(m_exposureControlParam);
95  m_whiteBalanceParam = new EnumCameraParameter("ImageSource.IO.Sensor.WhiteBalance=%s",
96  "root.ImageSource.I0.Sensor.WhiteBalance=(.*)", false, kWhiteBalanceChoices, sizeof(kWhiteBalanceChoices)/sizeof(kWhiteBalanceChoices[0]));
97  m_parameters.push_back(m_whiteBalanceParam);
98 
99  m_paramChangedSem = semBCreate (SEM_Q_PRIORITY, SEM_EMPTY);
100  m_socketPossessionSem = semBCreate (SEM_Q_PRIORITY, SEM_FULL);
101 
102  m_paramTask.Start((int)this);
103 }
104 
109 {
110  m_paramTask.Stop();
111 
112  semDelete(m_socketPossessionSem);
113  semDelete(m_paramChangedSem);
114 
115  delete m_whiteBalanceParam;
116  delete m_exposureControlParam;
117  delete m_resolutionParam;
118  delete m_rotationParam;
119  delete m_maxFPSParam;
120  delete m_compressionParam;
121  delete m_exposurePriorityParam;
122  delete m_colorLevelParam;
123  delete m_brightnessParam;
124 }
125 
130 {
131  return thisPtr->ParamTaskFunction();
132 }
133 
139 // TODO: need to synchronize the actual setting of parameters (the assignment statement)
141 {
142  static bool firstTime = true;
143 
144  while (true)
145  {
146  semTake(m_socketPossessionSem, WAIT_FOREVER);
147  if (firstTime)
148  {
149  while (ReadCamParams() == 0);
150  firstTime = false;
151  }
152  bool restartRequired = false;
153 
154  ParameterVector_t::iterator it = m_parameters.begin();
155  ParameterVector_t::iterator end = m_parameters.end();
156  for(; it != end; it++)
157  {
158  bool changed = false;
159  char param[150];
160  restartRequired |= (*it)->CheckChanged(changed, param);
161  if (changed)
162  {
163  UpdateCamParam(param);
164  }
165  }
166  if (restartRequired)
167  {
168  RestartCameraTask();
169  }
170  semGive(m_socketPossessionSem);
171  }
172  return 0;
173 }
174 
180 {
181  m_brightnessParam->SetValue(brightness);
182  semGive(m_paramChangedSem);
183 }
184 
190 {
191  return m_brightnessParam->GetValue();
192 }
193 
198 void AxisCameraParams::WriteWhiteBalance(WhiteBalance_t whiteBalance)
199 {
200  m_whiteBalanceParam->SetValue(whiteBalance);
201  semGive(m_paramChangedSem);
202 }
203 
208 AxisCameraParams::WhiteBalance_t AxisCameraParams::GetWhiteBalance()
209 {
210  return (WhiteBalance_t) m_whiteBalanceParam->GetValue();
211 }
212 
218 {
219  m_colorLevelParam->SetValue(colorLevel);
220  semGive(m_paramChangedSem);
221 }
222 
228 {
229  return m_colorLevelParam->GetValue();
230 }
231 
236 void AxisCameraParams::WriteExposureControl(Exposure_t exposureControl)
237 {
238  m_exposureControlParam->SetValue(exposureControl);
239  semGive(m_paramChangedSem);
240 }
241 
246 AxisCameraParams::Exposure_t AxisCameraParams::GetExposureControl()
247 {
248  return (Exposure_t) m_exposureControlParam->GetValue();
249 }
250 
255 void AxisCameraParams::WriteResolution(Resolution_t resolution)
256 {
257  m_resolutionParam->SetValue(resolution);
258  semGive(m_paramChangedSem);
259 }
260 
265 AxisCameraParams::Resolution_t AxisCameraParams::GetResolution()
266 {
267  return (Resolution_t) m_resolutionParam->GetValue();
268 }
269 
277 void AxisCameraParams::WriteExposurePriority(int exposurePriority)
278 {
279  m_exposurePriorityParam->SetValue(exposurePriority);
280  semGive(m_paramChangedSem);
281 }
282 
283 int AxisCameraParams::GetExposurePriority()
284 {
285  return m_exposurePriorityParam->GetValue();
286 }
287 
293 void AxisCameraParams::WriteRotation(Rotation_t rotation)
294 {
295  m_rotationParam->SetValue(rotation);
296  semGive(m_paramChangedSem);
297 }
298 
303 AxisCameraParams::Rotation_t AxisCameraParams::GetRotation()
304 {
305  return (Rotation_t) m_rotationParam->GetValue();
306 }
307 
314 {
315  m_compressionParam->SetValue(compression);
316  semGive(m_paramChangedSem);
317 }
318 
324 {
325  return m_compressionParam->GetValue();
326 }
327 
334 {
335  m_maxFPSParam->SetValue(maxFPS);
336  semGive(m_paramChangedSem);
337 }
338 
344 {
345  return m_maxFPSParam->GetValue();
346 }
347 
354 int AxisCameraParams::UpdateCamParam(const char* param)
355 {
356  const char *requestString =
357  "GET /axis-cgi/admin/param.cgi?action=update&%s HTTP/1.1\n\
358 User-Agent: HTTPStreamClient\n\
359 Connection: Keep-Alive\n\
360 Cache-Control: no-cache\n\
361 Authorization: Basic RlJDOkZSQw==\n\n";
362  char completedRequest[1024];
363  sprintf(completedRequest, requestString, param);
364  // Send request
365  int camSocket = CreateCameraSocket(completedRequest);
366  if (camSocket == ERROR)
367  {
368  printf("UpdateCamParam failed: %s\n", param);
369  return 0;
370  }
371  close(camSocket);
372  return 1;
373 }
374 
380 {
381  const char * requestString =
382  "GET /axis-cgi/admin/param.cgi?action=list HTTP/1.1\n\
383 User-Agent: HTTPStreamClient\n\
384 Connection: Keep-Alive\n\
385 Cache-Control: no-cache\n\
386 Authorization: Basic RlJDOkZSQw==\n\n";
387 
388  int camSocket = CreateCameraSocket(requestString);
389  if (camSocket == ERROR)
390  {
391  return 0;
392  }
393  // Allocate on the heap since it is very large and only needed once
394  char *readBuffer = new char[27000];
395  int totalRead = 0;
396  while (1)
397  {
398  wpi_assert(totalRead < 26000);
399  int bytesRead = recv(camSocket, &readBuffer[totalRead], 1000, 0);
400  if (bytesRead == ERROR)
401  {
402  wpi_setErrnoErrorWithContext("Failed to read image header");
403  close(camSocket);
404  return 0;
405  }
406  else if (bytesRead <= 0)
407  {
408  break;
409  }
410  totalRead += bytesRead;
411  }
412  readBuffer[totalRead] = '\0';
413 
414  ParameterVector_t::iterator it = m_parameters.begin();
415  ParameterVector_t::iterator end = m_parameters.end();
416  for(; it != end; it++)
417  {
418  (*it)->GetParamFromString(readBuffer, totalRead);
419  }
420  close(camSocket);
421  delete [] readBuffer;
422  return 1;
423 }
424 
425 /*
426  * Create a socket connected to camera
427  * Used to create a connection to the camera by both AxisCameraParams and AxisCamera.
428  * @param requestString The initial request string to send upon successful connection.
429  * @return ERROR if failed, socket handle if successful.
430  */
431 int AxisCameraParams::CreateCameraSocket(const char *requestString)
432 {
433  int sockAddrSize;
434  struct sockaddr_in serverAddr;
435  int camSocket;
436  /* create socket */
437  if ((camSocket = socket(AF_INET, SOCK_STREAM, 0)) == ERROR)
438  {
439  wpi_setErrnoErrorWithContext("Failed to create the camera socket");
440  return ERROR;
441  }
442 
443  sockAddrSize = sizeof(struct sockaddr_in);
444  bzero((char *) &serverAddr, sockAddrSize);
445  serverAddr.sin_family = AF_INET;
446  serverAddr.sin_len = (u_char) sockAddrSize;
447  serverAddr.sin_port = htons(80);
448 
449  serverAddr.sin_addr.s_addr = m_ipAddress;
450 
451  /* connect to server */
452  struct timeval connectTimeout;
453  connectTimeout.tv_sec = 5;
454  connectTimeout.tv_usec = 0;
455  if (connectWithTimeout(camSocket, (struct sockaddr *) &serverAddr, sockAddrSize, &connectTimeout) == ERROR)
456  {
457  wpi_setErrnoErrorWithContext("Failed to connect to the camera");
458  close(camSocket);
459  return ERROR;
460  }
461  int sent = send(camSocket, requestString, strlen(requestString), 0);
462  if (sent == ERROR)
463  {
464  wpi_setErrnoErrorWithContext("Failed to send a request to the camera");
465  close(camSocket);
466  return ERROR;
467  }
468  return camSocket;
469 }
void WriteExposurePriority(int)
bool Start(uint32_t arg0=0, uint32_t arg1=0, uint32_t arg2=0, uint32_t arg3=0, uint32_t arg4=0, uint32_t arg5=0, uint32_t arg6=0, uint32_t arg7=0, uint32_t arg8=0, uint32_t arg9=0)
Definition: Task.cpp:52
Resolution_t GetResolution()
void SetValue(int value)
AxisCameraParams(const char *ipAddress)
uint16_t GetTeamNumber()
void WriteExposureControl(Exposure_t)
static int s_ParamTaskFunction(AxisCameraParams *thisPtr)
Exposure_t GetExposureControl()
bool Stop()
Definition: Task.cpp:81
WhiteBalance_t GetWhiteBalance()
void WriteResolution(Resolution_t)
static DriverStation * GetInstance()
Rotation_t GetRotation()
void WriteWhiteBalance(WhiteBalance_t whiteBalance)
int UpdateCamParam(const char *param)
void WriteRotation(Rotation_t)