From a1e72bdb047cc86bac21b6918b705c615e56f662 Mon Sep 17 00:00:00 2001
From: Greg Balke <11024792+gbalke@users.noreply.github.com>
Date: Tue, 6 Oct 2020 20:05:11 -0700
Subject: [PATCH] Comms Testing Update (#98)

* Frequency test update

* Adding buffer clear after failed communication

* Error reporting and additional CLI options

* Adjust to match python scripts ordering

* Extra Whitespace
---
 .../src/BLDCControllerClient.cpp              |  1 +
 .../BLDCDriver_communication_freq_test.cpp    | 66 +++++++++++--------
 2 files changed, 41 insertions(+), 26 deletions(-)

diff --git a/blue_hardware_drivers/src/BLDCControllerClient.cpp b/blue_hardware_drivers/src/BLDCControllerClient.cpp
index 0980e30c..ab2fffae 100644
--- a/blue_hardware_drivers/src/BLDCControllerClient.cpp
+++ b/blue_hardware_drivers/src/BLDCControllerClient.cpp
@@ -72,6 +72,7 @@ void BLDCControllerClient::exchange() {
         if (err != "")
         {
           std::string elapsed = std::to_string(fsec(Time::now() - last_successful_transmission).count());
+          resetBuffer();
           throw comms_error("Board " + std::to_string((int)board_id) + ": " + err + 
                             "\n\t\t\t\t\tTime since last successful comms: " + elapsed);
         }
diff --git a/blue_hardware_drivers/src/BLDCDriver_communication_freq_test.cpp b/blue_hardware_drivers/src/BLDCDriver_communication_freq_test.cpp
index 72f8cf0f..01429017 100644
--- a/blue_hardware_drivers/src/BLDCDriver_communication_freq_test.cpp
+++ b/blue_hardware_drivers/src/BLDCDriver_communication_freq_test.cpp
@@ -31,27 +31,7 @@ float get_period() {
   return period.toSec();
 }
 
-int main(int argc, char **argv) {
-  ros::init(argc, argv, "comms", ros::init_options::AnonymousName);
-
-  ros::NodeHandle n;
-  std::vector<comm_id_t> board_list;
-  std::map<comm_id_t, std::vector<double> > velocity_history_mapping;
-  board_list.push_back(1); // BASE
-  //board_list.push_back(2);
-  //board_list.push_back(3);
-  //board_list.push_back(4);
-  //board_list.push_back(5);
-  //board_list.push_back(6);
-  //board_list.push_back(7);
-  //board_list.push_back(8);
-
-  char* port = argv[1];
-  BLDCControllerClient device;
-  try {
-    device.init(port, board_list);
-  } catch (std::exception& e) { ROS_ERROR("Please Provide USB\n%s\n", e.what()); }
-
+void enumerate_boards(BLDCControllerClient &device, std::vector<comm_id_t> &board_list) {
   // Put all boards into bootloader!
   int count = 0;
   while (count < 3) {
@@ -105,12 +85,45 @@ int main(int argc, char **argv) {
       }
     }
   }
+}
+
+int main(int argc, char **argv) {
+  ros::init(argc, argv, "comms", ros::init_options::AnonymousName);
+
+  ros::NodeHandle n;
+  std::vector<comm_id_t> board_list;
+  std::map<comm_id_t, std::vector<double>> velocity_history_mapping;
+
+
+  char* port = argv[1];
+  char* board_nums = argv[2];
+  char* enumerate = argv[3];
+
+  std::string num = "";
+  for (unsigned int i = 0; board_nums[i] != '\0'; i++) {
+    if (board_nums[i] == ',')
+      continue;
+    comm_id_t id_num = std::atoi(board_nums + i);
+    board_list.push_back(id_num);
+  }
 
+  BLDCControllerClient device;
+  try {
+    device.init(port, board_list);
+  } catch (std::exception& e) { ROS_ERROR("Please Provide USB\n%s\n", e.what()); }
+
+  try {
+    if (*enumerate == 'T') {
+      enumerate_boards(device, board_list);
+    }
+  } catch (std::exception& e) { ROS_ERROR("Please choose whether to enumerate (T/F)\n%s\n", e.what()); }
+
+  bool success = false;
   // Command the motor to 0 before initializing it.
   for (auto id : board_list) {
     success = false;
     while (!success) {
-      try { 
+      try {
         device.queueSetCommand(id, 0);
         device.exchange();
         success = true;
@@ -142,7 +155,7 @@ int main(int argc, char **argv) {
   int counter = 0;
   ros::Rate r(CONTROL_LOOP_FREQ);
 
-  int num_packets_per_log = 100;
+  int num_packets_per_log = 1000;
   int errors = 0;
   int num_packets = 0;
   while (ros::ok()) {
@@ -152,7 +165,7 @@ int main(int argc, char **argv) {
       }
       try { device.exchange(); }
       catch(comms_error e) {
-        ROS_ERROR("%s\n", e.what());
+        //ROS_ERROR("%s\n", e.what());
         device.clearQueue();
         errors++;
         continue;
@@ -165,11 +178,12 @@ int main(int argc, char **argv) {
     }
     num_packets += num_packets_per_log;
     dt = get_period() / (float) num_packets_per_log;
-    ROS_INFO("comm time: dt: %f, freq: %f", dt, 1.0 / dt);
+    ROS_INFO("comm time dt: %f, freq: %f, error rate: %.2f%%",
+             dt, 1.0 / dt, float(errors)/num_packets_per_log * 100);
+    errors = 0;
   }
   r.sleep();
 
 
   return 0;
 }
-