#include "cameraconfig.h"
#include <mil.h>
#include "mil3dcam_sickrangere.h"
#include "sickcamera_rangere.h"
#if !USE_REAL_CAMERA
MIL_INT CreateCameraSystem(SSickCameraSystem* pCameraSystem)
{
return SICK_CAMERA_OK;
}
MIL_INT InitCameraSystem(SSickCameraSystem* pCameraSystem)
{
return SICK_CAMERA_OK;
}
MIL_INT FillParams(SSickCameraSystem* pCameraSystem,
RangerParams* pConvParams,
const std::string&)
{
pConvParams->LaserMeasurementMode = M_RANGER_Hi3D_MODE;
pConvParams->RangeStreamOffset = 10752;
pConvParams->LaserIntensityStreamOffset = 9216;
pConvParams->RangeSizeBit = 16;
pConvParams->LaserIntensitySizeBit = 8;
pConvParams->LaserScatterSizeBit = NO_INFORMATION;
pConvParams->LaserScatterStreamOffset = NO_INFORMATION;
pConvParams->GrayStreamOffset = NO_INFORMATION;
pConvParams->HiResGrayStreamOffset = NO_INFORMATION;
pConvParams->ColorStreamOffset = NO_INFORMATION;
pConvParams->HiResColorStreamOffset = 0;
pConvParams->ScatterStreamOffset = NO_INFORMATION;
pConvParams->ImageROISizeX = 1536;
pConvParams->ImageROIOffsetX = 0;
pConvParams->ImageROISizeY = 512;
pConvParams->ImageROIOffsetY = 0;
pConvParams->MeasROISizeX = 1536;
pConvParams->MeasROIOffsetX = 0;
pConvParams->LaserROISizeY = 464;
pConvParams->LaserROIOffsetY = 48;
pConvParams->ProfileDataSizeByte = 13824;
pConvParams->FlipY = 1;
pConvParams->FixedPoint = 4;
pConvParams->ROIOffsetX = 0;
pConvParams->ROIOffsetY = 48;
pConvParams->CycleTimeMicroseconds = 5000;
return SICK_CAMERA_OK;
}
void CloseDown(SSickCameraSystem* pCameraSystem)
{
}
#else
#include "icon_auto_link.h"
#include "icon_api.h"
#include "ethernetcamera.h"
#include "framegrabber.h"
#include "dataformat.h"
using namespace std;
using namespace icon;
static const string BasePath = "<ROOT><CAMERA name='" CAMERA_NAME "'>";
static const string ImageConfigPath =
BasePath + "<CONFIGURATION name='" IMAGE_MODE_NAME "'>";
static const string ImageCompPath =
ImageConfigPath + "<COMPONENT name='" IMAGE_COMP_NAME "'>";
static const string MeasConfigPath =
BasePath + "<CONFIGURATION name='" MEASUREMENT_MODE_NAME "'>";
static const string ColorCompPath =
MeasConfigPath + "<COMPONENT name='" COLOR_COMP_NAME "'>";
static const string HiresColorCompPath =
MeasConfigPath + "<COMPONENT name='" HIRES_COLOR_COMP_NAME "'>";
static const string NumberOfColumnsName = "number of columns";
static const string NumberOfRowsName = "number of rows";
static const string RangeAxisName = "range axis";
static const string SubPixelingName = "sub pixeling";
static const string NumberOfThrsName = "number of thrs";
static const string OutputSelectName = "output select";
static const string AcquistionDirectionName = "acquistion direction";
static const string StartColumnName = "start column";
static const string StartRowName = "start row";
static const string CycleTimeName = "cycle time";
static const string EnableScatterName = "enable scatter";
static const string BalanceGreenName = "balance green";
static const string GainGreenName = "gain green";
static const string BalanceBlueName = "balance blue";
static const string GainBlueName = "gain blue";
static const string GainRedName = "gain red";
static const string ExposureTimeRedName = "exposure time red";
static const string HorThrValueType = "HorThr";
static const string HorMaxValueType = "HorMax";
static const string HorMaxThrValueType = "HorMaxThr";
static const string Hi3DCOGValueType = "COG";
static const string Hi3DValueType = "DCM";
static const string ColorValueType = "RGB";
static const string HiResColorValue = "HiresRGB";
static const string GrayValueType = "Gray";
static const string HiResGrayValue = "HiresGray";
static const string ScatterValueType = "Scatter";
static const string ImageValueType = "image";
static const string ByteTypeName = "BYTE";
static const string WordTypeName = "WORD";
static const char* const XSumSubCompName = "XSum";
static const char* const SumSubCompName = "Sum";
static const char* const RangeSubCompName = "Range";
static const char* const IntensitySubCompName = "Intensity";
static const char* const ScatterSubCompName = "Scatter";
#define FRAME_BUFFER_SIZE 50
#define CONVEYOR_STARTUP_DELAY_MS 300
#define MAX_FRAME_GRABBER 4
static FrameGrabber* ActiveFrameGrabber[MAX_FRAME_GRABBER] = {NULL, NULL, NULL, NULL};
MIL_INT CreateCamera(EthernetCamera** ppCam);
MIL_INT StopCamera(EthernetCamera* pCam);
MIL_INT GetCameraDataFormat(EthernetCamera* pCam, DataFormat* pDataFormat);
MIL_INT GetCameraMilIntParam(EthernetCamera* pCam,
const string& rPath,
const string& rParamName,
MIL_INT* pIntValue);
MIL_INT SetCameraMilIntParam(EthernetCamera* pCam,
const string& rPath,
const string& rParamName,
MIL_INT IntValue);
MIL_INT CreateFrameGrabber(FrameGrabber** ppFrameGrab);
MIL_INT StartGrab(SSickCameraSystem* pCameraSystem, MIL_ID StartStageEvent = M_NULL);
MIL_INT StopGrab(SSickCameraSystem* pCameraSystem);
void ProcessCallback(IconBuffer *pBuffer, FrameGrabber* pFrameGrab, void* UserData);
MIL_INT GetNewActiveIndex();
bool IsActive(FrameGrabber* pFrameGrab);
bool RemoveActiveFrameGrabber(FrameGrabber* pFrameGrab);
MIL_INT FillHorThrParams(RangerParams* pConvParams,
MIL_INT MeasStreamOffset,
const Component* pMeasComponent,
RangerE* pCam,
const string MeasCompPath );
MIL_INT FillHorMaxParams(RangerParams* pConvParams,
MIL_INT MeasStreamOffset,
const Component* pMeasComponent,
RangerE* pCam,
const string MeasCompPath);
MIL_INT FillHorMaxThrParams(RangerParams* pConvParams,
MIL_INT MeasStreamOffset,
const Component* pMeasComponent,
RangerE* pCam,
const string MeasCompPath );
MIL_INT FillHi3DParams(RangerParams* pConvParams,
MIL_INT MeasStreamOffset,
const Component* pMeasComponent,
RangerE* pCam,
const string MeasCompPath );
MIL_INT FillHi3DCogParams(RangerParams* pConvParams,
MIL_INT MeasStreamOffset,
const Component* pMeasComponent,
RangerE* pCam,
const string MeasCompPath );
MIL_INT CreateCameraSystem(SSickCameraSystem* pCameraSystem)
{
if(CreateCamera(&pCameraSystem->pCam) == SICK_CAMERA_NOT_OK)
return SICK_CAMERA_NOT_OK;
return CreateFrameGrabber(&(pCameraSystem->pFrameGrab));
}
MIL_INT CreateCamera(EthernetCamera** ppCam)
{
*ppCam = NULL;
Camera* pGeneric = createCamera(EthernetCamera::cameraType, "MyCamera");
if(pGeneric == NULL || pGeneric->getCameraType() != EthernetCamera::cameraType)
{
delete pGeneric;
return SICK_CAMERA_NOT_OK;
}
*ppCam = static_cast<EthernetCamera*>(pGeneric);
return SICK_CAMERA_OK;
}
MIL_INT CreateFrameGrabber(FrameGrabber** ppFrameGrab)
{
*ppFrameGrab = NULL;
*ppFrameGrab = createFrameGrabber("FGEthernetFast", "MyGrabber");
if(*ppFrameGrab == NULL)
{
delete *ppFrameGrab;
return SICK_CAMERA_NOT_OK;
}
return SICK_CAMERA_OK;
}
MIL_INT InitCameraSystem(SSickCameraSystem* pCameraSystem)
{
int Ret;
FGEthernetFastParameters* pFrameGrabberParams = static_cast<FGEthernetFastParameters*>(pCameraSystem->pFrameGrab->getParameters());
Ret = pCameraSystem->pCam->setComParameters(CAMERA_IP,
pFrameGrabberParams->getFrameGrabberPort(),
pFrameGrabberParams->getRedundancyPort(),
EthernetCamera::HIGH_PERFORMANCE_DATA_CHANNEL);
if (Ret != Camera::E_ALL_OK)
return SICK_CAMERA_NOT_OK;
Ret = pCameraSystem->pCam->init();
if(Ret != Camera::E_ALL_OK)
return SICK_CAMERA_NOT_OK;
pFrameGrabberParams->setCameraIP(CAMERA_IP);
pFrameGrabberParams->setBufferSize(FRAME_BUFFER_SIZE);
pFrameGrabberParams->setRecoveryInterval(10);
int CameraDataPort;
Ret = pCameraSystem->pCam->getCameraDataPort(CameraDataPort);
if(Ret != Camera::E_ALL_OK)
return SICK_CAMERA_NOT_OK;
else
pFrameGrabberParams->setCameraPort(CameraDataPort);
if(Ret != Camera::E_ALL_OK)
return SICK_CAMERA_NOT_OK;
return SICK_CAMERA_OK;
}
MIL_INT FillParams(SSickCameraSystem* pCameraSystem,
RangerParams* pConvParams,
const string& rParamFilename)
{
MIL_INT Ret;
int Ret2;
Ret = SwitchGrabMode(pCameraSystem, IMAGE_MODE_NAME);
if (Ret != SICK_CAMERA_OK)
return Ret;
Ret2 = pCameraSystem->pCam->fileLoadParameters(rParamFilename);
if(Ret2 != Camera::E_ALL_OK)
return SICK_CAMERA_NOT_OK;
Ret = GetCameraMilIntParam(pCameraSystem->pCam, ImageConfigPath, StartColumnName, &pConvParams->ImageROIOffsetX);
if (Ret != SICK_CAMERA_OK)
return Ret;
Ret = GetCameraMilIntParam(pCameraSystem->pCam, ImageCompPath, StartRowName, &pConvParams->ImageROIOffsetY);
if (Ret != SICK_CAMERA_OK)
return Ret;
Ret = GetCameraMilIntParam(pCameraSystem->pCam, ImageConfigPath, NumberOfColumnsName, &pConvParams->ImageROISizeX);
if (Ret != SICK_CAMERA_OK)
return Ret;
Ret = GetCameraMilIntParam(pCameraSystem->pCam, ImageCompPath, NumberOfRowsName, &pConvParams->ImageROISizeY);
if (Ret != SICK_CAMERA_OK)
return Ret;
Ret = SwitchGrabMode(pCameraSystem, MEASUREMENT_MODE_NAME);
if (Ret != SICK_CAMERA_OK)
return Ret;
DataFormat CameraMeasFormat;
Ret = GetCameraDataFormat(pCameraSystem->pCam, &CameraMeasFormat);
if (Ret != SICK_CAMERA_OK)
return Ret;
if (CameraMeasFormat.getConfigType() != DataFormat::Linescan_T)
return SICK_CAMERA_NOT_OK;
pConvParams->ProfileDataSizeByte = CameraMeasFormat.dataSize();
Ret = GetCameraMilIntParam(pCameraSystem->pCam, MeasConfigPath, CycleTimeName, &pConvParams->CycleTimeMicroseconds);
if (Ret != SICK_CAMERA_OK)
return Ret;
Ret = GetCameraMilIntParam(pCameraSystem->pCam, MeasConfigPath, NumberOfColumnsName, &pConvParams->MeasROISizeX);
if (Ret != SICK_CAMERA_OK)
return Ret;
Ret = GetCameraMilIntParam(pCameraSystem->pCam, MeasConfigPath, StartColumnName, &pConvParams->MeasROIOffsetX);
if (Ret != SICK_CAMERA_OK)
return Ret;
pConvParams->ROIOffsetX = (pConvParams->ImageROIOffsetX + pConvParams->ImageROISizeX) - (pConvParams->MeasROIOffsetX + pConvParams->MeasROISizeX);
pConvParams->GrayStreamOffset = NO_INFORMATION;
pConvParams->HiResGrayStreamOffset = NO_INFORMATION;
pConvParams->ColorStreamOffset = NO_INFORMATION;
pConvParams->HiResColorStreamOffset = NO_INFORMATION;
pConvParams->ScatterStreamOffset = NO_INFORMATION;
pConvParams->LaserMeasurementMode = NO_INFORMATION;
for(uint_t CompIdx = 0; CompIdx < CameraMeasFormat.numComponents(); CompIdx++)
{
const Component* pMeasComponent = CameraMeasFormat.getComponent(CompIdx);
string MeasCompName = pMeasComponent->getName();
string MeasValueTypeString = pMeasComponent->getValueType();
MIL_INT MeasStreamOffset = CameraMeasFormat.getComponentOffset(CompIdx);
const string MeasCompPath = MeasConfigPath + "<COMPONENT name='" + MeasCompName + "'>";
bool IsLaser = true;
if (MeasValueTypeString == HorThrValueType)
Ret = FillHorThrParams(pConvParams, MeasStreamOffset, pMeasComponent, pCameraSystem->pCam, MeasCompPath);
else if (MeasValueTypeString == HorMaxValueType)
Ret = FillHorMaxParams(pConvParams, MeasStreamOffset, pMeasComponent, pCameraSystem->pCam, MeasCompPath);
else if (MeasValueTypeString == HorMaxThrValueType)
Ret = FillHorMaxThrParams(pConvParams, MeasStreamOffset, pMeasComponent, pCameraSystem->pCam, MeasCompPath);
else if (MeasValueTypeString == Hi3DValueType)
Ret = FillHi3DParams(pConvParams, MeasStreamOffset, pMeasComponent, pCameraSystem->pCam, MeasCompPath);
else if (MeasValueTypeString == Hi3DCOGValueType)
Ret = FillHi3DCogParams(pConvParams, MeasStreamOffset, pMeasComponent, pCameraSystem->pCam, MeasCompPath);
else
{
IsLaser = false;
if (MeasValueTypeString == GrayValueType)
pConvParams->GrayStreamOffset = MeasStreamOffset;
else if(MeasValueTypeString == HiResGrayValue)
pConvParams->HiResGrayStreamOffset = MeasStreamOffset;
else if(MeasValueTypeString == ColorValueType)
pConvParams->ColorStreamOffset = MeasStreamOffset;
else if(MeasValueTypeString == HiResColorValue)
pConvParams->HiResColorStreamOffset = MeasStreamOffset;
else if(MeasValueTypeString == ScatterValueType)
pConvParams->ScatterStreamOffset = MeasStreamOffset;
else
return SICK_CAMERA_NOT_OK;
}
if(IsLaser)
{
MIL_INT MeasROIOffsetY;
Ret = GetCameraMilIntParam(pCameraSystem->pCam, MeasCompPath, StartRowName, &MeasROIOffsetY);
if (Ret != SICK_CAMERA_OK)
return Ret;
MIL_INT MeasROISizeY;
Ret = GetCameraMilIntParam(pCameraSystem->pCam, MeasCompPath, NumberOfRowsName, &MeasROISizeY);
if (Ret != SICK_CAMERA_OK)
return Ret;
pConvParams->LaserROIOffsetY = MeasROIOffsetY;
pConvParams->LaserROISizeY = MeasROISizeY;
pConvParams->ROIOffsetY = MeasROIOffsetY - pConvParams->ImageROIOffsetY;
}
}
return SICK_CAMERA_OK;
}
MIL_INT SetColorGain(SSickCameraSystem* pCameraSystem, MIL_DOUBLE RedMean, MIL_DOUBLE GreenMean, MIL_DOUBLE BlueMean, bool UseHighres)
{
const std::string& CompPath = UseHighres ? HiresColorCompPath : ColorCompPath;
MIL_INT GainRed = RedMean < 64 ? 4 : (RedMean < 85 ? 3 : 1);
MIL_INT Ret = SetCameraMilIntParam(pCameraSystem->pCam, CompPath, GainRedName, GainRed);
if(Ret != Camera::E_ALL_OK)
return SICK_CAMERA_NOT_OK;
MIL_INT GainGreen = GreenMean < 64 ? 4 : (GreenMean < 85 ? 3 : 1);
Ret = SetCameraMilIntParam(pCameraSystem->pCam, CompPath, GainGreenName, GainGreen);
if(Ret != Camera::E_ALL_OK)
return SICK_CAMERA_NOT_OK;
MIL_INT GainBlue = BlueMean < 64 ? 4 : (BlueMean < 85 ? 3 : 1);
Ret = SetCameraMilIntParam(pCameraSystem->pCam, CompPath, GainBlueName, GainBlue);
if(Ret != Camera::E_ALL_OK)
return SICK_CAMERA_NOT_OK;
return SICK_CAMERA_OK;
}
MIL_INT SetWhiteBalancing(SSickCameraSystem* pCameraSystem, MIL_DOUBLE RedMean, MIL_DOUBLE GreenMean, MIL_DOUBLE BlueMean, bool UseHighres)
{
MIL_INT Ret;
const std::string& CompPath = UseHighres ? HiresColorCompPath : ColorCompPath;
MIL_INT BalanceGreen = (MIL_INT)((RedMean/GreenMean*100.0) + 0.5);
MIL_INT BalanceBlue = (MIL_INT)((RedMean/BlueMean*100.0) + 0.5);
MIL_INT ExposureTimeRed;
Ret = GetCameraMilIntParam(pCameraSystem->pCam, CompPath, ExposureTimeRedName, &ExposureTimeRed);
if(Ret != Camera::E_ALL_OK)
return SICK_CAMERA_NOT_OK;
MIL_INT CycleTime;
Ret = GetCameraMilIntParam(pCameraSystem->pCam, MeasConfigPath, CycleTimeName, &CycleTime);
if(Ret != Camera::E_ALL_OK)
return SICK_CAMERA_NOT_OK;
ExposureTimeRed = CycleTime - ExposureTimeRed < 100 ? CycleTime - 100 : ExposureTimeRed;
MIL_INT NewExposureTimeGreen = (MIL_INT)(BalanceGreen > 100 ? (MIL_DOUBLE)ExposureTimeRed / BalanceGreen * 100: ExposureTimeRed);
MIL_INT NewExposureTimeBlue = (MIL_INT)(BalanceBlue > 100 ? (MIL_DOUBLE)ExposureTimeRed / BalanceBlue * 100: ExposureTimeRed);
Ret = SetCameraMilIntParam(pCameraSystem->pCam, CompPath, ExposureTimeRedName, NewExposureTimeGreen < NewExposureTimeBlue ? NewExposureTimeGreen : NewExposureTimeBlue);
if(Ret != Camera::E_ALL_OK)
return SICK_CAMERA_NOT_OK;
Ret = SetCameraMilIntParam(pCameraSystem->pCam, CompPath, BalanceGreenName, BalanceGreen);
if(Ret != Camera::E_ALL_OK)
return SICK_CAMERA_NOT_OK;
Ret = SetCameraMilIntParam(pCameraSystem->pCam, CompPath, BalanceBlueName, BalanceBlue);
if(Ret != Camera::E_ALL_OK)
return SICK_CAMERA_NOT_OK;
return SICK_CAMERA_OK;
}
void CloseDown(SSickCameraSystem* pCameraSystem)
{
StopGrab(pCameraSystem);
if(pCameraSystem->pCam != NULL)
{
pCameraSystem->pCam->close();
delete pCameraSystem->pCam;
}
if(pCameraSystem->pFrameGrab != NULL)
{
pCameraSystem->pFrameGrab->disconnect();
delete pCameraSystem->pFrameGrab;
}
closeApi();
}
void FrameGrabberProcess(SSickCameraSystem* pCameraSystem, MIL_INT Operation, MIL_INT NbGrab , MIL_ID StartStageEvent )
{
StopGrab(pCameraSystem);
if(Operation & M_STOP)
return;
FGEthernetFastParameters* pFGEThernetParams = static_cast<FGEthernetFastParameters*>(pCameraSystem->pFrameGrab->getParameters());
if(Operation & M_START && pFGEThernetParams->getMode() == FrameGrabber::FGCALLBACK)
{
StartGrab(pCameraSystem, StartStageEvent);
}
else
{
MIL_INT Increment = 0;
MIL_INT GrabCount = 1;
if(Operation == M_SEQUENCE && NbGrab != M_DEFAULT)
{
Increment = 1;
GrabCount = NbGrab;
}
StartGrab(pCameraSystem, StartStageEvent);
IconBuffer* pBuffer;
MIL_INT Ret = pCameraSystem->pFrameGrab->getNextIconBuffer(&pBuffer, ULONG_MAX);
for(MIL_INT FrameIdx = 0; FrameIdx < GrabCount && IsActive(pCameraSystem->pFrameGrab); FrameIdx += Increment)
{
MIL_INT Ret = pCameraSystem->pFrameGrab->getNextIconBuffer(&pBuffer, ULONG_MAX);
ProcessCallback(pBuffer, pCameraSystem->pFrameGrab, pFGEThernetParams->getUserData());
}
StopGrab(pCameraSystem);
}
}
MIL_INT SwitchGrabMode(SSickCameraSystem* pCameraSystem,
const std::string& rModeName,
MIL_UINT NbScansPerGrab ,
MIL_INT GrabType ,
FrameGrabberHookStruct* pFrameGrabberHook )
{
int Ret;
MIL_INT Ret2;
Ret2 = StopGrab(pCameraSystem);
if(Ret2 != SICK_CAMERA_OK)
return Ret2;
if(pCameraSystem->pFrameGrab->isConnected())
{
Ret = pCameraSystem->pFrameGrab->disconnect();
if(Ret != FrameGrabber::E_ALL_OK)
return SICK_CAMERA_NOT_OK;
}
Ret = pCameraSystem->pCam->setActiveConfiguration(rModeName);
if(Ret != Camera::E_ALL_OK)
return SICK_CAMERA_NOT_OK;
FGEthernetFastParameters* pFGEThernetParams = static_cast<FGEthernetFastParameters*>(pCameraSystem->pFrameGrab->getParameters());
string CameraDataFormat;
Ret = pCameraSystem->pCam->getDataFormat("", CameraDataFormat);
if(Ret == Camera::E_ALL_OK)
{
unsigned long PacketSize;
Ret = pCameraSystem->pCam->getPacketSize(PacketSize);
if(Ret == Camera::E_ALL_OK)
pFGEThernetParams->setDataFormat(CameraDataFormat, PacketSize);
}
pFGEThernetParams->setNoScans((int)NbScansPerGrab);
pCameraSystem->pCam->setBufferHeight((unsigned long)NbScansPerGrab);
pFGEThernetParams->setMode(GrabType == M_ASYNCHRONOUS? FrameGrabber::FGCALLBACK : FrameGrabber::FGPOLLING);
pFGEThernetParams->setCallback(ProcessCallback);
pFGEThernetParams->setUserData(pFrameGrabberHook);
Ret = pCameraSystem->pFrameGrab->connect();
if(Ret != FrameGrabber::E_ALL_OK)
return SICK_CAMERA_NOT_OK;
return SICK_CAMERA_OK;
}
MIL_INT StopCamera(EthernetCamera* pCam)
{
int Ret, Status;
Ret = pCam->checkCamStatus(Status);
if(Ret != Camera::E_ALL_OK)
return SICK_CAMERA_NOT_OK;
if(Status != EthernetCamera::Stopped)
{
Ret = pCam->stop();
if(Ret != Camera::E_ALL_OK)
return SICK_CAMERA_NOT_OK;
}
return SICK_CAMERA_OK;
}
MIL_INT GetCameraDataFormat(EthernetCamera* pCam, DataFormat* pDataFormat)
{
int Ret;
string DataFormatString;
Ret = pCam->getDataFormat("", DataFormatString);
if(Ret != Camera::E_ALL_OK)
return SICK_CAMERA_NOT_OK;
Ret = pDataFormat->init(DataFormatString.c_str());
if(Ret != Camera::E_ALL_OK)
return SICK_CAMERA_NOT_OK;
return SICK_CAMERA_OK;
}
MIL_INT GetCameraMilIntParam(EthernetCamera* pCam,
const string& rPath,
const string& rParamName,
MIL_INT* pIntValue)
{
string ParamValueString;
int Ret = pCam->getParameterValue(rPath, rParamName, ParamValueString);
if (Ret != Camera::E_ALL_OK)
return SICK_CAMERA_NOT_OK;
*pIntValue = atol( ParamValueString.c_str() );
return SICK_CAMERA_OK;
}
MIL_INT SetCameraMilIntParam(EthernetCamera* pCam,
const string& rPath,
const string& rParamName,
MIL_INT IntValue)
{
bool UpdateRequired, DataFormatChanged;
char ParamStringArray[256];
_itoa_s((int)IntValue, ParamStringArray, 10);
string ParamString = ParamStringArray;
int Ret = pCam->setParameterValue(rPath, rParamName, ParamString, UpdateRequired, DataFormatChanged);
if(Ret != Camera::E_ALL_OK)
return SICK_CAMERA_NOT_OK;
return SICK_CAMERA_OK;
}
void ProcessCallback(IconBuffer *pBuffer, FrameGrabber* pFrameGrab, void* UserData)
{
FrameGrabberHookStruct* pFrameGrabHookStruct = (FrameGrabberHookStruct*) UserData;
MIL_ID MilRangerImage = MbufCreate2d(pFrameGrabHookStruct->MilSystem,
pBuffer->getWidth(),
pBuffer->getHeight(),
8 + M_UNSIGNED,
M_IMAGE + M_PROC,
M_HOST_ADDRESS + M_PITCH_BYTE,
pBuffer->getWidth(),
pBuffer->getWritePointer(0),
M_NULL);
(*(pFrameGrabHookStruct->HookHandlerPtr))(0, MilRangerImage, pFrameGrabHookStruct->UserDataPtr);
MbufFree(MilRangerImage);
}
MIL_INT StartGrab(SSickCameraSystem* pCameraSystem, MIL_ID StartStageEvent )
{
MIL_INT Index = GetNewActiveIndex();
if(Index != -1)
{
MIL_INT Ret;
ActiveFrameGrabber[Index] = pCameraSystem->pFrameGrab;
Ret = pCameraSystem->pFrameGrab->startGrab();
if(Ret != FrameGrabber::E_ALL_OK)
return SICK_CAMERA_NOT_OK;
Ret = pCameraSystem->pCam->start();
if(Ret != Camera::E_ALL_OK)
return SICK_CAMERA_NOT_OK;
if(StartStageEvent)
{
MthrControl(StartStageEvent, M_EVENT_SET, M_SIGNALED);
MosSleep(CONVEYOR_STARTUP_DELAY_MS);
}
}
else
return SICK_CAMERA_NOT_OK;
return SICK_CAMERA_OK;
}
MIL_INT StopGrab(SSickCameraSystem* pCameraSystem)
{
MIL_INT Ret;
MIL_INT Ret2;
if(RemoveActiveFrameGrabber(pCameraSystem->pFrameGrab))
{
Ret = pCameraSystem->pFrameGrab->stopGrab();
if(Ret != FrameGrabber::E_ALL_OK)
return SICK_CAMERA_NOT_OK;
int Status;
Ret2 = pCameraSystem->pCam->checkCamStatus(Status);
if(Ret2 != Camera::E_ALL_OK)
return SICK_CAMERA_NOT_OK;
if(Status != RangerE::Stopped)
{
Ret = pCameraSystem->pCam->stop();
if(Ret != Camera::E_ALL_OK)
return SICK_CAMERA_NOT_OK;
}
}
return SICK_CAMERA_OK;
}
MIL_INT GetNewActiveIndex()
{
for(MIL_INT i = 0; i < MAX_FRAME_GRABBER; i++)
{
if(ActiveFrameGrabber[i] == NULL)
return i;
}
return -1;
}
bool IsActive(FrameGrabber* pFrameGrab)
{
for(MIL_INT i = 0; i < MAX_FRAME_GRABBER; i++)
{
if(ActiveFrameGrabber[i] == pFrameGrab)
return true;
}
return false;
}
bool RemoveActiveFrameGrabber(FrameGrabber* pFrameGrab)
{
for(MIL_INT i = 0; i < MAX_FRAME_GRABBER; i++)
{
if(ActiveFrameGrabber[i] == pFrameGrab)
{
ActiveFrameGrabber[i] = NULL;
return true;
}
}
return false;
}
MIL_INT FillHorThrParams(RangerParams* pConvParams,
MIL_INT MeasStreamOffset,
const Component* pMeasComponent,
RangerE* pCam,
const string MeasCompPath )
{
MIL_INT Ret;
pConvParams->LaserMeasurementMode = RANGER_HOR_THR_MODE;
pConvParams->RangeStreamOffset = MeasStreamOffset +
pMeasComponent->getSubComponentOffset(RangeSubCompName);
pConvParams->LaserIntensityStreamOffset = NO_INFORMATION;
pConvParams->LaserIntensitySizeBit = NO_INFORMATION;
pConvParams->LaserScatterStreamOffset = NO_INFORMATION;
pConvParams->LaserScatterSizeBit = NO_INFORMATION;
if ( pConvParams->RangeStreamOffset == DataFormat::E_BAD_NAME )
return SICK_CAMERA_NOT_OK;
MIL_INT NumberOfThrsValue;
Ret = GetCameraMilIntParam(pCam, MeasCompPath, NumberOfThrsName, &NumberOfThrsValue);
if (Ret != SICK_CAMERA_OK)
return Ret;
MIL_INT OutputSelectValue;
Ret = GetCameraMilIntParam(pCam, MeasCompPath, OutputSelectName, &OutputSelectValue);
if (Ret != SICK_CAMERA_OK)
return Ret;
switch(NumberOfThrsValue)
{
case 1:
pConvParams->FixedPoint = 1;
break;
case 2:
switch(OutputSelectValue)
{
case 1:
case 2:
case 3:
pConvParams->FixedPoint = 1;
break;
case 0:
case 4:
pConvParams->FixedPoint = 2;
break;
case 10:
return SICK_CAMERA_NOT_OK;
default:
return SICK_CAMERA_NOT_OK;
}
break;
default:
return SICK_CAMERA_NOT_OK;
}
const SubComponent* pRangeSubComponent =
pMeasComponent->getNamedSubComponent(RangeSubCompName);
if (pRangeSubComponent == NULL)
return SICK_CAMERA_NOT_OK;
string RangeSizeBitString = pRangeSubComponent->getValueType();
if (RangeSizeBitString == ByteTypeName)
pConvParams->RangeSizeBit = 8;
else if (RangeSizeBitString == WordTypeName)
pConvParams->RangeSizeBit = 16;
else
return SICK_CAMERA_NOT_OK;
MIL_INT RangeAxisValue;
Ret = GetCameraMilIntParam(pCam, MeasCompPath, RangeAxisName, &RangeAxisValue);
if (Ret != SICK_CAMERA_OK)
return Ret;
if (RangeAxisValue != 0 && RangeAxisValue != 1)
return SICK_CAMERA_NOT_OK;
MIL_INT AcquistionDirectionValue;
Ret = GetCameraMilIntParam(pCam, MeasCompPath, AcquistionDirectionName,
&AcquistionDirectionValue);
if (Ret != SICK_CAMERA_OK)
return Ret;
if (AcquistionDirectionValue != 0 && AcquistionDirectionValue != 1)
return SICK_CAMERA_NOT_OK;
if (AcquistionDirectionValue == RangeAxisValue)
pConvParams->FlipY = DO_FLIP_Y_AXIS;
else
pConvParams->FlipY = DO_NOT_FLIP_Y_AXIS;
if ( pConvParams->RangeSizeBit == 16 &&
RangeAxisValue == 0 &&
pConvParams->LaserROISizeY != 512 )
return SICK_CAMERA_NOT_OK;
return SICK_CAMERA_OK;
}
MIL_INT FillHorMaxParams(RangerParams* pConvParams,
MIL_INT MeasStreamOffset,
const Component* pMeasComponent,
RangerE* pCam,
const string MeasCompPath)
{
MIL_INT Ret;
pConvParams->LaserMeasurementMode = RANGER_HOR_MAX_MODE;
pConvParams->RangeStreamOffset = MeasStreamOffset +
pMeasComponent->getSubComponentOffset(RangeSubCompName);
pConvParams->LaserIntensityStreamOffset = MeasStreamOffset +
pMeasComponent->getSubComponentOffset(IntensitySubCompName);
pConvParams->LaserIntensitySizeBit = 8;
pConvParams->LaserScatterStreamOffset = NO_INFORMATION;
pConvParams->LaserScatterSizeBit = NO_INFORMATION;
if ( pConvParams->RangeStreamOffset == DataFormat::E_BAD_NAME ||
pConvParams->LaserIntensityStreamOffset == DataFormat::E_BAD_NAME )
return SICK_CAMERA_NOT_OK;
Ret = GetCameraMilIntParam(pCam, MeasCompPath, SubPixelingName,
&pConvParams->FixedPoint);
if (Ret != SICK_CAMERA_OK)
return Ret;
if (pConvParams->FixedPoint != 0)
return SICK_CAMERA_NOT_OK;
const SubComponent* pRangeSubComponent =
pMeasComponent->getNamedSubComponent(RangeSubCompName);
if (pRangeSubComponent == NULL)
return SICK_CAMERA_NOT_OK;
string RangeSizeBitString = pRangeSubComponent->getValueType();
if (RangeSizeBitString == ByteTypeName)
pConvParams->RangeSizeBit = 8;
else if (RangeSizeBitString == WordTypeName)
pConvParams->RangeSizeBit = 16;
else
return SICK_CAMERA_NOT_OK;
pConvParams->FlipY = DO_FLIP_Y_AXIS;
if ( pConvParams->RangeSizeBit == 16 &&
pConvParams->LaserROISizeY != 512 )
return SICK_CAMERA_NOT_OK;
return SICK_CAMERA_OK;
}
MIL_INT FillHorMaxThrParams(RangerParams* pConvParams,
MIL_INT MeasStreamOffset,
const Component* pMeasComponent,
RangerE* pCam,
const string MeasCompPath )
{
MIL_INT Ret;
pConvParams->LaserMeasurementMode = RANGER_HOR_MAX_AND_THRES_MODE;
pConvParams->RangeStreamOffset = MeasStreamOffset +
pMeasComponent->getSubComponentOffset(RangeSubCompName);
pConvParams->LaserIntensityStreamOffset = MeasStreamOffset +
pMeasComponent->getSubComponentOffset(IntensitySubCompName);
pConvParams->LaserIntensitySizeBit = 8;
pConvParams->LaserScatterStreamOffset = NO_INFORMATION;
pConvParams->LaserScatterSizeBit = NO_INFORMATION;
if ( pConvParams->RangeStreamOffset == DataFormat::E_BAD_NAME ||
pConvParams->LaserIntensityStreamOffset == DataFormat::E_BAD_NAME )
return SICK_CAMERA_NOT_OK;
pConvParams->FixedPoint = 1;
const SubComponent* pRangeSubComponent =
pMeasComponent->getNamedSubComponent(RangeSubCompName);
if (pRangeSubComponent == NULL)
return SICK_CAMERA_NOT_OK;
string RangeSizeBitString = pRangeSubComponent->getValueType();
if (RangeSizeBitString == ByteTypeName)
pConvParams->RangeSizeBit = 8;
else if (RangeSizeBitString == WordTypeName)
pConvParams->RangeSizeBit = 16;
else
return SICK_CAMERA_NOT_OK;
MIL_INT RangeAxisValue;
Ret = GetCameraMilIntParam(pCam, MeasCompPath, RangeAxisName, &RangeAxisValue);
if (Ret != SICK_CAMERA_OK)
return Ret;
if (RangeAxisValue != 0 && RangeAxisValue != 1)
return SICK_CAMERA_NOT_OK;
MIL_INT AcquistionDirectionValue;
Ret = GetCameraMilIntParam(pCam, MeasCompPath, AcquistionDirectionName,
&AcquistionDirectionValue);
if (Ret != SICK_CAMERA_OK)
return Ret;
if (AcquistionDirectionValue != 0 && AcquistionDirectionValue != 1)
return SICK_CAMERA_NOT_OK;
if (AcquistionDirectionValue == RangeAxisValue)
pConvParams->FlipY = DO_FLIP_Y_AXIS;
else
pConvParams->FlipY = DO_NOT_FLIP_Y_AXIS;
if ( pConvParams->RangeSizeBit == 16 &&
RangeAxisValue == 0 &&
pConvParams->LaserROISizeY != 512 )
return SICK_CAMERA_NOT_OK;
return SICK_CAMERA_OK;
}
MIL_INT FillHi3DParams(RangerParams* pConvParams,
MIL_INT MeasStreamOffset,
const Component* pMeasComponent,
RangerE* pCam,
const string MeasCompPath )
{
MIL_INT Ret;
pConvParams->LaserMeasurementMode = M_RANGER_Hi3D_MODE;
pConvParams->RangeStreamOffset = MeasStreamOffset +
pMeasComponent->getSubComponentOffset(RangeSubCompName);
pConvParams->LaserIntensityStreamOffset = MeasStreamOffset +
pMeasComponent->getSubComponentOffset(IntensitySubCompName);
pConvParams->LaserIntensitySizeBit = 8;
if ( pConvParams->RangeStreamOffset == DataFormat::E_BAD_NAME ||
pConvParams->LaserIntensityStreamOffset == DataFormat::E_BAD_NAME )
return SICK_CAMERA_NOT_OK;
MIL_INT EnableScatterValue;
Ret = GetCameraMilIntParam(pCam, MeasCompPath, EnableScatterName, &EnableScatterValue);
if(Ret =! SICK_CAMERA_NOT_OK)
return Ret;
if(EnableScatterValue == 0)
{
pConvParams->LaserScatterStreamOffset = NO_INFORMATION;
pConvParams->LaserScatterSizeBit = NO_INFORMATION;
}
else if(EnableScatterValue == 1)
{
pConvParams->LaserScatterStreamOffset = MeasStreamOffset +
pMeasComponent->getSubComponentOffset(ScatterSubCompName);
pConvParams->LaserScatterSizeBit = 8;
}
else
return SICK_CAMERA_NOT_OK;
pConvParams->FixedPoint = RANGER_E55_DEFAULT_HI3D_FIXED_POINT;
pConvParams->RangeSizeBit = 16;
MIL_INT RangeAxisValue;
Ret = GetCameraMilIntParam(pCam, MeasCompPath, RangeAxisName, &RangeAxisValue);
if (Ret != SICK_CAMERA_OK)
return Ret;
if (RangeAxisValue == 0)
pConvParams->FlipY = DO_FLIP_Y_AXIS;
else if (RangeAxisValue == 1)
pConvParams->FlipY = DO_NOT_FLIP_Y_AXIS;
else
return SICK_CAMERA_NOT_OK;
return SICK_CAMERA_OK;
}
MIL_INT FillHi3DCogParams(RangerParams* pConvParams,
MIL_INT MeasStreamOffset,
const Component* pMeasComponent,
RangerE* pCam,
const string MeasCompPath )
{
MIL_INT Ret;
pConvParams->LaserMeasurementMode = M_RANGER_Hi3D_COG_MODE;
pConvParams->RangeStreamOffset = MeasStreamOffset +
pMeasComponent->getSubComponentOffset(XSumSubCompName);
pConvParams->LaserIntensityStreamOffset = MeasStreamOffset +
pMeasComponent->getSubComponentOffset( SumSubCompName);
pConvParams->LaserIntensitySizeBit = 16;
pConvParams->LaserScatterStreamOffset = NO_INFORMATION;
pConvParams->LaserScatterSizeBit = NO_INFORMATION;
if ( pConvParams->RangeStreamOffset == DataFormat::E_BAD_NAME ||
pConvParams->LaserIntensityStreamOffset == DataFormat::E_BAD_NAME )
return SICK_CAMERA_NOT_OK;
pConvParams->FixedPoint = RANGER_E55_DEFAULT_COG_FIXED_POINT;
pConvParams->RangeSizeBit = 16;
MIL_INT RangeAxisValue;
Ret = GetCameraMilIntParam(pCam, MeasCompPath, RangeAxisName, &RangeAxisValue);
if (Ret != SICK_CAMERA_OK)
return Ret;
if (RangeAxisValue == 0)
pConvParams->FlipY = DO_FLIP_Y_AXIS;
else if (RangeAxisValue == 1)
pConvParams->FlipY = DO_NOT_FLIP_Y_AXIS;
else
return SICK_CAMERA_NOT_OK;
return SICK_CAMERA_OK;
}
#endif