pull out more data

sdk/master_board_sdk/benchmark/communication_benchmark.cpp

@@ -32,6 +32,7 @@ struct ThreadSettings
     CommunicationData benchmark_data;
     LatencyEstimator latency_estimator;
     RtTimer loop_duration_estimator;
+    RtTimer ctrl_loop_duration_estimator;
     PD pd_ctrl;
 };
 
@@ -44,6 +45,7 @@ void* communication_benchmark(void* arg)
     MasterBoardInterface& robot_if = *(thread_settings->robot_if_ptr);
     LatencyEstimator& latency_estimator = thread_settings->latency_estimator;
     RtTimer& loop_duration_estimator = thread_settings->loop_duration_estimator;
+    RtTimer& ctrl_loop_duration_estimator = thread_settings->ctrl_loop_duration_estimator;    
     CommunicationData& benchmark_data = thread_settings->benchmark_data;
     PD& pd_ctrl = thread_settings->pd_ctrl;
 
@@ -84,6 +86,7 @@ void* communication_benchmark(void* arg)
     double start_time = 0.0;
     double current_time = 0.0;
     double motor_ref = 0.0;
+    loop_duration_estimator.tic();
     while (!robot_if.IsTimeout() && thread_settings->ctrl_state != -1)
     {
         current_time = rt_clock_sec();
@@ -93,7 +96,7 @@ void* communication_benchmark(void* arg)
             last += thread_settings->dt;
             cpt += 1;
             cpt_print += 1;
-            loop_duration_estimator.tic();
+            ctrl_loop_duration_estimator.tic();
             latency_estimator.update_received_list(
                 robot_if.GetLastRecvCmdIndex(), robot_if.GetCmdPacketIndex());
             // Read sensor data sent by the masterboard
@@ -188,14 +191,18 @@ void* communication_benchmark(void* arg)
                     100.0 * static_cast<double>(robot_if.GetSensorsLost()) /
                     static_cast<double>(robot_if.GetSensorsSent());
                 benchmark_data.time_list_[cpt] = current_time - start_time;
+                benchmark_data.last_rcv_index_[cpt] = robot_if.GetLastRecvCmdIndex();
+                benchmark_data.cmd_index_[cpt] = robot_if.GetCmdPacketIndex();
             }
             latency_estimator.update_sent_list(robot_if.GetCmdPacketIndex());
             // Send the reference currents to the master board
             robot_if.SendCommand();
+            double ctrl_loop_duration = ctrl_loop_duration_estimator.tac();
             double loop_duration = loop_duration_estimator.tac_tic();
             if (cpt < static_cast<int>(benchmark_data.loop_duration_s_.size()))
             {
                 benchmark_data.loop_duration_s_[cpt] = loop_duration;
+                benchmark_data.ctrl_loop_duration_s_[cpt] = ctrl_loop_duration;
             }
         }
     }
@@ -231,11 +238,11 @@ void* communication_benchmark(void* arg)
 
 int main(int argc, char* argv[])
 {
-    if (argc != 2)
+    if (argc != 4)
     {
         throw std::runtime_error(
-            "Wrong number of argument, please enter the network interface "
-            "name.");
+            "Wrong number of argument, please add: 'the network interface "
+            "name' 'the distance between the mb and the pc' 'the wifi card name'.");
     }
     ThreadSettings thread_settings;
 
@@ -251,7 +258,9 @@ int main(int argc, char* argv[])
         thread_settings.dt,
         thread_settings.network_interface,
         thread_settings.robot_if_ptr->GetProtocolVersion(),
-        thread_settings.duration_s);
+        thread_settings.duration_s,
+        std::atof(argv[3]),
+        argv[2]);
     thread_settings.pd_ctrl.set_gains(0.2, 0.05);
 
     printf("Executing the Benchmark.\n");

sdk/master_board_sdk/benchmark/communication_benchmark_plotter.py

@@ -37,6 +37,9 @@ def main(path_to_data_folder_in):
     cmd_ratio_list = data["cmd_ratio_list"].to_list()
     sensor_ratio_list = data["sensor_ratio_list"].to_list()
     loop_duration_s = data["loop_duration_s"].to_list()
