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; }