diff --git a/sdk/master_board_sdk/CMakeLists.txt b/sdk/master_board_sdk/CMakeLists.txt index 9c7f624f..cc223308 100644 --- a/sdk/master_board_sdk/CMakeLists.txt +++ b/sdk/master_board_sdk/CMakeLists.txt @@ -42,6 +42,7 @@ IF(BUILD_PYTHON_INTERFACE) FINDPYTHON() SEARCH_FOR_BOOST_PYTHON(REQUIRED) ENDIF(BUILD_PYTHON_INTERFACE) +find_package(Boost REQUIRED COMPONENTS filesystem) # ---------------------------------------------------- # --- INCLUDE ---------------------------------------- @@ -103,6 +104,17 @@ IF(BUILD_PYTHON_INTERFACE) PYTHON_BUILD(example com_analyser.py) ENDIF(BUILD_PYTHON_INTERFACE) +# --- Benchmark ---------------------------------------------------------------- +ADD_EXECUTABLE(${PROJECT_NAME}_benchmark_communication benchmark/communication_benchmark.cpp) +TARGET_LINK_LIBRARIES(${PROJECT_NAME}_benchmark_communication ${PROJECT_NAME} Boost::filesystem) +INSTALL(TARGETS ${PROJECT_NAME}_benchmark_communication DESTINATION bin) +# Install the python script for the plotting +INSTALL( + FILES benchmark/communication_benchmark_plotter.py + RENAME ${PROJECT_NAME}_benchmark_communication_plotter + PERMISSIONS OWNER_EXECUTE OWNER_WRITE OWNER_READ + DESTINATION bin +) # --- CHECK EXAMPLES ----------------------------------------------------------- ADD_EXECUTABLE(master_board_example example/example.cpp) diff --git a/sdk/master_board_sdk/benchmark/communication_benchmark.cpp b/sdk/master_board_sdk/benchmark/communication_benchmark.cpp new file mode 100644 index 00000000..2512f1b3 --- /dev/null +++ b/sdk/master_board_sdk/benchmark/communication_benchmark.cpp @@ -0,0 +1,274 @@ +/** + * BSD 3-Clause License + * + * Copyright (c) 2019, LAAS-CNRS, New York University and Max Planck + * Gesellschaft. + * All rights reserved. + */ + +#include +#include +#include "master_board_sdk/benchmark/communication_data.hpp" +#include "master_board_sdk/benchmark/execute_bash_command.hpp" +#include "master_board_sdk/benchmark/latency_estimator.hpp" +#include "master_board_sdk/benchmark/pd.hpp" +#include "master_board_sdk/benchmark/real_time_thread.hpp" +#include "master_board_sdk/benchmark/real_time_timer.hpp" +#include "master_board_sdk/master_board_interface.h" + +using namespace master_board_sdk::benchmark; + +struct ThreadSettings +{ + std::string network_interface = ""; + int ctrl_state = 0; + int n_udriver_controlled = 1; // Current number of controled drivers. + double dt = 0.001; //  Time step + double duration_s = 20.0; // Duration in seconds, init included + int nb_iteration = static_cast(duration_s / dt); + + // Interface objects: + std::unique_ptr robot_if_ptr; + CommunicationData benchmark_data; + LatencyEstimator latency_estimator; + RtTimer loop_duration_estimator; + RtTimer ctrl_loop_duration_estimator; + PD pd_ctrl; +}; + +void* communication_benchmark(void* arg) +{ + // input parameters + ThreadSettings* thread_settings = static_cast(arg); + + // Shortcuts: + 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; + + //  We enable each controler driver and its two associated motors + for (int i = 0; i < thread_settings->n_udriver_controlled; ++i) + { + robot_if.GetDriver(i)->motor1->SetCurrentReference(0); + robot_if.GetDriver(i)->motor2->SetCurrentReference(0); + robot_if.GetDriver(i)->motor1->Enable(); + robot_if.GetDriver(i)->motor2->Enable(); + robot_if.GetDriver(i)->EnablePositionRolloverError(); + robot_if.GetDriver(i)->SetTimeout(5); + robot_if.GetDriver(i)->Enable(); + } + // Start sending the "get ready command" + double last = rt_clock_sec(); + while (!robot_if.IsTimeout() && !robot_if.IsAckMsgReceived()) + { + if ((rt_clock_sec() - last) > thread_settings->dt) + { + last = rt_clock_sec(); + robot_if.SendInit(); + } + } + // Check if the interface is properly connected. + if (robot_if.IsTimeout()) + { + printf("-- Timeout while waiting for ack. --\n"); + exit(0); + } + + std::vector init_pos(2 * thread_settings->n_udriver_controlled); + + printf("-- ack acquired. --\n"); + last = rt_clock_sec() - thread_settings->dt; + int cpt = 0; // Counting iterations. + int cpt_print = 0; + 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(); + // Fix the control period. + if ((current_time - last) > thread_settings->dt) + { + last += thread_settings->dt; + cpt += 1; + cpt_print += 1; + ctrl_loop_duration_estimator.tic(); + latency_estimator.update_received_list( + robot_if.GetLastRecvCmdIndex(), robot_if.GetCmdPacketIndex()); + // Read sensor data sent by the masterboard + robot_if.ParseSensorData(); + + //  If the system is not ready + if (thread_settings->ctrl_state == 0) + { + int ctrl_state = 1; + // for all motors on a connected slave + // Check if all motors are enabled and ready + for (int i = 0; i < thread_settings->n_udriver_controlled * 2; + ++i) + { + if (!(robot_if.GetMotor(i)->IsEnabled() && + robot_if.GetMotor(i)->IsReady())) + { + ctrl_state = 0; + } + } + for (int i = 0; i < 2 * thread_settings->n_udriver_controlled; + ++i) + { + init_pos[i] = robot_if.motors[i].get_position(); + } + thread_settings->ctrl_state = ctrl_state; + cpt = 0; // Counting iterations. + start_time = current_time; + } + // running control + else + { + if ((current_time - start_time) > thread_settings->duration_s) + { + thread_settings->ctrl_state = -1; + for (int i = 0; + i < thread_settings->n_udriver_controlled * 2; + ++i) + { + robot_if.GetMotor(i)->SetCurrentReference(0); + } + } + else + { + for (int i = 0; + i < thread_settings->n_udriver_controlled * 2; + ++i) + { + if ((i % 2 == 0) && + robot_if.GetDriver(i / 2)->error_code == 0xF) + { + continue; + } + if (robot_if.GetMotor(i)->IsEnabled()) + { + motor_ref = + init_pos[i] + + 4 * M_PI * sin(2 * M_PI * 0.5 * current_time); + robot_if.GetMotor(i)->SetCurrentReference( + pd_ctrl.compute( + robot_if.motors[i].GetPosition(), + robot_if.motors[i].GetVelocity(), + motor_ref)); + } + } + } + } + // Display system state and communication status, + // once every 100 iterations of the main loop + if (cpt_print % 1000 == 0) + { + robot_if.PrintStats(); + printf("%f > %f\n", + (current_time - start_time), + thread_settings->duration_s); + } + if (cpt < static_cast(benchmark_data.cmd_lost_list_.size())) + { + benchmark_data.cmd_lost_list_[cpt] = robot_if.GetCmdLost(); + benchmark_data.sensor_lost_list_[cpt] = + robot_if.GetSensorsLost(); + benchmark_data.cmd_ratio_list_[cpt] = + 100.0 * static_cast(robot_if.GetCmdLost()) / + static_cast(robot_if.GetCmdSent()); + benchmark_data.sensor_ratio_list_[cpt] = + 100.0 * static_cast(robot_if.GetSensorsLost()) / + static_cast(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(benchmark_data.loop_duration_s_.size())) + { + benchmark_data.loop_duration_s_[cpt] = loop_duration; + benchmark_data.ctrl_loop_duration_s_[cpt] = ctrl_loop_duration; + } + } + } + // Shut down the interface between the computer and the master + // board + robot_if.Stop(); + if (robot_if.IsTimeout()) + { + printf( + "-- Masterboard timeout detected. --\n" + "-- " + "Either the masterboard has been shutdown, " + "or there has been a connection issue with the cable/wifi." + " --\n"); + } + benchmark_data.latency_ = latency_estimator.get_latency_measurement(); + // Plot histograms and graphs + for (int i = 0; + i < static_cast(benchmark_data.histogram_sensor_.size()); + ++i) + { + benchmark_data.histogram_sensor_[i] = + robot_if.GetSensorHistogram(i); + benchmark_data.histogram_cmd_[i] = robot_if.GetCmdHistogram(i); + } + printf("-- Dump the collected data --\n"); + benchmark_data.dump_data(); + return nullptr; +} + +int main(int argc, char* argv[]) +{ + if (argc != 4) + { + throw std::runtime_error( + "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; + + thread_settings.network_interface = argv[1]; + thread_settings.robot_if_ptr = std::make_unique( + thread_settings.network_interface); + // Initialization of the interface between the computer and the master board + thread_settings.robot_if_ptr->Init(); + thread_settings.latency_estimator.initialize( + thread_settings.robot_if_ptr->GetCmdPacketIndex()); + thread_settings.benchmark_data.initialize( + thread_settings.nb_iteration, + thread_settings.dt, + thread_settings.network_interface, + thread_settings.robot_if_ptr->GetProtocolVersion(), + 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"); + thread_settings.ctrl_state = 0; + RealTimeThread rt_thread; + rt_thread.create_realtime_thread(&communication_benchmark, + &thread_settings); + rt_thread.join(); + + printf("Extract the plots from the collected data."); + std::string output; + { + std::ostringstream oss; + oss << argv[0] << "_plotter " + << thread_settings.benchmark_data.data_folder_; + execute_bash_command(oss.str()); + } + printf("End of program!\n"); + return 0; +} diff --git a/sdk/master_board_sdk/benchmark/communication_benchmark.py b/sdk/master_board_sdk/benchmark/communication_benchmark.py new file mode 100644 index 00000000..8fdb067d --- /dev/null +++ b/sdk/master_board_sdk/benchmark/communication_benchmark.py @@ -0,0 +1,442 @@ +# coding: utf8 + +from pathlib import Path +import argparse +import math +import os +import sys +import time +from copy import deepcopy +from time import clock +import yaml + +import libmaster_board_sdk_pywrap as mbs +import matplotlib.pyplot as plt +from matplotlib.offsetbox import AnchoredText +import numpy as np +import platform +import subprocess + + +class LatencyEstimator(object): + def __init__(self, duration, dt, last_recv_cmd_index): + super().__init__() + + self._overflow_cmd_cpt = 0 + self._first_cmd_index = last_recv_cmd_index + self._cmd_packet_index = last_recv_cmd_index + self._last_cmd_packet_index = last_recv_cmd_index + + self._sent_list = (int(duration / dt) + 2) * [0.0] + self._received_list = (int(duration / dt) + 2) * [0.0] + + self._latency = [] + + def update_received_list(self, last_recv_cmd_index, current_cmd_packet_index): + if last_recv_cmd_index > current_cmd_packet_index: + last_recv_cmd_index = ( + self._overflow_cmd_cpt - 1 + ) * 65536 + last_recv_cmd_index + else: + last_recv_cmd_index = self._overflow_cmd_cpt * 65536 + last_recv_cmd_index + + if ( + last_recv_cmd_index >= self._first_cmd_index + and self._received_list[last_recv_cmd_index - self._first_cmd_index] == 0 + ): + self._received_list[last_recv_cmd_index - self._first_cmd_index] = clock() + + def update_sent_list(self, current_cmd_packet_index): + diff = current_cmd_packet_index - self._last_cmd_packet_index + if diff < 0: + self._overflow_cmd_cpt += 1 + diff = 65536 + current_cmd_packet_index - self._last_cmd_packet_index + self._cmd_packet_index += diff + self._last_cmd_packet_index = current_cmd_packet_index + self._sent_list[self._cmd_packet_index - self._first_cmd_index] = clock() + + def get_latency_measurement(self): + self._latency = [] + for i in range(1, len(self._sent_list) - 1): + if self._received_list[i] != 0 and self._sent_list[i] != 0: + self._latency.append( + 1000 * (self._received_list[i] - self._sent_list[i]) + ) + else: + self._latency.append(0) # 0 means not sent or not received + return self._latency + + +class DurationEstimator(object): + def __init__(self) -> None: + super().__init__() + self._previous_time = 0.0 + self._current_time = 0.0 + + def initialize(self): + self._previous_time = clock() + + def get_duration(self): + self._current_time = clock() + if self._previous_time != 0.0: + return 1000 * (self._current_time - self._previous_time) + self._previous_time = self._current_time + + def get_current_time(self): + return 1000 * self._current_time + + +class BenchmarkData(object): + def __init__(self): + + # Data storage. + + # 100Hz data buffer + self.time_list = [] + self.cmd_lost_list = [] + self.sensor_lost_list = [] + self.cmd_ratio_list = [] + self.sensor_ratio_list = [] + + # Histograms + self.histogram_sensor = [] + self.histogram_cmd = [] + + # Communication latency + self.latency = [] + + # Control time loop + self.loop_duration = [] + + # Data folder to dump the data/figures. + self._data_folder = ( + Path("/tmp") + / "communication_benchmark" + / deepcopy(time.strftime("%Y-%m-%d_%H-%M-%S")) + ) + if not self._data_folder.is_dir(): + self._data_folder.mkdir(parents=True, exist_ok=True) + + # Metadata and statistics. + self.metadata = {} + self.metadata["controller"] = {} + self.metadata["system"] = {} + self.metadata["latency_statistics"] = {} + + def initialize(self, dt, network_interface, protocol_version): + # collect infos. + self.dt = dt + self.network_interface = network_interface + + # get the wifi channel + if self.network_interface[0] == "w": + freq = subprocess.check_output( + "iwlist " + self.network_interface + " channel | grep Frequency", + shell=True, + ) + self.wifi_channel = (str(freq).split("(")[1]).split(")")[0] + else: + self.wifi_channel = "None" + + # Controller infos. + self.metadata["controller"]["control_period"] = self.dt + self.metadata["controller"]["network_interface"] = self.network_interface + self.metadata["controller"]["wifi_channel"] = self.wifi_channel + + # System infos. + self.metadata["system"]["date_time"] = time.strftime("%c") + self.metadata["system"]["linux_distribution"] = ( + platform.linux_distribution()[0] + + " " + + platform.linux_distribution()[1] + + " " + + platform.linux_distribution()[2] + ) + self.metadata["system"]["computer"] = os.uname()[1] + self.metadata["system"]["protocole_version"] = protocol_version + + def dump_data(self): + + self._dump_latency() + self._dump_histograms_and_graphs() + self._dump_loop_duration() + self._dump_metadata() + print( + "The graphs and metadata have been stored in ", + self._data_folder, + ) + + def _dump_metadata(self): + """Dump the metada in a yaml format to be human readable.""" + yaml_text = yaml.dump(self.metadata, default_flow_style=False) + with open(str(self._data_folder / "metadata.yaml"), "a") as f: + f.write(yaml_text) + f.close() + + def _dump_latency(self): + # computing avg and std for non zero values + nonzero = [self.latency[i] for i in np.nonzero(self.latency)[0]] + average = 0 + std = 0 + if len(nonzero) != 0: + average = np.mean(nonzero) + std = np.std(nonzero) + self.metadata["latency_statistics"]["average"] = float(average) + self.metadata["latency_statistics"]["standard_deviation"] = float(std) + print("latency average latency: %f ms" % average) + print("latency standard deviation: %f ms" % std) + + print(print("-- Dump latency graph --")) + plt.figure("wifi-ethernet latency", figsize=(20, 15), dpi=200) + anchored_text = AnchoredText( + "average latency: %f ms\nstandard deviation: %f ms" % (average, std), + loc=2, + prop=dict(fontsize="xx-large"), + ) + if len(self.latency) > 5000: + ax1 = plt.subplot(2, 1, 1) + else: + ax1 = plt.subplot(1, 1, 1) + ax1.plot(self.latency, ".") + ax1.set_xlabel("index", fontsize="xx-large") + ax1.set_ylabel("latency (ms)", fontsize="xx-large") + ax1.add_artist(anchored_text) + + # plotting zoomed version to see pattern + if len(self.latency) > 5000: + ax2 = plt.subplot(2, 1, 2) + ax2.plot(self.latency, ".") + ax2.set_xlabel("index", fontsize="xx-large") + ax2.set_ylabel("latency (ms)", fontsize="xx-large") + ax2.set_xlim(len(self.latency) / 2, len(self.latency) / 2 + 2000) + ax2.set_ylim(-0.1, 2.1) + + if self.wifi_channel != "None": + plt.suptitle( + "Wifi communication latency: " + self.wifi_channel, + fontsize="xx-large", + ) + else: + plt.suptitle("Ethernet communication latency", fontsize="xx-large") + plt.savefig(str(self._data_folder / "wifi_eth_latency.png")) + + def _dump_histograms_and_graphs(self): + # Plot histograms and graphs + + plt.figure("wifi-ethernet statistics", figsize=(20, 15), dpi=200) + + ax3 = plt.subplot(3, 2, 1) + ax3.bar(range(1, 21), self.histogram_cmd) + ax3.set_xlabel( + "Number of consecutive lost command packets", fontsize="xx-large" + ) + ax3.set_ylabel("Number of occurences", fontsize="xx-large") + ax3.set_xticks(range(1, 21)) + + ax4 = plt.subplot(3, 2, 2) + ax4.bar(range(1, 21), self.histogram_sensor) + ax4.set_xlabel("Number of consecutive lost sensor packets", fontsize="xx-large") + ax4.set_ylabel("Number of occurences", fontsize="xx-large") + ax4.set_xticks(range(1, 21)) + + ax5 = plt.subplot(3, 2, 3) + ax5.plot(self.time_list, self.cmd_lost_list) + ax5.set_xlabel("time (s)", fontsize="xx-large") + ax5.set_ylabel("lost commands", fontsize="xx-large") + + ax6 = plt.subplot(3, 2, 4) + ax6.plot(self.time_list, self.sensor_lost_list) + ax6.set_xlabel("time (s)", fontsize="xx-large") + ax6.set_ylabel("lost sensors", fontsize="xx-large") + + ax7 = plt.subplot(3, 2, 5) + ax7.plot(self.time_list, self.cmd_ratio_list) + ax7.set_xlabel("time (s)", fontsize="xx-large") + ax7.set_ylabel("ratio command loss (%)", fontsize="xx-large") + + ax8 = plt.subplot(3, 2, 6) + ax8.plot(self.time_list, self.sensor_ratio_list) + ax8.set_xlabel("time (s)", fontsize="xx-large") + ax8.set_ylabel("ratio sensor loss (%)", fontsize="xx-large") + + if self.wifi_channel != "None": + plt.suptitle("Wifi statistics: " + self.wifi_channel, fontsize="xx-large") + + else: + plt.suptitle("Ethernet statistics", fontsize="xx-large") + plt.savefig(str(self._data_folder / "wifi_eth_stats.png")) + + def _dump_loop_duration(self): + """plotting the evolution of command loop duration""" + + plt.figure("loop duration", figsize=(20, 15), dpi=200) + anchored_text = AnchoredText( + "average duration: %f ms\nstandard deviation: %f ms" + % (np.mean(self.loop_duration), np.std(self.loop_duration)), + loc=2, + prop=dict(fontsize="xx-large"), + ) + ax9 = plt.subplot(1, 1, 1) + ax9.plot(self.loop_duration, ".") + 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(str(self._data_folder / "command_loop_duration.png")) + + +def communication_benchmark(name_interface): + # input paramters + n_udriver_controlled = 1 # Current number of controled drivers + cpt = 0 # Counting iterations. + dt = 0.001 #  Time step + state = 0 # State of the system (ready (1) or not (0)) + duration = 20 # Duration in seconds, init included + + # Check input parameters + if duration == 0: + print("-- Null duration selected, end of script --") + return + print("-- Start of example script --") + + #  Set the process to highest priority (from -20 highest to +20 lowest) + os.nice(-20) + + # Create the communication interface to the robot. + robot_if = mbs.MasterBoardInterface(name_interface) + # Initialization of the interface between the computer and the master board + robot_if.Init() + #  We enable each controler driver and its two associated motors + for i in range(n_udriver_controlled): + robot_if.GetDriver(i).motor1.SetCurrentReference(0) + robot_if.GetDriver(i).motor2.SetCurrentReference(0) + robot_if.GetDriver(i).motor1.Enable() + robot_if.GetDriver(i).motor2.Enable() + robot_if.GetDriver(i).EnablePositionRolloverError() + robot_if.GetDriver(i).SetTimeout(5) + robot_if.GetDriver(i).Enable() + + # Start sending the "get ready command" + last = clock() + while not robot_if.IsTimeout() and not robot_if.IsAckMsgReceived(): + if (clock() - last) > dt: + last = clock() + robot_if.SendInit() + + # contrary to c++, in python it is interesting to build arrays + # with connected motors indexes so we can simply go through them in main loop + # indexes of the motors on each connected slaves + motors_spi_connected_indexes = [] + # Check if the interface is properly connected. + if robot_if.IsTimeout(): + print("-- Timeout while waiting for ack. --") + else: + # fill the connected motors indexes array + for i in range(n_udriver_controlled): + if robot_if.GetDriver(i).is_connected: + # if slave i is connected then motors 2i and 2i+1 are potentially connected + motors_spi_connected_indexes.append(2 * i) + motors_spi_connected_indexes.append(2 * i + 1) + + latency_estimator = LatencyEstimator(duration, dt, robot_if.GetCmdPacketIndex()) + loop_duration_estimator = DurationEstimator() + benchmark_data = BenchmarkData() + + benchmark_data.initialize(dt, name_interface, str(robot_if.GetProtocolVersion())) + loop_duration_estimator.initialize() + while (not robot_if.IsTimeout()) and ( + clock() < duration + ): # Stop after 15 seconds (around 5 seconds are used at the start for calibration) + + latency_estimator.update_received_list( + robot_if.GetLastRecvCmdIndex(), robot_if.GetCmdPacketIndex() + ) + + # Fix the control period. + if (clock() - last) > dt: + last += dt + cpt += 1 + + # Read sensor data sent by the masterboard + robot_if.ParseSensorData() + + if state == 0: #  If the system is not ready + state = 1 + # for all motors on a connected slave + # Check if all motors are enabled and ready + for i in motors_spi_connected_indexes: + if not ( + robot_if.GetMotor(i).IsEnabled() + and robot_if.GetMotor(i).IsReady() + ): + state = 0 + # If the system is ready + else: + + # for all motors on a connected slave + for i in motors_spi_connected_indexes: + if i % 2 == 0 and robot_if.GetDriver(i // 2).error_code == 0xF: + # print("Transaction with SPI{} failed".format(i // 2)) + continue # user should decide what to do in that case, here we ignore that motor + if robot_if.GetMotor(i).IsEnabled(): + # Set reference currents, TODO create a proper controller here. + robot_if.GetMotor(i).SetCurrentReference(0) + + # Display system state and communication status, + # once every 100 iterations of the main loop + if cpt % 100 == 0: + print(chr(27) + "[2J") + robot_if.PrintMotors() + robot_if.PrintMotorDrivers() + robot_if.PrintStats() + sys.stdout.flush() # for Python 2, use print( .... , flush=True) for Python 3 + + benchmark_data.cmd_lost_list.append(robot_if.GetCmdLost()) + benchmark_data.sensor_lost_list.append(robot_if.GetSensorsLost()) + benchmark_data.cmd_ratio_list.append( + 100.0 * robot_if.GetCmdLost() / robot_if.GetCmdSent() + ) + benchmark_data.sensor_ratio_list.append( + 100.0 * robot_if.GetSensorsLost() / robot_if.GetSensorsSent() + ) + benchmark_data.time_list.append(clock()) + + latency_estimator.update_sent_list(robot_if.GetCmdPacketIndex()) + robot_if.SendCommand() # Send the reference currents to the master board + benchmark_data.loop_duration.append(loop_duration_estimator.get_duration()) + + robot_if.Stop() # Shut down the interface between the computer and the master board + if robot_if.IsTimeout(): + print("-- Masterboard timeout detected. --") + print( + "-- ", + "Either the masterboard has been shutdown, ", + "or there has been a connection issue with the cable/wifi." " --", + ) + + benchmark_data.latency = latency_estimator.get_latency_measurement() + # Plot histograms and graphs + for i in range(20): + benchmark_data.histogram_sensor.append(robot_if.GetSensorHistogram(i)) + benchmark_data.histogram_cmd.append(robot_if.GetCmdHistogram(i)) + + print("-- Dump the collected data --") + benchmark_data.dump_data() + + print("-- End of example script --") + + +def main(): + parser = argparse.ArgumentParser(description="Example masterboard use in python.") + parser.add_argument( + "-i", + "--interface", + required=True, + help='Name of the interface (use ifconfig in a terminal), for instance "enp1s0"', + ) + + communication_benchmark(parser.parse_args().interface) + + +if __name__ == "__main__": + main() diff --git a/sdk/master_board_sdk/benchmark/communication_benchmark_data_reader.py b/sdk/master_board_sdk/benchmark/communication_benchmark_data_reader.py new file mode 100644 index 00000000..5540c8fa --- /dev/null +++ b/sdk/master_board_sdk/benchmark/communication_benchmark_data_reader.py @@ -0,0 +1,318 @@ +# coding: utf8 + +import argparse +import math +import os +import sys +import time +from time import clock + +import libmaster_board_sdk_pywrap as mbs +import matplotlib.pyplot as plt +from matplotlib.offsetbox import AnchoredText +import numpy as np +import platform +import subprocess + + +def communication_benchmark(name_interface): + N_UDRIVER_CONTROLED = 1 # Current number of controled drivers + + cpt = 0 # Counting iterations. + dt = 0.001 #  Time step + state = 0 # State of the system (ready (1) or not (0)) + duration = 30 # Duration in seconds, init included + + cmd_lost_list = [] + sensor_lost_list = [] + cmd_ratio_list = [] + sensor_ratio_list = [] + time_list = [] + histogram_sensor = [] + histogram_cmd = [] + + if (duration == 0): + print("Null duration selected, end of script") + return + + sent_list = [0.0 for i in range(int(duration/dt)+2)] + received_list = [0.0 for i in range(int(duration/dt)+2)] + loop_duration = [] + + # contrary to c++, in python it is interesting to build arrays + # with connected motors indexes so we can simply go through them in main loop + motors_spi_connected_indexes = [] # indexes of the motors on each connected slaves + + print("-- Start of example script --") + + os.nice(-20) #  Set the process to highest priority (from -20 highest to +20 lowest) + robot_if = mbs.MasterBoardInterface(name_interface) + robot_if.Init() # Initialization of the interface between the computer and the master board + for i in range(N_UDRIVER_CONTROLED): #  We enable each controler driver and its two associated motors + robot_if.GetDriver(i).motor1.SetCurrentReference(0) + robot_if.GetDriver(i).motor2.SetCurrentReference(0) + robot_if.GetDriver(i).motor1.Enable() + robot_if.GetDriver(i).motor2.Enable() + robot_if.GetDriver(i).EnablePositionRolloverError() + robot_if.GetDriver(i).SetTimeout(5) + robot_if.GetDriver(i).Enable() + + last = clock() + prev_time = 0 + + while (not robot_if.IsTimeout() and not robot_if.IsAckMsgReceived()): + if ((clock() - last) > dt): + last = clock() + robot_if.SendInit() + + if robot_if.IsTimeout(): + print("Timeout while waiting for ack.") + else: + + # fill the connected motors indexes array + for i in range(N_UDRIVER_CONTROLED): + if robot_if.GetDriver(i).is_connected: + # if slave i is connected then motors 2i and 2i+1 are potentially connected + motors_spi_connected_indexes.append(2 * i) + motors_spi_connected_indexes.append(2 * i + 1) + + overflow_cmd_cpt = 0 + first_cmd_index = robot_if.GetCmdPacketIndex() + cmd_packet_index = first_cmd_index + last_cmd_packet_index = first_cmd_index + + while ((not robot_if.IsTimeout()) + and (clock() < duration)): # Stop after 15 seconds (around 5 seconds are used at the start for calibration) + + if (robot_if.GetLastRecvCmdIndex() > robot_if.GetCmdPacketIndex()): + last_recv_cmd_index = (overflow_cmd_cpt-1) * 65536 + robot_if.GetLastRecvCmdIndex() + else: + last_recv_cmd_index = overflow_cmd_cpt * 65536 + robot_if.GetLastRecvCmdIndex() + + if (last_recv_cmd_index >= first_cmd_index and received_list[last_recv_cmd_index-first_cmd_index] == 0): + received_list[last_recv_cmd_index-first_cmd_index] = clock() + + if ((clock() - last) > dt): + last += dt + cpt += 1 + + robot_if.ParseSensorData() # Read sensor data sent by the masterboard + + if (state == 0): #  If the system is not ready + state = 1 + + # for all motors on a connected slave + for i in motors_spi_connected_indexes: # Check if all motors are enabled and ready + if not (robot_if.GetMotor(i).IsEnabled() and robot_if.GetMotor(i).IsReady()): + state = 0 + + else: # If the system is ready + + # for all motors on a connected slave + for i in motors_spi_connected_indexes: + + if i % 2 == 0 and robot_if.GetDriver(i // 2).error_code == 0xf: + #print("Transaction with SPI{} failed".format(i // 2)) + continue #user should decide what to do in that case, here we ignore that motor + + if robot_if.GetMotor(i).IsEnabled(): + robot_if.GetMotor(i).SetCurrentReference(0) # Set reference currents + + if ((cpt % 100) == 0): # Display state of the system once every 100 iterations of the main loop + print(chr(27) + "[2J") + # To read IMU data in Python use robot_if.imu_data_accelerometer(i), robot_if.imu_data_gyroscope(i) + # or robot_if.imu_data_attitude(i) with i = 0, 1 or 2 + robot_if.PrintStats() + sys.stdout.flush() # for Python 2, use print( .... , flush=True) for Python 3 + + cmd_lost_list.append(robot_if.GetCmdLost()) + sensor_lost_list.append(robot_if.GetSensorsLost()) + cmd_ratio_list.append(100.*robot_if.GetCmdLost()/robot_if.GetCmdSent()) + sensor_ratio_list.append(100.*robot_if.GetSensorsLost()/robot_if.GetSensorsSent()) + time_list.append(clock()) + + current_time = clock() + + diff = robot_if.GetCmdPacketIndex() - last_cmd_packet_index + if diff < 0: + overflow_cmd_cpt += 1 + diff = 65536 + robot_if.GetCmdPacketIndex() - last_cmd_packet_index + cmd_packet_index += diff + last_cmd_packet_index = robot_if.GetCmdPacketIndex() + + robot_if.SendCommand() # Send the reference currents to the master board + + sent_list[cmd_packet_index-first_cmd_index] = current_time + if (prev_time != 0) : + loop_duration.append(1000 * (current_time - prev_time)) + prev_time = current_time + + robot_if.Stop() # Shut down the interface between the computer and the master board + + if robot_if.IsTimeout(): + print("Masterboard timeout detected.") + print("Either the masterboard has been shut down or there has been a connection issue with the cable/wifi.") + if cpt == 0: + print("-- End of example script --") + return + + + # creation of the folder where the graphs will be stored + if not os.path.isdir("../graphs"): + os.mkdir("../graphs") + + dir_name = time.strftime("%m") + time.strftime("%d") + time.strftime("%H") + time.strftime("%M") + time.strftime("%S") + if os.path.isdir("../graphs/" + dir_name): + count = 1 + while os.path.isdir("../graphs/" + dir_name + "-" + str(count)): + count += 1 + dir_name += "-" + str(count) + + + os.mkdir("../graphs/" + dir_name) + + + latency = [] + for i in range(1, len(sent_list)-1): + if (received_list[i] != 0 and sent_list[i] != 0): + latency.append(1000 * (received_list[i] - sent_list[i])) + else: + latency.append(0) # 0 means not sent or not received + + # computing avg and std for non zero values + nonzero = [latency[i] for i in np.nonzero(latency)[0]] + average = 0 + std = 0 + if len(nonzero) != 0: + average = np.mean(nonzero) + print("average latency: %f ms" %average) + std = np.std(nonzero) + print("standard deviation: %f ms" %std) + + + plt.figure("wifi-ethernet latency", figsize=(20,15), dpi=200) + + anchored_text = AnchoredText("average latency: %f ms\nstandard deviation: %f ms" %(average, std), loc=2, prop=dict(fontsize='xx-large')) + + if len(latency) > 5000: + ax1 = plt.subplot(2, 1, 1) + else: + ax1 = plt.subplot(1, 1, 1) + ax1.plot(latency, '.') + ax1.set_xlabel("index", fontsize='xx-large') + ax1.set_ylabel("latency (ms)", fontsize='xx-large') + ax1.add_artist(anchored_text) + + # plotting zoomed version to see pattern + if len(latency) > 5000: + ax2 = plt.subplot(2, 1, 2) + ax2.plot(latency, '.') + ax2.set_xlabel("index", fontsize='xx-large') + ax2.set_ylabel("latency (ms)", fontsize='xx-large') + ax2.set_xlim(len(latency)/2, len(latency)/2 + 2000) + ax2.set_ylim(-0.1, 2.1) + + if (name_interface[0] == 'w'): + freq = subprocess.check_output("iwlist " + name_interface +" channel | grep Frequency", shell = True) + channel = (str(freq).split('(')[1]).split(')')[0] + plt.suptitle("Wifi communication latency: " + channel, fontsize='xx-large') + + else : + plt.suptitle("Ethernet communication latency", fontsize='xx-large') + + + plt.savefig("../graphs/" + dir_name + '/' + dir_name + "-wifieth-latency.png") + + + #Plot histograms and graphs + for i in range(20): + histogram_sensor.append(robot_if.GetSensorHistogram(i)) + histogram_cmd.append(robot_if.GetCmdHistogram(i)) + + plt.figure("wifi-ethernet statistics", figsize=(20,15), dpi=200) + + ax3 = plt.subplot(3, 2, 1) + ax3.bar(range(1, 21), histogram_cmd) + ax3.set_xlabel("Number of consecutive lost command packets", fontsize='xx-large') + ax3.set_ylabel("Number of occurences", fontsize='xx-large') + ax3.set_xticks(range(1, 21)) + + ax4 = plt.subplot(3, 2, 2) + ax4.bar(range(1, 21), histogram_sensor) + ax4.set_xlabel("Number of consecutive lost sensor packets", fontsize='xx-large') + ax4.set_ylabel("Number of occurences", fontsize='xx-large') + ax4.set_xticks(range(1, 21)) + + ax5 = plt.subplot(3, 2, 3) + ax5.plot(time_list, cmd_lost_list) + ax5.set_xlabel("time (s)", fontsize='xx-large') + ax5.set_ylabel("lost commands", fontsize='xx-large') + + ax6 = plt.subplot(3, 2, 4) + ax6.plot(time_list, sensor_lost_list) + ax6.set_xlabel("time (s)", fontsize='xx-large') + ax6.set_ylabel("lost sensors", fontsize='xx-large') + + ax7 = plt.subplot(3, 2, 5) + ax7.plot(time_list, cmd_ratio_list) + ax7.set_xlabel("time (s)", fontsize='xx-large') + ax7.set_ylabel("ratio command loss (%)", fontsize='xx-large') + + ax8 = plt.subplot(3, 2, 6) + ax8.plot(time_list, sensor_ratio_list) + ax8.set_xlabel("time (s)", fontsize='xx-large') + ax8.set_ylabel("ratio sensor loss (%)", fontsize='xx-large') + + if (name_interface[0] == 'w'): + plt.suptitle("Wifi statistics: " + channel, fontsize='xx-large') + + else : + plt.suptitle("Ethernet statistics", fontsize='xx-large') + + + plt.savefig("../graphs/" + dir_name + '/' + dir_name + "-wifieth-stats.png") + + + # plotting the evolution of command loop duration + plt.figure("loop duration", figsize=(20, 15), dpi=200) + anchored_text = AnchoredText("average duration: %f ms\nstandard deviation: %f ms" %(np.mean(loop_duration), np.std(loop_duration)), loc=2, prop=dict(fontsize='xx-large')) + + ax9 = plt.subplot(1, 1, 1) + ax9.plot(loop_duration, '.') + 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("../graphs/" + dir_name + '/' + dir_name + "-command-loop-duration.png") + + + # creation of the text file with system info + text_file = open("../graphs/" + dir_name + '/' + dir_name + "-system-info.txt", 'w') + text_file.write("Current date and time: " + time.strftime("%c") + '\n') + text_file.write("Linux distribution: " + platform.linux_distribution()[0] + ' ' + platform.linux_distribution()[1] + ' ' + platform.linux_distribution()[2] + '\n') + text_file.write("Computer: " + os.uname()[1] + '\n') + text_file.write("Interface: " + name_interface + '\n') + if (name_interface[0] == 'w'): + text_file.write("Wifi channel: " + channel + '\n') + text_file.write("Protocol version: " + str(robot_if.GetProtocolVersion()) + '\n') + text_file.write("Script duration: " + str(duration) + '\n') + text_file.close() + + print("The graphs have been stored in 'master_board_sdk/graphs' with a text file containing the system info, in a folder named from the current date and time.") + print("-- End of example script --") + + +def main(): + parser = argparse.ArgumentParser(description='Example masterboard use in python.') + parser.add_argument('-i', + '--interface', + required=True, + help='Name of the interface (use ifconfig in a terminal), for instance "enp1s0"') + + communication_benchmark(parser.parse_args().interface) + + +if __name__ == "__main__": + main() diff --git a/sdk/master_board_sdk/benchmark/communication_benchmark_plotter.py b/sdk/master_board_sdk/benchmark/communication_benchmark_plotter.py new file mode 100644 index 00000000..72f314fe --- /dev/null +++ b/sdk/master_board_sdk/benchmark/communication_benchmark_plotter.py @@ -0,0 +1,196 @@ +#! /usr/bin/env python3 +""" +BSD 3-Clause License + +Copyright (c) 2019, LAAS-CNRS, New York University and Max Planck +Gesellschaft. +All rights reserved. +""" +import argparse +from pathlib import Path +import pandas +import yaml +import pyaml +import numpy as np +from matplotlib.offsetbox import AnchoredText +from matplotlib import pyplot as plt + + +def main(path_to_data_folder_in): + path_to_data_folder = Path(path_to_data_folder_in) + assert path_to_data_folder.exists() + + # display the metadata + print("Metada:") + print("---") + with open(path_to_data_folder / "meta_data.yaml") as yaml_file: + metadata = yaml.safe_load(yaml_file) + print(pyaml.dump(metadata)) + print("---") + print() + + # Read the data + data = pandas.read_csv(path_to_data_folder / "data.dat", delimiter=" ") + time_list = data["time_list"].to_list() + cmd_lost_list = data["cmd_lost_list"].to_list() + sensor_lost_list = data["sensor_lost_list"].to_list() + 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" + ].to_list() + # + hitograms = pandas.read_csv(path_to_data_folder / "histograms.dat", delimiter=" ") + histogram_sensor = hitograms["histogram_sensor"].to_list() + histogram_cmd = hitograms["histogram_cmd"].to_list() + + ############ + + print("-- Dump latency graph --") + plt.figure("wifi-ethernet latency", figsize=(20, 15), dpi=200) + anchored_text = AnchoredText( + "average latency: %f ms\nstandard deviation: %f ms" + % ( + metadata["latency_statistics"]["average"], + metadata["latency_statistics"]["standard_deviation"], + ), + loc=2, + prop=dict(fontsize="xx-large"), + ) + if len(latency) > 5000: + ax1 = plt.subplot(2, 1, 1) + else: + ax1 = plt.subplot(1, 1, 1) + ax1.plot(latency, ".") + ax1.set_xlabel("index", fontsize="xx-large") + ax1.set_ylabel("latency (ms)", fontsize="xx-large") + ax1.add_artist(anchored_text) + if len(latency) > 5000: + ax2 = plt.subplot(2, 1, 2) + ax2.plot(latency, ".") + ax2.set_xlabel("index", fontsize="xx-large") + ax2.set_ylabel("latency (ms)", fontsize="xx-large") + ax2.set_ylim(-0.1, 2.1) + if metadata["control"]["wifi_channel"] != "None": + plt.suptitle( + "Wifi communication latency: " + metadata["control"]["wifi_channel"], + fontsize="xx-large", + ) + else: + plt.suptitle("Ethernet communication latency", fontsize="xx-large") + plt.savefig(path_to_data_folder / "wifi_eth_latency.png") + + ############ + + print("Plot histograms and graphs") + plt.figure("wifi-ethernet statistics", figsize=(20, 15), dpi=200) + ax3 = plt.subplot(3, 2, 1) + ax3.bar(range(1, 21), histogram_cmd) + ax3.set_xlabel("Number of consecutive lost command packets", fontsize="xx-large") + ax3.set_ylabel("Number of occurences", fontsize="xx-large") + ax3.set_xticks(range(1, 21)) + ax4 = plt.subplot(3, 2, 2) + ax4.bar(range(1, 21), histogram_sensor) + ax4.set_xlabel("Number of consecutive lost sensor packets", fontsize="xx-large") + ax4.set_ylabel("Number of occurences", fontsize="xx-large") + ax4.set_xticks(range(1, 21)) + ax5 = plt.subplot(3, 2, 3) + ax5.plot(time_list, cmd_lost_list) + ax5.set_xlabel("time (s)", fontsize="xx-large") + ax5.set_ylabel("lost commands", fontsize="xx-large") + ax6 = plt.subplot(3, 2, 4) + ax6.plot(time_list, sensor_lost_list) + ax6.set_xlabel("time (s)", fontsize="xx-large") + ax6.set_ylabel("lost sensors", fontsize="xx-large") + ax7 = plt.subplot(3, 2, 5) + ax7.plot(time_list, cmd_ratio_list) + ax7.set_xlabel("time (s)", fontsize="xx-large") + ax7.set_ylabel("ratio command loss (%)", fontsize="xx-large") + ax8 = plt.subplot(3, 2, 6) + ax8.plot(time_list, sensor_ratio_list) + ax8.set_xlabel("time (s)", fontsize="xx-large") + ax8.set_ylabel("ratio sensor loss (%)", fontsize="xx-large") + if metadata["control"]["wifi_channel"] != "None": + plt.suptitle( + "Wifi statistics: " + metadata["control"]["wifi_channel"], + fontsize="xx-large", + ) + else: + plt.suptitle("Ethernet statistics", fontsize="xx-large") + plt.savefig(path_to_data_folder / "wifi_eth_stats.png") + + ######### + + 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" + % (np.mean(loop_duration_s), np.std(loop_duration_s)), + loc=2, + prop=dict(fontsize="xx-large"), + ) + ax9 = plt.subplot(1, 1, 1) + ax9.plot(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 / "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.") + + def dir_path(string): + if Path(string).is_dir(): + return Path(string).resolve() + else: + raise NotADirectoryError(string) + + parser.add_argument( + "path_to_data_folder", + metavar="path_to_data_folder", + type=dir_path, + help="The path to the benchmark data.", + ) + + args = parser.parse_args() + main(args.path_to_data_folder) diff --git a/sdk/master_board_sdk/include/master_board_sdk/benchmark/communication_data.hpp b/sdk/master_board_sdk/include/master_board_sdk/benchmark/communication_data.hpp new file mode 100644 index 00000000..fd4887bd --- /dev/null +++ b/sdk/master_board_sdk/include/master_board_sdk/benchmark/communication_data.hpp @@ -0,0 +1,235 @@ +/** + * BSD 3-Clause License + * + * Copyright (c) 2019, LAAS-CNRS, New York University and Max Planck + * Gesellschaft. + * All rights reserved. + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include "master_board_sdk/benchmark/execute_bash_command.hpp" +#include "master_board_sdk/benchmark/real_time_timer.hpp" + +namespace master_board_sdk +{ +namespace benchmark +{ +class CommunicationData +{ +public: + CommunicationData() + { + } + + ~CommunicationData() + { + printf("Delete CommunicationData!\n"); + fflush(stdout); + } + + void initialize(const int& size, + const double& dt, + const std::string& network_interface, + const int& protocol_version, + 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); + histogram_cmd_.resize(20, 0); + + // Data folder to dump the data/figures. + data_folder_ = get_log_dir("master_board_sdk"); + + // Metadata and statistics. + 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( + "iwlist " + network_interface_ + " channel | grep Frequency"); + } + else + { + wifi_channel_ = "None"; + } + distribution_ = execute_bash_command("uname -a"); + protocol_version_ = protocol_version; + latence_average_ = 0.0; + latence_stdev_ = 0.0; + } + + void get_latency_statistics(const std::deque& latency, + double& average, + double& stdev) + { + std::vector latency_pruned; + latency_pruned.reserve(latency.size()); + for (std::size_t i = 0; i < latency.size(); ++i) + { + if (latency[i] > 0.0) + { + latency_pruned.push_back(latency[i]); + } + } + + std::vector& v = latency_pruned; + double sum = std::accumulate(v.begin(), v.end(), 0.0); + average = sum / static_cast(v.size()); + + std::vector diff(v.size()); + std::transform(v.begin(), + v.end(), + diff.begin(), + std::bind2nd(std::minus(), average)); + double sq_sum = + std::inner_product(diff.begin(), diff.end(), diff.begin(), 0.0); + stdev = std::sqrt(sq_sum / static_cast(v.size())); + } + + void dump_data() + { + // Writing in files + std::ofstream myfile; + + // Storing data + myfile.open(data_folder_ + "/" + "data.dat"); + myfile << "time_list " + << "cmd_lost_list " + << "sensor_lost_list " + << "cmd_ratio_list " + << "sensor_ratio_list " + << "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] << " " // + << ctrl_loop_duration_s_[i] << " " // + << last_rcv_index_[i] << " " // + << cmd_index_[i] << " " // + << std::endl; + } + myfile.close(); + + myfile.open(data_folder_ + "/" + "latency.dat"); + myfile << "latency" << std::endl; + for (std::size_t i = 0; i < latency_.size(); ++i) + { + myfile << latency_[i] << std::endl; + } + myfile.close(); + + // Storing the Histograms + myfile.open(data_folder_ + "/" + "histograms.dat"); + myfile << "histogram_sensor histogram_cmd" << std::endl; + for (std::size_t i = 0; i < histogram_sensor_.size(); ++i) + { + myfile << histogram_sensor_[i] << " " << histogram_cmd_[i] + << std::endl; + } + myfile.close(); + + // metadata + get_latency_statistics(latency_, latence_average_, latence_stdev_); + myfile.open(data_folder_ + "/" + "meta_data.yaml"); + myfile << "control:" << std::endl + << " controller_period: " << 0.001 << std::endl + << " network_interface: " << network_interface_ << std::endl + << " 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; + myfile.close(); + printf("Data dumped in %s\n", data_folder_.c_str()); + fflush(stdout); + } + + std::string get_log_dir(const std::string& app_name) + { + std::string home_dir = std::string(getenv("HOME")) + "/"; + date_ = RtTimer::get_current_date_str(); + std::string log_dir = home_dir + app_name + "/" + date_ + "/"; + + boost::filesystem::create_directory(home_dir + app_name); + boost::filesystem::create_directory(log_dir); + + return log_dir; + } + + // Histograms + std::vector histogram_sensor_; + std::vector histogram_cmd_; + + // Data + std::vector last_rcv_index_; + std::vector cmd_index_; + std::vector cmd_lost_list_; + std::vector sensor_lost_list_; + std::vector time_list_; + std::vector cmd_ratio_list_; + std::vector sensor_ratio_list_; + std::deque latency_; + std::vector loop_duration_s_; + std::vector ctrl_loop_duration_s_; + + // data_folder + std::string data_folder_; + + // metadata controller + double control_period_; + std::string network_interface_; + std::string wifi_channel_; + double experiment_duration_; + + // metadata latency + double latence_average_; + double latence_stdev_; + + // metadata system + std::string wifi_card_name_; + double distance_; + std::string distribution_; + int protocol_version_; + std::string date_; +}; + +} // namespace benchmark +} // namespace master_board_sdk diff --git a/sdk/master_board_sdk/include/master_board_sdk/benchmark/execute_bash_command.hpp b/sdk/master_board_sdk/include/master_board_sdk/benchmark/execute_bash_command.hpp new file mode 100644 index 00000000..d12869fa --- /dev/null +++ b/sdk/master_board_sdk/include/master_board_sdk/benchmark/execute_bash_command.hpp @@ -0,0 +1,41 @@ +/** + * BSD 3-Clause License + * + * Copyright (c) 2019, LAAS-CNRS, New York University and Max Planck + * Gesellschaft. + * All rights reserved. + */ + +#pragma once + +#include +#include + +namespace master_board_sdk +{ +namespace benchmark +{ +std::string execute_bash_command(const std::string& cmd) +{ + auto pPipe = ::popen(cmd.c_str(), "r"); + if (pPipe == nullptr) + { + throw std::runtime_error("Cannot open pipe"); + } + + std::array buffer; + + std::string result; + + while (!std::feof(pPipe)) + { + auto bytes = std::fread(buffer.data(), 1, buffer.size(), pPipe); + result.append(buffer.data(), bytes); + } + + ::pclose(pPipe); + return result; +} + +} // namespace benchmark +} // namespace master_board_sdk diff --git a/sdk/master_board_sdk/include/master_board_sdk/benchmark/latency_estimator.hpp b/sdk/master_board_sdk/include/master_board_sdk/benchmark/latency_estimator.hpp new file mode 100644 index 00000000..46279365 --- /dev/null +++ b/sdk/master_board_sdk/include/master_board_sdk/benchmark/latency_estimator.hpp @@ -0,0 +1,134 @@ +/** + * BSD 3-Clause License + * + * Copyright (c) 2019, LAAS-CNRS, New York University and Max Planck + * Gesellschaft. + * All rights reserved. + */ + +#pragma once + +#include +#include "master_board_sdk/benchmark/real_time_timer.hpp" + +namespace master_board_sdk +{ +namespace benchmark +{ +class LatencyEstimator +{ +public: + LatencyEstimator() + { + start_ = false; + overflow_cmd_cpt_ = 0; + first_cmd_index_ = 0; + cmd_packet_index_ = 0; + last_cmd_packet_index_ = 0; + + sent_list_.clear(); + received_list_.clear(); + latency_.clear(); + } + + ~LatencyEstimator() + { + printf("Delete LatencyEstimator!\n"); + fflush(stdout); + } + + void initialize(int last_recv_cmd_index) + { + overflow_cmd_cpt_ = 0; + first_cmd_index_ = last_recv_cmd_index; + cmd_packet_index_ = last_recv_cmd_index; + last_cmd_packet_index_ = last_recv_cmd_index; + start_ = true; + } + + void update_received_list(int last_recv_cmd_index, + int current_cmd_packet_index) + { + int index = 0; + if (last_recv_cmd_index > current_cmd_packet_index) + { + index = (overflow_cmd_cpt_ - 1) * 65536 + last_recv_cmd_index; + } + else + { + index = overflow_cmd_cpt_ * 65536 + last_recv_cmd_index; + } + index -= first_cmd_index_; + + if (index >= 0) + { + // We make sure to resize the deque properly before affecting the + // values. + while (index >= static_cast(received_list_.size())) + { + received_list_.push_back(0.0); + } + if (received_list_[index] == 0) + { + received_list_[index] = rt_clock_sec(); + } + } + } + + void update_sent_list(int current_cmd_packet_index) + { + int diff = current_cmd_packet_index - last_cmd_packet_index_; + if (diff < 0) + { + overflow_cmd_cpt_ += 1; + diff = 65536 + current_cmd_packet_index - last_cmd_packet_index_; + } + cmd_packet_index_ += diff; + last_cmd_packet_index_ = current_cmd_packet_index; + int index = cmd_packet_index_ - first_cmd_index_; + assert(index >= 0); + + // We make sure to resize the deque properly before affecting the + // values. + while (index >= static_cast(sent_list_.size())) + { + sent_list_.push_back(0.0); + } + sent_list_[index] = rt_clock_sec(); + } + + const std::deque& get_latency_measurement() + { + latency_ = std::deque( + std::min(sent_list_.size(), received_list_.size()), 0.0); + + for (std::size_t i = 0; + ((i < sent_list_.size()) && (i < received_list_.size()) && + (i < latency_.size())); + ++i) + { + if (received_list_[i] != 0 && sent_list_[i] != 0) + { + latency_[i] = 1000 * (received_list_[i] - sent_list_[i]); + } + else + { + latency_[i] = 0.0; // 0 means not sent or not received + } + } + return latency_; + } + +private: + bool start_; + int overflow_cmd_cpt_; + int first_cmd_index_; + int cmd_packet_index_; + int last_cmd_packet_index_; + std::deque sent_list_; + std::deque received_list_; + std::deque latency_; +}; + +} // namespace benchmark +} // namespace master_board_sdk diff --git a/sdk/master_board_sdk/include/master_board_sdk/benchmark/pd.hpp b/sdk/master_board_sdk/include/master_board_sdk/benchmark/pd.hpp new file mode 100644 index 00000000..6666a922 --- /dev/null +++ b/sdk/master_board_sdk/include/master_board_sdk/benchmark/pd.hpp @@ -0,0 +1,59 @@ +/** + * BSD 3-Clause License + * + * Copyright (c) 2019, LAAS-CNRS, New York University and Max Planck + * Gesellschaft. + * All rights reserved. + */ + +#pragma once + +namespace master_board_sdk +{ +namespace benchmark +{ +class PD +{ +public: + PD() + { + position_error_ = 0.0; + force_ = 0.0; + set_gains(0.0, 0.0); + } + + PD(double kp, double kd) + { + position_error_ = 0.0; + force_ = 0.0; + set_gains(kp, kd); + } + + void set_gains(double kp, double kd) + { + kp_ = kp; + kd_ = kd; + } + + double compute(const double& position, + const double& velocity, + const double& position_target) + { + position_error_ = position_target - position; + force_ = position_error_ * kp_ - velocity * kd_; + return force_; + } + +private: + double kp_; + double kd_; + double integral_; + double integral_saturation_; + + // tmp var + double position_error_; + double force_; +}; + +} // namespace benchmark +} // namespace master_board_sdk diff --git a/sdk/master_board_sdk/include/master_board_sdk/benchmark/real_time_thread.hpp b/sdk/master_board_sdk/include/master_board_sdk/benchmark/real_time_thread.hpp new file mode 100644 index 00000000..1c755201 --- /dev/null +++ b/sdk/master_board_sdk/include/master_board_sdk/benchmark/real_time_thread.hpp @@ -0,0 +1,381 @@ +/** + * BSD 3-Clause License + * + * Copyright (c) 2019, New York University and Max Planck Gesellschaft. + * All rights reserved. + */ + +#pragma once + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + +namespace master_board_sdk +{ +namespace benchmark +{ +/** + * @brief This class is a data structure allowing the user to share + * configurations among threads. These parameter allows you to generate + * real threads in xenomai and rt_preempt. The same code is compatible with + * Mac and ubuntu but will run non-real time threads. + * + * warning : initial version, copy pasted from : + * https://wiki.linuxfoundation.org/realtime/documentation/howto/applications/application_base + * I did not study things now, so this likely needs improvement (alternative: + * https://rt.wiki.kernel.org/index.php/Threaded_RT-application_with_memory_locking_and_stack_handling_example) + * note: if failed as mlockall, run executable with sudo or be part of the + * real_time group or xenomai group. + */ +class RealTimeThreadParameters +{ +public: + /** + * @brief Construct a new RealTimeThreadParameters object + */ + RealTimeThreadParameters() + { + keyword_ = "real_time_thread"; + priority_ = 80; + stack_size_ = 50 * PTHREAD_STACK_MIN; + cpu_id_.clear(); + delay_ns_ = 0; + block_memory_ = true; + cpu_dma_latency_ = 0; + } + /** + * @brief Destroy the RealTimeThreadParameters object + */ + ~RealTimeThreadParameters() + { + } + +public: + /** @brief Used in xenomai to define the thread id. */ + std::string keyword_; + + /** @brief Defines the thread priority from 0 to 100. */ + int priority_; + + /** @brief Define the stack size. */ + int stack_size_; + + /** @brief Define the cpu affinity. Which means on which cpu(s) the thread + * is going to run */ + std::vector cpu_id_; + + /** @brief indicate on which cpu the thread will run (xenomai only) */ + int dedicated_cpu_id_; + + /** @brief @todo Unknow Xenomai parameter */ + int delay_ns_; + + /** @brief Defines if the thread should block the memory in a "page" or if + * several pages can be use. Switching memory page is time consumming and + * a non real time operation. */ + bool block_memory_; + + /** @brief Maximum desired latency of the CPU in microseconds. Set to 0 to + * get best real-time performance. Set to any negative value if you do not + * want the thread to change the CPU latency. */ + int cpu_dma_latency_; +}; + +/** + * @brief This class allows you to spawn thread. Its parameter are defined + * above. + */ +class RealTimeThread +{ +public: + /** + * @brief Construct a new ThreadInfo object + */ + RealTimeThread() + { + std::string rt_preempt_error_message_ = + "NOTE: This program must be executed with special permission to " + "get " + "the required real time permissions.\n" + "Either use sudo or be part of the \'realtime\' group" + "Aborting thread creation."; + thread_.reset(nullptr); + } + + /** + * @brief We do not allow copies of this object + */ + RealTimeThread(const benchmark::RealTimeThread& other) = delete; + + /** + * @brief Destroy the RealTimeThread object. + */ + ~RealTimeThread() + { + join(); + thread_.reset(nullptr); + { + printf("Delete RealTimeThread!\n"); + fflush(stdout); + } + } + + /** + * @brief create_realtime_thread spawns a real time thread if the OS allows + * it. + * @param[in] thread_function: the executing function for the thread. + * @param[in] args: arguments to be passed to the thread. + * @return the error code. + */ + int create_realtime_thread(void* (*thread_function)(void*), void* args) + { + if (thread_ != nullptr) + { + printf("Thread already running"); + } + + if (parameters_.cpu_dma_latency_ >= 0) + { + set_cpu_dma_latency(parameters_.cpu_dma_latency_); + } + + thread_.reset(new pthread_t()); + + if (parameters_.block_memory_) + { + block_memory(); + } + + struct sched_param param; + pthread_attr_t attr; + int ret; + + ret = pthread_attr_init(&attr); + if (ret) + { + printf("%s %d\n", + ("init pthread attributes failed. Ret=" + + rt_preempt_error_message_) + .c_str(), + ret); + return ret; + } + + /* Set a specific stack size */ + ret = pthread_attr_setstacksize(&attr, parameters_.stack_size_); + if (ret) + { + printf("%s %d\n", + ("pthread setstacksize failed. Ret=" + + rt_preempt_error_message_) + .c_str(), + ret); + return ret; + } + + /* Set scheduler policy and priority of pthread */ + ret = pthread_attr_setschedpolicy(&attr, SCHED_FIFO); + // ret = pthread_attr_setschedpolicy(&attr, SCHED_RR); // WARNING LAAS + // is using this one!!!! + if (ret) + { + printf("%s %d\n", + ("pthread setschedpolicy failed. Ret=" + + rt_preempt_error_message_) + .c_str(), + ret); + return ret; + } + param.sched_priority = parameters_.priority_; + ret = pthread_attr_setschedparam(&attr, ¶m); + if (ret) + { + printf("%s %d\n", + ("pthread setschedparam failed. Ret=" + + rt_preempt_error_message_) + .c_str(), + ret); + return ret; + } + /* Use scheduling parameters of attr */ + ret = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED); + if (ret) + { + printf("%s %d\n", + ("pthread setinheritsched failed. Ret=" + + rt_preempt_error_message_) + .c_str(), + ret); + return ret; + } + + /* Create a pthread with specified attributes */ + ret = pthread_create(thread_.get(), &attr, thread_function, args); + if (ret) + { + printf("%s %d\n", + ("create pthread failed. Ret=" + rt_preempt_error_message_) + .c_str(), + ret); + return ret; + } + + if (parameters_.cpu_id_.size() > 0) + { + cpu_set_t cpuset; + CPU_ZERO(&cpuset); + for (unsigned i = 0; i < parameters_.cpu_id_.size(); ++i) + { + CPU_SET(parameters_.cpu_id_[i], &cpuset); + } + ret = pthread_setaffinity_np(*thread_, sizeof(cpu_set_t), &cpuset); + if (ret) + { + printf("%s %d\n", + ("Associate thread to a specific cpu failed. Ret=" + + rt_preempt_error_message_) + .c_str(), + ret); + } + + int get_aff_error = 0; + get_aff_error = + pthread_getaffinity_np(*thread_, sizeof(cpu_set_t), &cpuset); + if (get_aff_error) + { + printf("%s %d\n", + ("Check the thread cpu affinity failed. Ret=" + + rt_preempt_error_message_) + .c_str(), + ret); + } + printf("Set returned by pthread_getaffinity_np() contained: "); + for (unsigned j = 0; j < CPU_SETSIZE; j++) + { + if (CPU_ISSET(j, &cpuset)) + { + printf("CPU %d, ", j); + } + } + printf("\n"); + } + + std::vector cpu_affinities(1, 0); + fix_current_process_to_cpu(cpu_affinities, getpid()); + return ret; + } + + /** + * @brief join join the real time thread + * @return the error code. + */ + int join() + { + int ret = 0; + if (thread_ != nullptr) + { + /* Join the thread and wait until it is done */ + ret = pthread_join(*thread_, nullptr); + if (ret) + { + printf("join pthread failed.\n"); + } + thread_.reset(nullptr); + } + return ret; + } + + /** + * @brief block_memory block the current and futur memory pages. + * see + * https://wiki.linuxfoundation.org/realtime/documentation/howto/applications/memory#memory-locking + * for further explanation. + */ + void block_memory() + { + /* Lock memory */ + if (mlockall(MCL_CURRENT | MCL_FUTURE) == -1) + { + printf("mlockall failed: %s\n", strerror(errno)); + exit(-2); + } + } + + bool fix_current_process_to_cpu(std::vector& cpu_affinities, int pid) + { + if (cpu_affinities.size() > 0) + { + cpu_set_t mask; + CPU_ZERO(&mask); + for (unsigned i = 0; i < cpu_affinities.size(); ++i) + { + if (cpu_affinities[i] > -1) + { + CPU_SET(cpu_affinities[i], &mask); + } + } + pid_t process_pid = static_cast(pid); + int ret = sched_setaffinity(process_pid, sizeof(mask), &mask); + if (ret) + { + printf("sched_setaffinity failed. Ret= %d\n", ret); + return false; + } + else + { + return true; + } + } + printf("fix_current_process_to_cpu: Nothing to be done.\n"); + return true; + } + + bool set_cpu_dma_latency(int max_latency_us) + { + int fd; + + fd = open("/dev/cpu_dma_latency", O_WRONLY); + if (fd < 0) + { + perror("open /dev/cpu_dma_latency"); + return false; + } + if (write(fd, &max_latency_us, sizeof(max_latency_us)) != + sizeof(max_latency_us)) + { + perror("write to /dev/cpu_dma_latency"); + return false; + } + + return true; + } + + /** + * @brief Paramter of the real time thread + */ + RealTimeThreadParameters parameters_; + +private: + std::unique_ptr thread_; + + std::string rt_preempt_error_message_; +}; + +} // namespace benchmark +} // namespace master_board_sdk diff --git a/sdk/master_board_sdk/include/master_board_sdk/benchmark/real_time_timer.hpp b/sdk/master_board_sdk/include/master_board_sdk/benchmark/real_time_timer.hpp new file mode 100644 index 00000000..a8501ccf --- /dev/null +++ b/sdk/master_board_sdk/include/master_board_sdk/benchmark/real_time_timer.hpp @@ -0,0 +1,445 @@ +/** + * @file timer.hpp + * @author Maximilien Naveau (maximilien.naveau@gmail.com) + * license License BSD-3-Clause + * @copyright Copyright (c) 2019, New York University and Max Planck + * Gesellschaft. + * @date 2019-05-22 + * + * @brief Some tools to measure (ellapsed) time. + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + + +namespace master_board_sdk +{ +namespace benchmark +{ +/** + * @brief Simple renaming to get the number of days passed out of the date. + */ +typedef std::chrono::duration< + int, + std::ratio_multiply >::type> + days; + +/** + * @brief The timer class is a simple time measurement class that measure + * between tic and tac and with a memory buffer of a certain size. + */ +class RtTimer +{ +public: + /** + * @brief timer constructor + */ + RtTimer() + { + // initialize the tic and tac times by the current time + tic_time_ = std::numeric_limits::quiet_NaN(); + // initialize the memory buffer size, allocate memory and set counter to + // zero. + set_memory_size(60000); + // default name + name_ = "timer"; + // reset all the statistic memebers + min_elapsed_time_ = std::numeric_limits::infinity(); + max_elapsed_time_ = -std::numeric_limits::infinity(); + avg_elapsed_time_ = 0.0; + second_moment_elapsed_time_ = 0.0; + count_ = 0; + } + + /** + * @brief tic measures the time when it is called. This is to be used with + * the tac method that will return the time elapsed between tic and tac. + */ + void tic() + { + // get the current time + tic_time_ = RtTimer::get_current_time_sec(); + } + + /** + * @brief tac is to be used after tic has been called. + * @return the duration in seconds between the call of tic() and the call of + * tac(). if tic() has not been called previously this will return nan + */ + double tac() + { + double tac_time = RtTimer::get_current_time_sec(); + double time_interval = tac_time - tic_time_; + + log_time_interval(time_interval); + + return time_interval; + } + + /** + * @brief this is like a tac() followed by a tic(), making sure the + * previous tac_time becomes the tic_time + */ + double tac_tic() + { + double tac_time = RtTimer::get_current_time_sec(); + double time_interval = tac_time - tic_time_; + + log_time_interval(time_interval); + + tic_time_ = tac_time; + + return time_interval; + } + + /** + * @brief Save the time interval measured + * + * @param time_interval + */ + void log_time_interval(double time_interval) + { + if (std::isnan(time_interval)) return; + + // Only store into the buffer if the buffer is non-zero. + if (memory_buffer_size_ != 0) + { + // check if the buffer is full + if (count_ >= time_measurement_buffer_.size()) + { + time_measurement_buffer_.pop_front(); + time_measurement_buffer_.push_back(time_interval); + } + else + { + // save the current time elapsed + time_measurement_buffer_[count_] = time_interval; + } + } + + // increase the count + ++count_; + // compute some statistics + min_elapsed_time_ = time_interval < min_elapsed_time_ + ? time_interval + : min_elapsed_time_; + max_elapsed_time_ = time_interval > max_elapsed_time_ + ? time_interval + : max_elapsed_time_; + avg_elapsed_time_ = + (double(count_ - 1) * avg_elapsed_time_ + time_interval) / + double(count_); + second_moment_elapsed_time_ = + (double(count_ - 1) * second_moment_elapsed_time_ + + time_interval * time_interval) / + double(count_); + } + + /** + * IOSTREAM functions + */ + + /** + * @brief dump_tic_tac_measurements writes in a file the time elapsed + * between every tick + * @param file_name is the path to the file. + */ + void dump_measurements(std::string file_name) const + { + try + { + std::ofstream log_file(file_name, std::ios::binary | std::ios::out); + log_file.precision(10); + for (unsigned i = 0; + i < std::min(count_, (unsigned long)memory_buffer_size_); + ++i) + { + log_file << i << " " << time_measurement_buffer_[i] + << std::endl; + } + log_file.flush(); + log_file.close(); + } + catch (...) + { + printf( + "fstream Error in dump_tic_tac_measurements(): " + "no time measurment saved\n"); + } + } + + /** + * @brief print_statistics display in real time the statistics of the time + * measurements acquiered so far. + */ + void print_statistics() const + { + printf("%s --------------------------------\n", name_.c_str()); + printf( + "count: %ld\n" + "min_elapsed_sec: %f\n" + "max_elapsed_sec: %f\n" + "avg_elapsed_sec: %f\n" + "std_dev_elapsed_sec: %f\n", + count_, + get_min_elapsed_sec(), + get_max_elapsed_sec(), + get_avg_elapsed_sec(), + get_std_dev_elapsed_sec()); + printf("--------------------------------------------\n"); + } + + /** + * SETTERS + */ + + /** + * @brief set_memory_size sets the buffer size. It resets all value of the + * buffer to zero. + * !! WARNING non real time method. !! + * @param memory_buffer_size is the size use to reset the size of the + */ + void set_memory_size(const unsigned memory_buffer_size) + { + count_ = 0; + memory_buffer_size_ = memory_buffer_size; + time_measurement_buffer_.resize(memory_buffer_size_, 0.0); + } + + /** + * @brief set_name modify the name of the object for display purposes. + * @param name is the new name of the object. + */ + void set_name(std::string name) + { + name_ = name; + } + + /** + * GETTERS + */ + + /** + * @brief get_min_elapsed_sec + * @return a copy of the minimum elapsed times + */ + double get_min_elapsed_sec() const + { + return min_elapsed_time_; + } + + /** + * @brief get_max_elapsed_sec + * @return a copy of the maximum elapsed times + */ + double get_max_elapsed_sec() const + { + return max_elapsed_time_; + } + + /** + * @brief get_avg_elapsed_sec + * @return a copy of the average elapsed time + */ + double get_avg_elapsed_sec() const + { + return avg_elapsed_time_; + } + + /** + * @brief get_std_dev_elapsed_sec + * @return a copy of the standard deviation of the elapsed times + */ + double get_std_dev_elapsed_sec() const + { + return std::sqrt(second_moment_elapsed_time_ - + avg_elapsed_time_ * avg_elapsed_time_); + } + +protected: + /** + * @brief tic_time_ time at which tic() was called + */ + double tic_time_; + + /** + * @brief time_measurement_buffer_ this is a chained list of double + */ + std::deque time_measurement_buffer_; + + /** + * @brief count_time_buffer_ is a counter that manages the + * time_measurement_buffer_ fill in. + */ + long unsigned count_; + + /** + * @brief memory_buffer_size_ is the max size of the memory buffer. + */ + unsigned memory_buffer_size_; + + /** + * @brief min_elapsed_time_ is the minimum measured elapsed time + */ + double min_elapsed_time_; + + /** + * @brief max_elapsed_time_ is the maximum measured elapsed time + */ + double max_elapsed_time_; + + /** + * @brief avg_elapsed_time_ is the average measured elapsed time + */ + double avg_elapsed_time_; + + /** + * @brief avg_elapsed_time_ is the second moment measured elapsed + * time + */ + double second_moment_elapsed_time_; + + /** + * @brief name_ of the timer object + */ + std::string name_; + + /** + * Some utilities + */ +public: + /** + * @brief get_current_time_sec gives the current time in double and in + * seconds + * @return + */ + static double get_current_time_sec() + { + struct timespec now; + clock_gettime(CLOCK_REALTIME, &now); + return static_cast(now.tv_sec) + + static_cast(now.tv_nsec) / 1e9; + } + + /** + * @brief get_current_time_ms gives the current time in double and in + * milli seconds + * @return + */ + static double get_current_time_ms() + { + return 1e3 * get_current_time_sec(); + } + + /** + * @brief puts the current thread to sleep for the duration + * of "sleep_duration_us" micro-seconds. + * @param sleep_time_us is the sleeping duration asked in micro-seconds. + * @return 0 on success, error code otherwise + */ + static int sleep_microseconds(int sleep_duration_us) + { + usleep(sleep_duration_us); + return 0; + } + + /** + * @brief timespec_add_sec posix type of a date in time. + * @param date_spec is the date to be changed + * @param duration_sec the duration to be added to "t" in seconds + */ + static void timespec_add_sec(struct timespec& date_spec, + const double duration_sec) + { + double total_time_sec = duration_sec + + static_cast(date_spec.tv_nsec) / 1e9 + + static_cast(date_spec.tv_sec); + sec_to_timespec(total_time_sec, date_spec); + } + + /** + * @brief sleep_sec puts the current thread to sleep for the duration + * of "sleep_time_sec" seconds. + * @param sleep_time_sec is the sleeping duration asked in seconds. + */ + static void sleep_sec(const double& sleep_time_sec) + { + struct timespec abs_target_time; + clock_gettime(CLOCK_REALTIME, &abs_target_time); + timespec_add_sec(abs_target_time, sleep_time_sec); + clock_nanosleep( + CLOCK_REALTIME, TIMER_ABSTIME, &abs_target_time, nullptr); + } + + /** + * @brief sleep_ms puts the current thread to sleep for the duration + * of "sleep_time_sec" seconds. + * @param sleep_time_ms is the sleeping duration asked in seconds. + */ + static void sleep_ms(const double& sleep_time_ms) + { + sleep_sec(1e-3 * sleep_time_ms); + } + + /** + * @brief sec_to_timespec converts a double representing the time in seconds + * to a struct timespec. + * @param[in] date_sec is the time in sec to be converted. + * @param[out] date_spec is the converted structure. + */ + static void sec_to_timespec(double date_sec, struct timespec& date_spec) + { + date_sec += 0.5e-9; + date_spec.tv_sec = static_cast(date_sec); + date_spec.tv_nsec = static_cast( + (date_sec - static_cast(date_spec.tv_sec)) * 1e9); + } + + /** + * @brief sleep_until_sec puts the threads to sleep until the date + * "date_sec" is reached. + * @param date_sec is the date until when to sleep in seconds. + */ + static void sleep_until_sec(const double& date_sec) + { + struct timespec abs_target_time; + sec_to_timespec(date_sec, abs_target_time); + clock_nanosleep( + CLOCK_REALTIME, TIMER_ABSTIME, &abs_target_time, nullptr); + } + + /** + * @brief get_current_date_str get the current date and format it in a + * string with "year_month_day_hour_minute_sec" + */ + static std::string get_current_date_str() + { + std::ostringstream oss; + auto now = std::chrono::system_clock::now(); + std::time_t now_c = std::chrono::system_clock::to_time_t(now); + struct tm* parts = std::localtime(&now_c); + oss << (int)(1900 + parts->tm_year) << "-"; + oss << std::setfill('0') << std::setw(2) << 1 + parts->tm_mon << "-"; + oss << std::setfill('0') << std::setw(2) << parts->tm_mday << "_"; + oss << std::setfill('0') << std::setw(2) << parts->tm_hour << "-"; + oss << std::setfill('0') << std::setw(2) << parts->tm_min << "-"; + oss << std::setfill('0') << std::setw(2) << parts->tm_sec; + return oss.str(); + } +}; + +double rt_clock_sec() +{ + return RtTimer::get_current_time_sec(); +} + +} // namespace benchmark +} // namespace master_board_sdk diff --git a/sdk/master_board_sdk/src/Link_manager.cpp b/sdk/master_board_sdk/src/Link_manager.cpp index 5cf6f98f..3948cf20 100644 --- a/sdk/master_board_sdk/src/Link_manager.cpp +++ b/sdk/master_board_sdk/src/Link_manager.cpp @@ -139,7 +139,7 @@ void *LINK_manager::sock_recv_thread(void *p_arg) int LINK_manager::send(uint8_t *payload, int len) { - uint8_t raw_bytes[LEN_RAWBYTES_MAX]; + uint8_t raw_bytes[LEN_RAWBYTES_MAX] = {0}; //Not the most fastest way to do this : // copy the payload in the packet array and then copy it back into the buffer... @@ -153,7 +153,7 @@ int LINK_manager::send(uint8_t *payload, int len) int LINK_manager::send() { - uint8_t raw_bytes[LEN_RAWBYTES_MAX]; + uint8_t raw_bytes[LEN_RAWBYTES_MAX] = {0}; int raw_len = mypacket->toBytes(raw_bytes, LEN_RAWBYTES_MAX); diff --git a/sdk/master_board_sdk/src/master_board_interface.cpp b/sdk/master_board_sdk/src/master_board_interface.cpp index 6ffec64e..1e38621e 100644 --- a/sdk/master_board_sdk/src/master_board_interface.cpp +++ b/sdk/master_board_sdk/src/master_board_interface.cpp @@ -361,9 +361,9 @@ void MasterBoardInterface::ParseSensorData() // zero. Check for this small velocity and set the velocity to zero. // See also: https://github.com/open-dynamic-robot-initiative/master-board/issues/92 for (int j = 0; j < 2; j++) { - int16_t &velocity = sensor_packet.dual_motor_driver_sensor_packets[i].velocity[j]; + int16_t velocity = sensor_packet.dual_motor_driver_sensor_packets[i].velocity[j]; if (velocity == 1 || velocity == -1) { - velocity = 0; + sensor_packet.dual_motor_driver_sensor_packets[i].velocity[j] = 0; } }