+    ctrl_loop_duration_s = data["ctrl_loop_duration_s"].to_list()
+    last_rcv_index = data["last_rcv_index"].to_list()
+    cmd_index = data["cmd_index"].to_list()
     #
     latency = pandas.read_csv(path_to_data_folder / "latency.dat", delimiter=" ")[
         "latency"
@@ -123,7 +126,25 @@ def main(path_to_data_folder_in):
 
     #########
 
-    print("Plotting the evolution of command loop duration.")
+    print("Plotting the evolution of ctrl loop duration.")
+    plt.figure("Ctrl loop duration", figsize=(20, 15), dpi=200)
+    anchored_text = AnchoredText(
+        "average duration: %f ms\nstandard deviation: %f ms"
+        % (np.mean(ctrl_loop_duration_s), np.std(ctrl_loop_duration_s)),
+        loc=2,
+        prop=dict(fontsize="xx-large"),
+    )
+    ax9 = plt.subplot(1, 1, 1)
+    ax9.plot(ctrl_loop_duration_s, ".")
+    ax9.set_xlabel("iteration", fontsize="xx-large")
+    ax9.set_ylabel("loop duration (ms)", fontsize="xx-large")
+    ax9.add_artist(anchored_text)
+    plt.suptitle("Command loop duration", fontsize="xx-large")
+    plt.savefig(path_to_data_folder / "ctrl_command_loop_duration.png")
+
+    #########
+
+    print("Plotting the evolution of full loop duration.")
     plt.figure("Loop duration", figsize=(20, 15), dpi=200)
     anchored_text = AnchoredText(
         "average duration: %f ms\nstandard deviation: %f ms"
@@ -139,6 +160,21 @@ def main(path_to_data_folder_in):
     plt.suptitle("Command loop duration", fontsize="xx-large")
     plt.savefig(path_to_data_folder / "command_loop_duration.png")
 
+    #########
+
+    print("Command indexes.")
+    plt.figure("Command indexes", figsize=(20, 15), dpi=200)
+    plt.suptitle("Last received command index", fontsize="xx-large")
+    ax = plt.subplot(2, 1, 1)
+    ax.plot(cmd_index, "-")
+    ax.set_xlabel("iteration", fontsize="xx-large")
+    ax.set_ylabel("Command index", fontsize="xx-large")
+    ax = plt.subplot(2, 1, 2)
+    ax.plot(last_rcv_index, "-")
+    ax.set_xlabel("iteration", fontsize="xx-large")
+    ax.set_ylabel("Last received command index", fontsize="xx-large")
+    plt.savefig(path_to_data_folder / "command_index.png")
+
 
 if __name__ == "__main__":
     parser = argparse.ArgumentParser(description="Display data and dump images.")

sdk/master_board_sdk/include/master_board_sdk/benchmark/communication_data.hpp

@@ -38,16 +38,21 @@ class CommunicationData
                     const double& dt,
                     const std::string& network_interface,
                     const int& protocol_version,
-                    const double experiment_duration)
+                    const double& experiment_duration,
+                    const double& distance,
+                    const std::string& wifi_card_name)
     {
         // Data storage.
         time_list_.resize(size, 0.0);
+        last_rcv_index_.resize(size, 0);
+        cmd_index_.resize(size, 0);
         cmd_lost_list_.resize(size, 0);
         sensor_lost_list_.resize(size, 0);
         cmd_ratio_list_.resize(size, 0.0);
         sensor_ratio_list_.resize(size, 0.0);
         latency_.clear();
         loop_duration_s_.resize(size, 0.0);
+        ctrl_loop_duration_s_.resize(size, 0.0);
 
         // Histograms
         histogram_sensor_.resize(20, 0);
@@ -60,6 +65,8 @@ class CommunicationData
         control_period_ = dt;
         network_interface_ = network_interface;
         experiment_duration_ = experiment_duration;
+        distance_ = distance;
+        wifi_card_name_ = wifi_card_name;
         if (network_interface_.at(0) == 'w')
         {
             wifi_channel_ = execute_bash_command(
@@ -115,12 +122,21 @@ class CommunicationData
                << "sensor_lost_list "
                << "cmd_ratio_list "
                << "sensor_ratio_list "
-               << "loop_duration_s\n";
+               << "loop_duration_s "
+               << "ctrl_loop_duration_s "
+               << "last_rcv_index "
+               << "cmd_index " << std::endl;
         for (std::size_t i = 0; i < time_list_.size(); ++i)
         {
-            myfile << time_list_[i] << " " << cmd_lost_list_[i] << " "
-                   << sensor_lost_list_[i] << " " << cmd_ratio_list_[i] << " "
-                   << sensor_ratio_list_[i] << " " << loop_duration_s_[i]
+            myfile << time_list_[i] << " "             //
+                   << cmd_lost_list_[i] << " "         //
+                   << sensor_lost_list_[i] << " "      //
+                   << cmd_ratio_list_[i] << " "        //
+                   << sensor_ratio_list_[i] << " "     //
+                   << loop_duration_s_[i] << " "       //
+                   << ctrl_loop_duration_s_[i] << " "  //
+                   << last_rcv_index_[i] << " "        //
+                   << cmd_index_[i] << " "             //
                    << std::endl;
         }
         myfile.close();
@@ -152,10 +168,12 @@ class CommunicationData
                << "    wifi_channel: " << wifi_channel_ << std::endl
                << "    experiment_duration: " << experiment_duration_
                << std::endl
+               << "    distance: " << distance_ << std::endl
                << "latency_statistics:" << std::endl
                << "    average: " << latence_average_ << std::endl
                << "    standard_deviation: " << latence_stdev_ << std::endl
                << "system:" << std::endl
+               << "    wifi_card_name: " << wifi_card_name_ << std::endl
                << "    distribution_: " << distribution_ << std::endl
                << "    date_time: " << date_ << std::endl
                << "    protocole_version: '3'" << std::endl;
@@ -181,13 +199,16 @@ class CommunicationData
     std::vector<int> histogram_cmd_;
 
     // Data
+    std::vector<int> last_rcv_index_;
+    std::vector<int> cmd_index_;
     std::vector<int> cmd_lost_list_;
     std::vector<int> sensor_lost_list_;
     std::vector<double> time_list_;
     std::vector<double> cmd_ratio_list_;
     std::vector<double> sensor_ratio_list_;
     std::deque<double> latency_;
     std::vector<double> loop_duration_s_;
+    std::vector<double> ctrl_loop_duration_s_;
 
     // data_folder
     std::string data_folder_;
@@ -203,6 +224,8 @@ class CommunicationData
     double latence_stdev_;
 
     // metadata system
+    std::string wifi_card_name_;
+    double distance_;
     std::string distribution_;
     int protocol_version_;
     std::string date_;