00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022 #include "surveyor_driver.h"
00023
00024
00025 Driver*
00026 Surveyor_Init(ConfigFile *cf, int section)
00027 {
00028 return ((Driver *) (new Surveyor(cf, section)));
00029 }
00030
00031
00032 void
00033 Surveyor_Register(DriverTable *table)
00034 {
00035 table->AddDriver("surveyor", Surveyor_Init);
00036 }
00037
00042 Surveyor::Surveyor(ConfigFile *cf, int section) :
00043 Driver(cf, section, true, PLAYER_MSGQUEUE_DEFAULT_MAXLEN)
00044 {
00045 memset(&this->position_addr, 0, sizeof(player_devaddr_t));
00046 memset(&this->camera_addr, 0, sizeof(player_devaddr_t));
00047 memset(&this->ir_addr, 0, sizeof(player_devaddr_t));
00048 memset(&this->dio_addr, 0, sizeof(player_devaddr_t));
00049
00050
00051 if (cf->ReadDeviceAddr(&(this->position_addr), section, "provides",
00052 PLAYER_POSITION2D_CODE, -1, NULL) == 0)
00053 {
00054 if (this->AddInterface(this->position_addr) != 0)
00055 {
00056 PLAYER_ERROR("Could not add Position2D interface for SRV-1");
00057 this->SetError(-1);
00058 return;
00059 }
00060 }
00061
00062
00063 if (cf->ReadDeviceAddr(&(this->camera_addr), section, "provides",
00064 PLAYER_CAMERA_CODE, -1, NULL) == 0)
00065 {
00066
00067 const char *imagetype = cf->ReadString(section, "image_size",
00068 "320x240");
00069 if (imagetype[0] == '3')
00070 {
00071 this->setup_image_mode = SRV1_IMAGE_BIG;
00072 }
00073 else if (imagetype[0] == '1')
00074 {
00075 this->setup_image_mode = SRV1_IMAGE_MED;
00076 }
00077 else
00078 {
00079 this->setup_image_mode = SRV1_IMAGE_SMALL;
00080 }
00081 if (this->AddInterface(this->camera_addr) != 0)
00082 {
00083 PLAYER_ERROR("Could not add Camera interface for SRV-1");
00084 this->SetError(-1);
00085 return;
00086 }
00087 }
00088
00089
00090
00091 this->portname = cf->ReadString(section, "port", "/dev/ttyUSB0");
00092
00093 this->srvdev = NULL;
00094
00095
00096 puts("Constructor is done!");
00097 }
00098
00099
00100
00101
00102
00103
00104 int
00105 Surveyor::Setup()
00106 {
00107 this->srvdev = srv1_create(this->portname);
00108
00109 if (!srv1_init(this->srvdev))
00110 {
00111 srv1_destroy(this->srvdev);
00112 this->srvdev = NULL;
00113 PLAYER_ERROR("could not connect to SRV-1");
00114 return -1;
00115 }
00116
00117 this->srvdev->image_mode = this->setup_image_mode;
00118 printf("image_mode = '%c' \n", this->srvdev->image_mode);
00119
00120
00121 this->StartThread();
00122
00123
00124 puts("Setup is done!");
00125
00126 return 0;
00127 }
00128
00129 int
00130 Surveyor::Shutdown()
00131 {
00132 puts("Shutting surveyor driver down");
00133 this->StopThread();
00134 srv1_destroy(this->srvdev);
00135 this->srvdev = NULL;
00136 return 0;
00137 }
00138
00139 void
00140 Surveyor::Main()
00141 {
00142 for (;;)
00143 {
00144
00145 this->ProcessMessages();
00146
00147
00148 if (!srv1_read_sensors(this->srvdev))
00149 {
00150 PLAYER_ERROR("failed to retrieve sensors from SRV-1");
00151 srv1_destroy(this->srvdev);
00152 return;
00153 }
00154 printf("\nCARLOS: before Publishing()\n");
00155
00157
00158 player_position2d_data_t posdata;
00159 memset(&posdata, 0, sizeof(posdata));
00160
00161 posdata.vel.px = this->srvdev->vx;
00162 posdata.vel.pa = this->srvdev->va;
00163
00164 this->Publish(this->position_addr, PLAYER_MSGTYPE_DATA,
00165 PLAYER_POSITION2D_DATA_STATE, (void*) &posdata, sizeof(posdata),
00166 NULL);
00167
00168
00170
00171 player_camera_data_t camdata;
00172 memset(&camdata, 0, sizeof(camdata));
00173
00174 switch (this->srvdev->image_mode)
00175 {
00176 case SRV1_IMAGE_SMALL:
00177 camdata.width = 80;
00178 camdata.height = 64;
00179 break;
00180 case SRV1_IMAGE_MED:
00181 camdata.width = 160;
00182 camdata.height = 128;
00183 break;
00184 case SRV1_IMAGE_BIG:
00185 camdata.width = 320;
00186 camdata.height = 240;
00187 break;
00188 }
00189
00190 camdata.fdiv = 1;
00191 camdata.bpp = 24;
00192 camdata.format = PLAYER_CAMERA_FORMAT_RGB888;
00193 camdata.compression = PLAYER_CAMERA_COMPRESS_JPEG;
00194
00195
00196
00197
00198
00199
00200 if (this->srvdev->image_mode != SRV1_IMAGE_OFF)
00201 {
00202 camdata.image_count = this->srvdev->frame_size;
00203
00204 if (camdata.image == NULL)
00205 {
00206 camdata.image = (uint8_t *) malloc(camdata.image_count);
00207 }
00208 else
00209 {
00210 camdata.image = (uint8_t *) realloc(camdata.image,
00211 camdata.image_count);
00212 }
00213 memcpy(camdata.image, this->srvdev->frame, camdata.image_count);
00214
00215
00216
00217
00218 }
00219
00220 this->Publish(this->camera_addr, PLAYER_MSGTYPE_DATA,
00221 PLAYER_CAMERA_DATA_STATE, (void*) &camdata, sizeof(camdata),
00222 NULL);
00223
00224
00225
00226
00227 usleep(SRVMIN_CYCLE_TIME);
00228 }
00229 }
00230
00231 int
00232 Surveyor::ProcessMessage(QueuePointer &resp_queue, player_msghdr *hdr,
00233 void *data)
00234 {
00235
00236
00237
00238
00239
00240 if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD,
00241 PLAYER_POSITION2D_CMD_VEL, this->position_addr))
00242 {
00243 printf("\nCARLOS: I'm Matching Message\n");
00244
00245
00246 player_position2d_cmd_vel_t position_cmd;
00247 position_cmd = *(player_position2d_cmd_vel_t *) data;
00248 PLAYER_MSG2(2,"sending motor commands %f %f", position_cmd.vel.px, position_cmd.vel.pa);
00249
00250 if (!srv1_set_speed(this->srvdev, position_cmd.vel.px,
00251 position_cmd.vel.pa))
00252 {
00253 PLAYER_ERROR("failed to set speed on SRV-1");
00254 }
00255
00256 return 0;
00257 }
00258 else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
00259 PLAYER_POSITION2D_REQ_MOTOR_POWER, this->position_addr))
00260 {
00261 this->Publish(this->position_addr, resp_queue,
00262 PLAYER_MSGTYPE_RESP_ACK, PLAYER_POSITION2D_REQ_MOTOR_POWER);
00263 return 0;
00264 }
00265 else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
00266 PLAYER_POSITION2D_REQ_GET_GEOM, this->position_addr))
00267 {
00268
00269 memset(&pos_geom, 0, sizeof pos_geom);
00270
00271
00272 pos_geom.size.sl = SRV1_DIAMETER;
00273 pos_geom.size.sw = SRV1_DIAMETER;
00274
00275 this->Publish(this->position_addr, resp_queue,
00276 PLAYER_MSGTYPE_RESP_ACK, PLAYER_POSITION2D_REQ_GET_GEOM,
00277 (void*) &pos_geom, sizeof pos_geom, NULL);
00278 return 0;
00279 }
00280 else
00281 {
00282 return -1;
00283 }
00284
00285
00286
00287 }
00288
00290
00291
00292
00293 extern "C"
00294 {
00295 int
00296 player_driver_init(DriverTable* table)
00297 {
00298 puts("Surveyor driver initializing");
00299 Surveyor_Register(table);
00300 puts("Surveyor driver done");
00301 return (0);
00302 }
00303 }