sonar2ir.cc

Go to the documentation of this file.
00001 /*
00002  * Sonar2IR Player driver
00003  */
00004 
00067 #ifdef HAVE_CONFIG_H
00068 #include "config.h"
00069 #endif
00070 
00071 #include <string.h>
00072 #include <libplayercore/playercore.h>
00073 
00074 #define MAX_IR_SAMPLES(sonar_samples) \
00075    ((sonar_samples) <= PLAYER_IR_MAX_SAMPLES \
00076     ? (sonar_samples) : PLAYER_IR_MAX_SAMPLES)
00077 
00078 class Sonar2IR : public Driver 
00079 {
00080   private:
00081    // State data
00082    player_devaddr_t ir_id, sonar_id;
00083    player_ir_data_t ir_data;
00084    player_ir_pose_t ir_pose;
00085    player_sonar_power_config_t sonar_power;
00086    Device *sonar;
00087    bool republish_data,
00088         republish_pose,
00089         republish_power,
00090         republish_geom;
00091 
00092    // Publish state data.
00093    void PutData(void);
00094 
00095   public:
00096    Sonar2IR(ConfigFile *cf, int section);
00097    ~Sonar2IR(void);
00098 
00099    // Thread life-cycle
00100    void Main();
00101    int Setup();
00102    int Shutdown();
00103 
00104    // Message handling
00105    int Subscribe(player_devaddr_t id);
00106    int Unsubscribe(player_devaddr_t id);
00107    int ProcessMessage(MessageQueue *queue, player_msghdr *msghdr, void *data);
00108 };
00109 
00110 
00112 // Driver setup and shutdown
00114 
00115 Sonar2IR::Sonar2IR(ConfigFile* cf, int section)
00116     : Driver(cf, section, true, PLAYER_MSGQUEUE_DEFAULT_MAXLEN)
00117 {
00118    // zero ids, so that we'll know later which interfaces were requested
00119    memset(&ir_id, 0, sizeof(ir_id));
00120    memset(&sonar_id, 0, sizeof(sonar_id));
00121    subscriptions = 0;
00122    republish_data = republish_pose = false;
00123 
00124    if (cf->ReadDeviceAddr(&ir_id, section, "provides",
00125                           PLAYER_IR_CODE, -1, NULL) == 0) {
00126       if (this->AddInterface(ir_id) != 0) {
00127          this->SetError(-1);
00128          return;
00129       }
00130    }
00131 
00132    if (cf->ReadDeviceAddr(&sonar_id, section, "requires",
00133                           PLAYER_SONAR_CODE, -1, NULL) != 0) {
00134       this->SetError(-1);
00135       return;
00136    }
00137 }
00138 
00139 int Sonar2IR::Setup()
00140 {
00141    if (Device::MatchDeviceAddress(sonar_id, ir_id)) {
00142       PLAYER_ERROR("subscribing to self");
00143       return -1;
00144    }
00145 
00146    if (!(sonar = deviceTable->GetDevice(sonar_id))) {
00147       PLAYER_ERROR("sonar device unavailable");
00148       return -1;
00149    }
00150 
00151    if (sonar->Subscribe(InQueue) != 0) {
00152       PLAYER_ERROR("sonar device subscription failed");
00153       return -1;
00154    }
00155 
00156    StartThread();
00157    return 0;
00158 }
00159 
00160 int Sonar2IR::Shutdown()
00161 {
00162    StopThread();
00163    sonar->Unsubscribe(InQueue);
00164    return 0;
00165 }
00166 
00167 Sonar2IR::~Sonar2IR (void)
00168 {
00169 }
00170 
00171 void
00172 Sonar2IR::Main()
00173 {
00174    for(;;) {
00175       pthread_testcancel();
00176 
00177       // Wait for sonar messages
00178       InQueue->Wait();
00179 
00180       // Handle pending messages
00181       ProcessMessages();
00182 
00183       PutData();
00184    }
00185 }
00186 
00187 
00189 // Message processing
00191 
00192 int
00193 Sonar2IR::ProcessMessage(MessageQueue *queue, player_msghdr *hdr, void *data)
00194 {
00195    if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_DATA,
00196                              PLAYER_SONAR_DATA_RANGES, sonar_id)) {
00197       Lock();
00198       if (!republish_data) {
00199          player_sonar_data_t *sonar_data = (player_sonar_data_t *) data;
00200 
00201          memset(&ir_data, 0, sizeof(ir_data));
00202 
00203          ir_data.ranges_count = MAX_IR_SAMPLES(sonar_data->ranges_count);
00204 
00205          for (uint32_t i = 0; i < ir_data.ranges_count; i++) {
00206             ir_data.ranges[i] = sonar_data->ranges[i];
00207          }
00208 
00209          republish_data = true;
00210       }
00211       Unlock();
00212       return 0;
00213    }
00214 
00215    // The sonar interface docs suggest that data can arrive with both
00216    // message IDs.
00217    if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_DATA,
00218                              PLAYER_SONAR_DATA_GEOM, sonar_id) ||
00219        Message::MatchMessage(hdr, PLAYER_MSGTYPE_DATA,
00220                              PLAYER_SONAR_REQ_GET_GEOM, sonar_id)) {
00221       Lock();
00222       if (!republish_pose) {
00223          player_sonar_geom_t *sonar_geom = (player_sonar_geom_t *) data;
00224 
00225          memset(&ir_pose, 0, sizeof(ir_pose));
00226 
00227          ir_pose.poses_count = MAX_IR_SAMPLES(sonar_geom->poses_count);
00228 
00229          for (uint32_t i = 0; i < ir_pose.poses_count; i++) {
00230             ir_pose.poses[i] = sonar_geom->poses[i];
00231          }
00232 
00233          republish_pose = true;
00234       }
00235       Unlock();
00236       return 0;
00237    }
00238 
00239    // Forward requests to the sonar device
00240    if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_IR_POSE, ir_id)) {
00241       Lock();
00242       republish_geom = true;
00243       Unlock();
00244       return 0;
00245    }
00246 
00247    if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_IR_POWER, ir_id)) {
00248       Lock();
00249       if (!republish_power) {
00250          player_ir_power_req_t *ir_power;
00251 
00252          ir_power = (player_ir_power_req_t *) data;
00253          sonar_power.state = ir_power->state;
00254          republish_power = true;
00255       }
00256       Unlock();
00257       return 0;
00258    }
00259 
00260    return -1;
00261 }
00262    
00263 
00265 // Publish / Subscribe / Unsubscribe
00267 
00268 void
00269 Sonar2IR::PutData(void)
00270 {
00271    Lock();
00272    if (republish_data) {
00273       Unlock();
00274       Publish(ir_id, NULL, PLAYER_MSGTYPE_DATA,
00275               PLAYER_IR_DATA_RANGES,
00276               (void *) &ir_data, sizeof(ir_data));
00277       Lock();
00278       republish_data = republish_pose = false;
00279    }
00280    Unlock();
00281 
00282    Lock();
00283    if (republish_pose) {
00284       Unlock();
00285       Publish(ir_id, NULL, PLAYER_MSGTYPE_DATA, PLAYER_IR_POSE,
00286               (void *) &ir_pose, sizeof(ir_pose));
00287       Lock();
00288       republish_pose = false;
00289    }
00290    Unlock();
00291 
00292    Lock();
00293    if (republish_geom) {
00294       Unlock();
00295       Publish(sonar_id, NULL, PLAYER_MSGTYPE_REQ, PLAYER_SONAR_REQ_GET_GEOM);
00296       Lock();
00297       republish_pose = false;
00298    }
00299    Unlock();
00300 
00301    Lock();
00302    if (republish_power) {
00303       Unlock();
00304       Publish(sonar_id, NULL, PLAYER_MSGTYPE_REQ, PLAYER_SONAR_REQ_POWER,
00305               (void *) &sonar_power, sizeof(sonar_power));
00306       Lock();
00307       republish_pose = false;
00308    }
00309    Unlock();
00310 }
00311 
00312 int
00313 Sonar2IR::Subscribe(player_devaddr_t id)
00314 {
00315    return Driver::Subscribe(id);
00316 }
00317 
00318 int
00319 Sonar2IR::Unsubscribe(player_devaddr_t id)
00320 {
00321    return Driver::Unsubscribe(id);
00322 }
00323 
00324 
00326 // Driver module registration and initializaion
00328 
00329 Driver*
00330 Sonar2IR_Init(ConfigFile* cf, int section)
00331 {
00332    return (Driver*)(new Sonar2IR(cf, section));
00333 }
00334 
00335 void Sonar2IR_Register(DriverTable* table)
00336 {
00337    table->AddDriver("sonar2ir", Sonar2IR_Init);
00338 }
00339 
00340 /* Need the extern to avoid C++ name-mangling  */
00341 extern "C" {
00342 
00343 int player_driver_init(DriverTable* table)
00344 {
00345    Sonar2IR_Register(table);
00346    return 0;
00347 }
00348 
00349 }

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