diff --git a/dfall_ws/src/dfall_pkg/src/nodes/ViconDataPublisher.cpp b/dfall_ws/src/dfall_pkg/src/nodes/ViconDataPublisher.cpp
index 1361db48e83642531859073e5810d176f8fe5262..be04f3de12f27ea6762ce2ed125fd919b982156d 100755
--- a/dfall_ws/src/dfall_pkg/src/nodes/ViconDataPublisher.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/ViconDataPublisher.cpp
@@ -103,17 +103,34 @@ int main(int argc, char* argv[]) {
// Inform the user this is about to be attempted
ROS_INFO_STREAM("[VICON DATA PUBLISHER] Connecting to Vicon host with name: " << hostName );
+ // Initiliase a variable for counting the number of connection attempts
+ int num_failed_vicon_connection_attempts = 0;
+
+ // Initialise a variable for the connection status
+ bool vicon_connection_isok = false;
+
// Attempt to connect in a while loop
while (!vicon_client.IsConnected().Connected)
{
// Get the connection status
- bool ok = (vicon_client.Connect(hostName).Result == Result::Success);
+ bool vicon_connection_isok = (vicon_client.Connect(hostName).Result == Result::Success);
- if (!ok)
+ if (!vicon_connection_isok)
{
- // If failed: inform the user and wait a bit
- ROS_ERROR("[VICON DATA PUBLISHER] ERROR - connection failed... attempting again in 1 second");
- ros::Duration(1.0).sleep();
+ // Increment the counter for failed connection attempts
+ num_failed_vicon_connection_attempts++;
+ if (num_failed_vicon_connection_attempts<3)
+ {
+ // If failed: inform the user and wait a bit
+ ROS_ERROR("[VICON DATA PUBLISHER] ERROR - connection failed... attempting again in 1 second");
+ ros::Duration(1.0).sleep();
+ }
+ else
+ {
+ // If failed: inform the user and stop trying
+ ROS_ERROR("[VICON DATA PUBLISHER] ERROR - connection failed... no longer attempting to connect.");
+ break;
+ }
}
else
{
@@ -124,122 +141,137 @@ int main(int argc, char* argv[]) {
- // SPECIFY THE SETTING OF THE VICON CLIENT
+ // IF NO CONNECTION, THEN GO INTO IDLE MODE
+ if (!vicon_connection_isok)
+ {
+ // Inform the user
+ ROS_ERROR("[VICON DATA PUBLISHER] ERROR - connection not established, this node is now idling.");
+ // Go into an idling loop
+ while (ros::ok())
+ {
+ ros::Duration(0.1).sleep();
+ }
+ }
+ else
+ {
- // Set "stream mode" parameter
- // > OPTIONS: { ServerPush , ClientPull }
- // > The "ServerPush" option should have the lowest latency
- vicon_client.SetStreamMode(ViconDataStreamSDK::CPP::StreamMode::ServerPush);
+ // SPECIFY THE SETTING OF THE VICON CLIENT
- // Set what type of data should be provided
- vicon_client.EnableSegmentData();
- vicon_client.EnableMarkerData();
- vicon_client.EnableUnlabeledMarkerData();
- vicon_client.EnableDeviceData();
+ // Set "stream mode" parameter
+ // > OPTIONS: { ServerPush , ClientPull }
+ // > The "ServerPush" option should have the lowest latency
+ vicon_client.SetStreamMode(ViconDataStreamSDK::CPP::StreamMode::ServerPush);
- // Set the inertial frame convention such that positive
- // Z points upwards
- vicon_client.SetAxisMapping(Direction::Forward, Direction::Left, Direction::Up);
+ // Set what type of data should be provided
+ vicon_client.EnableSegmentData();
+ vicon_client.EnableMarkerData();
+ vicon_client.EnableUnlabeledMarkerData();
+ vicon_client.EnableDeviceData();
+ // Set the inertial frame convention such that positive
+ // Z points upwards
+ vicon_client.SetAxisMapping(Direction::Forward, Direction::Left, Direction::Up);
- //
- while (ros::ok())
- {
- // Get a frame
- while (vicon_client.GetFrame().Result != Result::Success) {
- // Sleep a little so that we don't lumber the CPU with a busy poll
- ros::Duration(0.001).sleep();
- }
- // Initilise a "ViconData" struct
- // > This is defined in the "ViconData.msg" file
- ViconData viconData;
+ // Enter an endless loop while ROS is still "ok"
+ while (ros::ok())
+ {
+ // Get a frame
+ while (vicon_client.GetFrame().Result != Result::Success) {
+ // Sleep a little so that we don't lumber the CPU with a busy poll
+ ros::Duration(0.001).sleep();
+ }
- // Unlabeled markers, for GUI
- unsigned int unlabeledMarkerCount = vicon_client.GetUnlabeledMarkerCount().MarkerCount;
+ // Initilise a "ViconData" struct
+ // > This is defined in the "ViconData.msg" file
+ ViconData viconData;
- UnlabeledMarker marker;
- for(int unlabeledMarkerIndex = 0; unlabeledMarkerIndex < unlabeledMarkerCount; unlabeledMarkerIndex++)
- {
+ // Unlabeled markers, for GUI
+ unsigned int unlabeledMarkerCount = vicon_client.GetUnlabeledMarkerCount().MarkerCount;
- Output_GetUnlabeledMarkerGlobalTranslation OutputTranslation =
- vicon_client.GetUnlabeledMarkerGlobalTranslation(unlabeledMarkerIndex);
+ UnlabeledMarker marker;
+ for(int unlabeledMarkerIndex = 0; unlabeledMarkerIndex < unlabeledMarkerCount; unlabeledMarkerIndex++)
+ {
- marker.index = unlabeledMarkerIndex;
- marker.x = OutputTranslation.Translation[0]/1000.0f;
- marker.y = OutputTranslation.Translation[1]/1000.0f;
- marker.z = OutputTranslation.Translation[2]/1000.0f;
+ Output_GetUnlabeledMarkerGlobalTranslation OutputTranslation =
+ vicon_client.GetUnlabeledMarkerGlobalTranslation(unlabeledMarkerIndex);
- viconData.markers.push_back(marker);
- }
+ marker.index = unlabeledMarkerIndex;
+ marker.x = OutputTranslation.Translation[0]/1000.0f;
+ marker.y = OutputTranslation.Translation[1]/1000.0f;
+ marker.z = OutputTranslation.Translation[2]/1000.0f;
- unsigned int subjectCount = vicon_client.GetSubjectCount().SubjectCount;
-
- // //Process the data and publish on topic
- for (int index = 0; index < subjectCount; index++) {
- std::string subjectName = vicon_client.GetSubjectName(index).SubjectName;
- std::string segmentName = vicon_client.GetSegmentName(subjectName, 0).SegmentName;
-
-
- //continue only if the received frame is for the correct crazyflie
- Output_GetSegmentGlobalTranslation outputTranslation =
- vicon_client.GetSegmentGlobalTranslation(subjectName, segmentName);
- //ROS_INFO_STREAM("translation occluded: " << outputTranslation.Occluded);
-
- Output_GetSegmentGlobalRotationQuaternion outputRotation =
- vicon_client.GetSegmentGlobalRotationQuaternion(subjectName, segmentName);
- //ROS_INFO_STREAM("translation occluded: " << outputRotation.Occluded);
-
- //calculate position and rotation of Crazyflie
- double quat_x = outputRotation.Rotation[0];
- double quat_y = outputRotation.Rotation[1];
- double quat_z = outputRotation.Rotation[2];
- double quat_w = outputRotation.Rotation[3];
-
- //TODO check whether this transformation is correct
- double roll = atan2(2 * (quat_w * quat_x + quat_y * quat_z), 1 - 2 * (quat_x * quat_x + quat_y * quat_y));
- double pitch = asin(2 * (quat_w * quat_y - quat_z * quat_x));
- double yaw = atan2(2 * (quat_w * quat_z + quat_x * quat_y), 1 - 2 * (quat_y * quat_y + quat_z * quat_z));
-
- //calculate time until frame data was received
- Output_GetLatencyTotal outputLatencyTotal = vicon_client.GetLatencyTotal();
- double totalViconLatency;
- if (outputLatencyTotal.Result == Result::Success) {
- totalViconLatency = outputLatencyTotal.Total;
- } else {
- totalViconLatency = 0;
+ viconData.markers.push_back(marker);
}
-
- //build message
- CrazyflieData cfData;
- cfData.crazyflieName = subjectName;
-
- cfData.occluded = outputTranslation.Occluded;
-
- cfData.x = outputTranslation.Translation[0] / 1000.0f;
- cfData.y = outputTranslation.Translation[1] / 1000.0f;
- cfData.z = outputTranslation.Translation[2] / 1000.0f;
- cfData.roll = roll;
- cfData.pitch = pitch;
- cfData.yaw = yaw;
- cfData.acquiringTime = totalViconLatency;
- // if(!outputTranslation.Occluded)
- viconData.crazyflies.push_back(cfData);
+ unsigned int subjectCount = vicon_client.GetSubjectCount().SubjectCount;
+
+ // //Process the data and publish on topic
+ for (int index = 0; index < subjectCount; index++) {
+ std::string subjectName = vicon_client.GetSubjectName(index).SubjectName;
+ std::string segmentName = vicon_client.GetSegmentName(subjectName, 0).SegmentName;
+
+
+ //continue only if the received frame is for the correct crazyflie
+ Output_GetSegmentGlobalTranslation outputTranslation =
+ vicon_client.GetSegmentGlobalTranslation(subjectName, segmentName);
+ //ROS_INFO_STREAM("translation occluded: " << outputTranslation.Occluded);
+
+ Output_GetSegmentGlobalRotationQuaternion outputRotation =
+ vicon_client.GetSegmentGlobalRotationQuaternion(subjectName, segmentName);
+ //ROS_INFO_STREAM("translation occluded: " << outputRotation.Occluded);
+
+ //calculate position and rotation of Crazyflie
+ double quat_x = outputRotation.Rotation[0];
+ double quat_y = outputRotation.Rotation[1];
+ double quat_z = outputRotation.Rotation[2];
+ double quat_w = outputRotation.Rotation[3];
+
+ //TODO check whether this transformation is correct
+ double roll = atan2(2 * (quat_w * quat_x + quat_y * quat_z), 1 - 2 * (quat_x * quat_x + quat_y * quat_y));
+ double pitch = asin(2 * (quat_w * quat_y - quat_z * quat_x));
+ double yaw = atan2(2 * (quat_w * quat_z + quat_x * quat_y), 1 - 2 * (quat_y * quat_y + quat_z * quat_z));
+
+ //calculate time until frame data was received
+ Output_GetLatencyTotal outputLatencyTotal = vicon_client.GetLatencyTotal();
+ double totalViconLatency;
+ if (outputLatencyTotal.Result == Result::Success) {
+ totalViconLatency = outputLatencyTotal.Total;
+ } else {
+ totalViconLatency = 0;
+ }
+
+
+ //build message
+ CrazyflieData cfData;
+ cfData.crazyflieName = subjectName;
+
+ cfData.occluded = outputTranslation.Occluded;
+
+ cfData.x = outputTranslation.Translation[0] / 1000.0f;
+ cfData.y = outputTranslation.Translation[1] / 1000.0f;
+ cfData.z = outputTranslation.Translation[2] / 1000.0f;
+ cfData.roll = roll;
+ cfData.pitch = pitch;
+ cfData.yaw = yaw;
+ cfData.acquiringTime = totalViconLatency;
+ // if(!outputTranslation.Occluded)
+ viconData.crazyflies.push_back(cfData);
+ }
+ viconDataPublisher.publish(viconData);
}
- viconDataPublisher.publish(viconData);
- }
- // END OF "while (ros::ok())"
+ // END OF "while (ros::ok())"
- // The code only reaches this point if the while loop is
- // broken, hence disable and diconnect the Vicon client
- vicon_client.DisableSegmentData();
- vicon_client.DisableMarkerData();
- vicon_client.DisableUnlabeledMarkerData();
- vicon_client.DisableDeviceData();
+ // The code only reaches this point if the while loop is
+ // broken, hence disable and diconnect the Vicon client
+ vicon_client.DisableSegmentData();
+ vicon_client.DisableMarkerData();
+ vicon_client.DisableUnlabeledMarkerData();
+ vicon_client.DisableDeviceData();
- vicon_client.Disconnect();
+ vicon_client.Disconnect();
+ }
#endif
}