wonderwall.cc

Go to the documentation of this file.
00001 /*
00002  * Sonar2IR Player driver
00003  */
00004 
00005 #include "sonar2ir.h"
00006 
00007 
00009 // Driver module registration and initializaion
00011 
00012 Driver*
00013 Sonar2IR_Init(ConfigFile* cf, int section)
00014 {
00015    return (Driver*)(new Sonar2IR(cf, section));
00016 }
00017 
00018 void Sonar2IR_Register(DriverTable* table)
00019 {
00020    table->AddDriver("sonar2ir", Sonar2IR_Init);
00021 }
00022 
00023 /* Need the extern to avoid C++ name-mangling  */
00024 extern "C" {
00025 
00026 int player_driver_init(DriverTable* table)
00027 {
00028    Sonar2IR_Register(table);
00029    return 0;
00030 }
00031 
00032 }
00033 
00035 // Driver setup and shutdown
00037 
00038 Sonar2IR::Sonar2IR(ConfigFile* cf, int section)
00039     : Driver(cf, section, true, PLAYER_MSGQUEUE_DEFAULT_MAXLEN)
00040 {
00041    // zero ids, so that we'll know later which interfaces were requested
00042    memset(&ir_id, 0, sizeof(ir_id));
00043    memset(&sonar_id, 0, sizeof(sonar_id));
00044    subscriptions = 0;
00045    republish_data = republish_pose = false;
00046 
00047    if (cf->ReadDeviceAddr(&ir_id, section, "provides",
00048                           PLAYER_IR_CODE, -1, NULL) == 0) {
00049       if (this->AddInterface(ir_id) != 0) {
00050          this->SetError(-1);
00051          return;
00052       }
00053    }
00054 
00055    if (cf->ReadDeviceAddr(&sonar_id, section, "requires",
00056                           PLAYER_SONAR_CODE, -1, NULL) != 0) {
00057       this->SetError(-1);
00058       return;
00059    }
00060 }
00061 
00062 int Sonar2IR::Setup()
00063 {
00064    // Subscribe to the sonar.
00065    if (Device::MatchDeviceAddress(sonar_id, ir_id)) {
00066       PLAYER_ERROR("subscribing to self");
00067       return -1;
00068    }
00069 
00070    if (!(sonar_dev = deviceTable->GetDevice(sonar_id))) {
00071       PLAYER_ERROR("sonar device unavailable");
00072       return -1;
00073    }
00074 
00075    if (sonar_dev->Subscribe(InQueue) != 0) {
00076       PLAYER_ERROR("sonar device subscription failed");
00077       return -1;
00078    }
00079 
00080    StartThread();
00081    return 0;
00082 }
00083 
00084 int Sonar2IR::Shutdown()
00085 {
00086    StopThread();
00087    sonar_dev->Unsubscribe(InQueue);
00088    return 0;
00089 }
00090 
00091 Sonar2IR::~Sonar2IR (void)
00092 {
00093 }
00094 
00095 void
00096 Sonar2IR::Main()
00097 {
00098    for(;;) {
00099       pthread_testcancel();
00100 
00101       // Wait for sonar messages
00102       InQueue->Wait();
00103 
00104       // Handle pending messages
00105       ProcessMessages();
00106 
00107       PutData();
00108    }
00109 }
00110 
00111 
00113 // Message processing
00115 
00116 int
00117 Sonar2IR::ProcessMessage(MessageQueue *queue, player_msghdr *hdr, void *data)
00118 {
00119    if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_DATA,
00120                              PLAYER_SONAR_DATA_RANGES, sonar_id)) {
00121       Lock();
00122       if (!republish_data) {
00123          player_sonar_data_t *sonar_data = (player_sonar_data_t *) data;
00124 
00125          memset(&ir_data, 0, sizeof(ir_data));
00126 
00127          ir_data.ranges_count = sonar_data->ranges_count;
00128          if (ir_data.ranges_count > PLAYER_IR_MAX_SAMPLES)
00129             ir_data.ranges_count = PLAYER_IR_MAX_SAMPLES;
00130 
00131          for (uint32_t i = 0; i < ir_data.ranges_count; i++) {
00132             ir_data.ranges[i] = sonar_data->ranges[i];
00133          }
00134 
00135          republish_data = true;
00136       }
00137       Unlock();
00138       return 0;
00139    }
00140 
00141    if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_DATA,
00142                              PLAYER_SONAR_DATA_GEOM, sonar_id)) {
00143       Lock();
00144       if (!republish_pose) {
00145          player_sonar_geom_t *sonar_geom = (player_sonar_geom_t *) data;
00146 
00147          memset(&ir_pose, 0, sizeof(ir_pose));
00148 
00149          ir_pose.poses_count = sonar_geom->poses_count;
00150          if (ir_pose.poses_count > PLAYER_IR_MAX_SAMPLES)
00151             ir_pose.poses_count = PLAYER_IR_MAX_SAMPLES;
00152 
00153          for (uint32_t i = 0; i < ir_pose.poses_count; i++) {
00154             ir_pose.poses[i] = sonar_geom->poses[i];
00155          }
00156 
00157          republish_pose = true;
00158       }
00159       Unlock();
00160       return 0;
00161    }
00162 
00163    // Forward requests to the sonar device
00164    if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
00165                              PLAYER_IR_POSE, ir_id) ||
00166       Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
00167                              PLAYER_IR_POWER, ir_id)) {
00168       Publish(sonar_id, NULL, hdr->type, hdr->subtype,
00169               data, hdr->size);
00170       return 0;
00171    }
00172 
00173    return -1;
00174 }
00175 
00176 int
00177 Sonar2IR::HandleConfig(MessageQueue* queue,
00178                player_msghdr * hdr,
00179                void * data)
00180 {
00181    PLAYER_WARN("unknown config request to sonar2ir driver");
00182    return -1;
00183 }
00184 
00185 int
00186 Sonar2IR::HandleCommand(player_msghdr *hdr, void* data)
00187 {
00188    PLAYER_WARN("unknown command to sonar2ir driver");
00189    return -1;
00190 }
00191 
00192 
00194 // Publish / Subscribe / Unsubscribe
00196 
00197 int
00198 Sonar2IR::Subscribe(player_devaddr_t id)
00199 {
00200    int result;
00201 
00202    if ((result = Driver::Subscribe(id)) == 0) {
00203       if (Device::MatchDeviceAddress(id, ir_id)) {
00204          subscriptions++;
00205       }
00206    }
00207 
00208    return result;
00209 }
00210 
00211 int
00212 Sonar2IR::Unsubscribe(player_devaddr_t id)
00213 {
00214    int result;
00215 
00216    if ((result = Driver::Unsubscribe(id)) == 0) {
00217       if (Device::MatchDeviceAddress(id, ir_id)) {
00218          subscriptions--;
00219          assert(subscriptions >= 0);
00220       }
00221    }
00222 
00223    return result;
00224 }
00225 
00226 void
00227 Sonar2IR::PutData(void)
00228 {
00229    Lock();
00230    if (republish_data) {
00231       Unlock();
00232       Publish(ir_id, NULL, PLAYER_MSGTYPE_DATA,
00233               PLAYER_IR_DATA_RANGES,
00234               (void *) &ir_data, sizeof(ir_data), NULL);
00235       Lock();
00236       republish_data = republish_pose = false;
00237    }
00238    Unlock();
00239 
00240    Lock();
00241    if (republish_pose) {
00242       Unlock();
00243       Publish(ir_id, NULL, PLAYER_MSGTYPE_DATA, PLAYER_IR_POSE,
00244               (void *) &ir_pose, sizeof(ir_pose), NULL);
00245       Lock();
00246       republish_pose = false;
00247    }
00248    Unlock();
00249 }

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