ersp.cc

Go to the documentation of this file.
00001 /*
00002  * ERSP  Player driver
00003  */
00004 
00054 #include "ersp.h"
00055 #include "log.h"
00056 
00057 
00059 // Driver setup and shutdown
00061 
00062 ERSP::ERSP(ConfigFile* cf, int section)
00063       : Driver(cf, section, true, PLAYER_MSGQUEUE_DEFAULT_MAXLEN)
00064 {
00065    // zero ids, so that we'll know later which interfaces were requested
00066    memset(&this->position_id, 0, sizeof(player_devaddr_t));
00067    memset(&this->bumper_id, 0, sizeof(player_devaddr_t));
00068    memset(&this->ir_range_id, 0, sizeof(player_devaddr_t));
00069    memset(&this->ir_binary_id, 0, sizeof(player_devaddr_t));
00070    memset(&this->power_id, 0, sizeof(player_devaddr_t));
00071 
00072    this->position_subscriptions = 0;
00073    this->bumper_subscriptions = 0;
00074    this->ir_range_subscriptions = 0;
00075    this->ir_binary_subscriptions = 0;
00076    this->power_subscriptions = 0;
00077    this->running = false;
00078 
00079    if (cf->ReadDeviceAddr(&(this->position_id), section, "provides",
00080                      PLAYER_POSITION2D_CODE, -1, NULL) == 0) {
00081       if (this->AddInterface(this->position_id) != 0) {
00082          this->SetError(-1);
00083          return;
00084       }
00085    }
00086  
00087    if (cf->ReadDeviceAddr(&(this->bumper_id), section, "provides",
00088                      PLAYER_BUMPER_CODE, -1, "bumper") == 0) {
00089       if (this->AddInterface(this->bumper_id) != 0) {
00090          this->SetError(-1);
00091          return;
00092       }
00093    }
00094 
00095    if (cf->ReadDeviceAddr(&(this->ir_range_id), section, "provides",
00096                      PLAYER_IR_CODE, -1, NULL) == 0) {
00097       if (this->AddInterface(this->ir_range_id) != 0) {
00098          this->SetError(-1);
00099          return;
00100       }
00101    }
00102 
00103    if (cf->ReadDeviceAddr(&(this->ir_binary_id), section, "provides",
00104                      PLAYER_BUMPER_CODE, -1, "ir") == 0) {
00105       if (this->AddInterface(this->ir_binary_id) != 0) {
00106          this->SetError(-1);
00107          return;
00108       }
00109    }
00110 
00111 #ifdef POWER
00112    if (cf->ReadDeviceAddr(&(this->power_id), section, "provides",
00113                      PLAYER_POWER_CODE, -1, NULL) == 0) {
00114       if (this->AddInterface(this->power_id) != 0) {
00115          this->SetError(-1);
00116          return;
00117       }
00118    }
00119 #endif
00120 }
00121 
00122 int ERSP::SetupERSP()
00123 {
00124    static const struct ersp_dev ersp_devs[] = {
00125 
00126 #define ERSP_INTERFACE(ROBOT, TYPE, NAME)
00127 #define ERSP_INTERFACE_END
00128 #define ERSP_DEV(ROBOT, ID, TYPE, NAME) \
00129        { ROBOT##_##TYPE##_##ID, ROBOT##_##ID, ERSP_##TYPE, NAME },
00130 
00131 #include ERSP_INCLUDE
00132 
00133 #undef ERSP_INTERFACE
00134 #undef ERSP_INTERFACE_END
00135 #undef ERSP_DEV
00136 
00137    };
00138    devs = ersp_devs;
00139 
00140    if (this->running)
00141       return 0;
00142 
00143 #ifdef HAVE_ERSP
00144    Evolution::Result result;
00145 
00146    // Get the ERSP resource manager
00147    manager = new Evolution::ResourceManager(NULL, &result); 
00148    if (result != Evolution::RESULT_SUCCESS) {
00149       LOG_ERR("Cannot create a resource manager.");
00150       return -1;
00151    }
00152 
00153    result = manager->get_resource_container(0, &container);
00154    if (result != Evolution::RESULT_SUCCESS) {
00155       LOG_ERR("Cannot create a resource container.");
00156       return -1;
00157    }
00158 
00159    for (unsigned int i = 0; i < ERSP_DEVICES; i++) {
00160       /* XXX: Ordered by ersp_dev_type_t. */
00161       static const char *interfaces[] = {
00162          Evolution::IDriveSystem::INTERFACE_ID,
00163          Evolution::IAvoidance::INTERFACE_ID,
00164          Evolution::IBattery::INTERFACE_ID,
00165          Evolution::IBumpSensor::INTERFACE_ID,
00166          Evolution::IBumpSensor::INTERFACE_ID,
00167          Evolution::IRangeSensor::INTERFACE_ID,
00168       };
00169       void **handle = &devices[i].handle;
00170       const char *id = devs[i].id;
00171       const char *interface = interfaces[devs[i].type];
00172 
00173       result = container->obtain_interface(Evolution::NO_TICKET,
00174                                   id, interface, handle);
00175       if (result != Evolution::RESULT_SUCCESS) {
00176          LOG_ERR("Cannot create a %s system.", id);
00177          return -1;
00178       }
00179    }
00180 
00181    devices[SCORPION_AVOID_AVOID].avoid->disable_avoidance(Evolution::NO_TICKET);
00182 #endif /* HAVE_ERSP */
00183 
00184    return 0;
00185 }
00186 
00187 /* Should return 1 on error, and 0 on success. */
00188 int ERSP::Setup()
00189 {
00190    LOG_INFO("Driver setup");
00191 
00192    if (SetupERSP()) {
00193 #ifdef HAVE_ERSP
00194       // FIXME: This might need to be changed so that
00195       // nothing is freed until the destructor is called
00196       // and instead have the SetupERSP conditionally
00197       // initialize variables.
00198       if (manager != NULL)
00199          delete manager;
00200 #endif /* HAVE_ERSP */
00201 
00202       return 1;
00203    }
00204 
00205    // Now spawn reading thread
00206    // Prospone this to here since ::Shutdown will not be called if
00207    // an error condition is returned.
00208    this->StartThread();
00209    this->running = true;
00210 
00211    LOG_INFO("Driver setup done");
00212 
00213     return 0;
00214 }
00215 
00216 int ERSP::Shutdown()
00217 {
00218    LOG_INFO("Driver shutdown");
00219 
00220    if (!this->running)
00221       return 0;
00222 
00223    this->StopThread();
00224    this->running = false;
00225 
00226    LOG_INFO("Driver has been shutdown");
00227 
00228    return 0;
00229 }
00230 
00231 ERSP::~ERSP (void)
00232 {
00233 #ifdef HAVE_ERSP
00234    /* Deallocate the Resource Manager.
00235     *
00236     * This means that the ERSP library will be initialized and ready
00237     * throughout the life of the player server.
00238     *
00239     * FIXME: We still need to ensure that the various interfaces
00240     * (driver, ...) are reset so the robot doesn't do anything. */
00241 
00242    if (manager != NULL)
00243       delete manager;
00244 #endif /* HAVE_ERSP */
00245 }
00246 
00247 void
00248 ERSP::Main()
00249 {
00250 #ifdef HAVE_ERSP
00251    Evolution::Timestamp timestamp;
00252    Evolution::Result result;
00253 #endif /* HAVE_ERSP */
00254    time_t prev_time = 0;
00255 
00256    for(;;) {
00257       pthread_testcancel();
00258 
00259       this->Lock();
00260 
00261       if (this->bumper_subscriptions != 0) {
00262          player_bumper_data_t *bumper = &ersp_data.bumper;
00263 
00264          memset(bumper, 0, sizeof(*bumper));
00265 
00266          for (unsigned int i = 0; i < ERSP_DEVICES; i++) {
00267             bool triggered = false;
00268 
00269             if (devs[i].type != ERSP_BUMP)
00270                continue;
00271 
00272 #ifdef HAVE_ERSP
00273             result = devices[i].bumper->is_triggered(Evolution::NO_TICKET,
00274                                                          &timestamp,
00275                                            &triggered);
00276             if (result != Evolution::RESULT_SUCCESS)
00277                LOG_ERR("Error reading from binary sensor %s",
00278                      devs[i].id);
00279 #endif /* HAVE_ERSP */
00280 
00281             bumper->bumpers[bumper->bumpers_count++] = (uint8_t) triggered == true;
00282          }
00283       }
00284 
00285       if (this->ir_range_subscriptions != 0) {
00286          player_ir_data_t *ir = &ersp_data.ir_range;
00287 
00288          memset(ir, 0, sizeof(*ir));
00289 
00290          for (unsigned int i = 0; i < ERSP_DEVICES; i++) {
00291             double distance = 0;
00292 
00293             if (devs[i].type != ERSP_IR_RANGE)
00294                continue;
00295 
00296 #ifdef HAVE_ERSP
00297             result = devices[i].range->get_distance_reading(Evolution::NO_TICKET,
00298                                         &timestamp, &distance);
00299             if (result != Evolution::RESULT_SUCCESS)
00300                LOG_ERR("Error reading from range sensor %s",
00301                      devs[i].id);
00302 #endif /* HAVE_ERSP */
00303 
00304             /* Convert to meters */
00305             distance = distance / 100;
00306             if (distance > IR_MAX_RANGE) {
00307                distance = IR_MAX_RANGE;
00308             }
00309 
00310             ir->ranges[ir->ranges_count++] = (float) distance;
00311          }
00312       }
00313 
00314       if (this->ir_binary_subscriptions != 0) {
00315          player_bumper_data_t *ir = &ersp_data.ir_binary;
00316 
00317          memset(ir, 0, sizeof(*ir));
00318 
00319          for (unsigned int i = 0; i < ERSP_DEVICES; i++) {
00320             bool triggered = false;
00321 
00322             if (devs[i].type != ERSP_IR_BINARY)
00323                continue;
00324 
00325 #ifdef HAVE_ERSP
00326             result = devices[i].bumper->is_triggered(Evolution::NO_TICKET,
00327                                                          &timestamp,
00328                                            &triggered);
00329             if (result != Evolution::RESULT_SUCCESS)
00330                LOG_ERR("Error reading from binary sensor %s",
00331                      devs[i].id);
00332 #endif /* HAVE_ERSP */
00333 
00334             ir->bumpers[ir->bumpers_count++] = (uint8_t) triggered == true;
00335          }
00336       }
00337 
00338 #ifdef POWER
00339       if (this->power_subscriptions != 0) {
00340          player_power_data_t *power = &ersp_data.power;
00341          double percent_charge = 0, voltage = 0;
00342 
00343          memset(power, 0, sizeof(*power));
00344          power->valid = PLAYER_POWER_MASK_VOLTS | PLAYER_POWER_MASK_PERCENT;
00345 
00346 #ifdef HAVE_ERSP
00347          result = devices[SCORPION_BATTERY_BATTERY].battery->get_percent_charge_left(Evolution::NO_TICKET,
00348             &timestamp, &percent_charge);
00349          if (result != Evolution::RESULT_SUCCESS)
00350             LOG_ERR("Error reading from battery");
00351 #endif /* HAVE_ERSP */
00352          power->percent = (float) percent_charge;
00353 
00354 #ifdef HAVE_ERSP
00355          result = devices[SCORPION_BATTERY_BATTERY].battery->get_voltage(Evolution::NO_TICKET,
00356             &timestamp, &voltage);
00357          if (result != Evolution::RESULT_SUCCESS)
00358             LOG_ERR("Error reading from battery");
00359 #endif /* HAVE_ERSP */
00360          power->volts = (float) percent_charge;
00361       }
00362 #endif
00363 
00364       this->Unlock();
00365 
00366       if (this->position_subscriptions ||
00367           this->bumper_subscriptions ||
00368          this->ir_range_subscriptions ||
00369          this->ir_binary_subscriptions ||
00370          this->power_subscriptions) {
00371          time_t now = time(NULL);
00372 
00373          /* Limit to only publishing every second. */
00374          if (now > prev_time) {
00375             prev_time = now;
00376          }
00377       }
00378             PutData();
00379 
00380       // Handle pending messages
00381       if (!this->InQueue->Empty()) {
00382          LOG_DBG("Processing messages");
00383          ProcessMessages();
00384       }
00385    }
00386 }
00387 
00388 
00390 // Message processing
00392 
00393 int
00394 ERSP::ProcessMessage(MessageQueue * resp_queue,
00395                 player_msghdr * hdr,
00396                 void * data)
00397 {
00398    // Look for configuration requests
00399    if (hdr->type == PLAYER_MSGTYPE_REQ)
00400       return this->HandleConfig(resp_queue, hdr, data);
00401 
00402    if (hdr->type == PLAYER_MSGTYPE_CMD)
00403       return this->HandleCommand(hdr, data);
00404 
00405    return -1;
00406 }
00407 
00408 int
00409 ERSP::HandleConfig(MessageQueue* resp_queue,
00410                player_msghdr * hdr,
00411                void * data)
00412 {
00413    // check for position config requests
00414    if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
00415                        PLAYER_POSITION2D_REQ_SET_ODOM,
00416                        this->position_id)) {
00417 
00418       player_position2d_set_odom_req_t* set_odom_req;
00419 
00420       if (hdr->size != sizeof(player_position2d_set_odom_req_t)) {
00421          LOG_WARN("Arg to odometry set requests wrong size; ignoring");
00422          return -1;
00423       }
00424 
00425       set_odom_req = (player_position2d_set_odom_req_t*) data;
00426 
00427       LOG_INFO("req set odom");
00428 
00429       this->Publish(this->position_id, resp_queue,
00430                  PLAYER_MSGTYPE_RESP_ACK,
00431                  PLAYER_POSITION2D_REQ_SET_ODOM);
00432       return 0;
00433    }
00434    else if (Message::MatchMessage(hdr,PLAYER_MSGTYPE_REQ,
00435                            PLAYER_POSITION2D_REQ_MOTOR_POWER,
00436                            this->position_id)) {
00437 
00438       player_position2d_power_config_t* power_config;
00439 
00440       /* motor state change request
00441        *   1 = enable motors
00442        *   0 = disable motors (default)
00443        */
00444       if (hdr->size != sizeof(player_position2d_power_config_t)) {
00445          LOG_WARN("Arg to motor state change request wrong size; ignoring");
00446          return -1;
00447       }
00448 
00449       power_config = (player_position2d_power_config_t*) data;
00450 
00451       LOG_INFO("req motor power");
00452 
00453       this->Publish(this->position_id, resp_queue,
00454                  PLAYER_MSGTYPE_RESP_ACK,
00455                  PLAYER_POSITION2D_REQ_MOTOR_POWER);
00456       return 0;
00457    }
00458    else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
00459                            PLAYER_POSITION2D_REQ_RESET_ODOM,
00460                            this->position_id)) {
00461       /* reset position to 0,0,0: no args */
00462       if (hdr->size != 0) {
00463          PLAYER_WARN("Arg to reset position request is wrong size; ignoring");
00464          return(-1);
00465       }
00466 
00467       LOG_INFO("req reset odom");
00468 
00469       this->Publish(this->position_id, resp_queue,
00470                  PLAYER_MSGTYPE_RESP_ACK,
00471                  PLAYER_POSITION2D_REQ_RESET_ODOM);
00472       return 0;
00473    }
00474    else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
00475                            PLAYER_POSITION2D_REQ_GET_GEOM,
00476                            this->position_id)) {
00477       /* Return the robot geometry. */
00478       if (hdr->size != 0) {
00479          LOG_WARN("Arg get robot geom is wrong size; ignoring");
00480          return(-1);
00481       }
00482 
00483       player_position2d_geom_t geom;
00484 
00485       // FIXME
00486       memset(&geom, 0, sizeof(geom));
00487 
00488       LOG_INFO("req get geom");
00489 
00490       this->Publish(this->position_id, resp_queue,
00491                  PLAYER_MSGTYPE_RESP_ACK,
00492                  PLAYER_POSITION2D_REQ_GET_GEOM,
00493                  (void*) &geom, sizeof(geom), NULL);
00494       return 0;
00495    }
00496    else if (Message::MatchMessage(hdr,PLAYER_MSGTYPE_REQ,
00497                            PLAYER_POSITION2D_REQ_VELOCITY_MODE,
00498                            this->position_id)) {
00499 
00500       player_position2d_velocity_mode_config_t* velmode_config;
00501 
00502       /* velocity control mode:
00503        *   0 = direct wheel velocity control (default)
00504        *   1 = separate translational and rotational control
00505        */
00506       if (hdr->size != sizeof(player_position2d_velocity_mode_config_t)) {
00507          LOG_WARN("Arg to velocity control mode change "
00508                 "request is wrong size; ignoring");
00509          return -1;
00510       }
00511 
00512       velmode_config = (player_position2d_velocity_mode_config_t*) data;
00513 
00514       LOG_INFO("req velocity mode config");
00515 
00516       this->Publish(this->position_id, resp_queue,
00517                  PLAYER_MSGTYPE_RESP_ACK,
00518                  PLAYER_POSITION2D_REQ_VELOCITY_MODE);
00519       return 0;
00520    }
00521    else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
00522                            PLAYER_BUMPER_GET_GEOM,
00523                            this->bumper_id)) {
00524       /* Return the bumper geometry. */
00525       if(hdr->size != 0) {
00526          PLAYER_WARN("Arg get bumper geom is wrong size; ignoring");
00527          return -1;
00528       }
00529 
00530       player_bumper_geom_t geom;
00531 
00532       geom.bumper_def_count = 2;
00533 
00534       geom.bumper_def[0].pose.px = 0;
00535       geom.bumper_def[0].pose.py = 0;
00536       geom.bumper_def[0].pose.pa = 0;
00537       geom.bumper_def[0].length = 0.020;
00538       geom.bumper_def[0].radius = 0;
00539    
00540       geom.bumper_def[0].pose.px = 0;
00541       geom.bumper_def[0].pose.py = 0;
00542       geom.bumper_def[0].pose.pa = 0;
00543       geom.bumper_def[0].length = 0.020;
00544       geom.bumper_def[0].radius = 0;
00545 
00546       this->Publish(this->bumper_id, resp_queue,
00547                  PLAYER_MSGTYPE_RESP_ACK, PLAYER_BUMPER_GET_GEOM,
00548                  (void *) &geom, sizeof(geom), NULL);
00549 
00550       return 0;
00551    }
00552    else {
00553       LOG_WARN("unknown config request to ersp driver");
00554       return -1;
00555    }
00556 }
00557 
00558 int
00559 ERSP::HandleCommand(player_msghdr *hdr, void* data)
00560 {
00561    if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD,
00562                        PLAYER_POSITION2D_CMD_VEL,
00563                        this->position_id)) {
00564 
00565       const double SPEED_ACCELERATION = 20;
00566       const double ANGULAR_ACCELERATION = M_PI / 2.0; // radians/sec^2
00567       player_position2d_cmd_vel_t position_cmd;
00568       double speed, turn_rate;
00569 
00570       // get and send the latest motor command
00571       position_cmd = *(player_position2d_cmd_vel_t*)data;
00572 
00573       // m/sec -> cm/sec
00574       speed = position_cmd.vel.px * 100;
00575 
00576       // rad/sec
00577       turn_rate = position_cmd.vel.pa;
00578 
00579       LOG_INFO("Handle velocity cmd: speed=%f, turn_rate=%f, sacc=%f, tacc=%f",
00580           speed, turn_rate, SPEED_ACCELERATION, ANGULAR_ACCELERATION);
00581 
00582 #ifdef HAVE_ERSP
00583       // Player uses m/sec while ERSP uses cm/s
00584       devices[SCORPION_DRIVE_DRIVE].driver->move_and_turn(Evolution::NO_TICKET,
00585                            speed, SPEED_ACCELERATION,
00586                            turn_rate, ANGULAR_ACCELERATION);
00587 #endif /* HAVE_ERSP */
00588 
00589       return 0;
00590    }
00591 #if 0
00592    else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD,
00593                            PLAYER_POSITION2D_CMD_VEL,
00594                            this->position_id)) {
00595 
00596       ersp_driver->move_and_turn(Evolution::NO_TICKET, speed,
00597                            0, turn_rate, 0);
00598 
00599       return 0;
00600    }
00601 #endif
00602 
00603    return -1;
00604 }
00605 
00606 
00608 // Publish / Subscribe / Unsubscribe
00610 
00611 int
00612 ERSP::Subscribe(player_devaddr_t id)
00613 {
00614    int result;
00615 
00616    // Do the subscription ...
00617    if ((result = Driver::Subscribe(id)) == 0) {
00618       // ... and increment the appropriate subscription counter
00619       if (Device::MatchDeviceAddress(id, this->position_id)) {
00620          this->position_subscriptions++;
00621          LOG_INFO("%d position subscriptions",
00622                 this->position_subscriptions);
00623       }
00624       else if (Device::MatchDeviceAddress(id, this->bumper_id)) {
00625          this->bumper_subscriptions++;
00626          LOG_INFO("%d bumper subscriptions",
00627                 this->bumper_subscriptions);
00628       }
00629       else if (Device::MatchDeviceAddress(id, this->ir_range_id)) {
00630          this->ir_range_subscriptions++;
00631          LOG_INFO("%d ir subscriptions",
00632                 this->ir_range_subscriptions);
00633       }
00634       else if (Device::MatchDeviceAddress(id, this->ir_binary_id)) {
00635          this->ir_binary_subscriptions++;
00636          LOG_INFO("%d ir subscriptions",
00637                 this->ir_binary_subscriptions);
00638       }
00639 #ifdef POWER
00640       else if (Device::MatchDeviceAddress(id, this->power_id)) {
00641          this->power_subscriptions++;
00642          LOG_INFO("%d power subscriptions", this->power_subscriptions);
00643       }
00644 #endif
00645    }
00646 
00647    return result;
00648 }
00649 
00650 int
00651 ERSP::Unsubscribe(player_devaddr_t id)
00652 {
00653    int result;
00654 
00655    // Do the unsubscription ...
00656    if ((result = Driver::Unsubscribe(id)) == 0) {
00657       // ... and decrement the appropriate subscription counter
00658       if (Device::MatchDeviceAddress(id, this->position_id)) {
00659          this->position_subscriptions--;
00660          assert(this->position_subscriptions >= 0);
00661          LOG_INFO("%d position subscriptions",
00662                 this->position_subscriptions);
00663       }
00664       else if (Device::MatchDeviceAddress(id, this->bumper_id)) {
00665          this->bumper_subscriptions--;
00666          assert(this->bumper_subscriptions >= 0);
00667          LOG_INFO("%d bumper subscriptions",
00668                 this->bumper_subscriptions);
00669       }
00670       else if (Device::MatchDeviceAddress(id, this->ir_range_id)) {
00671          this->ir_range_subscriptions--;
00672          assert(this->ir_range_subscriptions >= 0);
00673          LOG_INFO("%d ir subscriptions",
00674                 this->ir_range_subscriptions);
00675       }
00676       else if (Device::MatchDeviceAddress(id, this->ir_binary_id)) {
00677          this->ir_binary_subscriptions--;
00678          assert(this->ir_binary_subscriptions >= 0);
00679          LOG_INFO("%d ir subscriptions",
00680                 this->ir_binary_subscriptions);
00681       }
00682 #ifdef POWER
00683       else if (Device::MatchDeviceAddress(id, this->power_id)) {
00684          this->power_subscriptions--;
00685          assert(this->power_subscriptions >= 0);
00686          LOG_INFO("%d power subscriptions", this->power_subscriptions);
00687       }
00688 #endif
00689    }
00690 
00691    return result;
00692 }
00693 
00694 void
00695 ERSP::PutData(void)
00696 {
00697    LOG_INFO("Publishing odometry data");
00698 
00699    // FIXME: Use {position,bumper}_subscriptions to only
00700    // send conditionally on the number of subscribers.
00701 
00702    if (position_subscriptions)
00703    this->Publish(this->position_id, NULL,
00704               PLAYER_MSGTYPE_DATA,
00705               PLAYER_POSITION2D_DATA_STATE,
00706               (void*)&(this->ersp_data.position),
00707               sizeof(player_position2d_data_t),
00708               NULL);
00709  
00710    if (bumper_subscriptions)
00711    this->Publish(this->bumper_id, NULL,
00712               PLAYER_MSGTYPE_DATA,
00713               PLAYER_BUMPER_DATA_STATE,
00714               (void*)&(this->ersp_data.bumper),
00715               sizeof(player_bumper_data_t),
00716               NULL);
00717    
00718    if (ir_range_subscriptions)
00719    this->Publish(this->ir_range_id, NULL,
00720               PLAYER_MSGTYPE_DATA,
00721               PLAYER_IR_DATA_RANGES,
00722               (void*)&(this->ersp_data.ir_range),
00723               sizeof(player_ir_data_t),
00724               NULL);
00725 
00726    if (ir_binary_subscriptions)
00727    this->Publish(this->ir_binary_id, NULL,
00728               PLAYER_MSGTYPE_DATA,
00729               PLAYER_BUMPER_DATA_STATE,
00730               (void*)&(this->ersp_data.ir_binary),
00731               sizeof(player_bumper_data_t),
00732               NULL);
00733 
00734 #ifdef POWER
00735    if (power_subscriptions)
00736    this->Publish(this->power_id, NULL,
00737               PLAYER_MSGTYPE_DATA,
00738               PLAYER_POWER_DATA_STATE,
00739               (void*)&(this->ersp_data.power),
00740               sizeof(player_power_data_t),
00741               NULL);
00742 #endif   
00743 }
00744 
00745 
00747 // Driver module registration and initializaion
00749 
00750 Driver*
00751 ERSP_Init(ConfigFile* cf, int section)
00752 {
00753    return (Driver*)(new ERSP(cf, section));
00754 }
00755 
00756 void ERSP_Register(DriverTable* table)
00757 {
00758    table->AddDriver("ersp", ERSP_Init);
00759 }
00760 
00761 /* Need the extern to avoid C++ name-mangling  */
00762 extern "C" {
00763 
00764 int player_driver_init(DriverTable* table)
00765 {
00766    LOG_INFO("Registering ERSP driver.");
00767    ERSP_Register(table);
00768 
00769    return 0;
00770 }
00771 
00772 }

Generated on Sat Jan 13 03:11:09 2007 for ERSP Player driver by  doxygen 1.5.1