diff --git a/demo/scout_demo/scout_mini_omni_demo.cpp b/demo/scout_demo/scout_mini_omni_demo.cpp index 0e91c26..c9148b8 100644 --- a/demo/scout_demo/scout_mini_omni_demo.cpp +++ b/demo/scout_demo/scout_mini_omni_demo.cpp @@ -32,7 +32,7 @@ int main(int argc, char **argv) { std::unique_ptr scout; ProtocolDectctor detector; - detector.Connect("can0"); + detector.Connect(device_name); auto proto = detector.DetectProtocolVersion(5); if (proto == ProtocolVersion::AGX_V1) { std::cout << "Detected protocol: AGX_V1" << std::endl; diff --git a/demo/scout_demo/scout_robot_demo.cpp b/demo/scout_demo/scout_robot_demo.cpp index 8d8441a..d5cb7d0 100644 --- a/demo/scout_demo/scout_robot_demo.cpp +++ b/demo/scout_demo/scout_robot_demo.cpp @@ -50,18 +50,21 @@ int main(int argc, char **argv) { std::unique_ptr scout; ProtocolDectctor detector; - detector.Connect("can0"); - auto proto = detector.DetectProtocolVersion(5); - if (proto == ProtocolVersion::AGX_V1) { - std::cout << "Detected protocol: AGX_V1" << std::endl; - scout = std::unique_ptr( - new ScoutRobot(ProtocolVersion::AGX_V1, is_scout_mini)); - } else if (proto == ProtocolVersion::AGX_V2) { - std::cout << "Detected protocol: AGX_V2" << std::endl; - scout = std::unique_ptr( - new ScoutRobot(ProtocolVersion::AGX_V2, is_scout_mini)); + if (detector.Connect(device_name)) { + auto proto = detector.DetectProtocolVersion(5); + if (proto == ProtocolVersion::AGX_V1) { + std::cout << "Detected protocol: AGX_V1" << std::endl; + scout = std::unique_ptr( + new ScoutRobot(ProtocolVersion::AGX_V1, is_scout_mini)); + } else if (proto == ProtocolVersion::AGX_V2) { + std::cout << "Detected protocol: AGX_V2" << std::endl; + scout = std::unique_ptr( + new ScoutRobot(ProtocolVersion::AGX_V2, is_scout_mini)); + } else { + std::cout << "Detected protocol: UNKONWN" << std::endl; + return -1; + } } else { - std::cout << "Detected protocol: UNKONWN" << std::endl; return -1; } diff --git a/demo/utils_demo/demo_protocol_detector.cpp b/demo/utils_demo/demo_protocol_detector.cpp index 7765ddc..ebcc286 100644 --- a/demo/utils_demo/demo_protocol_detector.cpp +++ b/demo/utils_demo/demo_protocol_detector.cpp @@ -16,16 +16,19 @@ using namespace westonrobot; int main(int argc, char **argv) { ProtocolDectctor detector; - detector.Connect("can0"); + if (detector.Connect("can0")) { + auto proto = detector.DetectProtocolVersion(5); - auto proto = detector.DetectProtocolVersion(5); - - if (proto == ProtocolVersion::AGX_V1) { - std::cout << "Detected protocol: AGX_V1" << std::endl; - } else if (proto == ProtocolVersion::AGX_V2) { - std::cout << "Detected protocol: AGX_V2" << std::endl; + if (proto == ProtocolVersion::AGX_V1) { + std::cout << "Detected protocol: AGX_V1" << std::endl; + } else if (proto == ProtocolVersion::AGX_V2) { + std::cout << "Detected protocol: AGX_V2" << std::endl; + } else { + std::cout << "Detected protocol: UNKONWN" << std::endl; + } } else { - std::cout << "Detected protocol: UNKONWN" << std::endl; + std::cout << "Failed to open port" << std::endl; + return -1; } return 0; diff --git a/include/ugv_sdk/details/async_port/async_port_base.hpp b/include/ugv_sdk/details/async_port/async_port_base.hpp index ab3eab0..e7a269c 100644 --- a/include/ugv_sdk/details/async_port/async_port_base.hpp +++ b/include/ugv_sdk/details/async_port/async_port_base.hpp @@ -34,9 +34,10 @@ class AsyncPortBase { io_thread_ = std::thread([this]() { io_context_.run(); }); return true; } - std::cerr << "Failed to setup port, please check if specified port exits " - "or if you have proper permissions to access it" - << std::endl; + std::cerr + << "[ERROR] Failed to setup port, please check if specified port exits " + "or if you have proper permissions to access it" + << std::endl; return false; }; virtual void StopService() {} diff --git a/include/ugv_sdk/utilities/protocol_detector.hpp b/include/ugv_sdk/utilities/protocol_detector.hpp index c418b77..07ed071 100644 --- a/include/ugv_sdk/utilities/protocol_detector.hpp +++ b/include/ugv_sdk/utilities/protocol_detector.hpp @@ -18,8 +18,7 @@ namespace westonrobot { class ProtocolDectctor { public: - void Connect(std::string can_name); - void Connect(std::string uart_name, uint32_t baudrate); + bool Connect(std::string can_name); ProtocolVersion DetectProtocolVersion(uint32_t timeout_sec); diff --git a/src/async_port/async_can.cpp b/src/async_port/async_can.cpp index e0fcd68..e88cab5 100644 --- a/src/async_port/async_can.cpp +++ b/src/async_port/async_can.cpp @@ -35,7 +35,10 @@ bool AsyncCAN::SetupPort() { memcpy(ifr.ifr_name, port_.c_str(), iface_name_size); const int ioctl_result = ioctl(can_fd_, SIOCGIFINDEX, &ifr); - if (ioctl_result < 0) StopService(); + if (ioctl_result < 0) { + StopService(); + return false; + } struct sockaddr_can addr; memset(&addr, 0, sizeof(addr)); @@ -44,12 +47,15 @@ bool AsyncCAN::SetupPort() { const int bind_result = bind(can_fd_, (struct sockaddr *)&addr, sizeof(addr)); - if (bind_result < 0) StopService(); + if (bind_result < 0) { + StopService(); + return false; + } port_opened_ = true; std::cout << "Start listening to port: " << port_ << std::endl; } catch (std::system_error &e) { - std::cout << e.what(); + std::cout << e.what() << std::endl; return false; } diff --git a/src/async_port/async_serial.cpp b/src/async_port/async_serial.cpp index 00d246a..fcd561d 100644 --- a/src/async_port/async_serial.cpp +++ b/src/async_port/async_serial.cpp @@ -55,9 +55,8 @@ bool AsyncSerial::SetupPort() { port_opened_ = true; std::cout << "Start listening to port: " << port_ << "@" << baud_rate_ << std::endl; - } catch (std::system_error &e) { - std::cout << e.what(); + std::cout << e.what() << std::endl; return false; } diff --git a/src/utilities/protocol_detector.cpp b/src/utilities/protocol_detector.cpp index cd93e72..860a190 100644 --- a/src/utilities/protocol_detector.cpp +++ b/src/utilities/protocol_detector.cpp @@ -11,13 +11,12 @@ #include "ugv_sdk/utilities/stopwatch.hpp" namespace westonrobot { -void ProtocolDectctor::Connect(std::string can_name) { +bool ProtocolDectctor::Connect(std::string can_name) { can_ = std::make_shared(can_name); can_->SetReceiveCallback( std::bind(&ProtocolDectctor::ParseCANFrame, this, std::placeholders::_1)); - can_->StartListening(); + return can_->StartListening(); } -void ProtocolDectctor::Connect(std::string uart_name, uint32_t baudrate) {} ProtocolVersion ProtocolDectctor::DetectProtocolVersion(uint32_t timeout_sec) { msg_v1_detected_ = false;