diff --git a/panthera_cpp/motor_cpp/src/hardware/robot.cpp b/panthera_cpp/motor_cpp/src/hardware/robot.cpp index 245ed8b..4c7f615 100755 --- a/panthera_cpp/motor_cpp/src/hardware/robot.cpp +++ b/panthera_cpp/motor_cpp/src/hardware/robot.cpp @@ -5,6 +5,7 @@ #include #include #include +#include namespace hightorque_robot { @@ -293,10 +294,13 @@ namespace hightorque_robot { int pid, vid; int r = 0; - struct sp_port *port; + struct sp_port *port = nullptr; try { - sp_get_port_by_name(name, &port); + if (sp_get_port_by_name(name, &port) != SP_OK || port == nullptr) + { + return -3; + } if (sp_get_port_usb_vid_pid(port, &vid, &pid) != SP_OK) { r = -1; @@ -344,8 +348,10 @@ namespace hightorque_robot { std::cerr << e.what() << '\n'; r = -3; - sp_close(port); - sp_free_port(port); + if (port) { + sp_close(port); + sp_free_port(port); + } } return r; @@ -402,6 +408,14 @@ namespace hightorque_robot } } + if (str.empty()) + { + std::string msg = "No compatible serial ports found. " + "Connect the arm and ensure USB VID is 0xCAF1 or 0xCAE1 (PID 0xFFFF)."; + std::cerr << "\033[1;31m" << msg << "\033[0m" << std::endl; + throw std::runtime_error(msg); + } + for(auto board_params: robot_params.CANboards) { auto cp_num = board_params.second.CANport_num;