// Update the filter with some new sensor observation
void pf_update_sensor(pf_t *pf, pf_sensor_model_fn_t sensor_fn, void *sensor_data)
{
int i;
pf_sample_set_t *set;
pf_sample_t *sample;
double total;
set = pf->sets + pf->current_set;
// Compute the sample weights
total = (*sensor_fn) (sensor_data, set);
if (total > 0.0)
{
// Normalize weights
for (i = 0; i < set->sample_count; i++)
{
sample = set->samples + i;
sample->weight /= total;
}
}
else
{
PLAYER_WARN("pdf has zero probability");
// Handle zero total
for (i = 0; i < set->sample_count; i++)
{
sample = set->samples + i;
sample->weight = 1.0 / set->sample_count;
}
}
return;
}
EpuckDriver::EpuckDriver(ConfigFile* cf, int section)
:ThreadedDriver(cf, section, true, PLAYER_MSGQUEUE_DEFAULT_MAXLEN)
,EXPECTED_EPUCK_SIDE_VERSION(300) // Version 3.0
{
std::string strSerialPort(cf->ReadString(section, "port", "/dev/rfcomm0"));
this->serialPort = new SerialPort(strSerialPort);
//-------------------- POSITION2D
if(cf->ReadDeviceAddr(&this->position2dAddr, section, "provides",
PLAYER_POSITION2D_CODE, -1, NULL) == 0)
{
if(this->AddInterface(this->position2dAddr) != 0)
{
this->SetError(-1);
return;
}
this->epuckPosition2d.reset(new EpuckPosition2d(this->serialPort));
}
//-------------------- IR
if(cf->ReadDeviceAddr(&this->irAddr, section, "provides",
PLAYER_IR_CODE, -1, NULL) == 0)
{
if(this->AddInterface(this->irAddr) != 0)
{
this->SetError(-1);
return;
}
this->epuckIR.reset(new EpuckIR(this->serialPort));
}
//-------------------- CAMERA
if(cf->ReadDeviceAddr(&this->cameraAddr, section, "provides",
PLAYER_CAMERA_CODE , -1, NULL) == 0)
{
if(this->AddInterface(this->cameraAddr) != 0)
{
this->SetError(-1);
return;
}
int sensor_x1 = cf->ReadInt(section, "sensor_x1", 240);
int sensor_y1 = cf->ReadInt(section, "sensor_y1", 160);
int sensor_width = cf->ReadInt(section, "sensor_width", 160);
int sensor_height = cf->ReadInt(section, "sensor_height", 160);
int zoom_fact_width = cf->ReadInt(section, "zoom_fact_width", 4);
int zoom_fact_height = cf->ReadInt(section, "zoom_fact_height", 4);
EpuckCamera::ColorModes color_mode;
std::string strColorMode(cf->ReadString(section,
"color_mode",
"GREY_SCALE_MODE"));
if(strColorMode == "GREY_SCALE_MODE")
{
color_mode = EpuckCamera::GREY_SCALE_MODE;
}
else if(strColorMode == "RGB_565_MODE")
{
color_mode = EpuckCamera::RGB_565_MODE;
}
else if(strColorMode == "YUV_MODE")
{
color_mode = EpuckCamera::YUV_MODE;
}
else
{
PLAYER_WARN("Invalid camera color mode, using default grey scale mode.");
color_mode = EpuckCamera::GREY_SCALE_MODE;
}
this->epuckCamera.reset(new EpuckCamera(this->serialPort,
sensor_x1, sensor_y1,
sensor_width, sensor_height,
zoom_fact_width, zoom_fact_height,
color_mode));
}
//-------------------- RING LEDS
const char* key[]={"ring_led0", "ring_led1", "ring_led2", "ring_led3",
"ring_led4", "ring_led5", "ring_led6", "ring_led7"};
for(unsigned led = 0; led < EpuckLEDs::RING_LEDS_NUM; ++led)
{
if(cf->ReadDeviceAddr(&this->ringLEDAddr[led], section, "provides",
PLAYER_BLINKENLIGHT_CODE, -1, key[led]) == 0)
{
if(this->AddInterface(this->ringLEDAddr[led]) != 0)
{
this->SetError(-1);
return;
}
if(this->epuckLEDs.get() == NULL)
this->epuckLEDs.reset(new EpuckLEDs(this->serialPort));
}
}
//-------------------- FRONT LED
if(cf->ReadDeviceAddr(&this->frontLEDAddr, section, "provides",
PLAYER_BLINKENLIGHT_CODE, -1, "front_led") == 0)
{
if(this->AddInterface(this->frontLEDAddr) != 0)
{
this->SetError(-1);
return;
}
if(this->epuckLEDs.get() == NULL)
this->epuckLEDs.reset(new EpuckLEDs(this->serialPort));
}
//-------------------- BODY LED
if(cf->ReadDeviceAddr(&this->bodyLEDAddr, section, "provides",
PLAYER_BLINKENLIGHT_CODE, -1, "body_led") == 0)
{
if(this->AddInterface(this->bodyLEDAddr) != 0)
{
this->SetError(-1);
return;
}
if(this->epuckLEDs.get() == NULL)
this->epuckLEDs.reset(new EpuckLEDs(this->serialPort));
}
}
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;
}
}
//------------------------------------------------------------------------------
// Process all messages for this driver.
int PingerDriver::ProcessMessage( QueuePointer& respQueue,
player_msghdr* pHeader, void* pData )
{
if ( Message::MatchMessage(
pHeader, PLAYER_MSGTYPE_DATA, PLAYER_OPAQUE_DATA_STATE, mOpaqueID ) )
{
player_opaque_data_t* pOpaqueData = (player_opaque_data_t*)pData;
if ( pOpaqueData->data_count <= mBuffer.GetFreeSpace() )
{
mBuffer.TryToAddBytes( pOpaqueData->data, pOpaqueData->data_count );
}
else
{
PLAYER_WARN( "Pinger driver buffer is full. Dropping data" );
}
return 0;
}
else if ( Message::MatchMessage(
pHeader, PLAYER_MSGTYPE_CMD, PLAYER_DSPIC_CMD_SAY, this->device_addr ) )
{
player_dspic_cmd_say* pCmd = (player_dspic_cmd_say*)pData;
U8* str;
player_opaque_data_t mData;
printf( "Got %s\n", pCmd->string );
pic_cmd_t thecmd = picDecodeCmd(pCmd->string);
if (thecmd!=picCMD_UNKNOWN)
switch(thecmd) {
case picFIRE_TRANSDUCER_0:
mData.data_count = 4;
str = new U8[4];
str[0] = (U8)'F';
str[1] = (U8)'S';
str[2] = (U8)'0';
str[3] = (U8)'\r';
mData.data = str;
transducer = 0;
mpOpaque->PutMsg(this->InQueue,
PLAYER_MSGTYPE_CMD,
PLAYER_OPAQUE_CMD_DATA,
&mData, 0, NULL);
state = stWaitingActiveECho;
delete [] str;
break;
case picFIRE_TRANSDUCER_1:
mData.data_count = 4;
str = new U8[4];
str[0] = (U8)'F';
str[1] = (U8)'S';
str[2] = (U8)'1';
str[3] = (U8)'\r';
mData.data = str;
transducer = 1;
mpOpaque->PutMsg(this->InQueue,
PLAYER_MSGTYPE_CMD,
PLAYER_OPAQUE_CMD_DATA,
&mData, 0, NULL);
state = stWaitingActiveECho;
delete [] str;
break;
case picLISTEN_TRANSDUCER_0:
mData.data_count = 4;
str = new U8[4];
str[0] = (U8)'L';
str[1] = (U8)'S';
str[2] = (U8)'0';
str[3] = (U8)'\r';
mData.data = str;
transducer = 0;
mpOpaque->PutMsg(this->InQueue,
PLAYER_MSGTYPE_CMD,
PLAYER_OPAQUE_CMD_DATA,
&mData, 0, NULL);
state = stWaitingPassiveEcho;
delete [] str;
break;
case picLISTEN_TRANSDUCER_1:
mData.data_count = 4;
str = new U8[4];
str[0] = (U8)'L';
str[1] = (U8)'S';
str[2] = (U8)'1';
str[3] = (U8)'\r';
mData.data = str;
transducer = 1;
mpOpaque->PutMsg(this->InQueue,
PLAYER_MSGTYPE_CMD,
PLAYER_OPAQUE_CMD_DATA,
&mData, 0, NULL);
state = stWaitingPassiveEcho;
delete [] str;
break;
case picSEND_ANGULAR_VELOCITY:
mData.data_count = 4;
str = new U8[4];
str[0] = (U8)'A';
str[1] = (U8)'N';
str[2] = (U8)'G';
str[3] = (U8)'\r';
mData.data = str;
mpOpaque->PutMsg(this->InQueue,
PLAYER_MSGTYPE_CMD,
PLAYER_OPAQUE_CMD_DATA,
&mData, 0, NULL);
state = stWaitingAngVelocity;
delete [] str;
break;
case picSEND_ACCELERATION_X:
mData.data_count = 4;
str = new U8[4];
str[0] = (U8)'A';
str[1] = (U8)'C';
str[2] = (U8)'X';
str[3] = (U8)'\r';
mData.data = str;
mpOpaque->PutMsg(this->InQueue,
PLAYER_MSGTYPE_CMD,
PLAYER_OPAQUE_CMD_DATA,
&mData, 0, NULL);
state = stWaitingAcceleration;
delete [] str;
break;
case picSEND_ACCELERATION_Y:
mData.data_count = 4;
str = new U8[4];
str[0] = (U8)'A';
str[1] = (U8)'C';
str[2] = (U8)'Y';
str[3] = (U8)'\r';
mData.data = str;
mpOpaque->PutMsg(this->InQueue,
PLAYER_MSGTYPE_CMD,
PLAYER_OPAQUE_CMD_DATA,
&mData, 0, NULL);
state = stWaitingAcceleration;
delete [] str;
break;
}
return 0;
}
PLAYER_WARN( "Unhandled message\n" );
return -1;
}
//------------------------------------------------------------------------------
// Constructor. Retrieve options from the configuration file and do any
// pre-Setup() setup.
MotorDriver::MotorDriver( ConfigFile* pConfigFile, int section )
: ThreadedDriver( pConfigFile, section, false,
PLAYER_MSGQUEUE_DEFAULT_MAXLEN, PLAYER_POSITION3D_CODE ),
mbCompassAvailable( false ),
mLastDisplayedCompassAngleTimestamp( 0.0 ),
mbDepthSensorAvailable( false ),
mLastDisplayedDepthSensorDepthTimestamp( 0.0 ),
mbInitialisedPWM( false )
{
this->alwayson = true;
if ( !pwm_Initialize( 0xffff, PWMCLOCK_50MHZ, PWMIRQ_DISABLE ) )
{
printf( "Unable to initialise PWM library - %s\n", roboio_GetErrMsg() );
mbInitialisedPWM = false;
}
else
{
printf( "PWM library initialised\n" );
mbInitialisedPWM = true;
// Set the channels to produce a zero velocity PWM
pwm_SetPulse( LEFT_MOTOR_CHANNEL, PWM_FREQUENCY_US, ZERO_DUTY_CYCLE_US + LEFT_PWM_OFFSET );
pwm_SetPulse( RIGHT_MOTOR_CHANNEL, PWM_FREQUENCY_US, ZERO_DUTY_CYCLE_US + RIGHT_PWM_OFFSET );
pwm_SetPulse( FRONT_MOTOR_CHANNEL, PWM_FREQUENCY_US, ZERO_DUTY_CYCLE_US + FRONT_PWM_OFFSET );
pwm_SetPulse( BACK_MOTOR_CHANNEL, PWM_FREQUENCY_US, ZERO_DUTY_CYCLE_US + BACK_PWM_OFFSET );
pwm_SetPulse( TEST_CHANNEL, PWM_FREQUENCY_US, ZERO_DUTY_CYCLE_US );
pwm_SetCountingMode( LEFT_MOTOR_CHANNEL, PWM_CONTINUE_MODE );
pwm_SetCountingMode( RIGHT_MOTOR_CHANNEL, PWM_CONTINUE_MODE );
pwm_SetCountingMode( FRONT_MOTOR_CHANNEL, PWM_CONTINUE_MODE );
pwm_SetCountingMode( BACK_MOTOR_CHANNEL, PWM_CONTINUE_MODE );
pwm_SetCountingMode( TEST_CHANNEL, PWM_CONTINUE_MODE );
// Enable the pins
pwm_EnablePin( LEFT_MOTOR_CHANNEL );
pwm_EnablePin( FRONT_MOTOR_CHANNEL );
pwm_EnablePin( RIGHT_MOTOR_CHANNEL );
pwm_EnablePin( BACK_MOTOR_CHANNEL );
pwm_EnablePin( TEST_CHANNEL );
printf( "All go\n" );
//printf( "Outputting many pulses\n" );
pwm_EnableMultiPWM(0xffffffffL);
printf( "Enabled everything\n" );
}
mpCompass = NULL;
// See if we have an imu (compass) device
if ( pConfigFile->ReadDeviceAddr( &mCompassID, section, "requires",
PLAYER_IMU_CODE, -1, NULL ) != 0 )
{
PLAYER_WARN( "No Compass driver specified" );
}
else
{
mbCompassAvailable = true;
}
mpDepthSensor = NULL;
// See if we have a depth sensor
if ( pConfigFile->ReadDeviceAddr( &mDepthSensorID, section, "requires",
PLAYER_POSITION1D_CODE, -1, NULL ) != 0 )
{
PLAYER_WARN( "No Depth Sensor specified" );
}
else
{
mbDepthSensorAvailable = true;
}
}
/**
@brief Update the device data (the data going back to the client).
*/
void CameraV4L2::WriteData()
{
size_t image_size, size;
unsigned char * ptr1, * ptr2;
struct my_buffer *v4lBuffer = getFrameBuffer(fg);
if (v4lBuffer==NULL)
exit(1);
ptr1 = (unsigned char *)v4lBuffer->start;
ptr2 = this->data.image;
// Compute size of image
image_size = this->width * this->height * this->depth / 8;
// Set the image properties
this->data.width = htons(this->width);
this->data.height = htons(this->height);
this->data.bpp = this->depth;
this->data.compression = PLAYER_CAMERA_COMPRESS_RAW;
this->data.image_size = htonl(image_size);
if (image_size > sizeof(this->data.image)){
PLAYER_ERROR2("image_size <= sizeof(this->data.image) failed: %d > %d",
image_size, sizeof(this->data.image));
}
assert(image_size <= sizeof(this->data.image));
if (image_size > (size_t) v4lBuffer->length){
PLAYER_WARN("Frame size is smaller then expected");
image_size = (size_t) v4lBuffer->length;
}
//assert(image_size <= (size_t) v4lBuffer->length);
if (!flip_rb) {
memcpy(ptr2, ptr1, image_size);
} else {
int imgSize = ((this->width) * (this->height));
int i;
switch (v4l2_type_id)
{
case V4L2_PIX_FMT_RGB24:
case V4L2_PIX_FMT_BGR24:
for (i = 0; i < imgSize; i++)
{
ptr2[0] = ptr1[2];
ptr2[1] = ptr1[1];
ptr2[2] = ptr1[0];
ptr1 += 3;
ptr2 += 3;
}
break;
case V4L2_PIX_FMT_RGB32:
case V4L2_PIX_FMT_BGR32:
for (i = 0; i < imgSize; i++)
{
ptr2[0] = ptr1[2];
ptr2[1] = ptr1[1];
ptr2[2] = ptr1[0];
ptr2[3] = ptr1[3];
ptr1 += 4;
ptr2 += 4;
}
break;
default:
memcpy(ptr2, ptr1, image_size);
}
}
// Copy data to server
size = sizeof(this->data) - sizeof(this->data.image) + image_size;
struct timeval timestamp;
timestamp.tv_sec = this->tsec;
timestamp.tv_usec = this->tusec;
PutData((void*) &this->data, size, ×tamp);
giveBackFrameBuffer(fg, v4lBuffer);
return;
}
