Collecting Raw PhaseSpace Data for the Platforms (CPP) - Carleton-SRCL/SPOT GitHub Wiki
Sometimes it can be useful to simply collect raw positional data for the platforms without needing to rely on the platform computers and simply using C++. The simplest way to do this is to use the PhaseSpace_Server currently used to retrieve raw data, which it then sends out via UDP to all of the platforms. The C++ project is located at this link: https://www.dropbox.com/sh/0d5t0tp7i0umsmc/AAA2nrlK8wtYnJ_rRqDHSPyya?dl=0
The main function is divided into the system initialization and the data retrieval. When you open the project file and look to the main function, you will see the code below. This code starts by checking that the user provided an input when the executable was run. This input is the desired sample rate for the data, which can be run at any integer frequency up to 960 Hz. The rest of this code block simply initializes any required variables - see the comments next to each variable for a description of its purpose.
if (argc < 2) {
// print error message if no value is provided
std::cerr << "Error: no frequency value provided" << std::endl;
return 1; // exit program with error code
}
WSADATA w; /* Used to open windows connection */
unsigned short port_number = 31535; /* Port number to use */
int a1, a2, a3, a4; /* Components of address in xxx.xxx.xxx.xxx form */
int client_length; /* Length of client struct */
int bytes_received = -1; /* Bytes received from client */
SOCKET sd; /* Socket descriptor of server */
struct sockaddr_in server; /* Information about the server */
struct sockaddr_in client; /* Information about the client */
char platformID[BUFFER_SIZE]; /* Where to store received data */
struct hostent *hp; /* Information about this computer */
char host_name[256]; /* Name of the server */
double dataPacket[16] = { 0 }; /* Create array for data storage */
clock_t start;
double duration;
The next code block is used to establish a connection between the PhaseSpace computer and the ground station computer. The ground station does NOT need to be the lab computer - simply change the IP address for 'a1'-'a4' to match your laptop's IP address and you can run this on any computer. In brief, this code block opens a windows connection at the IP address provided, starts running the server on said IP address, and binds the socket. At this point, all of this code is C++ wireless communication code and is not strictly related to the PhaseSpace computer.
/* Below is the IP address for the groundstation computer. In this version,
it has been set to my laptop. This will have to be changed for any new groundstation. */
a1 = 192;
a2 = 168;
a3 = 1;
a4 = 104;
/* Open windows connection */
if (WSAStartup(0x0101, &w) != 0)
{
fprintf(stderr, "Could not open Windows connection.\n");
::exit(0);
}
/* Open a datagram socket */
sd = socket(AF_INET, SOCK_DGRAM, 0);
u_long mode = 1;
ioctlsocket(sd, FIONBIO, &mode);
if (sd == INVALID_SOCKET)
{
fprintf(stderr, "Could not create socket.\n");
WSACleanup();
::exit(0);
}
/* Clear out server struct */
memset((void *)&server, '\0', sizeof(struct sockaddr_in));
/* Set family and port */
server.sin_family = AF_INET;
server.sin_port = htons(port_number);
server.sin_addr.S_un.S_un_b.s_b1 = (unsigned char)a1;
server.sin_addr.S_un.S_un_b.s_b2 = (unsigned char)a2;
server.sin_addr.S_un.S_un_b.s_b3 = (unsigned char)a3;
server.sin_addr.S_un.S_un_b.s_b4 = (unsigned char)a4;
/* Print out server information */
printf("Server running on %u.%u.%u.%u\n", (unsigned char)server.sin_addr.S_un.S_un_b.s_b1,
(unsigned char)server.sin_addr.S_un.S_un_b.s_b2,
(unsigned char)server.sin_addr.S_un.S_un_b.s_b3,
(unsigned char)server.sin_addr.S_un.S_un_b.s_b4);
printf("Press CTRL + C to quit\n");
/* Bind address to socket */
if (bind(sd, (struct sockaddr *)&server, sizeof(struct sockaddr_in)) == -1)
{
fprintf(stderr, "Could not bind name to socket.\n");
closesocket(sd);
WSACleanup();
::exit(0);
}
This small snippet of code initializes the clients. Generally speaking this is only used for the experiments, as this code is required to send the data packet over UDP.
client_length = (int)sizeof(struct sockaddr_in);
/* Below is the IP address for the UDP client. */
std::vector<std::string> client_addresses = { "192.168.1.110", "192.168.1.111", "192.168.1.112", "192.168.1.104" };
// set up sockaddr_in struct for wireless computer
client.sin_family = AF_INET;
client.sin_port = htons(31534); // use port 1234 on wireless computer
The next code snippet contains the initialization for the PhaseSpace system. The code starts by defining the IP address of the PhaseSpace computer, and then it calls the open and initialize methods from the OWL API to connect the system and bind the socket. Strings are then defined for the different RED, BLACK, and BLUE LED trackers. The last line of code dictates which LED profile should be used. The profiles are pre-defined and 'all20' simply means that every LED that is found will be returned.
string address = "192.168.1.109";
OWL::Context owl;
OWL::Markers markers;
OWL::Rigids rigids;
if(owl.open(address) <= 0 || owl.initialize() <= 0) return 0;
std::string phaseSpaceOptions;
std::string tracker_id_RED_7_pos_string, tracker_id_RED_1_pos_string;
std::string tracker_id_RED_3_pos_string, tracker_id_RED_5_pos_string;
std::string tracker_id_BLACK_29_pos_string, tracker_id_BLACK_27_pos_string;
std::string tracker_id_BLACK_25_pos_string, tracker_id_BLACK_31_pos_string;
std::string tracker_id_BLUE_8_pos_string, tracker_id_BLUE_14_pos_string;
std::string tracker_id_BLUE_12_pos_string, tracker_id_BLUE_10_pos_string;
phaseSpaceOptions = "profile=all120";
The code then continues on to define the LED locations relative to the CG of the rigid bodies. These measurements will change depending on the configuration of the platforms. For example, the code below contains the CGs for the RED platform with a full tank of air and the reaction wheel.
tracker_id_RED_5_pos_string = "pos=125.509767,143.875167,0";
tracker_id_RED_3_pos_string = "pos=125.509767,-135.624833,0";
tracker_id_RED_1_pos_string = "pos=-154.990233,-135.624833,0";
tracker_id_RED_7_pos_string = "pos=-153.490233,144.375167,0";
tracker_id_BLACK_29_pos_string = "pos=130.251807,141.800150,0";
tracker_id_BLACK_27_pos_string = "pos=130.751807,-135.699850,0";
tracker_id_BLACK_25_pos_string = "pos=-146.748193,-135.199850,0";
tracker_id_BLACK_31_pos_string = "pos=-146.748193,143.300150,0";
tracker_id_BLUE_8_pos_string = "pos=140.000177,152.096588,0";
tracker_id_BLUE_14_pos_string = "pos=140.500177,-125.403412,0";
tracker_id_BLUE_12_pos_string = "pos=-136.999823,-124.903412,0";
tracker_id_BLUE_10_pos_string = "pos=-136.999823,153.596588,0";
The next section of the code creates the rigid bodies (4 LEDs make up a rigid body in our case), creates trackers, and assigns markers. This code abstracts what is actually going on, but it is not required knowledge. Of importance however, is the tracker ID's assigned. RED is ID #0, the arm is ID #1, BLACK is ID #2, and BLUE is ID #3.
const std::string myoptions = phaseSpaceOptions;
uint32_t tracker_id_RED = 0;
owl.createTracker(tracker_id_RED, "rigid", "RED_rigid");
/* Assign markers to the rigid body and indicate their positions
w.r.t the centre of mass (obtained from calibration text file) */
owl.assignMarker(tracker_id_RED, 5, "5", tracker_id_RED_5_pos_string); // top left
owl.assignMarker(tracker_id_RED, 3, "3", tracker_id_RED_3_pos_string); // top right
owl.assignMarker(tracker_id_RED, 1, "1", tracker_id_RED_1_pos_string); // bottom right
owl.assignMarker(tracker_id_RED, 7, "7", tracker_id_RED_7_pos_string); // bottom left
uint32_t tracker_id_BLACK = 2;
owl.createTracker(tracker_id_BLACK, "rigid", "BLACK_rigid");
/* Assign markers to the rigid body and indicate their positions
w.r.t the centre of mass (obtained from calibration text file) */
owl.assignMarker(tracker_id_BLACK, 29, "29", tracker_id_BLACK_29_pos_string); // top left
owl.assignMarker(tracker_id_BLACK, 27, "27", tracker_id_BLACK_27_pos_string); // top right
owl.assignMarker(tracker_id_BLACK, 25, "25", tracker_id_BLACK_25_pos_string); // bottom right
owl.assignMarker(tracker_id_BLACK, 31, "31", tracker_id_BLACK_31_pos_string); // bottom left
uint32_t tracker_id_BLUE = 3;
owl.createTracker(tracker_id_BLUE, "rigid", "BLUE_rigid");
/* Assign markers to the rigid body and indicate their positions
w.r.t the centre of mass (obtained from calibration text file) */
owl.assignMarker(tracker_id_BLUE, 8, "8", tracker_id_BLUE_8_pos_string); // top left
owl.assignMarker(tracker_id_BLUE, 14, "14", tracker_id_BLUE_14_pos_string); // top right
owl.assignMarker(tracker_id_BLUE, 12, "12", tracker_id_BLUE_12_pos_string); // bottom right
owl.assignMarker(tracker_id_BLUE, 10, "10", tracker_id_BLUE_10_pos_string); // bottom left
We then extract the frequency from the input argument, and pass that to the .frequency method. The streaming mode is then set (1 is TCP/IP, 2 is UDP broadcast to a specific IP, and 3 is UDP broadcast to all IP). A clock is started, and lastly a file is opened for data storage. Opening the data file is completely optional, it is also possible to simply send the data over UDP to wherever you need it. In this example we just want to log the data for later use.
int frequency = atoi(argv[1]); // convert first argument to integer
owl.frequency(frequency);
// start streaming
owl.streaming(1);
//start = clock();
auto t1 = Clock::now();
// Create an output file stream object
std::ofstream outFile;
outFile.open("RawPhaseSpaceData.csv", std::ios::out); // Open the file for output.
Now we enter the main loop of the program, which checks that the phasespace system is properly initialized and all required sockets are open. If there are no 'event' frames (i.e. no data), the loop skips to the next frame. If an 'event' is present, then it checks if the 'event' type is an ERROR. If it is not an error, then it checks if it is a FRAME (which contains the data). It then looks through the frames and find any 'rigids'. A 'rigid' is a predefined body (defined in the Master Client software), which uses multiple LEDs to create a rigid body with a center of mass. We have pre-defined 3 rigid bodies: RED, BLACK, and BLUE. The data (x, y, and orientation) are extracted from the frame and then written to a CSV file.
// main loop
while (owl.isOpen() && owl.property<int>("initialized"))
{
const OWL::Event* event = owl.nextEvent(1000);
if (!event) continue;
if (event->type_id() == OWL::Type::ERROR)
{
break;
}
else if (event->type_id() == OWL::Type::FRAME)
{
if (event->find("rigids", rigids) > 0)
{
for (OWL::Rigids::iterator r = rigids.begin(); r != rigids.end(); r++)
{
if (r->cond > 0)
{
auto t2 = Clock::now();
duration = std::chrono::duration_cast<std::chrono::nanoseconds>(t2 - t1).count();
dataPacket[0] = duration * 0.000000001;
if (r->id == 0)
{
//dataPacket[0] = event->time();
//duration = (std::clock() - start) / (double)CLOCKS_PER_SEC;
dataPacket[1] = r->pose[0];
dataPacket[2] = r->pose[1];
dataPacket[3] = atan2(2 * r->pose[4] * r->pose[5]
+ 2 * r->pose[3] * r->pose[6],
2 * r->pose[3] * r->pose[3] - 1
+ 2 * r->pose[4] * r->pose[4]);
}
if (r->id == 2)
{
dataPacket[4] = r->pose[0];
dataPacket[5] = r->pose[1];
dataPacket[6] = atan2(2 * r->pose[4] * r->pose[5]
+ 2 * r->pose[3] * r->pose[6],
2 * r->pose[3] * r->pose[3] - 1
+ 2 * r->pose[4] * r->pose[4]);
}
if (r->id == 3)
{
dataPacket[7] = r->pose[0];
dataPacket[8] = r->pose[1];
dataPacket[9] = atan2(2 * r->pose[4] * r->pose[5]
+ 2 * r->pose[3] * r->pose[6],
2 * r->pose[3] * r->pose[3] - 1
+ 2 * r->pose[4] * r->pose[4]);
}
}
}
}
// send dataPacket array to CSV file instead of wireless computer
if (outFile.is_open())
{
// Write the dataPacket values to the file, separated by commas
outFile << dataPacket[0];
for (int i = 1; i < 10; ++i)
{
outFile << "," << dataPacket[i];
}
outFile << "\n"; // End the line
}
}
} // while
The last section of the code simply closes the file that we wrote to, and then closes the OWL socket.
outFile.close(); // Close the file when you're done with it
owl.done();
owl.close();
return 0;