00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00039
00040
00041 #include "p_driver.h"
00042 using namespace Stg;
00043
00044 InterfaceRanger::InterfaceRanger( player_devaddr_t addr,
00045 StgDriver* driver,
00046 ConfigFile* cf,
00047 int section )
00048 : InterfaceModel( addr, driver, cf, section, "ranger" )
00049 {
00050 this->scan_id = 0;
00051 }
00052
00053 void InterfaceRanger::Publish( void )
00054 {
00055 ModelRanger* rgr = dynamic_cast<ModelRanger*>(this->mod);
00056
00057
00058
00059
00060
00061
00062 std::vector<ModelRanger::Sensor>& sensors = rgr->GetSensorsMutable();
00063
00064 player_ranger_data_range_t prange;
00065 memset( &prange, 0, sizeof(prange) );
00066
00067 player_ranger_data_intns_t pintens;
00068 memset( &pintens, 0, sizeof(pintens) );
00069
00070 std::vector<double> rv, iv;
00071
00072 if( sensors.size() == 1 )
00073 {
00074 prange.ranges_count = sensors[0].ranges.size();
00075 prange.ranges = prange.ranges_count ? &sensors[0].ranges[0] : NULL;
00076
00077 pintens.intensities_count = sensors[0].intensities.size();
00078 pintens.intensities = pintens.intensities_count ? &sensors[0].intensities[0] : NULL;
00079 }
00080 else
00081 {
00082 FOR_EACH( it, sensors )
00083 {
00084 if( it->ranges.size() )
00085 rv.push_back( it->ranges[0] );
00086
00087 if( it->intensities.size() )
00088 iv.push_back( it->intensities[0] );
00089 }
00090
00091 prange.ranges_count = rv.size();
00092 prange.ranges = rv.size() ? &rv[0] : NULL;
00093
00094 pintens.intensities_count = iv.size();
00095 pintens.intensities = iv.size() ? &iv[0] : NULL;
00096 }
00097
00098 if( prange.ranges_count )
00099 this->driver->Publish(this->addr,
00100 PLAYER_MSGTYPE_DATA,
00101 PLAYER_RANGER_DATA_RANGE,
00102 (void*)&prange, sizeof(prange), NULL);
00103
00104 if( pintens.intensities_count )
00105 this->driver->Publish(this->addr,
00106 PLAYER_MSGTYPE_DATA,
00107 PLAYER_RANGER_DATA_INTNS,
00108 (void*)&pintens, sizeof(pintens), NULL);
00109 }
00110
00111
00112 int InterfaceRanger::ProcessMessage(QueuePointer & resp_queue,
00113 player_msghdr_t* hdr,
00114 void* data)
00115 {
00116 ModelRanger* mod = (ModelRanger*)this->mod;
00117
00118
00119 if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
00120 PLAYER_RANGER_REQ_GET_CONFIG,
00121 this->addr))
00122 {
00123 if( hdr->size == 0 )
00124 {
00125
00126
00127 PRINT_WARN( "stageplugin ranger config describes only the first sensor of the ranger." );
00128
00129 player_ranger_config_t prc;
00130 bzero(&prc,sizeof(prc));
00131
00132 const ModelRanger::Sensor& s = mod->GetSensors()[0];
00133
00134 prc.min_angle = -s.fov/2.0;
00135 prc.max_angle = +s.fov/2.0;
00136 prc.angular_res = s.fov / (double)s.sample_count;
00137 prc.max_range = s.range.max;
00138 prc.min_range = s.range.min;
00139 prc.range_res = 1.0 / mod->GetWorld()->Resolution();
00140 prc.frequency = 1.0E6 / mod->GetInterval();
00141
00142 this->driver->Publish(this->addr, resp_queue,
00143 PLAYER_MSGTYPE_RESP_ACK,
00144 PLAYER_RANGER_REQ_GET_CONFIG,
00145 (void*)&prc, sizeof(prc), NULL);
00146 return(0);
00147 }
00148 else
00149 {
00150 PRINT_ERR2("config request len is invalid (%d != %d)", (int)hdr->size,0);
00151 return(-1);
00152 }
00153 }
00154 else
00155 if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
00156 PLAYER_RANGER_REQ_GET_GEOM,
00157 this->addr))
00158 {
00159 if(hdr->size == 0)
00160 {
00161 Geom geom = mod->GetGeom();
00162 Pose pose = mod->GetPose();
00163
00164 const std::vector<ModelRanger::Sensor>& sensors = mod->GetSensors();
00165
00166
00167 player_ranger_geom_t pgeom;
00168 bzero( &pgeom, sizeof(pgeom));
00169 pgeom.pose.px = pose.x;
00170 pgeom.pose.py = pose.y;
00171 pgeom.pose.pyaw = pose.a;
00172 pgeom.size.sl = geom.size.x;
00173 pgeom.size.sw = geom.size.y;
00174
00175 pgeom.element_poses_count = pgeom.element_sizes_count = sensors.size();
00176
00177 player_pose3d_t poses[ sensors.size() ];
00178 player_bbox3d_t sizes[ sensors.size() ];
00179
00180 for( size_t s=0; s<pgeom.element_poses_count; s++ )
00181 {
00182 poses[s].px = sensors[s].pose.x;
00183 poses[s].py = sensors[s].pose.y;
00184 poses[s].pz = sensors[s].pose.z;
00185 poses[s].proll = 0.0;
00186 poses[s].ppitch = 0.0;
00187 poses[s].pyaw = sensors[s].pose.a;
00188
00189 sizes[s].sw = sensors[s].size.x;
00190 sizes[s].sl = sensors[s].size.y;
00191 sizes[s].sh = sensors[s].size.z;
00192 }
00193
00194 pgeom.element_poses = poses;
00195 pgeom.element_sizes = sizes;
00196
00197 this->driver->Publish(this->addr, resp_queue,
00198 PLAYER_MSGTYPE_RESP_ACK,
00199 PLAYER_RANGER_REQ_GET_GEOM,
00200 (void*)&pgeom, sizeof(pgeom), NULL);
00201 return(0);
00202 }
00203 else
00204 {
00205 PRINT_ERR2("config request len is invalid (%d != %d)", (int)hdr->size,0);
00206 return(-1);
00207 }
00208 }
00209
00210
00211 PRINT_WARN2( "stage ranger doesn't support message %d:%d.",
00212 hdr->type, hdr->subtype);
00213 return(-1);
00214 }
00215
00216