tResult Emergency::Init(tInitStage eStage, __exception)
{
RETURN_IF_FAILED(cFilter::Init(eStage, __exception_ptr));
if (eStage == StageFirst)
{
cObjectPtr<IMediaType> typeSignal;
cObjectPtr<IMediaType> typeSignalOut;
RETURN_IF_FAILED(initInputPins(__exception_ptr, typeSignal));
RETURN_IF_FAILED(initOutputPins(typeSignalOut));
RETURN_IF_FAILED(typeSignal->GetInterface(IID_ADTF_MEDIA_TYPE_DESCRIPTION, (tVoid**) &this->coderDescriptionSignalInput));
RETURN_IF_FAILED_AND_LOG_ERROR_STR(createInputPin("front_ir_fusion", this->m_oIFrontIRF, typeSignal), "Failed creating IR Fusion!");
RETURN_IF_FAILED_AND_LOG_ERROR_STR(createOutputPin("decision", this->decisionPin, typeSignalOut), "Failed creating decision output!");
}
else if (eStage == StageNormal)
{
this->m_nEmergencyDistanceFront = GetPropertyInt(PROPERTY_EMERGENCY_DISTANCE_FRONT);
this->m_nEmergencyDistanceRear = GetPropertyInt(PROPERTY_EMERGENCY_DISTANCE_REAR);
this->m_nEmergencyDistanceFrontUss = GetPropertyInt(PROPERTY_EMERGENCY_DISTANCE_FRONT_USS);
}
else if (eStage == StageGraphReady)
{
cObjectPtr<IMediaSerializer> serializer;
RETURN_IF_FAILED(this->coderDescriptionSignalOutput->GetMediaSampleSerializer(&serializer));
m_size = serializer->GetDeserializedSize();
}
RETURN_NOERROR;
}
tResult Test::PropertyChanged(const char *name)
{
this->ip = GetPropertyStr(PROP_NAME_IP);
this->numberOfStopLines = GetPropertyInt(PROP_NAME_STOP_LINES);
this->delay = GetPropertyInt(PROP_NAME_DELAY);
this->driveSpeed = GetPropertyInt(PROP_NAME_DRIVE_SPEED);
this->smoothCurveValue = GetPropertyInt(PROP_NAME_SMOOTH_CURVE_VALUE);
this->ticksToStopLine = GetPropertyInt(PROP_NAME_TICKS_TO_STOP_LINE);
RETURN_NOERROR;
}
tResult Emergency::OnPinEvent(IPin *source, tInt eventCore, tInt param1, tInt param2, IMediaSample *mediaSample)
{
RETURN_IF_POINTER_NULL(source);
RETURN_IF_POINTER_NULL(mediaSample);
if (eventCore == IPinEventSink::PE_MediaSampleReceived)
{
if (source == &this->m_oIFrontIRF) //&& (steerAngle< 5 && steerAngle > -5) && !initSteerAngle
{
static tFloat32 signalValue = 0;
// this->m_nLastMSTime = pMediaSample->GetTime();
RETURN_IF_FAILED_AND_LOG_ERROR_STR(getValueFloat(mediaSample, signalValue), "Failed getting signalValue");
this->m_nEmergencyDistanceFront = GetPropertyInt(PROPERTY_EMERGENCY_DISTANCE_FRONT);
this->m_bEmergencybreak = (signalValue <= this->m_nEmergencyDistanceFront);
LOG_INFO(cString::Format("IRF _____ value: %d, steerangle: %d", signalValue, steerAngle));
if (this->m_bEmergencybreak)
{
RETURN_IF_FAILED_AND_LOG_ERROR_STR(sendDecision(EMERGENCY_STOP), "Cant send decision to DriveAlgorithm");
//break_and_wink(pMediaSample->GetTime(), true);
this->initSteerAngle = true;
}
else{
RETURN_IF_FAILED_AND_LOG_ERROR_STR(sendDecision(EMERGENCY_START), "Cant send decision to DriveAlgorithm");
}
}
}
RETURN_NOERROR;
}
// -------------------------------------------------------------------------------------------------
bool EmergencyFilter::stopCarNow(SensorData const & data) {
// -------------------------------------------------------------------------------------------------
float front_threshold = GetPropertyInt("Front distance threshold");
if (data.value_ir_l_fc < front_threshold) return true;
return false;
}
tResult StateControllerNew::Init(cFilter::tInitStage eStage, IException **__exception_ptr)
{
RETURN_IF_FAILED(cFilter::Init(eStage, __exception_ptr));
if (StageFirst == eStage)
{
RETURN_IF_FAILED(CreateDescriptions(__exception_ptr));
RETURN_IF_FAILED(CreateInputPins(__exception_ptr));
RETURN_IF_FAILED(CreateOutputPins(__exception_ptr));
}
else if (StageNormal == eStage)
{
controllerMode = static_cast<tControllerMode::ControllerModeEnum>(GetPropertyInt(CONTROLLER_MODE_PROPERTY));
logger.Log(cString::Format("StateController running in %s mode.", tControllerMode::ToString(controllerMode).c_str()).GetPtr());
if (tControllerMode::Manual == controllerMode && waitTimer == NULL)
{
tInt waitTime = 500 * 1000; // 500ms interval
waitTimer = _kernel->TimerCreate(tTimeStamp(waitTime), 0, static_cast<IRunnable *>(this), NULL, NULL, 0, 0,
cString::Format("%s.timer", OIGetInstanceName()));
}
}
else if (StageGraphReady == eStage)
{
currentManeuverID = 0;
sectionListIndex = -1;
maneuverListIndex = -1;
SetState(tCarState::Startup);
}
RETURN_NOERROR;
}
tResult cJuryModule::OnNotAus()
{
m_iNotAusCounter = 0;
__synchronized_obj(m_oCriticalSectionTimerNotAus);
m_hTimerNotAus = _kernel->TimerCreate(GetPropertyInt("Send time interval in sec")*1000000, 0, static_cast<IRunnable*>(this),
NULL, &m_iNotAusCounter, sizeof(m_iNotAusCounter), 0, adtf_util::cString::Format("%s.timerNotAus", OIGetInstanceName()));
RETURN_NOERROR;
}
tResult cJuryTransmitter::OnStop(tInt16 entryId)
{
m_Stop_entry_id = entryId;
m_hTimerStopCounter = 0;
__synchronized_obj(m_oCriticalSectionTimerStop);
m_hTimerStop = _kernel->TimerCreate(GetPropertyInt("Send time interval in sec")*1000000, 0, static_cast<IRunnable*>(this),
NULL, &m_hTimerStopCounter, sizeof(m_hTimerStopCounter), 0, adtf_util::cString::Format("%s.timerStop", OIGetInstanceName()));
RETURN_NOERROR;
}
tResult cJuryTransmitter::OnRequestReady(tInt16 entryId)
{
m_Request_Ready_entry_id = entryId;
m_hTimerRequestReadyCounter= 0;
__synchronized_obj(m_oCriticalSectionTimerRequestReady);
// Create a timer that will trigger the signal generation every 2 sec.
m_hTimerRequestReady = _kernel->TimerCreate(GetPropertyInt("Send time interval in sec")*1000000, 0, static_cast<IRunnable*>(this),
NULL, &m_hTimerRequestReadyCounter, sizeof(m_hTimerRequestReadyCounter), 0, adtf_util::cString::Format("%s.timerRequestReady", OIGetInstanceName()));
RETURN_NOERROR;
}
tResult cSimpleFusion::Init(tInitStage eStage, __exception)
{
RETURN_IF_FAILED(cFilter::Init(eStage, __exception_ptr));
if (eStage == StageFirst)
{
cObjectPtr<IMediaDescriptionManager> pDescManager;
RETURN_IF_FAILED(_runtime->GetObject(OID_ADTF_MEDIA_DESCRIPTION_MANAGER,IID_ADTF_MEDIA_DESCRIPTION_MANAGER,(tVoid**)&pDescManager,NULL));
tChar const * strDescSignalValue = pDescManager->GetMediaDescription("tSignalValue");
RETURN_IF_POINTER_NULL(strDescSignalValue);
pTypeSignalValue = new cMediaType(0, 0, 0, "tSignalValue", strDescSignalValue,IMediaDescription::MDF_DDL_DEFAULT_VERSION);
RETURN_IF_FAILED(pTypeSignalValue->GetInterface(IID_ADTF_MEDIA_TYPE_DESCRIPTION, (tVoid**)&m_pCoderDescSignalInput));
createOutputPin();
createInputPinIRLongeRange("ir_long_in");
createInputPinIRShortRange1("ir_short_in1");
createInputPinIRShortRange2("ir_short_in2");
createInputPinUSLeft("us_left_in");
createInputPinUSRight("us_right_in");
}
else if (eStage == StageNormal)
{
// In this stage you would do further initialisation and/or create your dynamic pins.
// Please take a look at the demo_dynamicpin example for further reference.
stateOfLongeRangeInputPin = GetPropertyInt(PROP_NAME_IR_LONGE_RANGE_INPUT_PIN_ACTIVE);
stateOfShortRangeInputPin1 = GetPropertyInt(PROP_NAME_IR_SHORT_RANGE_1_INPUT_PIN_ACTIVE);
stateOfShortRangeInputPin2 = GetPropertyInt(PROP_NAME_IR_SHORT_RANGE_2_INPUT_PIN_ACTIVE);
stateOfUSRightInputPin = GetPropertyInt(PROP_NAME_US_RIGHT_INPUT_PIN_ACTIVE);
stateOfUSLeftInputPin = GetPropertyInt(PROP_NAME_US_LEFT_INPUT_PIN_ACTIVE);
lengthOfMedianVector = GetPropertyInt(PROP_NAME_MEDIAN);
thresholdIRLongRange = GetPropertyInt(PROP_NAME_IR_LONG_RANGE_THRESHOLD);
thresholdIRShortRange = GetPropertyInt(PROP_NAME_IR_SHORT_RANGE_THRESHOLD);
thresholdUS = GetPropertyInt(PROP_NAME_US_THRESHOLD);
calculatedMedian();
}
else if (eStage == StageGraphReady)
{
}
RETURN_NOERROR;
}
tResult TemporalImage::Init(tInitStage eStage, __exception)
{
RETURN_IF_FAILED(cFilter::Init(eStage, __exception_ptr));
if (eStage == StageFirst)
{
RETURN_IF_FAILED(CreateInputPins(__exception_ptr));
RETURN_IF_FAILED(CreateOutputPins(__exception_ptr));
}
else if (eStage == StageNormal)
{
bufferSize = tUInt(GetPropertyInt(BUFFER_SIZE_PROPERTY));
method = tUInt8(GetPropertyInt(METHOD_PROPERTY));
buffer = vector<Mat>(bufferSize);
for (unsigned int i = 0; i < buffer.size(); ++i)
{
buffer[i] = Mat(0, 0, 0);
}
}
else if (eStage == StageGraphReady)
{
cObjectPtr<IMediaType> mediaType;
RETURN_IF_FAILED(videoInputPin.GetMediaType(&mediaType));
cObjectPtr<IMediaTypeVideo> videoType;
RETURN_IF_FAILED(mediaType->GetInterface(IID_ADTF_MEDIA_TYPE_VIDEO, (tVoid **) &videoType));
videoInputFormat = *(videoType->GetFormat());
videoOutputFormat = *(videoType->GetFormat());
videoOutputPin.SetFormat(&videoOutputFormat, NULL);
logger.Log(cString::Format("Input format: %d x %d @ %d Bit", videoInputFormat.nWidth, videoInputFormat.nHeight,
videoInputFormat.nBitsPerPixel).GetPtr(), false);
logger.Log(cString::Format("Output format: %d x %d @ %d Bit", videoOutputFormat.nWidth, videoOutputFormat.nHeight,
videoOutputFormat.nBitsPerPixel).GetPtr(), false);
}
RETURN_NOERROR;
}
void C4Object::VerticalBounds(C4Real &ctcoy)
{
// layer bounds
if (Layer && Layer->GetPropertyInt(P_BorderBound) & C4D_Border_Layer)
{
C4PropList* pActionDef = GetAction();
if (!pActionDef || pActionDef->GetPropertyP(P_Procedure) != DFA_ATTACH)
{
C4Real tbound = itofix(Layer->GetY() + Layer->Shape.GetY() - Shape.GetY()),
bbound = itofix(Layer->GetY() + Layer->Shape.GetY() + Layer->Shape.Hgt - (Shape.GetY() + Shape.Hgt));
if (ctcoy < tbound) StopAndContact(ctcoy, tbound, ydir, CNAT_Top);
if (ctcoy > bbound) StopAndContact(ctcoy, bbound, ydir, CNAT_Bottom);
}
}
// landscape bounds
C4Real tbound = itofix(0 - Shape.GetY()),
bbound = itofix(GBackHgt - (Shape.GetY() + Shape.Hgt));
if (ctcoy < tbound && GetPropertyInt(P_BorderBound) & C4D_Border_Top)
StopAndContact(ctcoy, tbound, ydir, CNAT_Top);
if (ctcoy > bbound && GetPropertyInt(P_BorderBound) & C4D_Border_Bottom)
StopAndContact(ctcoy, bbound, ydir, CNAT_Bottom);
}
// Stop the object and do contact calls if it collides with the border
void C4Object::SideBounds(C4Real &ctcox)
{
// layer bounds
if (Layer && Layer->GetPropertyInt(P_BorderBound) & C4D_Border_Layer)
{
C4PropList* pActionDef = GetAction();
if (!pActionDef || pActionDef->GetPropertyP(P_Procedure) != DFA_ATTACH)
{
C4Real lbound = itofix(Layer->GetX() + Layer->Shape.GetX() - Shape.GetX()),
rbound = itofix(Layer->GetX() + Layer->Shape.GetX() + Layer->Shape.Wdt - (Shape.GetX() + Shape.Wdt));
if (ctcox < lbound) StopAndContact(ctcox, lbound, xdir, CNAT_Left);
if (ctcox > rbound) StopAndContact(ctcox, rbound, xdir, CNAT_Right);
}
}
// landscape bounds
C4Real lbound = itofix(0 - Shape.GetX()),
rbound = itofix(GBackWdt - (Shape.GetX() + Shape.Wdt));
if (ctcox < lbound && GetPropertyInt(P_BorderBound) & C4D_Border_Sides)
StopAndContact(ctcox, lbound, xdir, CNAT_Left);
if (ctcox > rbound && GetPropertyInt(P_BorderBound) & C4D_Border_Sides)
StopAndContact(ctcox, rbound, xdir, CNAT_Right);
}
// -------------------------------------------------------------------------------------------------
float LaneFilter::calculateImageLuminosity(Mat const & image) {
// -------------------------------------------------------------------------------------------------
int scan_rate = GetPropertyInt("scan_rate");
float sum = 0;
int counter = 0;
for (int row = 0; row < image.rows; row += scan_rate) {
for (int col = 0; col < image.cols; col += scan_rate) {
Vec3b cur = image.at<Vec3b>(row, col);
sum += (1.0f / 3.0f) * (cur[0] + cur[1] + cur[2]);
counter++;
}
}
if (GetPropertyBool("debug")) LOG_INFO(cString::Format("Current luminosity: %f", sum / counter));
return sum / counter;
}
tVoid CurveDetector::setThresholds()
{
this->thresholdCanny1 = GetPropertyInt(PROP_NAME_CANNY_THRESHOLD_1);
this->thresholdCanny2 = GetPropertyInt(PROP_NAME_CANNY_THRESHOLD_2);
}
bool CGUIWindowFirstTimeUseCalibration::OnMessage(CGUIMessage& message)
{
bool retVal = CGUIWindowSettingsScreenCalibration::OnMessage(message);
switch (message.GetMessage())
{
case GUI_MSG_CLICKED:
{
switch(message.GetSenderId())
{
case TOP_LEFT_MOVER:
{
SetProperty("setting-state",SETTING_BOTTOM_RIGHT);
CLog::Log(LOGDEBUG,"CGUIWindowFirstTimeUseCalibration::OnMessage - GUI_MSG_CLICKED - Finish set TOP_LEFT_MOVER. Set property [setting-state=%d=SETTING_BOTTOM_RIGHT] (ftu)",GetPropertyInt("setting-state"));
SET_CONTROL_FOCUS(BOTTOM_RIGHT_MOVER,0);
}
break;
case BOTTOM_RIGHT_MOVER:
{
SetProperty("setting-state",VALIDATE_STATE);
CLog::Log(LOGDEBUG,"CGUIWindowFirstTimeUseCalibration::OnMessage - GUI_MSG_CLICKED - Finish set BOTTOM_RIGHT_MOVER. Set property [setting-state=%d=VALIDATE_STATE] (ftu)",GetPropertyInt("setting-state"));
SET_CONTROL_VISIBLE(TRY_AGAIN_BUTTON);
SET_CONTROL_VISIBLE(DONE_BUTTON);
SET_CONTROL_FOCUS(DONE_BUTTON,0);
}
break;
case TRY_AGAIN_BUTTON:
{
SET_CONTROL_FOCUS(TOP_LEFT_MOVER,0);
SetProperty("setting-state",SETTING_TOP_LEFT);
CLog::Log(LOGDEBUG,"CGUIWindowFirstTimeUseCalibration::OnMessage - GUI_MSG_CLICKED - click on TRY_AGAIN_BUTTON. Set property [setting-state=%d=SETTING_TOP_LEFT] (ftu)",GetPropertyInt("setting-state"));
}
break;
case DONE_BUTTON:
{
CLog::Log(LOGDEBUG,"CGUIWindowFirstTimeUseCalibration::OnMessage - GUI_MSG_CLICKED - click on DONE_BUTTON. Call Login() (ftu)");
g_stSettings.m_doneFTU2 = true;
g_settings.Save();
/////////////////////////////////////////////
// DONE_BUTTON was clicked -> call Login() //
/////////////////////////////////////////////
bool loginWasDone = g_application.GetBoxeeLoginManager().Login();
return loginWasDone;
}
break;
}
}
break;
}
return retVal;
}
void CGUIWindowFirstTimeUseCalibration::OnInitWindow()
{
CGUIWindowSettingsScreenCalibration::OnInitWindow();
SET_CONTROL_FOCUS(TOP_LEFT_MOVER,0);
SetProperty("setting-state",SETTING_TOP_LEFT);
CLog::Log(LOGDEBUG,"CGUIWindowFirstTimeUseCalibration::OnInitWindow - Exit function. [setting-state=%d=SETTING_TOP_LEFT] (ftu)",GetPropertyInt("setting-state"));
}
tResult cMarkerDetectFilter::ReadProperties(const tChar* strPropertyName)
{
if (NULL == strPropertyName || cString::IsEqual(strPropertyName, MD_COUNT))
{
count_lines_marker = GetPropertyInt(MD_COUNT);
}
if (NULL == strPropertyName || cString::IsEqual(strPropertyName, MD_REQUIRED_MARKER_SIZE))
{
f32RequiredArea = GetPropertyFloat(MD_REQUIRED_MARKER_SIZE);
}
if (NULL == strPropertyName || cString::IsEqual(strPropertyName, MD_GREY_TRESHOLD))
{
m_nThresholdValue = GetPropertyInt(MD_GREY_TRESHOLD);
}
if (NULL == strPropertyName || cString::IsEqual(strPropertyName, MD_RANGE_ROWS_R1))
{
row1 = GetPropertyInt(MD_RANGE_ROWS_R1);
}
if (NULL == strPropertyName || cString::IsEqual(strPropertyName, MD_RANGE_ROWS_R2))
{
row2 = GetPropertyInt(MD_RANGE_ROWS_R2);
}
if (NULL == strPropertyName || cString::IsEqual(strPropertyName, MD_RANGE_COLS_C1))
{
col1 = GetPropertyInt(MD_RANGE_COLS_C1);
}
if (NULL == strPropertyName || cString::IsEqual(strPropertyName, MD_RANGE_COLS_C2))
{
col2 = GetPropertyInt(MD_RANGE_COLS_C2);
}
if (NULL == strPropertyName || cString::IsEqual(strPropertyName, MD_RANGE_ROWS_R1_L))
{
row1_l = GetPropertyInt(MD_RANGE_ROWS_R1_L);
}
if (NULL == strPropertyName || cString::IsEqual(strPropertyName, MD_RANGE_ROWS_R2_L))
{
row2_l = GetPropertyInt(MD_RANGE_ROWS_R2_L);
}
if (NULL == strPropertyName || cString::IsEqual(strPropertyName, MD_RANGE_COLS_C1_L))
{
col1_l = GetPropertyInt(MD_RANGE_COLS_C1_L);
}
if (NULL == strPropertyName || cString::IsEqual(strPropertyName, MD_RANGE_COLS_C2_L))
{
col2_l = GetPropertyInt(MD_RANGE_COLS_C2_L);
}
if (NULL == strPropertyName || cString::IsEqual(strPropertyName, MD_RANGE_ROWS_R1_VER))
{
row1_ver = GetPropertyInt(MD_RANGE_ROWS_R1_VER);
}
if (NULL == strPropertyName || cString::IsEqual(strPropertyName, MD_RANGE_ROWS_R2_VER))
{
row2_ver = GetPropertyInt(MD_RANGE_ROWS_R2_VER);
}
if (NULL == strPropertyName || cString::IsEqual(strPropertyName, MD_RANGE_COLS_C1_VER))
{
col1_ver = GetPropertyInt(MD_RANGE_COLS_C1_VER);
}
if (NULL == strPropertyName || cString::IsEqual(strPropertyName, MD_RANGE_COLS_C2_VER))
{
col2_ver = GetPropertyInt(MD_RANGE_COLS_C2_VER);
}
RETURN_NOERROR;
}
tResult cMarkerDetectFilter::Init(tInitStage eStage, __exception)
{
RETURN_IF_FAILED(cFilter::Init(eStage, __exception_ptr));
if (eStage == StageFirst)
{
//create the video rgb input pin
RETURN_IF_FAILED(m_oPinInputVideo.Create("Video_RGB_input",IPin::PD_Input, static_cast<IPinEventSink*>(this)));
RETURN_IF_FAILED(RegisterPin(&m_oPinInputVideo));
//create the video rgb output pin
RETURN_IF_FAILED(m_oPinOutputVideo.Create("Video_RGB_output", IPin::PD_Output, static_cast<IPinEventSink*>(this)));
RETURN_IF_FAILED(RegisterPin(&m_oPinOutputVideo));
// create the description manager
cObjectPtr<IMediaDescriptionManager> pDescManager;
RETURN_IF_FAILED(_runtime->GetObject(OID_ADTF_MEDIA_DESCRIPTION_MANAGER,IID_ADTF_MEDIA_DESCRIPTION_MANAGER, (tVoid**)&pDescManager,__exception_ptr));
tChar const * strDescSignalLanacc_man = pDescManager->GetMediaDescription("tSignalValue");
RETURN_IF_POINTER_NULL(strDescSignalLanacc_man);
cObjectPtr<IMediaType> pTypeSignalmaneuver_lanacc = new cMediaType(0, 0, 0, "tSignalValue", strDescSignalLanacc_man, IMediaDescription::MDF_DDL_DEFAULT_VERSION);
RETURN_IF_FAILED(pTypeSignalmaneuver_lanacc->GetInterface(IID_ADTF_MEDIA_TYPE_DESCRIPTION, (tVoid**)&mediatype_Lan_Acc_Man));
RETURN_IF_FAILED(Lan_Acc_Man.Create("Lan_Acc_Man", pTypeSignalmaneuver_lanacc, static_cast<IPinEventSink*> (this)));
RETURN_IF_FAILED(RegisterPin(&Lan_Acc_Man));
tChar const * strDescSignalmaneuver_accspeed = pDescManager->GetMediaDescription("tSignalValue");
RETURN_IF_POINTER_NULL(strDescSignalmaneuver_accspeed);
cObjectPtr<IMediaType> pTypeSignalmaneuver_accspeed = new cMediaType(0, 0, 0, "tSignalValue", strDescSignalmaneuver_accspeed, IMediaDescription::MDF_DDL_DEFAULT_VERSION);
RETURN_IF_FAILED(pTypeSignalmaneuver_accspeed->GetInterface(IID_ADTF_MEDIA_TYPE_DESCRIPTION, (tVoid**)&mediatype_Lan_Acc_Speed));
RETURN_IF_FAILED(Lan_Acc_Speed.Create("Lane_Speed", pTypeSignalmaneuver_accspeed, static_cast<IPinEventSink*> (this)));
RETURN_IF_FAILED(RegisterPin(&Lan_Acc_Speed));
tChar const * strDescSignalmaneuver_idJury = pDescManager->GetMediaDescription("tSignalValue");
RETURN_IF_POINTER_NULL(strDescSignalmaneuver_idJury);
cObjectPtr<IMediaType> pTypeSignalmaneuver_idjury = new cMediaType(0, 0, 0, "tSignalValue", strDescSignalmaneuver_idJury, IMediaDescription::MDF_DDL_DEFAULT_VERSION);
RETURN_IF_FAILED(pTypeSignalmaneuver_idjury->GetInterface(IID_ADTF_MEDIA_TYPE_DESCRIPTION, (tVoid**)&mediatype_maneuverid_Jury));
RETURN_IF_FAILED(maneuverid_Jury.Create("maneuverid_Jury", pTypeSignalmaneuver_idjury, static_cast<IPinEventSink*> (this)));
RETURN_IF_FAILED(RegisterPin(&maneuverid_Jury));
tChar const * strDescSignalmaneuver_id = pDescManager->GetMediaDescription("tSignalValue");
RETURN_IF_POINTER_NULL(strDescSignalmaneuver_id);
cObjectPtr<IMediaType> pTypeSignalmaneuver_id = new cMediaType(0, 0, 0, "tSignalValue", strDescSignalmaneuver_id, IMediaDescription::MDF_DDL_DEFAULT_VERSION);
RETURN_IF_FAILED(pTypeSignalmaneuver_id->GetInterface(IID_ADTF_MEDIA_TYPE_DESCRIPTION, (tVoid**)&mediatype_maneuver_id));
RETURN_IF_FAILED(maneuver_id.Create("Maneuver_ID", pTypeSignalmaneuver_id, static_cast<IPinEventSink*> (this)));
RETURN_IF_FAILED(RegisterPin(&maneuver_id));
tChar const * strDescSignalmaneuver_finish = pDescManager->GetMediaDescription("tBoolSignalValue");
RETURN_IF_POINTER_NULL(strDescSignalmaneuver_finish);
cObjectPtr<IMediaType> pTypeSignalmaneuver_finish = new cMediaType(0, 0, 0, "tBoolSignalValue", strDescSignalmaneuver_finish, IMediaDescription::MDF_DDL_DEFAULT_VERSION);
RETURN_IF_FAILED(pTypeSignalmaneuver_finish->GetInterface(IID_ADTF_MEDIA_TYPE_DESCRIPTION, (tVoid**)&mediatype_maneuver_finish));
RETURN_IF_FAILED(maneuver_finish.Create("Maneuver_int_Finish", pTypeSignalmaneuver_finish, static_cast<IPinEventSink*> (this)));
RETURN_IF_FAILED(RegisterPin(&maneuver_finish));
// create the description for the road sign pin
tChar const * strDesc = pDescManager->GetMediaDescription("tRoadSign");
RETURN_IF_POINTER_NULL(strDesc);
cObjectPtr<IMediaType> pType = new cMediaType(0, 0, 0, "tRoadSign", strDesc, IMediaDescription::MDF_DDL_DEFAULT_VERSION);
// create the road sign OutputPin
RETURN_IF_FAILED(m_oPinRoadSign.Create("RoadSign", pType, this));
RETURN_IF_FAILED(RegisterPin(&m_oPinRoadSign));
// set the description for the road sign pin
RETURN_IF_FAILED(pType->GetInterface(IID_ADTF_MEDIA_TYPE_DESCRIPTION, (tVoid**)&m_pDescriptionRoadSign));
// create the description for the road sign pin
tChar const * strDescExt = pDescManager->GetMediaDescription("tRoadSignExt");
RETURN_IF_POINTER_NULL(strDescExt);
cObjectPtr<IMediaType> pTypeExt = new cMediaType(0, 0, 0, "tRoadSignExt", strDescExt, IMediaDescription::MDF_DDL_DEFAULT_VERSION);
// create the extended road sign OutputPin
RETURN_IF_FAILED(m_oPinRoadSignExt.Create("RoadSign_ext", pTypeExt, this));
RETURN_IF_FAILED(RegisterPin(&m_oPinRoadSignExt));
// set the description for the extended road sign pin
RETURN_IF_FAILED(pTypeExt->GetInterface(IID_ADTF_MEDIA_TYPE_DESCRIPTION, (tVoid**)&m_pDescriptionRoadSignExt));
}
else if (eStage == StageNormal)
{
// get the propeerties
ReadProperties(NULL);
m_iOutputMode = GetPropertyInt("Video Output Pin");
m_f32MarkerSize = static_cast<tFloat32>(GetPropertyFloat("Size of Markers"));
m_bDebugModeEnabled = GetPropertyBool("Debug Output to Console");
imshowflag = GetPropertyBool(MD_ENABLE_IMSHOW);
imshowflag_ver = GetPropertyBool(MD_ENABLE_IMSHOW_VER);
m_bCamaraParamsLoaded = tFalse;
//Get path of marker configuration file
cFilename fileConfig = GetPropertyStr("Dictionary File For Markers");
//create absolute path for marker configuration file
ADTF_GET_CONFIG_FILENAME(fileConfig);
fileConfig = fileConfig.CreateAbsolutePath(".");
//check if marker configuration file exits
if (fileConfig.IsEmpty() || !(cFileSystem::Exists(fileConfig)))
{
LOG_ERROR("Dictionary File for Markers not found");
RETURN_ERROR(ERR_INVALID_FILE);
}
else
{
//try to read the marker configuration file
if(m_Dictionary.fromFile(string(fileConfig))==false)
{
RETURN_ERROR(ERR_INVALID_FILE);
LOG_ERROR("Dictionary File for Markers could not be read");
}
if(m_Dictionary.size()==0)
{
RETURN_ERROR(ERR_INVALID_FILE);
LOG_ERROR("Dictionary File does not contain valid markers or could not be read sucessfully");
}
//set marker configuration file to highlyreliable markers class
if (!(HighlyReliableMarkers::loadDictionary(m_Dictionary)==tTrue))
{
//LOG_ERROR("Dictionary File could not be read for highly reliable markers");
}
}
//Get path of calibration file with camera paramters
cFilename fileCalibration = GetPropertyStr("Calibration File for used Camera"); ;
//Get path of calibration file with camera paramters
ADTF_GET_CONFIG_FILENAME(fileCalibration);
fileCalibration = fileCalibration.CreateAbsolutePath(".");
//check if calibration file with camera paramters exits
if (fileCalibration.IsEmpty() || !(cFileSystem::Exists(fileCalibration)))
{
LOG_ERROR("Calibration File for camera not found");
}
else
{
// read the calibration file with camera paramters exits and save to member variable
m_TheCameraParameters.readFromXMLFile(fileCalibration.GetPtr());
cv::FileStorage camera_data (fileCalibration.GetPtr(),cv::FileStorage::READ);
camera_data ["camera_matrix"] >> m_Intrinsics;
camera_data ["distortion_coefficients"] >> m_Distorsion;
m_bCamaraParamsLoaded = tTrue;
}
}
// todo
// Executed when a property is changed
tResult cSimpleFusion::PropertyChanged(const char *propertyName)
{
if(cString::Compare(propertyName, PROP_NAME_IR_LONGE_RANGE_INPUT_PIN_ACTIVE))
{
stateOfLongeRangeInputPin = GetPropertyInt(PROP_NAME_IR_LONGE_RANGE_INPUT_PIN_ACTIVE);
#ifdef _DEBUG
LOG_INFO("changed state of ir longe range input pin");
#endif
}
else if(cString::Compare(propertyName, PROP_NAME_IR_SHORT_RANGE_1_INPUT_PIN_ACTIVE))
{
stateOfShortRangeInputPin1 = GetPropertyInt(PROP_NAME_IR_SHORT_RANGE_1_INPUT_PIN_ACTIVE);
#ifdef _DEBUG
LOG_INFO("changed state of ir short range input pin 1");
#endif
}
else if(cString::Compare(propertyName, PROP_NAME_IR_SHORT_RANGE_2_INPUT_PIN_ACTIVE))
{
stateOfShortRangeInputPin2 = GetPropertyInt(PROP_NAME_IR_SHORT_RANGE_2_INPUT_PIN_ACTIVE);
#ifdef _DEBUG
LOG_INFO("changed state of ir short range input pin 2");
#endif
}
else if(cString::Compare(propertyName, PROP_NAME_US_LEFT_INPUT_PIN_ACTIVE))
{
stateOfUSLeftInputPin = GetPropertyInt(PROP_NAME_US_LEFT_INPUT_PIN_ACTIVE);
#ifdef _DEBUG
LOG_INFO("changed state of us left input pin");
#endif
}
else if(cString::Compare(propertyName, PROP_NAME_US_RIGHT_INPUT_PIN_ACTIVE))
{
stateOfUSRightInputPin = GetPropertyInt(PROP_NAME_US_RIGHT_INPUT_PIN_ACTIVE);
#ifdef _DEBUG
LOG_INFO("changed state of us right input pin");
#endif
}
else if(cString::Compare(propertyName, PROP_NAME_IR_LONG_RANGE_THRESHOLD))
{
thresholdIRLongRange = GetPropertyInt(PROP_NAME_IR_LONG_RANGE_THRESHOLD);
#ifdef _DEBUG
LOG_INFO("changed longe range threshold");
#endif
}
else if(cString::Compare(propertyName, PROP_NAME_IR_SHORT_RANGE_THRESHOLD))
{
thresholdIRShortRange = GetPropertyInt(PROP_NAME_IR_SHORT_RANGE_THRESHOLD);
#ifdef _DEBUG
LOG_INFO("changed short range threshold");
#endif
}
else if(cString::Compare(propertyName, PROP_NAME_US_THRESHOLD))
{
thresholdUS = GetPropertyInt(PROP_NAME_US_THRESHOLD);
#ifdef _DEBUG
LOG_INFO("changed US threshold");
#endif
}
else if(cString::Compare(propertyName, PROP_NAME_MEDIAN))
{
lengthOfMedianVector = GetPropertyInt(PROP_NAME_MEDIAN);
#ifdef _DEBUG
LOG_INFO("change median");
#endif
}
RETURN_NOERROR;
}
void CGUIListItem::IncrementProperty(const CStdString &strKey, int nVal)
{
int i = GetPropertyInt(strKey);
i += nVal;
SetProperty(strKey, i);
}
bool C4Object::ExecMovement() // Every Tick1 by Execute
{
// update in which material this object is
UpdateInMat();
// Containment check
if (Contained)
{
CopyMotion(Contained);
return true;
}
// General mobility check
if (Category & C4D_StaticBack) return false;
// Movement execution
if (Mobile) // Object is moving
{
// Move object
DoMovement();
// Demobilization check
if ((xdir==0) && (ydir==0) && (rdir==0)) Mobile=0;
// Check for stabilization
if (rdir==0) Stabilize();
}
else // Object is static
{
// Check for stabilization
Stabilize();
// Check for mobilization
if (!::Game.iTick10)
{
// Gravity mobilization
xdir=ydir=rdir=0;
Mobile=1;
}
}
// Enforce zero rotation
if (!Def->Rotateable) fix_r=Fix0;
// Out of bounds check
if ((!Inside<int32_t>(GetX() + Shape.GetX(), -Shape.Wdt, GBackWdt) && !(GetPropertyInt(P_BorderBound) & C4D_Border_Sides))
|| ((GetY() + Shape.GetY() > GBackHgt) && !(GetPropertyInt(P_BorderBound) & C4D_Border_Bottom)))
{
C4PropList* pActionDef = GetAction();
// Never remove attached objects: If they are truly outside landscape, their target will be removed,
// and the attached objects follow one frame later
if (!pActionDef || !Action.Target || pActionDef->GetPropertyP(P_Procedure) != DFA_ATTACH)
{
bool fRemove = true;
// never remove HUD objects
if (Category & C4D_Parallax)
{
int parX, parY;
GetParallaxity(&parX, &parY);
fRemove = false;
if (GetX()>GBackWdt || GetY()>GBackHgt) fRemove = true; // except if they are really out of the viewport to the right...
else if (GetX()<0 && !!parX) fRemove = true; // ...or it's not HUD horizontally and it's out to the left
else if (!parX && GetX()<-GBackWdt) fRemove = true; // ...or it's HUD horizontally and it's out to the left
}
if (fRemove)
{
AssignDeath(true);
AssignRemoval();
}
}
}
return true;
}
/*
* IRunnable implementation for manual mode
*/
tResult StateControllerNew::Run(tInt nActivationCode, const tVoid *pvUserData, tInt szUserDataSize, IException **__exception_ptr)
{
if (tControllerMode::Manual == controllerMode)
{
tCarState::CarStateEnum manualCarState = static_cast<tCarState::CarStateEnum>(GetPropertyInt(STATE_PROPERTY));
tManeuver::ManeuverEnum manualManeuver = static_cast<tManeuver::ManeuverEnum>(GetPropertyInt(MANEUVER_PROPERTY));
logger.Log(cString::Format("Manual: CarState: %s, Maneuver: %s", tCarState::ToString(manualCarState).c_str(),
tManeuver::ToString(manualManeuver).c_str()).GetPtr());
SendEnum(maneuverOutput, static_cast<tInt>(manualManeuver));
SendEnum(carStateOutput, static_cast<tInt>(manualCarState));
RETURN_NOERROR;
}
/* if (tControllerMode::Jury == controllerMode)
{
if (tCarState::GetReady == carState && AreAllModulesReady())
{
logger.Log("All modules ready.");
SetState(tCarState::Ready);
}
tManeuver::ManeuverEnum maneuver = tManeuver::M_UNKNOWN;
if (tCarState::Ready == carState || tCarState::Running == carState)
{
cString currentManeuverString = sectorList[sectionListIndex].maneuverList[maneuverListIndex].action;
maneuver = ConvertManeuver(currentManeuverString);
}
logger.Log(cString::Format("Jury: CarState: %s, Maneuver: %s", tCarState::ToString(carState).c_str(),
tManeuver::ToString(maneuver).c_str()).GetPtr());
SendEnum(maneuverOutput, static_cast<tInt>(maneuver));
SendEnum(carStateOutput, static_cast<tInt>(carState));
stateCar stateCar = Convert(carState);
tInt16 maneuverId = tInt16(currentManeuverID);
SendValue(driveStructOutput, stateCar, maneuverId);
}*/
RETURN_NOERROR;
}
tResult ROI::Init(tInitStage eStage, __exception)
{
RETURN_IF_FAILED(cFilter::Init(eStage, __exception_ptr));
if (eStage == StageFirst)
{
RETURN_IF_FAILED(InitDescriptions(__exception_ptr));
RETURN_IF_FAILED(CreateInputPins(__exception_ptr));
RETURN_IF_FAILED(CreateOutputPins(__exception_ptr));
}
else if (eStage == StageNormal)
{
hoodScanLineNumber = GetPropertyInt(HOOD_SCANLINE_NUMBER_PROPERTY);
roomScanLineNumber = GetPropertyInt(ROOM_SCANLINE_NUMBER_PROPERTY);
maxHoodDetectionCount = GetPropertyInt(MAX_HOOD_DETECTION_COUNT_PROPERTY);
rgbVideoManipulation = GetPropertyInt(RGB_VIDEO_MANIPULATION_PROPERTY);
depthVideoManipulation = GetPropertyInt(DEPTH_VIDEO_MANIPULATION_PROPERTY);
isHoodDetectionEnabled = GetPropertyBool(DETECT_HOOD_PROPERTY);
isRoomDetectionEnabled = GetPropertyBool(DETECT_ROOM_PROPERTY);
roomHeightManipulation = GetPropertyFloat(ROOM_HEIGHT_MANIPULATION_PROPERTY);
logger.Log(cString::Format("roomHeightManipulation: %d", roomHeightManipulation).GetPtr(), false);
logger.Log(cString::Format("hoodScanLineNumber: %d", hoodScanLineNumber).GetPtr(), false);
logger.Log(cString::Format("roomScanLineNumber: %d", roomScanLineNumber).GetPtr(), false);
logger.Log(cString::Format("processingWidthPercentage: %d", processingWidthPercentage).GetPtr(), false);
logger.Log(cString::Format("maxHoodDetectionCount: %d", maxHoodDetectionCount).GetPtr(), false);
logger.Log(cString::Format("rgbVideoManipulation: %d", rgbVideoManipulation).GetPtr(), false);
logger.Log(cString::Format("depthVideoManipulation: %d", depthVideoManipulation).GetPtr(), false);
logger.Log(cString::Format("isHoodDetectionEnabled: %d", isHoodDetectionEnabled).GetPtr(), false);
logger.Log(cString::Format("isRoomDetectionEnabled: %d", isRoomDetectionEnabled).GetPtr(), false);
}
else if (eStage == StageGraphReady)
{
// init RGB Video
cObjectPtr<IMediaType> rgbMediaType;
RETURN_IF_FAILED(rgbVideoInputPin.GetMediaType(&rgbMediaType));
cObjectPtr<IMediaTypeVideo> rgbVideoType;
RETURN_IF_FAILED(rgbMediaType->GetInterface(IID_ADTF_MEDIA_TYPE_VIDEO, (tVoid **) &rgbVideoType));
rgbVideoInputFormat = *(rgbVideoType->GetFormat());
rgbVideoOutputFormat = *(rgbVideoType->GetFormat());
rgbVideoOutputPin.SetFormat(&rgbVideoOutputFormat, NULL);
// init Depth Video
cObjectPtr<IMediaType> depthMediaType;
RETURN_IF_FAILED(depthVideoInputPin.GetMediaType(&depthMediaType));
cObjectPtr<IMediaTypeVideo> depthVideoType;
RETURN_IF_FAILED(depthMediaType->GetInterface(IID_ADTF_MEDIA_TYPE_VIDEO, (tVoid **) &depthVideoType));
depthVideoInputFormat = *(depthVideoType->GetFormat());
depthVideoOutputFormat = *(depthVideoType->GetFormat());
depthVideoOutputPin.SetFormat(&depthVideoOutputFormat, NULL);
logger.Log(cString::Format("RGB Input format: %d x %d @ %d Bit", rgbVideoInputFormat.nWidth, rgbVideoInputFormat.nHeight,
rgbVideoInputFormat.nBitsPerPixel).GetPtr(), false);
logger.Log(cString::Format("RGB Output format: %d x %d @ %d Bit", rgbVideoOutputFormat.nWidth, rgbVideoOutputFormat.nHeight,
rgbVideoOutputFormat.nBitsPerPixel).GetPtr(), false);
logger.Log(cString::Format("Depth Input format: %d x %d @ %d Bit", depthVideoInputFormat.nWidth, depthVideoInputFormat.nHeight,
depthVideoInputFormat.nBitsPerPixel).GetPtr(), false);
logger.Log(cString::Format("Depth Output format: %d x %d @ %d Bit", depthVideoOutputFormat.nWidth, depthVideoOutputFormat.nHeight,
depthVideoOutputFormat.nBitsPerPixel).GetPtr(), false);
if (depthVideoOutputFormat.nBitsPerPixel != 8)
{
THROW_ERROR_DESC(depthVideoOutputFormat.nBitsPerPixel, "Wrong depth video format. Use HTWK_Grayscale in front of this filter.");
}
// init processing parameters
processingData.processingWidth = depthVideoInputFormat.nWidth * (processingWidthPercentage / 100.0);
processingData.startOffset = (depthVideoInputFormat.nWidth - processingData.processingWidth) / 2;
processingData.hoodScanLineStepWidth = processingData.processingWidth / (hoodScanLineNumber - 1);
processingData.roomScanLineStepWidth = processingData.processingWidth / (roomScanLineNumber - 1);
logger.Log(cString::Format("hoodScanLineNumber: %d", hoodScanLineNumber).GetPtr(), false);
logger.Log(cString::Format("processingWidthPercentage: %d", processingWidthPercentage).GetPtr(), false);
logger.Log(cString::Format("processingWidth: %d", processingData.processingWidth).GetPtr(), false);
logger.Log(cString::Format("startOffset: %d", processingData.startOffset).GetPtr(), false);
logger.Log(cString::Format("hoodScanLineStepWidth: %d", processingData.hoodScanLineStepWidth).GetPtr(), false);
logger.Log(cString::Format("roomScanLineStepWidth: %d", processingData.roomScanLineStepWidth).GetPtr(), false);
}
RETURN_NOERROR;
}
tResult PIDController::ReadProperties(const tChar* strPropertyName)
{
if (NULL == strPropertyName || cString::IsEqual(strPropertyName, WSC_PROP_PID_KP))
{
m_f64PIDKp = GetPropertyFloat(WSC_PROP_PID_KP);
}
if (NULL == strPropertyName || cString::IsEqual(strPropertyName, WSC_PROP_PID_KD))
{
m_f64PIDKd = GetPropertyFloat(WSC_PROP_PID_KD);
}
if (NULL == strPropertyName || cString::IsEqual(strPropertyName, WSC_PROP_PID_KI))
{
m_f64PIDKi = GetPropertyFloat(WSC_PROP_PID_KI);
}
if (NULL == strPropertyName || cString::IsEqual(strPropertyName, WSC_PROP_PID_SAMPLE_TIME))
{
m_f64PIDSampleTime = GetPropertyFloat(WSC_PROP_PID_SAMPLE_TIME);
}
if (NULL == strPropertyName || cString::IsEqual(strPropertyName, WSC_PROP_PT1_OUTPUT_FACTOR))
{
m_f64PT1OutputFactor = GetPropertyFloat(WSC_PROP_PT1_OUTPUT_FACTOR);
}
if (NULL == strPropertyName || cString::IsEqual(strPropertyName, WSC_PROP_PT1_GAIN))
{
m_f64PT1Gain = GetPropertyFloat(WSC_PROP_PT1_GAIN);
}
if (NULL == strPropertyName || cString::IsEqual(strPropertyName, WSC_PROP_PT1_TIMECONSTANT))
{
m_f64PT1TimeConstant = GetPropertyFloat(WSC_PROP_PT1_TIMECONSTANT);
}
if (NULL == strPropertyName || cString::IsEqual(strPropertyName, "Controller Typ"))
{
m_i32ControllerMode = GetPropertyInt("Controller Typ");
}
if (NULL == strPropertyName || cString::IsEqual(strPropertyName, WSC_PROP_DEBUG_MODE))
{
m_bShowDebug = static_cast<tBool>(GetPropertyBool(WSC_PROP_DEBUG_MODE));
if(m_bShowDebug) {
LOG_INFO("PIDController: Debug mode is enabled");
}
}
if (NULL == strPropertyName || cString::IsEqual(strPropertyName, WSC_PROP_PT1_CORRECTION_FACTOR))
{
m_f64PT1CorrectionFactor = GetPropertyFloat(WSC_PROP_PT1_CORRECTION_FACTOR);
}
if (NULL == strPropertyName || cString::IsEqual(strPropertyName, WSC_PROP_PID_MINOUTPUT))
{
m_f64PIDMinimumOutput = GetPropertyFloat(WSC_PROP_PID_MINOUTPUT);
}
if (NULL == strPropertyName || cString::IsEqual(strPropertyName, WSC_PROP_PID_MAXOUTPUT))
{
m_f64PIDMaximumOutput = GetPropertyFloat(WSC_PROP_PID_MAXOUTPUT);
}
if (NULL == strPropertyName || cString::IsEqual(strPropertyName, WSC_PROP_PID_WINDUP))
{
m_i32AntiWindupMethod = GetPropertyInt(WSC_PROP_PID_WINDUP);
}
if (NULL == strPropertyName || cString::IsEqual(strPropertyName, WSC_PROP_PID_SETZERO))
{
m_bPID_Zero_Beh = GetPropertyBool(WSC_PROP_PID_SETZERO);
}
if (NULL == strPropertyName || cString::IsEqual(strPropertyName, WSC_PROP_PID_USEDIRINPUT))
{
m_bUseSetSpeedDirInfo = GetPropertyBool(WSC_PROP_PID_USEDIRINPUT);
}
RETURN_NOERROR;
}
