diff --git a/src/slam_wrapper.cpp b/src/slam_wrapper.cpp index 0724569..fe1da82 100644 --- a/src/slam_wrapper.cpp +++ b/src/slam_wrapper.cpp @@ -23,47 +23,39 @@ } void SlamWrapper::qt_notice_handler(const void *message) { - const auto seq = static_cast(message); - std::string str_; + int index_, begin_, end_, feedback_, arrive_; + auto seq = static_cast(message); + std::string str_, notice_; - int begin_ = seq->data().find("index:", 0); // Instruction unique identifier - int end_ = seq->data().find(";", begin_); + begin_ = seq->data().find("index:", 0); // Instruction unique identifier + end_ = seq->data().find(";", begin_); str_ = seq->data().substr(begin_ + 6, end_ - begin_ - 6); - char *endptr = nullptr; - if (*endptr != '\0') { - std::cerr << "Error converting index to integer" << std::endl; - return; - } - const int index = static_cast(strtol(str_.c_str(), &endptr, 10)); + index_ = atoi(str_.c_str()); begin_ = seq->data().find("notice:", 0); // Prompt message end_ = seq->data().find(";", begin_); - std::string notice_ = seq->data().substr(begin_ + 7, end_ - begin_ - 7); + notice_ = seq->data().substr(begin_ + 7, end_ - begin_ - 7); if (index <= 10000) { // Command execution feedback begin_ = seq->data().find("feedback:", 0); end_ = seq->data().find(";", begin_); str_ = seq->data().substr(begin_ + 9, end_ - begin_ - 9); - const long feedback_ = strtol(str_.c_str(), &endptr, 10); - if (*endptr != '\0') { - std::cerr << "Error converting feedback to integer" << std::endl; - return; - } - std::cout << "index:" << index << " feedback:" << feedback_ << " notice:" << notice_ << std::endl; + feedback_ = atoi(str_.c_str()); + if (feedback_ == 0 || feedback_ == -1) + std::cout << "\033[1;31m" + << "Command execution failed with index = " << index_ << "." + << "\033[0m"; + std::cout << notice_ << std::endl; } else if (index == 10001) { // Navigation feedback begin_ = seq->data().find("arrive:", 0); + begin_ = seq->data().find("arrive:", 0); end_ = seq->data().find(";", begin_); str_ = seq->data().substr(begin_ + 7, end_ - begin_ - 7); - // 巡检到达的位置,判断到达的是第几个点 - const long arrive = strtol(str_.c_str(), &endptr, 10); - if (*endptr != '\0') { - std::cerr << "Error converting arrive to integer" << std::endl; - return; - } - std::cout << "index:" << index << " arrive:" << arrive << " notice:" << notice_ << std::endl; - if (arrive == 1) { + arrive_ = atoi(str_.c_str()); + std::cout << " I arrived node " << arrive_ << ". " << notice_ << std::endl; + if (arrive_ == 1) { //发送暂停指令 pause_navigation();