GenericUDPLoggingClient::GenericUDPLoggingClient(ushort port, const char* multicastIP, PacketLogger* logger, const char* name, int subnet){
udp_params params = udp_params();
if(strlen(multicastIP)>0)
params.remote_ip = inet_addr(multicastIP);
params.local_port = port;
params.reuse_addr = true;
if(subnet>=0){
unsigned int addr;
if(find_subnet_adapter((unsigned int)subnet,addr))
params.local_ip = addr;
}
this->logger = logger;
this->name = name;
last_stats_print = getTime();
packetCnt = 0;
byteCnt=0;
conn = new udp_connection (params);
conn->set_callback (&UDPCallback, this);
printf("Generic UDP Listener Initialized. %s:%d\r\n",multicastIP,port);
}
SideRoadEdgeReceiver::SideRoadEdgeReceiver()
{
udp_params params = udp_params();
params.remote_ip = inet_addr(UDP_SIDEROADEDGE_ADDR);
params.local_port = UDP_SIDEROADEDGE_PORT;
params.reuse_addr = 1;
conn = new udp_connection (params);
conn->set_callback (MakeDelegate(this,&SideRoadEdgeReceiver::UDPCallback),conn);
printf("SideRoadEdgeMsg RX Interface Initialized. %s:%d\r\n",UDP_SIDEROADEDGE_ADDR,UDP_SIDEROADEDGE_PORT);
}
RoadFitterInterfaceSender::RoadFitterInterfaceSender()
{
udp_params params = udp_params();
params.remote_ip = inet_addr(UDP_RF_MULTICAST_ADDR);
params.local_port = 0;
params.multicast_loopback = true;
params.multicast_ttl=10;
conn = new udp_connection (params);
sequenceNumber=0;
printf("RF TX Interface Initialized. %s:%d\r\n",UDP_RF_MULTICAST_ADDR,UDP_RF_PORT);
}
SideRoadEdgeSender::SideRoadEdgeSender()
{
udp_params params = udp_params();
params.remote_ip = inet_addr(UDP_SIDEROADEDGE_ADDR);
params.local_port = 0;
params.multicast_loopback = true;
params.multicast_ttl=10;
conn = new udp_connection (params);
sequenceNumber=0;
printf("SideRoadEdgeMsg TX Interface Initialized. %s:%d\r\n",UDP_SIDEROADEDGE_ADDR,UDP_SIDEROADEDGE_PORT);
}
ActInterfaceReceiver::ActInterfaceReceiver()
{
packetNum=0;
udp_params params = udp_params();
params.remote_ip = inet_addr(UDP_ACTUATION_ADDR);
params.local_port = UDP_ACTUATION_PORT;
params.reuse_addr = 1;
conn = new udp_connection (params);
conn->set_callback (MakeDelegate(this,&ActInterfaceReceiver::UDPCallback),conn);
printf("Actuation RX Interface Initialized. %s:%d\r\n",UDP_ACTUATION_ADDR,UDP_ACTUATION_PORT);
}
CamSyncReceiver::CamSyncReceiver()
{
udp_params params = udp_params();
params.remote_ip = inet_addr(CAMSYNC_BROADCAST_IP);
params.local_port = CAMSYNC_BROADCAST_PORT;
params.reuse_addr = 1;
conn = new udp_connection (params);
conn->set_callback (MakeDelegate(this,&CamSyncReceiver::UDPCallback),conn);
sequenceNumber=0;
packetCount=0;
dropCount=0;
printf("Camera Sync RX Interface Initialized. %s:%d\r\n",CAMSYNC_BROADCAST_IP,CAMSYNC_BROADCAST_PORT);
}
RoadFitterInterfaceReceiver::RoadFitterInterfaceReceiver()
{
udp_params params = udp_params();
params.remote_ip = inet_addr(UDP_RF_MULTICAST_ADDR);
params.local_port = UDP_RF_PORT;
params.reuse_addr = 1;
conn = new udp_connection (params);
conn->set_callback (MakeDelegate(this,&RoadFitterInterfaceReceiver::UDPCallback),conn);
sequenceNumber=0;
packetCount=0;
dropCount=0;
printf("Road Fitter RX Interface Initialized. %s:%d\r\n",UDP_RF_MULTICAST_ADDR,UDP_RF_PORT);
}
SceneEstimatorInterfaceSender::SceneEstimatorInterfaceSender()
{
udp_params params = udp_params();
params.remote_ip = inet_addr(UDP_SCENE_EST_MULTICAST_ADDR);
params.local_port = 0;
params.multicast_loopback = true;
params.multicast_ttl=10;
conn = new udp_connection (params);
sequenceNumber=0;
sparseMsgTracked = malloc(MAX_PACKET_SIZE*10);
//TrackedClusterMsgSeqCount=0;
//LocalRoadModelMsgSeqCount=0;
printf("Scene Estimator TX Interface Initialized. %s:%d\r\n",UDP_SCENE_EST_MULTICAST_ADDR,UDP_SCENE_EST_PORT);
}
bool Sync1394Camera::InitCamera(int cameraID, SyncCamParams m_config, float x_offset, float y_offset, float yaw_offset, float fudge)
{
C1394Camera* camptr;
isRunning = false;
Sync1394Camera::config = m_config;
int effWidth = config.partialWidth;
int effHeight = config.partialHeight;
camptr = &camera;
if (config.isColor)
size = effWidth * effHeight * 3;
else
size = effWidth * effHeight;
if (config.BitDepth16) size *= 2;
buf = new unsigned char[size];
if(camptr->RefreshCameraList() == 0 )
{
printf("RefreshCameraList failed.\n");
return false;
}
if (cameraID == 0)
{
start_tick = clock();
}
//artagLoc->initARtagPose(640, 480, 200.0, x_offset, y_offset, yaw_offset, fudge);
artagLoc->initARtagPose(640, 480, 180.0, x_offset, y_offset, yaw_offset, fudge);
//artagLoc->initARtagPose(640, 480, 160.0, x_offset, y_offset, yaw_offset, fudge);
if(camptr->SelectCamera(cameraID)!=CAM_SUCCESS)
{
printf("Could not select camera\n" );
return false;
}
camId = cameraID;
if(camptr->InitCamera(true)!=CAM_SUCCESS)
{
printf("Could not init camera\n" );
return false;
}
if(camptr->SetVideoFormat(config.videoFormat)!=CAM_SUCCESS)
{
printf("Could not SetVideoFormat on camera\n" );
}
if(camptr->SetVideoMode(config.videoMode)!=CAM_SUCCESS)
{
printf("Could not SetVideoMode on camera\n");
}
if (config.usePartialScan == false)
{
if(camptr->SetVideoFrameRate(config.videoFrameRate)!=CAM_SUCCESS)
{
printf("Could not set video frame rate!");
return false;
} //30 fps
}
else if (config.usePartialScan)
{
unsigned short w, h;
C1394CameraControlSize* p_size_control = camptr->GetCameraControlSize();
p_size_control->GetSizeLimits(&w, &h);
if ((w>effWidth) || (h>effHeight))
{
printf("FATAL: Bad Partial Scan Size Specified! Specified w:%d h:%d, Max: w:%d h:%d",effWidth,effHeight,w,h);
return false;
}
if (config.isColor)
{
if( p_size_control->SetColorCode(COLOR_CODE_YUV422) != CAM_SUCCESS )
{
printf("SetColorCode failed.\n");
return false;
}
}
else
{
if( p_size_control->SetColorCode(config.BitDepth16?COLOR_CODE_Y16:COLOR_CODE_Y8) != CAM_SUCCESS )
{
printf("SetColorCode failed (BW).\n");
return false;
}
}
if( p_size_control->SetSize(effWidth,effHeight) != CAM_SUCCESS )
{
printf("SetSize failed.\n");
return false;
}
if( p_size_control->SetPos(config.partialLeft,config.partialTop) != CAM_SUCCESS )
{
printf("SetPos failed.\n");
return false;
}
if (p_size_control->SetBytesPerPacket (config.bytesPerPacket) != CAM_SUCCESS)
{
printf("SetBytesPerPacket failed.\n");
return false;
}
float interval=0.0; p_size_control->GetFrameInterval (&interval);
if (((float)config.syncFPS) > (1.0f/interval))
{
printf("WARNING: SyncFPS (%f) is greater than the actual camera FPS (%f)\n",(float)config.syncFPS,(1.0f/interval));
}
printf("Frame Interval is: %f",interval);
}
if (config.isSlave)
{
printf("Using SLAVE configuration.\n");
}
if (!config.AutoGain)
{
C1394CameraControl* ctrl = camptr->GetCameraControl (FEATURE_GAIN);
if (ctrl->SetAutoMode(false) != CAM_SUCCESS)
{
printf("Gain SetManualMode failed.\n");
}
ctrl->SetValue(GAINVAL);
if (ctrl->StatusAutoMode() == true)
{
printf("I HAVE AUTOGAIN ASSHOLE");
}
unsigned short gainval = 0;
ctrl->GetValue(&gainval);
if (DEBUG_SYNC1394) printf("gain: %d\n",gainval);
}
else
{
C1394CameraControl* ctrl = camptr->GetCameraControl (FEATURE_GAIN);
ctrl->SetValue(GAINVAL);
if (ctrl->SetAutoMode(true) != CAM_SUCCESS)
{
printf("Gain SetAutoMode failed.\n");
}
if (ctrl->StatusAutoMode() == true)
{
if (DEBUG_SYNC1394) printf("AutoGain successfully set!\n");
}
}
minGain=0;
maxGain=0;
GetGainMinMax( camptr, &minGain, &maxGain);
if (DEBUG_SYNC1394) printf("Min gain: %d, Max gain: %d\n",minGain,maxGain);
if (!config.AutoShutter)
{
C1394CameraControl* ctrl = camptr->GetCameraControl (FEATURE_SHUTTER);
if (ctrl->SetAutoMode(false) != CAM_SUCCESS)
{
printf("Shutter SetManualMode failed.\n");
}
ctrl->SetValue(SHUTTERVAL);
/*if (ctrl->SetValue(530) != CAM_SUCCESS)
{
printf("Shutter SetValue failed.\n");
}*/
if (ctrl->StatusAutoMode() == true)
{
printf("I HAVE AUTOSHUTTER ASSHOLE");
}
}
else
{
C1394CameraControl* ctrl = camptr->GetCameraControl (FEATURE_SHUTTER);
ctrl->SetValue(SHUTTERVAL);
if (ctrl->SetAutoMode(true) != CAM_SUCCESS)
{
printf("Shutter SetAutoMode failed.\n");
}
if (ctrl->StatusAutoMode() == true)
{
if (DEBUG_SYNC1394) printf("AutoShutter successfully set!\n");
}
}
if (!config.AutoBrightness)
{
C1394CameraControl* ctrl = camptr->GetCameraControl (FEATURE_BRIGHTNESS);
if (ctrl->SetAutoMode(false) != CAM_SUCCESS)
{
printf("Brightness SetManualMode failed.\n");
}
ctrl->SetValue(154);
if (ctrl->StatusAutoMode() == true)
{
printf("I HAVE AUTOBRIGHTNESS ASSHOLE");
}
unsigned short bval = 0;
ctrl->GetValue(&bval);
if (DEBUG_SYNC1394) printf("brightness: %d\n",bval);
}
else
{
C1394CameraControl* ctrl = camptr->GetCameraControl (FEATURE_BRIGHTNESS);
ctrl->SetValue(154);
if (ctrl->SetAutoMode(true) != CAM_SUCCESS)
{
printf("Brightness SetAutoMode failed.\n");
}
if (ctrl->StatusAutoMode() == true)
{
if (DEBUG_SYNC1394) printf("AutoBrightness successfully set!\n");
}
}
if (config.gammaEnable)
{
C1394CameraControl* ctrl = camptr->GetCameraControl (FEATURE_GAMMA);
if (ctrl->SetOnOff(true) != CAM_SUCCESS)
{
printf("Gamma SetOnOff failed.\n");
}
if (ctrl->SetValue(1) != CAM_SUCCESS)
{
printf("Gamma SetValue failed.\n");
}
}
minShutter=0;
maxShutter=0;
GetShutterMinMax(camptr, &minShutter, &maxShutter);
if (DEBUG_SYNC1394) printf("Min shutter: %d, Max shutter: %d\n",minShutter,maxShutter);
if (config.adjWB)
{
C1394CameraControl* ctrl = camptr->GetCameraControl (FEATURE_WHITE_BALANCE);
if (ctrl->SetValue(WHITEBALANCE_VALUE1,WHITEBALANCE_VALUE2) != CAM_SUCCESS)
{
printf("WhiteBalance SetValue failed.\n");
}
}
printf("Completed Init of Camera %d\n", camId);
if (config.eTrigEnabled)
{
C1394CameraControlTrigger* trig = camptr->GetCameraControlTrigger();
if (trig->SetTriggerSource (config.syncCamInput)!=CAM_SUCCESS)
printf("Could Not Set Trigger Source!\n");
else
{
if (trig->SetMode (0))
printf("Could Not Set Trigger Mode!\n");
else
{
if (trig->SetOnOff (true))
printf("Could Not Set Trigger ON!\n");
else
{
trig->Status();
printf("External Trigger Initialized.\n");
}
}
}
}
else
{
if (config.syncEnabled)
{
udp_params paramsRX = udp_params();
paramsRX.remote_ip = inet_addr(UDP_BROADCAST_IP);
paramsRX.local_port = UDP_BROADCAST_PORT;
paramsRX.reuse_addr = 1;
try
{
udpRX = new udp_connection(paramsRX);
}
catch (exception)
{
printf("Couldn't init UDP RX on %s:%d \n",UDP_BROADCAST_IP,paramsRX.local_port);
return false;
}
udpRX->set_callback(MakeDelegate(this,&Sync1394Camera::UDPCallback), udpRX);
udp_params paramsTX = udp_params();
try
{
udpTX = new udp_connection(paramsTX);
}
catch (exception)
{
printf("Couldn't init UDP TX on port %d\n",paramsTX.local_port);
return false;
}
if (config.isSlave == false)
{
char regmsg[] = {CAMERA_MSG_REGISTER, config.syncID & 0xFF, 0x00, 0x00, 0x00, 0x00,(paramsRX.local_port>>8)&0xff, paramsRX.local_port&0xff}; //register message
udpTX->send_message (regmsg,8,UDP_CONTROL_IP,UDP_CONTROL_PORT);
Sleep(100);
char fpsmsg[] = {CAMERA_MSG_SETFPS, config.syncID & 0xff, config.syncFPS &0xff};
udpTX->send_message (fpsmsg,3,UDP_CONTROL_IP,UDP_CONTROL_PORT);
Sleep(100);
char initmsg[] = {CAMERA_START, config.syncID & 0xFF}; //start message
udpTX->send_message (initmsg,2,UDP_CONTROL_IP,UDP_CONTROL_PORT);
if (config.ghettoSync == false)
{
C1394CameraControlTrigger* trig = camptr->GetCameraControlTrigger ();
unsigned short modein=0; unsigned short modeparam =0; unsigned short trigsrc=0;
if (trig->SetTriggerSource (config.syncCamInput)!=CAM_SUCCESS)
printf("Could Not Set Trigger Source!\n");
if (trig->SetMode (0))
printf("Could Not Set Trigger Mode!\n");
if (trig->SetOnOff (true))
printf("Could Not Set Trigger ON!\n");
}
}
if (DEBUG_SYNC1394) printf("Timing Initialized.\n");
}
