int TE::SetupLaser() { if (!(laser = deviceTable->GetDevice(laser_addr))) { PLAYER_ERROR("TE::SetupLaser Unable to locate suitable laser device"); return -1; } if (laser->Subscribe(InQueue) != 0) { PLAYER_ERROR("TE::SetupLaser Unable to subscribe to laser device"); return -1; } player_laser_geom_t* cfg; Message* msg; // Get the laser pose if (!(msg = laser->Request(InQueue, PLAYER_MSGTYPE_REQ, PLAYER_LASER_REQ_GET_GEOM, NULL, 0, NULL, false))) { PLAYER_ERROR("TE::SetupLaser Failed to get laser geometry"); return (-1); } // Store the laser pose cfg = (player_laser_geom_t*) msg->GetPayload(); laser_pose = cfg->pose; delete msg; num_laser_scans = 0; return 0; }
//////////////////////////////////////////////////////////////////////////////// // Set up the device (called by server thread). int CameraV4L2::Setup() { int err=0; int i; source = back_source; norm = back_norm; depth = back_depth; width = back_width; height = back_height; fieldType = back_fieldType; v4l2_type_id = back_v4l2_type_id; PLAYER_MSG1(0,"Camerav4l2: Setting up device %s", this->device); this->fg = fg2_createFrameGrabber(); // set initial settings if (fg2_open(this->fg, this->device)!=0) { PLAYER_ERROR1("unable to open %s", this->device); fg2_delete(&fg); return -1; } PLAYER_MSG0(1,"Camerav4l2: device opened, aplying settings"); err |= fg2_set_source(this->fg, this->source, this->sourceCh); err |= fg2_set_source_norm(this->fg, this->norm); if (err!=0){ PLAYER_ERROR("Camerav4l2: failed source or norm"); fg2_delete(&fg); return -1; } for(i=0; i<numOfCtls; i++){ fg2_setControlValue(fg, ctlNames[i], ctlVals[i]); } if (fg2_setPixelSettings(fg, this->width, this->height, this->v4l2_type_id, this->fieldType, this->depth)==0) { } else { PLAYER_ERROR("Camerav4l2: nie udalo sie ustawic podanych parametrow"); //fg2_delete(&fg); //return -1; } // finally start streaming with new settings if (fg2_startCapture(fg)!=0){ fg2_delete(&fg); return -1; } // Start the driver thread. this->StartThread(); return 0; }
int TE::SetupOdom() { // Setup the output position device if (!(odom = deviceTable->GetDevice(odom_addr))) { PLAYER_ERROR("TE::SetupOdom Unable to locate suitable output position device"); return -1; } if (odom->Subscribe(InQueue) != 0) { PLAYER_ERROR("TE::SetupOdom Unable to subscribe to output position device"); return -1; } // Setup the input position device if (!(localize = deviceTable->GetDevice(localize_addr))) { PLAYER_ERROR("TE::SetupOdom Unable to locate suitable input position device"); return -1; } if (localize->Subscribe(InQueue) != 0) { PLAYER_ERROR("TE::SetupOdom Unable to subscribe to input position device"); return -1; } // Get the odometry geometry Message* msg = 0; if (!(msg = odom->Request(InQueue, PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_GET_GEOM, NULL, 0, NULL, false)) || (msg->GetHeader()->size != sizeof (player_position2d_geom_t))) { PLAYER_ERROR("TE::SetupOdom Failed to get geometry of underlying position device"); if (msg) delete msg; return (-1); } player_position2d_geom_t* geom = (player_position2d_geom_t*) msg->GetPayload(); robot_geom = *geom; PLAYER_MSG5(0, "TE::SetupOdom Robot geom: %.3f %.3f %.3f %.3f %.3f", robot_geom.size.sl, robot_geom.size.sw, robot_geom.pose.px, robot_geom.pose.py, RTOD(robot_geom.pose.pa)); delete msg; memset(&odom_pose, 0, sizeof (player_pose_t)); memset(&odom_vel, 0, sizeof (player_pose_t)); odom_stall = false; return 0; }
//////////////////////////////////////////////////////////////////////////////// /// Process incoming requests int CameraV4L2::HandleRequests() { void *client; char request[PLAYER_MAX_REQREP_SIZE]; char outBuf[PLAYER_MAX_REQREP_SIZE]; int len; while ((len = GetConfig(&client, &request, sizeof(request),NULL)) > 0) { int ret = -1; PLAYER_MSG2(2,"Got Reguest %c (size: %d)", request[0], len); if (len>1 && (request[0]=='g' || request[0]=='G')){ // pobranie jakiejs wartosci PLAYER_MSG0(2,"Get type request"); ret = handleCommand(request, len, outBuf, PLAYER_MAX_REQREP_SIZE-1); if (ret==0) { outBuf[PLAYER_MAX_REQREP_SIZE-1] = '\0'; PLAYER_MSG1(2,"Sending Ack: %s", outBuf); if (PutReply(client, PLAYER_MSGTYPE_RESP_ACK, outBuf, strlen(outBuf)+1,NULL) != 0) PLAYER_ERROR("PutReply() failed"); } else { PLAYER_MSG0(2,"Sendinf NACK"); if (PutReply(client, PLAYER_MSGTYPE_RESP_NACK,NULL) != 0) PLAYER_ERROR("PutReply() failed"); } } else { if (len>0) { ret = handleCommand(request, len); } if (ret == 0){ PLAYER_MSG0(2,"Sending Ack"); if (PutReply(client, PLAYER_MSGTYPE_RESP_ACK,NULL) != 0) PLAYER_ERROR("PutReply() failed"); } else { PLAYER_MSG0(2,"Sending Nack"); if (PutReply(client, PLAYER_MSGTYPE_RESP_NACK,NULL) != 0) PLAYER_ERROR("PutReply() failed"); } } // if framegrabber is slow(1-10s), it is worth handling all commands at once //usleep(30000); } return 0; }
int EpuckDriver::MainSetup() { if(this->serialPort->initialize() == -1) { PLAYER_ERROR1("%s",this->serialPort->getError().c_str()); return -1; } // Request e-puck side program version this->serialPort->sendInt(0x01); if(this->serialPort->recvUnsigned() != this->EXPECTED_EPUCK_SIDE_VERSION) { PLAYER_ERROR("The e-puck side program version isn't the expected"); return -1; } if(epuckCamera.get() != NULL) { try { this->epuckCamera->Initialize(); std::string version = this->epuckCamera->GetCameraVersion(); PLAYER_MSG1(1,"E-puck camera initialized. Camera Version: %s", version.c_str()); } catch(std::exception &e) { PLAYER_ERROR1("%s", e.what()); return -1; } } return 0; }
int DynamicWindowController::setupLaser() { if(!(this->laser_device = deviceTable->GetDevice(this->laser_addr))) { PLAYER_ERROR("LAS1"); PLAYER_ERROR("unable to locate suitable laser device"); return -1; } if (this->laser_device->Subscribe(this->InQueue) != 0) { PLAYER_ERROR("LAS2"); PLAYER_ERROR("unable to subscribe to laser device"); return -1; } return 0; }
//------------------------------------------------------------------------------ PingerDriver::PingerDriver( ConfigFile* pConfigFile, int section ) : ThreadedDriver( pConfigFile, section, false, PLAYER_MSGQUEUE_DEFAULT_MAXLEN, PLAYER_DSPIC_CODE ), mBufferSize( "buffer_size", DEFAULT_BUFFER_SIZE, false ) { this->alwayson = true; mpOpaque = NULL; // We must have an opaque device if ( pConfigFile->ReadDeviceAddr( &mOpaqueID, section, "requires", PLAYER_OPAQUE_CODE, -1, NULL ) != 0 ) { PLAYER_ERROR( "No Opaque driver specified" ); SetError(-1); return; } // Read options from the configuration file RegisterProperty( "buffer_size", &mBufferSize, pConfigFile, section ); mBuffer.Init( mBufferSize.GetValue() ); // initializing serial incoming handling globals remainingBytes = 0; state = stIdle; transducer = 0; }
int DynamicWindowController::setupPosition() { if(!(this->position_device = deviceTable->GetDevice(this->position_addr))) { PLAYER_ERROR("POS1"); PLAYER_ERROR("unable to locate suitable position device"); return -1; } if(this->position_device->Subscribe(this->InQueue) != 0) { PLAYER_ERROR("POS2"); PLAYER_ERROR("unable to subscribe to position device"); return -1; } Message* msg; if(!(msg = this->position_device->Request(this->InQueue,PLAYER_MSGTYPE_REQ,PLAYER_POSITION2D_REQ_GET_GEOM,NULL, 0, NULL,false)) || (msg->GetHeader()->size != sizeof(player_position2d_geom_t))) { PLAYER_ERROR("POS3"); PLAYER_ERROR("failed to get geometry of underlying position device"); if(msg) delete msg; return -1; } player_position2d_geom_t* geom = (player_position2d_geom_t*)msg->GetPayload(); float robot_radius = MAX(geom->size.sl,geom->size.sw); robot_radius /= 2.0; dw.setRadius(robot_radius); delete msg; return 0; }
//------------------------------------------------------------------------------ // Set up the device. Return 0 if things go well, and -1 otherwise. int PingerDriver::MainSetup() { if ( Device::MatchDeviceAddress( mOpaqueID, this->device_addr ) ) { PLAYER_ERROR( "Attempting to subscribe to self" ); return -1; } mpOpaque = deviceTable->GetDevice( mOpaqueID ); if ( NULL == mpOpaque ) { PLAYER_ERROR( "Unable to locate suitable opaque device" ); return -1; } if ( mpOpaque->Subscribe( this->InQueue ) != 0 ) { PLAYER_ERROR( "Unable to subscribe to opaque device" ); return -1; } return 0; }
CameraSimulator::CameraSimulator(ConfigFile *cf, int section) : Driver(cf, section, true, PLAYER_MSGQUEUE_DEFAULT_MAXLEN, PLAYER_CAMERA_CODE) { this->position_device = NULL; if (cf->ReadDeviceAddr(&this->position_addr, section, "requires", PLAYER_POSITION2D_CODE, -1, NULL) != 0) { this->SetError(-1); return; } if (!(this->position_device = deviceTable->GetDevice(this->position_addr))) { PLAYER_ERROR("unable to locate suitable position device"); return; } if (this->position_device->Subscribe(this->InQueue) != 0) { PLAYER_ERROR("unable to subscribe to position device"); return; } this->data.image = NULL; camera_time = 0.0; width = cf->ReadInt(section, "width", 640); height = cf->ReadInt(section, "height", 480); // Load the image and prep it const char* imagename = cf->ReadString(section, "image", "image.png"); std::cout << "Loading image " << imagename << std::endl; image = cvLoadImage(imagename); std::cout << "image loaded. size = " << image.width() << " X " << image.height() << std::endl; ; this->data.image = new unsigned char[width * height * 3]; imageOutput = new OcvImage<RGBPixel<unsigned char> > (width, height, IPL_DEPTH_8U, 3); M = cvMat(2, 3, CV_32F, m); }
//------------------------------------------------------------------------------ // Set up the device. Return 0 if things go well, and -1 otherwise. int MotorDriver::MainSetup() { if ( mbCompassAvailable ) { if ( Device::MatchDeviceAddress( mCompassID, this->device_addr ) ) { PLAYER_ERROR( "Attempting to subscribe to self" ); return -1; } mpCompass = deviceTable->GetDevice( mCompassID ); if ( NULL == mpCompass ) { PLAYER_ERROR( "Unable to locate suitable compass device" ); return -1; } if ( mpCompass->Subscribe( this->InQueue ) != 0 ) { PLAYER_ERROR( "Unable to subscribe to compass device" ); return -1; } } if ( mbDepthSensorAvailable ) { if ( Device::MatchDeviceAddress( mDepthSensorID, this->device_addr ) ) { PLAYER_ERROR( "Attempting to subscribe to self" ); return -1; } mpDepthSensor = deviceTable->GetDevice( mDepthSensorID ); if ( NULL == mpDepthSensor ) { PLAYER_ERROR( "Unable to locate suitable Depth Sensor device" ); return -1; } if ( mpDepthSensor->Subscribe( this->InQueue ) != 0 ) { PLAYER_ERROR( "Unable to subscribe to Depth Sensor device" ); return -1; } } return 0; }
int player_sd_update(player_sd_t* sd, double timeout) { struct pollfd ufds[1]; int numready; int polltime; DNSServiceErrorType sdErr; player_sd_mdns_t* mdns = (player_sd_mdns_t*)sd->sdRef; if(!mdns->browseRef_valid) { PLAYER_ERROR("Can't update without a valid browsing session"); return(-1); } ufds[0].fd = DNSServiceRefSockFD(mdns->browseRef); ufds[0].events = POLLIN; if(timeout >= 0.0) polltime = (int)rint(timeout * 1e3); else polltime = -1; for(;;) { if((numready = poll(ufds,1,polltime)) < 0) { if(errno == EAGAIN) { // TODO: strictly speaking, we should decrement polltime here by // the time that has elapsed so far continue; } else { PLAYER_ERROR1("poll returned error: %s", strerror(errno)); return(-1); } } else if(numready > 0) { // Read all queued up responses if((sdErr = DNSServiceProcessResult(mdns->browseRef)) != kDNSServiceErr_NoError) { PLAYER_ERROR1("DNSServiceProcessResult returned error: %d", sdErr); return(-1); } while(mdns->flags & kDNSServiceFlagsMoreComing) { if((sdErr = DNSServiceProcessResult(mdns->browseRef)) != kDNSServiceErr_NoError) { PLAYER_ERROR1("DNSServiceProcessResult returned error: %d", sdErr); return(-1); } } break; } else { // timeout break; } } return(0); }
bool PropertyBag::AddProperty (const char *key, Property *property) { if (firstProperty == NULL) { if ((firstProperty = new PropertyNode) == NULL) { PLAYER_ERROR ("Failed to allocate memory for property node"); return false; } if ((firstProperty->key = strdup (key)) == NULL) { PLAYER_ERROR1 ("Failed to allocate memory for property key: %s", key); delete firstProperty; firstProperty = NULL; return false; } firstProperty->property = property; firstProperty->next = NULL; } else { // Walk to the end of the list, checking for an existing property as we go PropertyNode *lastProperty = firstProperty; while (lastProperty != NULL) { if (strcmp (lastProperty->key, key) == 0) { PLAYER_ERROR1 ("Property already registered: %s", key); return false; } if (lastProperty->next == NULL) { // This is the end of the list, break before we lose the pointer // Note that the while loop doesn't do this check because then // it wouldn't check the last item on the list to see if it's the // same key as the one being registered break; } lastProperty = lastProperty->next; } // Add the new property at this position (which should be the end of the list) if ((lastProperty->next = new PropertyNode) == NULL) { PLAYER_ERROR ("Failed to allocate memory for property node"); return false; } if ((lastProperty->next->key = strdup (key)) == NULL) { PLAYER_ERROR1 ("Failed to allocate memory for property key: %s", key); delete lastProperty->next; lastProperty->next = NULL; return false; } lastProperty = lastProperty->next; lastProperty->property = property; lastProperty->next = NULL; } return true; }
int EpuckDriver::ProcessMessage(QueuePointer &resp_queue, player_msghdr* hdr, void* data) { //-------------------- POSITION2D if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD, PLAYER_POSITION2D_CMD_VEL, this->position2dAddr)) { player_position2d_cmd_vel_t position_cmd_vel = *(player_position2d_cmd_vel_t *)data; this->epuckPosition2d->SetVel(position_cmd_vel.vel.px, position_cmd_vel.vel.pa); if(position_cmd_vel.vel.py != 0) { PLAYER_WARN("Ignored invalid sideways volocity command"); } return 0; }else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD, PLAYER_POSITION2D_CMD_CAR, this->position2dAddr)) { player_position2d_cmd_car_t position_cmd_car = *(player_position2d_cmd_car_t *)data; this->epuckPosition2d->SetVel(position_cmd_car.velocity, position_cmd_car.angle); return 0; }else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_GET_GEOM, this->position2dAddr)) { EpuckPosition2d::BodyGeometry epuckGeom = this->epuckPosition2d->GetGeometry(); player_position2d_geom_t playerGeom; playerGeom.pose.px = 0; playerGeom.pose.py = 0; playerGeom.pose.pyaw = 0; playerGeom.size.sw = epuckGeom.width; playerGeom.size.sl = epuckGeom.height; this->Publish(this->position2dAddr, resp_queue, PLAYER_MSGTYPE_RESP_ACK, PLAYER_POSITION2D_REQ_GET_GEOM, (void*)&playerGeom, sizeof(playerGeom), NULL); return 0; }else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_SET_ODOM, this->position2dAddr)) { player_position2d_set_odom_req_t* set_odom_req = (player_position2d_set_odom_req_t*)data; EpuckInterface::Triple epuckOdom; epuckOdom.x = set_odom_req->pose.px; epuckOdom.y = set_odom_req->pose.py; epuckOdom.theta = set_odom_req->pose.pa; this->epuckPosition2d->SetOdometry(epuckOdom); this->Publish(this->position2dAddr, resp_queue, PLAYER_MSGTYPE_RESP_ACK, PLAYER_POSITION2D_REQ_SET_ODOM); return 0; }else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_RESET_ODOM, this->position2dAddr)) { this->epuckPosition2d->ResetOdometry(); this->Publish(this->position2dAddr, resp_queue, PLAYER_MSGTYPE_RESP_ACK, PLAYER_POSITION2D_REQ_RESET_ODOM); return 0; } //-------------------- IR else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_IR_REQ_POSE, this->irAddr)) { std::vector<EpuckInterface::Triple> epuckGeom = this->epuckIR->GetGeometry(); player_ir_pose_t playerGeom; playerGeom.poses_count = epuckGeom.size(); playerGeom.poses = new player_pose3d_t[playerGeom.poses_count]; std::vector<EpuckInterface::Triple>::iterator it = epuckGeom.begin(); for(int count = 0; it < epuckGeom.end(); it++, count++) { playerGeom.poses[count].px = it->x; playerGeom.poses[count].py = it->y; playerGeom.poses[count].pyaw = it->theta; } this->Publish(this->irAddr, resp_queue, PLAYER_MSGTYPE_RESP_ACK, PLAYER_IR_REQ_POSE, (void*)&playerGeom, sizeof(playerGeom), NULL); delete[] playerGeom.poses; return 0; } //-------------------- ALL LEDs else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD, PLAYER_BLINKENLIGHT_CMD_POWER)) { //-------------------- RING LEDs for(unsigned led = 0; led < EpuckLEDs::RING_LEDS_NUM; ++led) { if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD, PLAYER_BLINKENLIGHT_CMD_POWER, this->ringLEDAddr[led])) { player_blinkenlight_cmd_power_t* blinkenlight_data = (player_blinkenlight_cmd_power_t*)data; this->epuckLEDs->SetRingLED(led, blinkenlight_data->enable); } } //-------------------- FRONT LED if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD, PLAYER_BLINKENLIGHT_CMD_POWER, this->frontLEDAddr)) { player_blinkenlight_cmd_power_t* blinkenlight_data = (player_blinkenlight_cmd_power_t*)data; this->epuckLEDs->SetFrontLED(blinkenlight_data->enable); } //-------------------- BODY LED if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD, PLAYER_BLINKENLIGHT_CMD_POWER, this->bodyLEDAddr)) { player_blinkenlight_cmd_power_t* blinkenlight_data = (player_blinkenlight_cmd_power_t*)data; this->epuckLEDs->SetBodyLED(blinkenlight_data->enable); } return 0; } else { PLAYER_ERROR("Epuck:: Unhandled message"); return -1; } }
//////////////////////////////////////////////////////////////////////////// // Load an occupancy grid int map_load_occ(map_t *map, const char *filename, double scale, int negate) { FILE *file; char magic[3]; int i, j; int ch, occ; int width, height, depth; map_cell_t *cell; // Open file file = fopen(filename, "r"); if (file == NULL) { fprintf(stderr, "%s: %s\n", strerror(errno), filename); return -1; } // Read ppm header fscanf(file, "%10s \n", magic); if (strcmp(magic, "P5") != 0) { fprintf(stderr, "incorrect image format; must be PGM/binary"); return -1; } // Ignore comments while ((ch = fgetc(file)) == '#') while (fgetc(file) != '\n'); ungetc(ch, file); // Read image dimensions fscanf(file, " %d %d \n %d \n", &width, &height, &depth); // Allocate space in the map if (map->cells == NULL) { map->scale = scale; map->size_x = width; map->size_y = height; map->cells = calloc(width * height, sizeof(map->cells[0])); } else { if (width != map->size_x || height != map->size_y) { PLAYER_ERROR("map dimensions are inconsistent with prior map dimensions"); return -1; } } // Read in the image for (j = height - 1; j >= 0; j--) { for (i = 0; i < width; i++) { ch = fgetc(file); // Black-on-white images if (!negate) { if (ch < depth / 4) occ = +1; else if (ch > 3 * depth / 4) occ = -1; else occ = 0; } // White-on-black images else { if (ch < depth / 4) occ = -1; else if (ch > 3 * depth / 4) occ = +1; else occ = 0; } if (!MAP_VALID(map, i, j)) continue; cell = map->cells + MAP_INDEX(map, i, j); cell->occ_state = occ; } } fclose(file); return 0; }
//////////////////////////////////////////////////////////////////////////// // Load a wifi signal strength map int map_load_wifi(map_t *map, const char *filename, int index) { FILE *file; char magic[3]; int i, j; int ch, level; int width, height, depth; map_cell_t *cell; // Open file file = fopen(filename, "r"); if (file == NULL) { fprintf(stderr, "%s: %s\n", strerror(errno), filename); return -1; } // Read ppm header fscanf(file, "%10s \n", magic); if (strcmp(magic, "P5") != 0) { fprintf(stderr, "incorrect image format; must be PGM/binary"); return -1; } // Ignore comments while ((ch = fgetc(file)) == '#') while (fgetc(file) != '\n'); ungetc(ch, file); // Read image dimensions fscanf(file, " %d %d \n %d \n", &width, &height, &depth); // Allocate space in the map if (map->cells == NULL) { map->size_x = width; map->size_y = height; map->cells = calloc(width * height, sizeof(map->cells[0])); } else { if (width != map->size_x || height != map->size_y) { PLAYER_ERROR("map dimensions are inconsistent with prior map dimensions"); return -1; } } // Read in the image for (j = height - 1; j >= 0; j--) { for (i = 0; i < width; i++) { ch = fgetc(file); if (!MAP_VALID(map, i, j)) continue; if (ch == 0) level = 0; else level = ch * 100 / 255 - 100; cell = map->cells + MAP_INDEX(map, i, j); cell->wifi_levels[index] = level; } } fclose(file); return 0; }