tResult FunctionDriver::Init(tInitStage eStage, __exception)
{
RETURN_IF_FAILED(cFilter::Init(eStage, __exception_ptr));
if (eStage == StageFirst)
{
// create all the pins
RETURN_IF_FAILED(CreateInputPins(__exception_ptr));
RETURN_IF_FAILED(CreateOutputPins(__exception_ptr));
}
else if (eStage == StageNormal)
{
speed = tFloat32(GetPropertyFloat(SPEED_PROPERTY));
sinWidth = tFloat32(GetPropertyFloat(SINWIDTH_PROPERTY));
sinLength = tFloat32(GetPropertyFloat(SINLENGTH_PROPERTY));
}
RETURN_NOERROR;
}
tResult YawToSteer::Init(tInitStage eStage, __exception)
{
RETURN_IF_FAILED(cFilter::Init(eStage, __exception_ptr));
if (eStage == StageFirst)
{
RETURN_IF_FAILED(CreateDescriptions(__exception_ptr));
RETURN_IF_FAILED(CreateInputPins(__exception_ptr));
RETURN_IF_FAILED(CreateOutputPins(__exception_ptr));
}
else if (eStage == StageNormal)
{
compensationValue = GetPropertyFloat(STEERING_COMPANSATION_FACTOR_PROPERTY);
deviationThreshold = GetPropertyFloat(DEVIATION_THRESHOLD_PROPERTY);
abruptVariationThreshold = GetPropertyFloat(ABRUPT_VARIATION_THRESHOLD_PROPERTY);
}
else if (eStage == StageGraphReady)
{
m_bIDsSignalSet = tFalse;
}
RETURN_NOERROR;
}
// -------------------------------------------------------------------------------------------------
tResult LaneFilter::processImage(IMediaSample* image_sample) {
// -------------------------------------------------------------------------------------------------
const tVoid* source_buffer;
bool enable_lights = false;
if (IS_OK(image_sample->Lock(&source_buffer))) {
int source_width = input_format_.nWidth;
int source_height = input_format_.nHeight;
Mat current_image(source_height, source_width, CV_8UC3, (uchar*)source_buffer);
enable_lights = calculateImageLuminosity(current_image) < GetPropertyFloat("lum_threshold");
// Detect the edges of the image by using a Canny Algo
Mat preprocessed;
lane_preprocessor_.set_gaussian_blur(Size(15,15), 2);
lane_preprocessor_.preprocess_image(current_image, preprocessed);
std::vector<Vector2> mapped_points;
Mat mapping_image;
lane_detector_.detect_lanes(preprocessed, mapping_image, mapped_points);
image_sample->Unlock(source_buffer);
transmitLanePoints(mapped_points);
}
if (GetPropertyBool("headlights_enabled")) {
if(enable_lights != headlights_on_) {
transmitHeadLight(enable_lights);
headlights_on_ = enable_lights;
}
}
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 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;
}
}
// create and register the output pin
RETURN_IF_FAILED(m_pin_output_accelerate.Create("accelerate_out", pTypeSignalValueOutput, static_cast<IPinEventSink*> (this)));
RETURN_IF_FAILED(RegisterPin(&m_pin_output_accelerate));
RETURN_IF_FAILED(m_pin_output_steer_angle.Create("steerAngle", pTypeSignalValueOutput, static_cast<IPinEventSink*> (this)));
RETURN_IF_FAILED(RegisterPin(&m_pin_output_steer_angle));
/* RETURN_IF_FAILED(m_pin_output_turn_left_enabled.Create("turnSignalLeftEnabled", pTypeBoolSignalValueOutput, static_cast<IPinEventSink*> (this)));
RETURN_IF_FAILED(RegisterPin(&m_pin_output_turn_left_enabled));
RETURN_IF_FAILED(m_pin_output_turn_right_enabled.Create("turnSignalRightEnabled", pTypeBoolSignalValueOutput, static_cast<IPinEventSink*> (this)));
RETURN_IF_FAILED(RegisterPin(&m_pin_output_turn_right_enabled));*/
// get properties
m_pparkluecke = tFloat32(GetPropertyFloat("Parallel Parkluecke"));
m_pdistanz_a = tFloat32(GetPropertyFloat("Parallel Distanz A"));
m_pdistanz_b = tFloat32(GetPropertyFloat("Parallel Distanz B"));
m_pwinkel1 = tFloat32(GetPropertyFloat("Parallel Winkel 1"));
m_pwinkel2 = tFloat32(GetPropertyFloat("Parallel Winkel 2"));
m_cparkluecke = tFloat32(GetPropertyFloat("Cross Parkluecke"));
m_cdistanz_a = tFloat32(GetPropertyFloat("Cross Distanz A"));
m_cdistanz_b = tFloat32(GetPropertyFloat("Cross Distanz B"));
m_cwinkel1 = tFloat32(GetPropertyFloat("Cross Winkel 1"));
m_cwinkel2 = tFloat32(GetPropertyFloat("Cross Winkel 2"));
//set distance, status and active
distance_a = 0;
active = 0;
status = 0;
gyro_a = 0;
tResult CarFollower::processSigns(SignStruct &data)
{
if (data.id == RoadSignTargetCar) {
signTimer=8;
//logger.log()<<"CarFollower::processSigns;"<<
// data.pos.x<<";"<<
// data.pos.y<<";"<<
// "ID;"<<data.id<<endl;
targetPose.x=data.pos.x;
targetPose.y=data.pos.y;
targetPose.speed=0;
targetPose.theta=data.theta;
logger.log()<<"TargetPose;"<<targetPose<<endl;
//average the delta velocity of target pose
tTimeStamp currTimeStamp=_clock->GetTime();
tFloat64 timeDiff=(currTimeStamp-lastTimeStamp);
lastTimeStamp=currTimeStamp;
//Calculate relative velocity
float relativeDistance=sqrt(pow(targetPose.x-currentPose.x,2)+pow(targetPose.y-currentPose.y,2));
float deltaRelativeDistance=relativeDistance-lastRelativeDistance;
lastRelativeDistance=relativeDistance;
float relativeVelocity;
if (timeDiff>0) {
relativeVelocity=(deltaRelativeDistance*1000000)/timeDiff;
} else {
relativeVelocity=0;
}
//Calculate relative velocity
float absoluteDistance=sqrt(pow(targetPose.x,2)+pow(targetPose.y,2));
float deltaAbsoluteDistance=absoluteDistance-lastAbsoluteDistance;
lastAbsoluteDistance=absoluteDistance;
float absoluteVelocity;
if (timeDiff>0) {
absoluteVelocity=(deltaAbsoluteDistance*1000000)/timeDiff;
} else {
absoluteVelocity=0;
}
/*if (timeDiff>250*1000)
{
//Target reacquired after long time (250 ms), do not calculate velocity
commandVelocity=0;
RETURN_NOERROR;
}*/
//Filter relative velocity
if (filterRelativeTargetVelocityCounter < VEL_FILTER_SIZE) {
filterRelativeTargetVelocity.push_back(relativeVelocity);
}
else
{
filterRelativeTargetVelocity[filterRelativeTargetVelocityCounter % VEL_FILTER_SIZE]=relativeVelocity;
}
float sum = 0;
for (size_t i = 0; i < filterRelativeTargetVelocity.size(); ++i) {
sum += filterRelativeTargetVelocity[i];
}
filterRelativeTargetVelocityCounter++;
targetRelativeVelocityFiltered=sum/filterRelativeTargetVelocity.size();
//Filter absolute velocity
if (filterAbsoluteTargetVelocityCounter < VEL_FILTER_SIZE) {
filterAbsoluteTargetVelocity.push_back(absoluteVelocity);
}
else
{
filterAbsoluteTargetVelocity[filterAbsoluteTargetVelocityCounter % VEL_FILTER_SIZE]=absoluteVelocity;
}
sum = 0;
for (size_t i = 0; i < filterAbsoluteTargetVelocity.size(); ++i) {
sum += filterAbsoluteTargetVelocity[i];
}
filterAbsoluteTargetVelocityCounter++;
targetAbsoluteVelocityFiltered=sum/filterAbsoluteTargetVelocity.size();
float D = GetPropertyFloat(PROP_SPACING);
float a = GetPropertyFloat(PROP_SPEEDSPACING);
float compensatePos = a*(relativeDistance-D)/D;
if (compensatePos<0)
{
compensatePos = 0;
}
if (relativeDistance<D)
{
commandVelocity=0.8*targetAbsoluteVelocityFiltered;
}
else
{
commandVelocity=currentPose.speed+0.8*compensatePos;
}
if (commandVelocity>1.5)
{
logger.log()<<"CommandVelocityExceedMax;"<<commandVelocity<<endl;
commandVelocity=1.5;
}
logger.log()<<"RelVelocity;"<<targetRelativeVelocityFiltered<<";AbsVelocity;"<<currentPose.speed<<endl;//<<";Time;"<<timeDiff<<";FilteredVelocity;"<<targetRelativeVelocityFiltered<<endl;
//logger.log()<<"AbsVelocity;Calc;"<<absVelocity<<";Distance;"<<absDistance<<";Time;"<<timeDiff<<";FilteredVelocity;"<<targetAbsoluteVelocityFiltered<<endl;
//logger.log()<<"CommandVelocity;"<<commandVelocity<<endl;
}
RETURN_NOERROR;
}
RETURN_IF_FAILED(pTypeSignalValueOutput->GetInterface(IID_ADTF_MEDIA_TYPE_DESCRIPTION, (tVoid**)&m_pCoderDescSignalOutput));
tChar const * strDescBoolSignalValue = pDescManager->GetMediaDescription("tBoolSignalValue");
RETURN_IF_POINTER_NULL(strDescBoolSignalValue);
cObjectPtr<IMediaType> pTypeBoolSignalValueOutput = new cMediaType(0, 0, 0, "tBoolSignalValue", strDescBoolSignalValue,IMediaDescription::MDF_DDL_DEFAULT_VERSION);
RETURN_IF_FAILED(pTypeBoolSignalValueOutput->GetInterface(IID_ADTF_MEDIA_TYPE_DESCRIPTION, (tVoid**)&m_pCoderDescBoolSignalOutput));
// create and register the output pin
RETURN_IF_FAILED(m_pin_output_accelerate.Create("accelerate_out", pTypeSignalValueOutput, static_cast<IPinEventSink*> (this)));
RETURN_IF_FAILED(RegisterPin(&m_pin_output_accelerate));
RETURN_IF_FAILED(m_pin_output_stuck.Create("got_stuck", pTypeBoolSignalValueOutput, static_cast<IPinEventSink*> (this)));
RETURN_IF_FAILED(RegisterPin(&m_pin_output_stuck));
// get properties
m_stopDistance = tFloat32(GetPropertyFloat("Hard Stop Distance"));
m_slowDistance15 = tFloat32(GetPropertyFloat("Slow down to 15 Distance"));
m_slowDistance25 = tFloat32(GetPropertyFloat("Slow down to 25 Distance"));
m_slowDistance30 = tFloat32(GetPropertyFloat("Slow down to 30 Distance"));
m_slowDistance_back = tFloat32(GetPropertyFloat("Back Distance"));
m_stuck_time = tFloat32(GetPropertyFloat("Stuck Time in s"));
m_stuck_distance = tFloat32(GetPropertyFloat("Stuck Distance"));
}
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.
}
else if (eStage == StageGraphReady)
{
// All pin connections have been established in this stage so you can query your pins
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;
}
