/** This function defines the connection routes to talk to Cortex.
*
* Machines can have more than one ethernet interface. This function
* is used to either set the ethernet interface to use, or to let
* the SDK auto-select the local interface, and/or the Cortex host.
* This function should only be called once at startup.
*
* \param szMyNicCardAddress - "a.b.c.d" or HostName. "" and NULL mean AutoSelect
*
* \param szCortexNicCardAddress - "a.b.c.d" or HostName. "" and NULL mean AutoSelect
*
* \return maReturnCode - RC_Okay, RC_ApiError, RC_NetworkError, RC_GeneralError
*/
int Cortex_Initialize(const char* szMyNicCardAddress,
const char* szCortexNicCardAddress)
{
int retval;
in_addr MyAddresses[10];
int nAddresses;
// Initialization only happens once.
if (bInitialized) {
LogMessage(VL_Warning, "Already Initialized");
return RC_GeneralError;
}
nAddresses = Cortex_GetAllOfMyAddresses((unsigned long *) MyAddresses, 10);
if (nAddresses < 0) {
LogMessage(VL_Error, "Unable to find my own machine");
return RC_NetworkError;
} else if (nAddresses == 0) {
LogMessage(VL_Error, "This machine has no ethernet interfaces");
return RC_NetworkError;
}
if (szMyNicCardAddress == NULL || szMyNicCardAddress[0] == 0) {
if (nAddresses > 1) {
LogMessage(
VL_Warning,
"The local machine has more than one ethernet interface. Using the first one found.");
}
MyNicCardAddress = MyAddresses[0];
LogMessage(VL_Info, "Initializing using my default ethernet address: %s",
inet_ntoa(MyNicCardAddress));
} else {
retval = ConvertToIPAddress(szMyNicCardAddress, &MyNicCardAddress);
if (retval != OK) {
LogMessage(VL_Error, "Unable to find MyNicCardAddress \"%s\"",
szMyNicCardAddress);
Cortex_Exit();
return RC_NetworkError;
}
LogMessage(VL_Info, "Initializing using my address: %s",
inet_ntoa(MyNicCardAddress));
}
if (ConvertToIPAddress(szCortexNicCardAddress, &CortexNicCardAddress) != OK) {
LogMessage(VL_Error, "Unable to convert \"%s\" to IP Address for Cortex",
szCortexNicCardAddress);
return RC_NetworkError;
}
memset(&CortexAddr, 0, sizeof(CortexAddr));
CortexAddr.sin_family = AF_INET; // host byte order
CortexAddr.sin_port = htons(wCortexPort); // short, network byte order
CortexAddr.sin_addr = CortexNicCardAddress;
LogMessage(VL_Info, "Initializing using Cortex host address: %s",
inet_ntoa(CortexNicCardAddress));
memset(&HostInfo, 0, sizeof(sHostInfo));
memcpy(HostInfo.HostMachineAddress, &CortexNicCardAddress, 4);
memset(&Polled_FrameOfData, 0, sizeof(sFrameOfData));
memset(&LatestFrameOfData, 0, sizeof(sFrameOfData));
// Get the semaphore technique initialized (CL)
if (sem_init(&EH_CommandConfirmed, 0, 0) != 0) {
LogMessage(VL_Error, "Unable to initialize semaphore: %d", errno);
}
Initialize_ListenForReplies();
usleep(10000);
Initialize_ListenForFramesOfData();
usleep(10000); // Give the ListenThread time to be ready for an answer to the startup packet
// Let the world know we are here.
PacketOut.iCommand = PKT2_HELLO_WORLD;
PacketOut.nBytes = sizeof(sMe);
strcpy(PacketOut.Data.Me.szName, "ClientTest");
memcpy(PacketOut.Data.Me.Version, MyVersionNumber, 4);
//Broadcast(&PacketOut);
//Broadcast(CommandSocket, wCortexPort, (char *)&PacketOut, PacketOut.nBytes + 4);
// At this point, if szCortexNicCardAddress was not specified,
// then CortexAddr should be the broadcast address.
// The ListenForReplies thread will hear the response and get the actual address
SendToCortex(&PacketOut);
usleep(100000); // sleep until threads are running
bInitialized = 1;
return RC_Okay;
}
// main driver
int main(int argc, char* argv[])
{
// initialize ros / variables
ros::init(argc, argv, "cortex_bridge");
ros::NodeHandle nh;
ros::ServiceServer service;
glob = ros::Time::now();
std::string myIP, cortexIP;
// Get IPs from param file
ros::NodeHandle param_nh("~");
if ( !param_nh.getParam("local_ip", myIP) )
ROS_ERROR ("Could not find IP of this machine in launch file.");
if ( !param_nh.getParam("cortex_ip", cortexIP) )
ROS_ERROR ("Could not find IP of cortex machine in launch file.");
// publisher for markers
#ifdef PUBLISH_VISUALIZATION_MARKERS
Cortex_markers = nh.advertise<visualization_msgs::MarkerArray>("vis_markers", 1000);
#else
Cortex_markers = nh.advertise<cortex_bridge::Markers>("novis_markers", 1000);
#endif
// broadcaster for transforms
Cortex_broadcaster = new tf::TransformBroadcaster();
// service to set origin for body
service = nh.advertiseService("cortexSetOrigin", setOriginCallback);
// set loop rate
ros::Rate loop_rate(150);
// Initialize cortex connection
if ( InitializeCortexConnection ( myIP.c_str(), cortexIP.c_str() ) != 0 )
{
ROS_INFO ( "Error: Unable to initialize ethernet communication" );
return -1;
}
// get all the possible bodies to track
NumBodiesOfInterest = GetKnownBodies ( bodyOfInterest );
if ( NumBodiesOfInterest < 0 )
{
// clean up cortex connection
Cortex_Exit();
ROS_ERROR ("Are you sure you added body defs in Cortex?");
return -1;
}
// init body offsets
bodyOriginOffset = new float*[NumBodiesOfInterest];
for ( int i = 0; i < NumBodiesOfInterest; ++i )
{
bodyOriginOffset[i] = new float[7];
for ( int j = 0; j < 7; ++j )
bodyOriginOffset[i][j] = 0.0;
}
// get cortex frame rate information
GetCortexFrameRate();
// set callbacks and handlers
InitializeCortexHandlers();
Cortex_SetDataHandlerFunc(newFrameCallback);
// call callbacks while ros::ok == true
ros::spin();
// clean up cortex connection
ROS_INFO ("Cortex_Exit");
Cortex_Exit();
// clean up memory
for ( int i = 0; i < NumBodiesOfInterest; ++i )
{
delete bodyOfInterest[i];
delete bodyOriginOffset[i];
}
delete bodyOfInterest;
delete bodyOriginOffset;
return 0;
}
