bool DPipe::ismouse()
{
vector<SDL_Point> points=vision();
if((event.button.x>points[0].x && event.button.x<points[1].x) && (event.button.y>points[0].y && event.button.y<points[2].y))
return true;
else
return false;
}
void ScavTaskColorShirt::executeTask(int timeout, TaskResult &result, std::string &record)
{
boost::thread motion( &ScavTaskColorShirt::motionThread, this);
boost::thread vision( &ScavTaskColorShirt::visionThread, this);
motion.join();
vision.join();
record = path_to_image;
result = SUCCEEDED;
}
void ScavTaskWhiteBoard::executeTask(int timeout, TaskResult &result, std::string &record) {
boost::thread motion( &ScavTaskWhiteBoard::motionThread, this);
boost::thread vision( &ScavTaskWhiteBoard::visionThread, this);
ros::Rate r(2);
while (ros::ok() and r.sleep()) {
if (task_completed) {
search_planner->cancelCurrentGoal();
break;
}
}
motion.detach();
vision.detach();
record = wb_path_to_image;
result = SUCCEEDED;
}
/// If @a base_value is set to true, do not fall back to movement().
int vision(bool base_value) const { return base_value ? vision_ : vision(); }
void Sensor2DeviceImpl::onTrackerMessage(Tracker2Message* message)
{
if (message->Type != Tracker2Message_Sensors)
return;
const float sampleIntervalTimeUnit = (1.0f / 1000.f);
double scaledSampleIntervalTimeUnit = sampleIntervalTimeUnit;
Tracker2Sensors& s = message->Sensors;
double absoluteTimeSeconds = 0.0;
if (SequenceValid)
{
UInt32 runningSampleCountDelta;
if (s.RunningSampleCount < LastRunningSampleCount)
{
// The running sample count on the device rolled around the 16 bit counter
// (expect to happen about once per minute), so RunningSampleCount
// needs a high word increment.
runningSampleCountDelta = ((((int)s.RunningSampleCount) + 0x10000) - (int)LastRunningSampleCount);
}
else
{
runningSampleCountDelta = (s.RunningSampleCount - LastRunningSampleCount);
}
absoluteTimeSeconds = LastSensorTime.TimeSeconds;
scaledSampleIntervalTimeUnit = TimeFilter.ScaleTimeUnit(sampleIntervalTimeUnit);
// If we missed a small number of samples, replicate the last sample.
if ((runningSampleCountDelta > LastNumSamples) && (runningSampleCountDelta <= 254))
{
if (HandlerRef.HasHandlers())
{
MessageBodyFrame sensors(this);
sensors.AbsoluteTimeSeconds = absoluteTimeSeconds - s.NumSamples * scaledSampleIntervalTimeUnit;
sensors.TimeDelta = (float) ((runningSampleCountDelta - LastNumSamples) * scaledSampleIntervalTimeUnit);
sensors.Acceleration = LastAcceleration;
sensors.RotationRate = LastRotationRate;
sensors.MagneticField = LastMagneticField;
sensors.Temperature = LastTemperature;
pCalibration->Apply(sensors);
HandlerRef.Call(sensors);
}
}
}
else
{
LastAcceleration = Vector3f(0);
LastRotationRate = Vector3f(0);
LastMagneticField= Vector3f(0);
LastTemperature = 0;
SequenceValid = true;
}
LastNumSamples = s.NumSamples;
LastRunningSampleCount = s.RunningSampleCount;
if (HandlerRef.HasHandlers())
{
MessageBodyFrame sensors(this);
UByte iterations = s.NumSamples;
if (s.NumSamples > 2)
{
iterations = 2;
sensors.TimeDelta = (float) ((s.NumSamples - 1) * scaledSampleIntervalTimeUnit);
}
else
{
sensors.TimeDelta = (float) scaledSampleIntervalTimeUnit;
}
for (UByte i = 0; i < iterations; i++)
{
sensors.AbsoluteTimeSeconds = absoluteTimeSeconds - ( iterations - 1 - i ) * scaledSampleIntervalTimeUnit;
sensors.Acceleration = AccelFromBodyFrameUpdate(s, i);
sensors.RotationRate = EulerFromBodyFrameUpdate(s, i);
sensors.MagneticField= MagFromBodyFrameUpdate(s);
sensors.Temperature = s.Temperature * 0.01f;
pCalibration->Apply(sensors);
HandlerRef.Call(sensors);
// TimeDelta for the last two sample is always fixed.
sensors.TimeDelta = (float) scaledSampleIntervalTimeUnit;
}
// Send pixel read only when frame timestamp changes.
if (LastFrameTimestamp != s.FrameTimestamp)
{
MessagePixelRead pixelRead(this);
// Prepare message for pixel read
pixelRead.PixelReadValue = s.FrameID;
pixelRead.RawFrameTime = s.FrameTimestamp;
pixelRead.RawSensorTime = s.SampleTimestamp;
pixelRead.SensorTimeSeconds = LastSensorTime.TimeSeconds;
pixelRead.FrameTimeSeconds = LastFrameTime.TimeSeconds;
HandlerRef.Call(pixelRead);
LastFrameTimestamp = s.FrameTimestamp;
}
UInt16 lowFrameCount = (UInt16) FullCameraFrameCount;
// Send message only when frame counter changes
if (lowFrameCount != s.CameraFrameCount)
{
// check for the rollover in the counter
if (s.CameraFrameCount < lowFrameCount)
FullCameraFrameCount += 0x10000;
// update the low bits
FullCameraFrameCount = (FullCameraFrameCount & ~0xFFFF) | s.CameraFrameCount;
MessageExposureFrame vision(this);
vision.CameraPattern = s.CameraPattern;
vision.CameraFrameCount = FullCameraFrameCount;
vision.CameraTimeSeconds = LastCameraTime.TimeSeconds;
HandlerRef.Call(vision);
}
LastAcceleration = sensors.Acceleration;
LastRotationRate = sensors.RotationRate;
LastMagneticField= sensors.MagneticField;
LastTemperature = sensors.Temperature;
//LastPixelRead = pixelRead.PixelReadValue;
//LastPixelReadTimeStamp = LastFrameTime;
}
else
{
if (s.NumSamples != 0)
{
UByte i = (s.NumSamples > 1) ? 1 : 0;
LastAcceleration = AccelFromBodyFrameUpdate(s, i);
LastRotationRate = EulerFromBodyFrameUpdate(s, i);
LastMagneticField = MagFromBodyFrameUpdate(s);
LastTemperature = s.Temperature * 0.01f;
}
}
}
