int main( int argc, char* argv[] )
{
// Program options
std::string HostName = "192.168.10.1:801";
int startup_delay = 10;
ros::init(argc, argv, "udp_enable");
ros::NodeHandle n;
ros::Rate loop_rate(1);
// log contains:
// version number
// log of framerate over time
// --multicast
// kill off internal app
std::string LogFile = "";
std::string MulticastAddress = "224.0.0.0:44801";
bool ConnectToMultiCast = false;
bool EnableMultiCast = true;
bool EnableHapticTest = false;
bool bReadCentroids = false;
std::vector<std::string> HapticOnList(0);
std::ofstream ofs;
// ROS parameters
std::string s;
// mutlicast address
if (n.getParam("vicon_multicast_address", s)) {
MulticastAddress = s;
//std::cout << "Got multicast " << MulticastAddress << " from parameter." << std::endl;
}
// own hostname
if (n.getParam("vicon_server_hostname", s)) {
HostName = s;
//std::cout << "Got hostname " << HostName << " from parameter." << std::endl;
}
// tracking object (this one is private as unique to each node)
// try the private thing using the "bare" method
if (ros::param::has("~startup_delay")) {
ros::param::get("~startup_delay", startup_delay);
}
ROS_INFO("Waiting for start-up delay of %d s", startup_delay);
sleep(startup_delay);
// Make a new client
Client MyClient;
// Connect to a server
ROS_INFO("Connecting to server %s", HostName.c_str());
while( !MyClient.IsConnected().Connected )
{
// Direct connection
bool ok = false;
if(ConnectToMultiCast)
{
// Multicast connection
ok = ( MyClient.ConnectToMulticast( HostName, MulticastAddress ).Result == Result::Success );
}
else
{
ok =( MyClient.Connect( HostName ).Result == Result::Success );
}
if(!ok)
{
std::cout << "Warning - connect failed..." << std::endl;
}
std::cout << ".";
sleep(1);
}
std::cout << std::endl;
// Enable some different data types
MyClient.EnableSegmentData();
MyClient.EnableMarkerData();
MyClient.EnableUnlabeledMarkerData();
MyClient.EnableDeviceData();
// Set the streaming mode
// MyClient.SetStreamMode( ViconDataStreamSDK::CPP::StreamMode::ClientPull );
// MyClient.SetStreamMode( ViconDataStreamSDK::CPP::StreamMode::ClientPullPreFetch );
MyClient.SetStreamMode( ViconDataStreamSDK::CPP::StreamMode::ServerPush );
// Set the global up axis
MyClient.SetAxisMapping( Direction::Forward,
Direction::Left,
Direction::Up ); // Z-up
// MyClient.SetGlobalUpAxis( Direction::Forward,
// Direction::Up,
// Direction::Right ); // Y-up
Output_GetAxisMapping _Output_GetAxisMapping = MyClient.GetAxisMapping();
std::cout << "Axis Mapping: X-" << Adapt( _Output_GetAxisMapping.XAxis )
<< " Y-" << Adapt( _Output_GetAxisMapping.YAxis )
<< " Z-" << Adapt( _Output_GetAxisMapping.ZAxis ) << std::endl;
// Discover the version number
Output_GetVersion _Output_GetVersion = MyClient.GetVersion();
std::cout << "Version: " << _Output_GetVersion.Major << "."
<< _Output_GetVersion.Minor << "."
<< _Output_GetVersion.Point << std::endl;
if( EnableMultiCast )
{
assert( MyClient.IsConnected().Connected );
MyClient.StopTransmittingMulticast();
sleep(1);
MyClient.StartTransmittingMulticast( HostName, MulticastAddress );
}
ROS_INFO("Starting multicast on address %s", MulticastAddress.c_str());
// wait for a key to be pressed
std::cout << "Multicast has been started" << std::endl;
std::cout << "Press Ctrl+C to stop" << std::endl;
while( ros::ok() )
{
// get ROS stuff done first
ros::spinOnce();
// a pause
loop_rate.sleep();
}
// shutting down again
if( EnableMultiCast )
{
MyClient.StopTransmittingMulticast();
}
MyClient.DisableSegmentData();
MyClient.DisableMarkerData();
MyClient.DisableUnlabeledMarkerData();
MyClient.DisableDeviceData();
if( bReadCentroids )
{
MyClient.DisableCentroidData();
}
// Disconnect and dispose
int t = clock();
std::cout << " Disconnecting..." << std::endl;
MyClient.Disconnect();
int dt = clock() - t;
double secs = (double) (dt)/(double)CLOCKS_PER_SEC;
std::cout << " Disconnect time = " << secs << " secs" << std::endl;
}
int main( int argc, char* argv[] )
{
//lcm
lcm::LCM lcm;
if(!lcm.good())
return 1;
vicon_state_t vicon_msg;
// Program options
std::string HostName = "192.168.0.102:801";
if( argc > 1 )
{
HostName = argv[1];
}
// log contains:
// version number
// log of framerate over time
// --multicast
// kill off internal app
std::string LogFile = "";
std::string MulticastAddress = "244.0.0.0:44801";
bool ConnectToMultiCast = false;
bool EnableMultiCast = false;
bool EnableHapticTest = false;
bool bReadCentroids = false;
std::vector<std::string> HapticOnList(0);
for(int a=2; a < argc; ++a)
{
std::string arg = argv[a];
if(arg == "--help")
{
std::cout << argv[0] << " <HostName>: allowed options include:\n --log_file <LogFile> --enable_multicast <MulticastAddress:Port> --connect_to_multicast <MulticastAddress:Port> --help --enable_haptic_test <DeviceName> --centroids" << std::endl;
return 0;
}
else if (arg=="--log_file")
{
if(a < argc)
{
LogFile = argv[a+1];
std::cout << "Using log file <"<< LogFile << "> ..." << std::endl;
++a;
}
}
else if (arg=="--enable_multicast")
{
EnableMultiCast = true;
if(a < argc)
{
MulticastAddress = argv[a+1];
std::cout << "Enabling multicast address <"<< MulticastAddress << "> ..." << std::endl;
++a;
}
}
else if (arg=="--connect_to_multicast")
{
ConnectToMultiCast = true;
if(a < argc)
{
MulticastAddress = argv[a+1];
std::cout << "connecting to multicast address <"<< MulticastAddress << "> ..." << std::endl;
++a;
}
}
else if (arg=="--enable_haptic_test")
{
EnableHapticTest = true;
++a;
if ( a < argc )
{
//assuming no haptic device name starts with "--"
while( a < argc && strncmp( argv[a], "--", 2 ) !=0 )
{
HapticOnList.push_back( argv[a] );
++a;
}
}
}
else if( arg=="--centroids" )
{
bReadCentroids = true;
}
else
{
std::cout << "Failed to understand argument <" << argv[a] << ">...exiting" << std::endl;
return 1;
}
}
std::ofstream ofs;
if(!LogFile.empty())
{
ofs.open(LogFile.c_str());
if(!ofs.is_open())
{
std::cout << "Could not open log file <" << LogFile << ">...exiting" << std::endl;
return 1;
}
}
// Make a new client
Client MyClient;
for(int i=0; i != 3; ++i) // repeat to check disconnecting doesn't wreck next connect
{
// Connect to a server
std::cout << "Connecting to " << HostName << " ..." << std::flush;
while( !MyClient.IsConnected().Connected )
{
// Direct connection
bool ok = false;
if(ConnectToMultiCast)
{
// Multicast connection
ok = ( MyClient.ConnectToMulticast( HostName, MulticastAddress ).Result == Result::Success );
}
else
{
ok =( MyClient.Connect( HostName ).Result == Result::Success );
}
if(!ok)
{
std::cout << "Warning - connect failed..." << std::endl;
}
std::cout << ".";
#ifdef WIN32
Sleep( 1000 );
#else
sleep(1);
#endif
}
// std::cout << std::endl;
// Enable some different data types
MyClient.EnableSegmentData();
MyClient.EnableMarkerData();
MyClient.EnableUnlabeledMarkerData();
MyClient.EnableDeviceData();
if( bReadCentroids )
{
MyClient.EnableCentroidData();
}
std::cout << "Segment Data Enabled: " << Adapt( MyClient.IsSegmentDataEnabled().Enabled ) << std::endl;
std::cout << "Marker Data Enabled: " << Adapt( MyClient.IsMarkerDataEnabled().Enabled ) << std::endl;
std::cout << "Unlabeled Marker Data Enabled: " << Adapt( MyClient.IsUnlabeledMarkerDataEnabled().Enabled ) << std::endl;
std::cout << "Device Data Enabled: " << Adapt( MyClient.IsDeviceDataEnabled().Enabled ) << std::endl;
std::cout << "Centroid Data Enabled: " << Adapt( MyClient.IsCentroidDataEnabled().Enabled ) << std::endl;
// Set the streaming mode
//MyClient.SetStreamMode( ViconDataStreamSDK::CPP::StreamMode::ClientPull );
// MyClient.SetStreamMode( ViconDataStreamSDK::CPP::StreamMode::ClientPullPreFetch );
MyClient.SetStreamMode( ViconDataStreamSDK::CPP::StreamMode::ServerPush );
// Set the global up axis
MyClient.SetAxisMapping( Direction::Forward,
Direction::Left,
Direction::Up ); // Z-up
// MyClient.SetGlobalUpAxis( Direction::Forward,
// Direction::Up,
// Direction::Right ); // Y-up
Output_GetAxisMapping _Output_GetAxisMapping = MyClient.GetAxisMapping();
std::cout << "Axis Mapping: X-" << Adapt( _Output_GetAxisMapping.XAxis )
<< " Y-" << Adapt( _Output_GetAxisMapping.YAxis )
<< " Z-" << Adapt( _Output_GetAxisMapping.ZAxis ) << std::endl;
// Discover the version number
Output_GetVersion _Output_GetVersion = MyClient.GetVersion();
std::cout << "Version: " << _Output_GetVersion.Major << "."
<< _Output_GetVersion.Minor << "."
<< _Output_GetVersion.Point << std::endl;
if( EnableMultiCast )
{
assert( MyClient.IsConnected().Connected );
MyClient.StartTransmittingMulticast( HostName, MulticastAddress );
}
size_t FrameRateWindow = 1000; // frames
size_t Counter = 0;
clock_t LastTime = clock();
// Loop until a key is pressed
#ifdef WIN32
while( !Hit() )
#else
while( true)
#endif
{
// Get a frame
// output_stream << "Waiting for new frame...";
while( MyClient.GetFrame().Result != Result::Success )
{
// Sleep a little so that we don't lumber the CPU with a busy poll
#ifdef WIN32
Sleep( 200 );
#else
sleep(1);
#endif
// output_stream << ".";
}
// output_stream << std::endl;
if(++Counter == FrameRateWindow)
{
clock_t Now = clock();
double FrameRate = (double)(FrameRateWindow * CLOCKS_PER_SEC) / (double)(Now - LastTime);
if(!LogFile.empty())
{
time_t rawtime;
struct tm * timeinfo;
time ( &rawtime );
timeinfo = localtime ( &rawtime );
ofs << "Frame rate = " << FrameRate << " at " << asctime (timeinfo)<< std::endl;
}
LastTime = Now;
Counter = 0;
}
// Get the frame number
Output_GetFrameNumber _Output_GetFrameNumber = MyClient.GetFrameNumber();
// output_stream << "Frame Number: " << _Output_GetFrameNumber.FrameNumber << std::endl;
if( EnableHapticTest == true )
{
for (size_t i = 0; i < HapticOnList.size(); ++ i)
{
if( Counter % 2 == 0 )
{
Output_SetApexDeviceFeedback Output= MyClient.SetApexDeviceFeedback( HapticOnList[i], true );
if( Output.Result == Result::Success )
{
// output_stream<< "Turn haptic feedback on for device: " << HapticOnList[i]<<std::endl;
}
else if( Output.Result == Result::InvalidDeviceName )
{
output_stream<< "Device doesn't exist: "<< HapticOnList[i]<<std::endl;
}
}
if( Counter % 20 == 0 )
{
Output_SetApexDeviceFeedback Output = MyClient.SetApexDeviceFeedback( HapticOnList[i], false);
if( Output.Result == Result::Success )
{
output_stream<< "Turn haptic feedback off for device: " << HapticOnList[i]<<std::endl;
}
}
}
}
// Count the number of subjects
unsigned int SubjectCount = MyClient.GetSubjectCount().SubjectCount;
// output_stream << "Subjects (" << SubjectCount << "):" << std::endl;
for( unsigned int SubjectIndex = 0 ; SubjectIndex < SubjectCount ; ++SubjectIndex )
{
// output_stream << " Subject #" << SubjectIndex << std::endl;
// Get the subject name
std::string SubjectName = MyClient.GetSubjectName( SubjectIndex ).SubjectName;
// output_stream << " Name: " << SubjectName << std::endl;
// Get the root segment
// std::string RootSegment = MyClient.GetSubjectRootSegmentName( SubjectName ).SegmentName;
// output_stream << " Root Segment: " << RootSegment << std::endl;
// Count the number of segments
unsigned int SegmentCount = MyClient.GetSegmentCount( SubjectName ).SegmentCount;
// output_stream << " Segments (" << SegmentCount << "):" << std::endl;
for( unsigned int SegmentIndex = 0 ; SegmentIndex < SegmentCount ; ++SegmentIndex )
{
// output_stream << " Segment #" << SegmentIndex << std::endl;
// Get the segment name
std::string SegmentName = MyClient.GetSegmentName( SubjectName, SegmentIndex ).SegmentName;
//chose the the object by name
if(SegmentName != OBJ_TO_TRACK) continue;
// output_stream << " Name: " << SegmentName << std::endl;
// Get the segment parent
std::string SegmentParentName = MyClient.GetSegmentParentName( SubjectName, SegmentName ).SegmentName;
// output_stream << " Parent: " << SegmentParentName << std::endl;
// Get the segment's children
unsigned int ChildCount = MyClient.GetSegmentChildCount( SubjectName, SegmentName ).SegmentCount;
// output_stream << " Children (" << ChildCount << "):" << std::endl;
for( unsigned int ChildIndex = 0 ; ChildIndex < ChildCount ; ++ChildIndex )
{
std::string ChildName = MyClient.GetSegmentChildName( SubjectName, SegmentName, ChildIndex ).SegmentName;
// output_stream << " " << ChildName << std::endl;
}
// Get the global segment translation
Output_GetSegmentGlobalTranslation _Output_GetSegmentGlobalTranslation =
MyClient.GetSegmentGlobalTranslation( SubjectName, SegmentName );
// output_stream << " Global Translation: (" << _Output_GetSegmentGlobalTranslation.Translation[ 0 ] << ", "
// << _Output_GetSegmentGlobalTranslation.Translation[ 1 ] << ", "
// << _Output_GetSegmentGlobalTranslation.Translation[ 2 ] << ") "
// << Adapt( _Output_GetSegmentGlobalTranslation.Occluded ) << std::endl;
// Get the global segment rotation in helical co-ordinates
// Output_GetSegmentGlobalRotationHelical _Output_GetSegmentGlobalRotationHelical =
// MyClient.GetSegmentGlobalRotationHelical( SubjectName, SegmentName );
// output_stream << " Global Rotation Helical: (" << _Output_GetSegmentGlobalRotationHelical.Rotation[ 0 ] << ", "
// << _Output_GetSegmentGlobalRotationHelical.Rotation[ 1 ] << ", "
// << _Output_GetSegmentGlobalRotationHelical.Rotation[ 2 ] << ") "
// << Adapt( _Output_GetSegmentGlobalRotationHelical.Occluded ) << std::endl;
// Get the global segment rotation as a matrix
Output_GetSegmentGlobalRotationMatrix _Output_GetSegmentGlobalRotationMatrix =
MyClient.GetSegmentGlobalRotationMatrix( SubjectName, SegmentName );
for (int iii = 0; iii < 9; iii ++) {
vicon_msg.DCM[iii] = _Output_GetSegmentGlobalRotationMatrix.Rotation[ iii ];
}
// output_stream << " Global Rotation Matrix: (" << _Output_GetSegmentGlobalRotationMatrix.Rotation[ 0 ] << ", "
// << _Output_GetSegmentGlobalRotationMatrix.Rotation[ 1 ] << ", "
// << _Output_GetSegmentGlobalRotationMatrix.Rotation[ 2 ] << ", "
// << _Output_GetSegmentGlobalRotationMatrix.Rotation[ 3 ] << ", "
// << _Output_GetSegmentGlobalRotationMatrix.Rotation[ 4 ] << ", "
// << _Output_GetSegmentGlobalRotationMatrix.Rotation[ 5 ] << ", "
// << _Output_GetSegmentGlobalRotationMatrix.Rotation[ 6 ] << ", "
// << _Output_GetSegmentGlobalRotationMatrix.Rotation[ 7 ] << ", "
// << _Output_GetSegmentGlobalRotationMatrix.Rotation[ 8 ] << ") "
// << Adapt( _Output_GetSegmentGlobalRotationMatrix.Occluded ) << std::endl;
// Get the global segment rotation in quaternion co-ordinates
Output_GetSegmentGlobalRotationQuaternion _Output_GetSegmentGlobalRotationQuaternion =
MyClient.GetSegmentGlobalRotationQuaternion( SubjectName, SegmentName );
// output_stream << " Global Rotation Quaternion: (" << _Output_GetSegmentGlobalRotationQuaternion.Rotation[ 0 ] << ", "
// << _Output_GetSegmentGlobalRotationQuaternion.Rotation[ 1 ] << ", "
// << _Output_GetSegmentGlobalRotationQuaternion.Rotation[ 2 ] << ", "
// << _Output_GetSegmentGlobalRotationQuaternion.Rotation[ 3 ] << ") "
// << Adapt( _Output_GetSegmentGlobalRotationQuaternion.Occluded ) << std::endl;
// Get the global segment rotation in EulerXYZ co-ordinates
Output_GetSegmentGlobalRotationEulerXYZ _Output_GetSegmentGlobalRotationEulerXYZ =
MyClient.GetSegmentGlobalRotationEulerXYZ( SubjectName, SegmentName );
// std::cout<<"global: "<<_Output_GetSegmentGlobalRotationEulerXYZ.Rotation[ 0 ]<<", "<<_Output_GetSegmentGlobalRotationEulerXYZ.Rotation[ 1 ]<<", "<<_Output_GetSegmentGlobalRotationEulerXYZ.Rotation[ 2 ]<<std::endl;
// output_stream << " Global Rotation EulerXYZ: (" << _Output_GetSegmentGlobalRotationEulerXYZ.Rotation[ 0 ] << ", "
// << _Output_GetSegmentGlobalRotationEulerXYZ.Rotation[ 1 ] << ", "
// << _Output_GetSegmentGlobalRotationEulerXYZ.Rotation[ 2 ] << ") "
// << Adapt( _Output_GetSegmentGlobalRotationEulerXYZ.Occluded ) << std::endl;
number = distribution(generator);
_Output_GetSegmentGlobalTranslation.Translation[ 0 ]+= 1000.0*number;
_Output_GetSegmentGlobalTranslation.Translation[ 1 ]+= 1000.0*number;
_Output_GetSegmentGlobalTranslation.Translation[ 2 ]+= 1000.0*number;
//lcm publish data
vicon_msg.timestamp = utime_now();
///////////////////filter position
// std::cout <<abs(_Output_GetSegmentGlobalTranslation.Translation[ 2 ] - pre_z)<<std::endl;
// if(abs(_Output_GetSegmentGlobalTranslation.Translation[ 0 ] - pre_x) <= POS_JUMP_THRES)
addArr(pos_x, _Output_GetSegmentGlobalTranslation.Translation[ 0 ], NUM_SAMPLES_MED);
// if(abs(_Output_GetSegmentGlobalTranslation.Translation[ 1 ] - pre_y) <= POS_JUMP_THRES)
addArr(pos_y, _Output_GetSegmentGlobalTranslation.Translation[ 1 ], NUM_SAMPLES_MED);
// if(abs(_Output_GetSegmentGlobalTranslation.Translation[ 2 ] - pre_z) <= POS_JUMP_THRES)
addArr(pos_z, _Output_GetSegmentGlobalTranslation.Translation[ 2 ], NUM_SAMPLES_MED);
addArr(pos_x_med, median(pos_x, NUM_SAMPLES_MED), NUM_SAMPLES_AVG);
addArr(pos_y_med, median(pos_y, NUM_SAMPLES_MED), NUM_SAMPLES_AVG);
addArr(pos_z_med, median(pos_z, NUM_SAMPLES_MED), NUM_SAMPLES_AVG);
addArr(pos_x_avg, average(pos_x_med, NUM_SAMPLES_AVG), NUM_SAMPLES_AVG);
addArr(pos_y_avg, average(pos_y_med, NUM_SAMPLES_AVG), NUM_SAMPLES_AVG);
addArr(pos_z_avg, average(pos_z_med, NUM_SAMPLES_AVG), NUM_SAMPLES_AVG);
vicon_msg.position[0] = pos_x_avg[NUM_SAMPLES_AVG - 1];
vicon_msg.position[1] = pos_y_avg[NUM_SAMPLES_AVG - 1];
vicon_msg.position[2] = pos_z_avg[NUM_SAMPLES_AVG - 1];
/////////////////filter attitude
///
///it seems that attitude is refreshed with half of the frequency as position
///
///
addArr(att_x, _Output_GetSegmentGlobalRotationEulerXYZ.Rotation[ 0 ], NUM_SAMPLES_MED);
addArr(att_y, _Output_GetSegmentGlobalRotationEulerXYZ.Rotation[ 1 ], NUM_SAMPLES_MED);
addArr(att_z, _Output_GetSegmentGlobalRotationEulerXYZ.Rotation[ 2 ], NUM_SAMPLES_MED);
addArr(att_x_med, median(att_x, NUM_SAMPLES_MED), NUM_SAMPLES_AVG);
addArr(att_y_med, median(att_y, NUM_SAMPLES_MED), NUM_SAMPLES_AVG);
addArr(att_z_med, median(att_z, NUM_SAMPLES_MED), NUM_SAMPLES_AVG);
addArr(att_x_avg, average(att_x_med, NUM_SAMPLES_AVG), NUM_SAMPLES_AVG);
addArr(att_y_avg, average(att_y_med, NUM_SAMPLES_AVG), NUM_SAMPLES_AVG);
addArr(att_z_avg, average(att_z_med, NUM_SAMPLES_AVG), NUM_SAMPLES_AVG);
vicon_msg.attitude[0] = att_x_avg[NUM_SAMPLES_AVG - 1];
vicon_msg.attitude[1] = att_y_avg[NUM_SAMPLES_AVG - 1];
vicon_msg.attitude[2] = att_z_avg[NUM_SAMPLES_AVG - 1];
// std::cout << "attitude[0]: "<<_Output_GetSegmentGlobalRotationEulerXYZ.Rotation[ 0 ]<<std::endl;
// std::cout << "attitude[1]: "<<_Output_GetSegmentGlobalRotationEulerXYZ.Rotation[ 1 ]<<std::endl;
// std::cout << "attitude[2]: "<<_Output_GetSegmentGlobalRotationEulerXYZ.Rotation[ 2 ]<<std::endl;
dt = (static_cast<double>(vicon_msg.timestamp - time_tmp))/1000; //dt in mseconds
// differentiate the positions to get velocities
dx_in_mm = vicon_msg.position[0] - pre_x;
dy_in_mm = vicon_msg.position[1] - pre_y;
dz_in_mm = vicon_msg.position[2] - pre_z;
// std::cout<<"dz: "<<dz_in_mm<<"\tdt: "<<dt <<std::endl;
vx = dx_in_mm/dt;
vy = dy_in_mm/dt;
vz = dz_in_mm/dt;
//differentiate the attitudes to get Euler rate
v_rho = ((double)vicon_msg.attitude[0] - pre_rho)/(dt/1e3);
v_theta = ((double)vicon_msg.attitude[1] - pre_theta)/(dt/1e3);
v_psi = ((double)vicon_msg.attitude[2] - pre_psi)/(dt/1e3);
///////////////////////filter the velocity
//collect data in to stack
//
//filter out unreasonable jump
// if(abs(vx - prev_vx) <= JUMP_THRESHOLD) {
addArr(vel_x, vx, NUM_SAMPLES_MED);
addArr(vel_x_med, median(vel_x, NUM_SAMPLES_MED), NUM_SAMPLES_AVG);
addArr(vel_x_avg, average(vel_x_med, NUM_SAMPLES_AVG), NUM_SAMPLES_AVG);
vx = vel_x_avg[NUM_SAMPLES_AVG-1];
prev_vx = vx;
// } else vx = prev_vx;
// if(abs(vy - prev_vy) <= JUMP_THRESHOLD) {
addArr(vel_y, vy, NUM_SAMPLES_MED);
addArr(vel_y_med, median(vel_y, NUM_SAMPLES_MED), NUM_SAMPLES_AVG);
addArr(vel_y_avg, average(vel_y_med, NUM_SAMPLES_AVG), NUM_SAMPLES_AVG);
vy = vel_y_avg[NUM_SAMPLES_AVG-1];
prev_vy = vy;
// } else vy = prev_vy;
// if(abs(vz - prev_vz) <= JUMP_THRESHOLD) {
addArr(vel_z, vz, NUM_SAMPLES_MED);
addArr(vel_z_med, median(vel_z, NUM_SAMPLES_MED), NUM_SAMPLES_AVG);
addArr(vel_z_avg, average(vel_z_med, NUM_SAMPLES_AVG), NUM_SAMPLES_AVG);
vz = vel_z_avg[NUM_SAMPLES_AVG-1];
prev_vz = vz;
// } else vz = prev_vz;
//filter the Euler rate
// if(abs(v_rho - v_rho_pre) <= deg2rad(JUMP_THRESHOLD_ATTITUDE)) {
if(abs(v_rho - v_rho_pre) <= 3) addArr(rho_vel, v_rho, NUM_SAMPLES_MED); // Jump between -180 to 180
addArr(rho_vel_med, median(rho_vel, NUM_SAMPLES_MED), NUM_SAMPLES_AVG);
addArr(rho_vel_avg, average(rho_vel_med, NUM_SAMPLES_AVG), NUM_SAMPLES_AVG);
v_rho = rho_vel_avg[NUM_SAMPLES_AVG - 1];
v_rho_pre = v_rho;
// } else v_rho = v_rho_pre;
// if(abs(v_theta - v_theta_pre) <= deg2rad(JUMP_THRESHOLD_ATTITUDE)) {
if(abs(v_theta - v_theta_pre) <= 3) addArr(theta_vel, v_theta, NUM_SAMPLES_MED);
addArr(theta_vel_med, median(theta_vel, NUM_SAMPLES_MED), NUM_SAMPLES_AVG);
addArr(theta_vel_avg, average(theta_vel_med, NUM_SAMPLES_AVG), NUM_SAMPLES_AVG);
v_theta = theta_vel_avg[NUM_SAMPLES_AVG - 1];
v_theta_pre = v_theta;
// } else v_theta = v_theta_pre;
// if(abs(v_psi - v_psi_pre) <= deg2rad(JUMP_THRESHOLD_ATTITUDE)) {
if(abs(v_psi - v_psi_pre) <= 3) addArr(psi_vel, v_psi, NUM_SAMPLES_MED);
addArr(psi_vel_med, median(psi_vel, NUM_SAMPLES_MED), NUM_SAMPLES_AVG);
addArr(psi_vel_avg, average(psi_vel_med, NUM_SAMPLES_AVG), NUM_SAMPLES_AVG);
v_psi = psi_vel_avg[NUM_SAMPLES_AVG - 1];
v_psi_pre = v_psi;
// } else v_psi = v_psi_pre;
vicon_msg.velocity[0] = vx;
vicon_msg.velocity[1] = vy;
vicon_msg.velocity[2] = vz;
vicon_msg.angular_vel[0] = v_rho;
vicon_msg.angular_vel[1] = v_theta;
vicon_msg.angular_vel[2] = v_psi;
lcm.publish("vicon_state", &vicon_msg);
//previous state
pre_x = (double)vicon_msg.position[0];
pre_y = (double)vicon_msg.position[1];
pre_z = (double)vicon_msg.position[2];
pre_rho = (double)vicon_msg.attitude[0];
pre_theta = (double)vicon_msg.attitude[1];
pre_psi = (double)vicon_msg.attitude[2];
time_tmp = vicon_msg.timestamp;
//usleep is crucial for accuracy
usleep(1e4);
}
}
if( bReadCentroids )
{
unsigned int CameraCount = MyClient.GetCameraCount().CameraCount;
output_stream << "Cameras(" << CameraCount << "):" << std::endl;
for( unsigned int CameraIndex = 0; CameraIndex < CameraCount; ++CameraIndex )
{
output_stream << " Camera #" << CameraIndex << ":" << std::endl;
const std::string CameraName = MyClient.GetCameraName( CameraIndex ).CameraName;
output_stream << " Name: " << CameraName << std::endl;
unsigned int CentroidCount = MyClient.GetCentroidCount( CameraName ).CentroidCount;
output_stream << " Centroids(" << CentroidCount << "):" << std::endl;
for( unsigned int CentroidIndex = 0; CentroidIndex < CentroidCount; ++CentroidIndex )
{
output_stream << " Centroid #" << CentroidIndex << ":" << std::endl;
Output_GetCentroidPosition _Output_GetCentroidPosition = MyClient.GetCentroidPosition( CameraName, CentroidIndex );
output_stream << " Position: (" << _Output_GetCentroidPosition.CentroidPosition[0] << ", "
<< _Output_GetCentroidPosition.CentroidPosition[1] << ")" << std::endl;
output_stream << " Radius: (" << _Output_GetCentroidPosition.Radius << ")" << std::endl;
}
}
}
}
if( EnableMultiCast )
{
MyClient.StopTransmittingMulticast();
}
MyClient.DisableSegmentData();
MyClient.DisableMarkerData();
MyClient.DisableUnlabeledMarkerData();
MyClient.DisableDeviceData();
if( bReadCentroids )
{
MyClient.DisableCentroidData();
}
// Disconnect and dispose
int t = clock();
std::cout << " Disconnecting..." << std::endl;
MyClient.Disconnect();
int dt = clock() - t;
double secs = (double) (dt)/(double)CLOCKS_PER_SEC;
std::cout << " Disconnect time = " << secs << " secs" << std::endl;
}
}
int main( int argc, char* argv[] )
{
//std::cout << "Starting Vicon UDP connector..." << std::endl << std::flush;
/* int ii;
for (ii=0; ii<=argc; ii++) {
//std::cout << argv[ii] << std::endl;
} */
// Program options
std::string HostName = "192.168.10.81";
std::string TargetSubjectName = "QAV_GREEN";
std::string ViconBaseFrame = "/world";
std::string MulticastAddress = "224.0.0.0:44801";
// scaling constant
double pos_scale = 1e-3;
// do the ROS setup
ros::init(argc, argv, "udp_client");
ros::NodeHandle n;
// ROS parameters
std::string s;
// mutlicast address
if (n.getParam("vicon_multicast_address", s)) {
MulticastAddress = s;
//std::cout << "Got multicast " << MulticastAddress << " from parameter." << std::endl;
}
// own hostname
if (n.getParam("vicon_client_hostname", s)) {
HostName = s;
//std::cout << "Got hostname " << HostName << " from parameter." << std::endl;
}
// vicon frame ID
if (n.getParam("vicon_tf_parent_frame", s)) {
ViconBaseFrame = s;
//std::cout << "Got frame ID " << ViconBaseFrame << " from parameter." << std::endl;
}
// tracking object (this one is private as unique to each node)
// try the private thing using the "bare" method
if (ros::param::has("~vicon_target_subject")) {
ros::param::get("~vicon_target_subject", TargetSubjectName);
}
// set up ROS publishing
std::string TopicName = "vicon/" + TargetSubjectName + "/" + TargetSubjectName;
ros::Publisher pose_pub = n.advertise<geometry_msgs::TransformStamped>(TopicName, 1000);
ros::Rate loop_rate(300);
//std::cout << "HostName: " << HostName << std::endl;
//std::cout << "Multicast: " << MulticastAddress << std::endl;
//std::cout << "Parent frame: " << ViconBaseFrame << std::endl;
//std::cout << "Target subject: " << TargetSubjectName << std::endl;
// initialize the transform
geometry_msgs::TransformStamped MyTransform;
MyTransform.header.frame_id = ViconBaseFrame;
//ROS_INFO("Publishing vicon object %s/%s on topic %s", TargetSubjectName.c_str(), TargetSubjectName.c_str(), TopicName.c_str());
// initialize transform broadcaster
tf::TransformBroadcaster TfBroadcaster;
tf::Transform MyTfTransform;
// log contains:
// version number
// log of framerate over time
// --multicast
// kill off internal app
bool ConnectToMultiCast = true;
bool EnableMultiCast = false;
bool EnableHapticTest = false;
bool bReadCentroids = false;
std::vector<std::string> HapticOnList(0);
// variables for data retrieval
Output_GetSegmentGlobalRotationQuaternion _Output_GetSegmentGlobalRotationQuaternion;
Output_GetSegmentGlobalTranslation _Output_GetSegmentGlobalTranslation;
Output_GetFrame _Output_GetFrame;
// Make a new client
Client MyClient;
// Connect to a server
ROS_INFO("Connecting to multicast %s as host %s", MulticastAddress.c_str(), HostName.c_str());
//std::cout << "Connecting to " << HostName << " ..." << std::flush;
while( !MyClient.IsConnected().Connected )
{
// Direct connection
bool ok = false;
if(ConnectToMultiCast)
{
// Multicast connection
ok = ( MyClient.ConnectToMulticast( HostName, MulticastAddress ).Result == Result::Success );
}
else
{
ok =( MyClient.Connect( HostName ).Result == Result::Success );
}
if(!ok)
{
////std::cout << "Warning - connect failed..." << std::endl;
ROS_WARN("Vicon connection failed...");
}
//std::cout << ".";
sleep(1);
}
//std::cout << "Connected" << std::endl;
// Enable some different data types
MyClient.EnableSegmentData();
//MyClient.EnableMarkerData();
//MyClient.EnableUnlabeledMarkerData();
//MyClient.EnableDeviceData();
if( bReadCentroids )
{
MyClient.EnableCentroidData();
}
//std::cout << "Segment Data Enabled: " << Adapt( MyClient.IsSegmentDataEnabled().Enabled ) << std::endl;
//std::cout << "Marker Data Enabled: " << Adapt( MyClient.IsMarkerDataEnabled().Enabled ) << std::endl;
//std::cout << "Unlabeled Marker Data Enabled: " << Adapt( MyClient.IsUnlabeledMarkerDataEnabled().Enabled ) << std::endl;
//std::cout << "Device Data Enabled: " << Adapt( MyClient.IsDeviceDataEnabled().Enabled ) << std::endl;
//std::cout << "Centroid Data Enabled: " << Adapt( MyClient.IsCentroidDataEnabled().Enabled ) << std::endl;
// Set the streaming mode
// MyClient.SetStreamMode( ViconDataStreamSDK::CPP::StreamMode::ClientPull );
// MyClient.SetStreamMode( ViconDataStreamSDK::CPP::StreamMode::ClientPullPreFetch );
MyClient.SetStreamMode( ViconDataStreamSDK::CPP::StreamMode::ServerPush );
// Set the global up axis
MyClient.SetAxisMapping( Direction::Forward,
Direction::Left,
Direction::Up ); // Z-up
// MyClient.SetGlobalUpAxis( Direction::Forward,
// Direction::Up,
// Direction::Right ); // Y-up
Output_GetAxisMapping _Output_GetAxisMapping = MyClient.GetAxisMapping();
//std::cout << "Axis Mapping: X-" << Adapt( _Output_GetAxisMapping.XAxis )
// << " Y-" << Adapt( _Output_GetAxisMapping.YAxis )
// << " Z-" << Adapt( _Output_GetAxisMapping.ZAxis ) << std::endl;
// Discover the version number
Output_GetVersion _Output_GetVersion = MyClient.GetVersion();
//std::cout << "Version: " << _Output_GetVersion.Major << "."
// << _Output_GetVersion.Minor << "."
// << _Output_GetVersion.Point << std::endl;
if( EnableMultiCast )
{
assert( MyClient.IsConnected().Connected );
MyClient.StartTransmittingMulticast( HostName, MulticastAddress );
}
//ROS_INFO("Publishing vicon object %s relative to %s", TargetSubjectName.c_str(), ViconBaseFrame.c_str());
ROS_INFO("Publishing vicon object %s", TargetSubjectName.c_str());
size_t FrameRateWindow = 1000; // frames
size_t Counter = 0;
clock_t LastTime = clock();
// Loop until a key is pressed
while( ros::ok() )
{
// get ROS stuff done first
ros::spinOnce();
// a pause
loop_rate.sleep();
//try {
// Get a frame
//ROS_INFO("Getting new frame...");
_Output_GetFrame = MyClient.GetFrame();
//ROS_INFO("Got frame");
if (_Output_GetFrame.Result != Result::Success) {
ROS_INFO("Missed frame");
//std::cout << "Didn't get new frame..." << std::endl << std::flush;
continue;
}
// Get the frame number
//Output_GetFrameNumber _Output_GetFrameNumber = MyClient.GetFrameNumber();
////std::cout << "Frame Number: " << _Output_GetFrameNumber.FrameNumber << std::endl;
//Output_GetFrameRate Rate = MyClient.GetFrameRate();
////std::cout << "Frame rate: " << Rate.FrameRateHz << std::endl;
// Get the timecode
//Output_GetTimecode _Output_GetTimecode = MyClient.GetTimecode();
/* //std::cout << "Timecode: "
<< _Output_GetTimecode.Hours << "h "
<< _Output_GetTimecode.Minutes << "m "
<< _Output_GetTimecode.Seconds << "s "
<< _Output_GetTimecode.Frames << "f "
<< _Output_GetTimecode.SubFrame << "sf "
<< Adapt( _Output_GetTimecode.FieldFlag ) << " "
<< _Output_GetTimecode.Standard << " "
<< _Output_GetTimecode.SubFramesPerFrame << " "
<< _Output_GetTimecode.UserBits << std::endl << std::endl;
*/
// Get the global rotation of the target segment
_Output_GetSegmentGlobalRotationQuaternion =
MyClient.GetSegmentGlobalRotationQuaternion( TargetSubjectName, TargetSubjectName );
// test if we got it OK
if (_Output_GetSegmentGlobalRotationQuaternion.Result == Result::Success) {
/* std::cout << "+=+= Global Rotation Quaternion of " << TargetSubjectName << "/" << TargetSubjectName
<< ": (" << _Output_GetSegmentGlobalRotationQuaternion.Rotation[ 0 ] << ", "
<< _Output_GetSegmentGlobalRotationQuaternion.Rotation[ 1 ] << ", "
<< _Output_GetSegmentGlobalRotationQuaternion.Rotation[ 2 ] << ", "
<< _Output_GetSegmentGlobalRotationQuaternion.Rotation[ 3 ] << ") "
<< Adapt( _Output_GetSegmentGlobalRotationQuaternion.Occluded ) << std::endl; */
}
else {
//std::cout << "Failed to get quaternion" << std::endl;
continue;
}
// Get the global segment translation
_Output_GetSegmentGlobalTranslation =
MyClient.GetSegmentGlobalTranslation( TargetSubjectName, TargetSubjectName );
// test if we got it OK
if (_Output_GetSegmentGlobalTranslation.Result == Result::Success) {
/* std::cout << "+=+= Global Translation of " << TargetSubjectName << "/" << TargetSubjectName
<< ": (" << _Output_GetSegmentGlobalTranslation.Translation[ 0 ] << ", "
<< _Output_GetSegmentGlobalTranslation.Translation[ 1 ] << ", "
<< _Output_GetSegmentGlobalTranslation.Translation[ 2 ] << ") "
<< Adapt( _Output_GetSegmentGlobalTranslation.Occluded ) << std::endl; */
}
else {
//std::cout << "Failed to get translation" << std::endl;
continue;
}
// populate the transform
MyTransform.transform.translation.x = _Output_GetSegmentGlobalTranslation.Translation[ 0 ] * pos_scale;
MyTransform.transform.translation.y = _Output_GetSegmentGlobalTranslation.Translation[ 1 ] * pos_scale;
MyTransform.transform.translation.z = _Output_GetSegmentGlobalTranslation.Translation[ 2 ] * pos_scale;
MyTransform.transform.rotation.x = _Output_GetSegmentGlobalRotationQuaternion.Rotation[ 0 ];
MyTransform.transform.rotation.y = _Output_GetSegmentGlobalRotationQuaternion.Rotation[ 1 ];
MyTransform.transform.rotation.z = _Output_GetSegmentGlobalRotationQuaternion.Rotation[ 2 ];
MyTransform.transform.rotation.w = _Output_GetSegmentGlobalRotationQuaternion.Rotation[ 3 ];
// publish the thing
pose_pub.publish(MyTransform);
// and the TF broadcast
MyTfTransform.setOrigin( tf::Vector3(_Output_GetSegmentGlobalTranslation.Translation[ 0 ] * pos_scale,
_Output_GetSegmentGlobalTranslation.Translation[ 1 ] * pos_scale,
_Output_GetSegmentGlobalTranslation.Translation[ 2 ] * pos_scale) );
MyTfTransform.setRotation( tf::Quaternion(_Output_GetSegmentGlobalRotationQuaternion.Rotation[ 0 ],
_Output_GetSegmentGlobalRotationQuaternion.Rotation[ 1 ],
_Output_GetSegmentGlobalRotationQuaternion.Rotation[ 2 ],
_Output_GetSegmentGlobalRotationQuaternion.Rotation[ 3 ]) );
TfBroadcaster.sendTransform(tf::StampedTransform(MyTfTransform, ros::Time::now(), ViconBaseFrame, TopicName));
//}
/* catch (...) {
ROS_INFO("Vicon problem");
}
*/
}
MyClient.DisableSegmentData();
MyClient.DisableMarkerData();
MyClient.DisableUnlabeledMarkerData();
MyClient.DisableDeviceData();
ROS_INFO("Disconnecting from Vicon...");
// Disconnect and dispose
int t = clock();
//std::cout << " Disconnecting..." << std::endl;
MyClient.Disconnect();
int dt = clock() - t;
double secs = (double) (dt)/(double)CLOCKS_PER_SEC;
//std::cout << " Disconnect time = " << secs << " secs" << std::endl;
}