From b35e4820f1dc4723aeaf7c7e18811fb6219ebc02 Mon Sep 17 00:00:00 2001 From: dingdingsong Date: Fri, 24 Nov 2023 20:17:16 +0800 Subject: [PATCH 01/12] [New]: Add bluetooth connect network MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 1.新增蓝牙配网节点，包括：封装蓝牙核心能力及ROS接口； 2.修改connector流程，修改语音及灯效、兼容新老版本APP； Signed-off-by: dingdingsong --- connector/include/connector/connector.hpp | 35 +- .../include/connector/ctrl_bluetooth.hpp | 69 ++ connector/src/connector.cpp | 214 ++++- connector/src/ctl_bluetooth.cpp | 132 +++ connector/src/ctrl_audio.cpp | 6 +- connector/src/ctrl_led.cpp | 19 + connector/src/main.cpp | 9 +- .../cyberdog_bluetooth_network/__init__.py | 0 .../__pycache__/__init__.cpython-36.pyc | Bin 0 -> 201 bytes .../bluetooth_node.py | 149 +++ .../cyberdog_bluetooth_network/bt_core.py | 848 ++++++++++++++++++ .../cyberdog_bluetooth_main.py | 38 + cyberdog_bluetooth_network/package.xml | 24 + .../resource/cyberdog_bluetooth_network | 0 cyberdog_bluetooth_network/setup.cfg | 4 + cyberdog_bluetooth_network/setup.py | 26 + ...test_copyright.cpython-36-pytest-6.2.5.pyc | Bin 0 -> 964 bytes .../test_flake8.cpython-36-pytest-6.2.5.pyc | Bin 0 -> 1022 bytes .../test_pep257.cpython-36-pytest-6.2.5.pyc | Bin 0 -> 974 bytes .../test/test_copyright.py | 23 + .../test/test_flake8.py | 25 + .../test/test_pep257.py | 23 + 22 files changed, 1609 insertions(+), 35 deletions(-) create mode 100644 connector/include/connector/ctrl_bluetooth.hpp create mode 100644 connector/src/ctl_bluetooth.cpp create mode 100644 cyberdog_bluetooth_network/cyberdog_bluetooth_network/__init__.py create mode 100644 cyberdog_bluetooth_network/cyberdog_bluetooth_network/__pycache__/__init__.cpython-36.pyc create mode 100644 cyberdog_bluetooth_network/cyberdog_bluetooth_network/bluetooth_node.py create mode 100644 cyberdog_bluetooth_network/cyberdog_bluetooth_network/bt_core.py create mode 100644 cyberdog_bluetooth_network/cyberdog_bluetooth_network/cyberdog_bluetooth_main.py create mode 100644 cyberdog_bluetooth_network/package.xml create mode 100644 cyberdog_bluetooth_network/resource/cyberdog_bluetooth_network create mode 100644 cyberdog_bluetooth_network/setup.cfg create mode 100644 cyberdog_bluetooth_network/setup.py create mode 100644 cyberdog_bluetooth_network/test/__pycache__/test_copyright.cpython-36-pytest-6.2.5.pyc create mode 100644 cyberdog_bluetooth_network/test/__pycache__/test_flake8.cpython-36-pytest-6.2.5.pyc create mode 100644 cyberdog_bluetooth_network/test/__pycache__/test_pep257.cpython-36-pytest-6.2.5.pyc create mode 100644 cyberdog_bluetooth_network/test/test_copyright.py create mode 100644 cyberdog_bluetooth_network/test/test_flake8.py create mode 100644 cyberdog_bluetooth_network/test/test_pep257.py diff --git a/connector/include/connector/connector.hpp b/connector/include/connector/connector.hpp index 55ca3d7..e4f318b 100644 --- a/connector/include/connector/connector.hpp +++ b/connector/include/connector/connector.hpp @@ -42,6 +42,10 @@ #include #include +#include +#include +#include + #include #include #include @@ -68,7 +72,7 @@ namespace cyberdog namespace interaction { namespace py = pybind11; -static const unsigned int LINE_MAX_SIZE {16384}; /*!< 行最大值:2kb */ +static const unsigned int LINE_MAX_SIZE {16384}; /*!< 行最大值:2kb */ /*! \file connector.hpp \brief 连接模块。 @@ -94,7 +98,11 @@ class Connector final : public rclcpp::Node using LedSrv = protocol::srv::LedExecute; /*!< [service 类型]LED 驱动服务 */ using CameraSrv = protocol::srv::CameraService; /*!< [service 类型]相机 驱动服务 */ - using TimeType = std::chrono::time_point; /*!< 超时 */ + using NotifyToAppMsg = protocol::msg::NotifyToApp; /*!< [topic 类型]通知APP连接状态消息 */ + using WIFIINFOMSG = protocol::msg::WifiInfo; + using BLUETOOTHSTATUSMSG = protocol::msg::BluetoothStatus; + + using TimeType = std::chrono::time_point; /*!< 超时 */ enum ShellEnum { shell = 1993, /*!< 异常结束, 执行shell命令错误 */ @@ -110,9 +118,8 @@ class Connector final : public rclcpp::Node const std::function, const std::function, const std::function, - const std::function); /*!< 初始化 */ + const std::function, + const std::function); /*!< 初始化 */ public: std::function CtrlAudio; /*!< [客户端]控制语音 */ @@ -120,6 +127,7 @@ class Connector final : public rclcpp::Node std::function CtrlLed; /*!< [客户端]控制led */ std::function CtrlWifi; /*!< [客户端]控制wifi */ + std::function CtrlAdvertising; /*!< [客户端]控制蓝牙 */ private: bool InitPythonInterpreter(); /*!< 初始化 python 解释器 */ @@ -183,6 +191,23 @@ class Connector final : public rclcpp::Node rclcpp::CallbackGroup::SharedPtr touch_cb_group_ {nullptr}; /*!< [回调组] touch */ rclcpp::CallbackGroup::SharedPtr img_cb_group_ {nullptr}; /*!< [回调组] img */ rclcpp::CallbackGroup::SharedPtr pub_cb_group_ {nullptr}; /*!< [回调组] pub */ + + void APPSendWiFiCallback(const protocol::msg::WifiInfo::SharedPtr msg); + void AppConnectState(const std_msgs::msg::Bool msg); + void BtStatusCallback(const protocol::msg::BluetoothStatus::SharedPtr msg); + bool WriteToFile( + const std::string ssid, + const std::string ip, + const std::string type); + + NotifyToAppMsg notify_to_app_msg_; + rclcpp::Subscription::SharedPtr wifi_info_sub_ {nullptr}; + rclcpp::Publisher::SharedPtr notify_to_app_pub_ {nullptr}; + rclcpp::Subscription::SharedPtr bluetooth_status_sub_ {nullptr}; + + std::string wifi_record_dir_ {""}; /*!< wifi 类型记录文件路径 */ + bool connect_network_status = true; + int connect_code = -1; }; // class Connector } // namespace interaction } // namespace cyberdog diff --git a/connector/include/connector/ctrl_bluetooth.hpp b/connector/include/connector/ctrl_bluetooth.hpp new file mode 100644 index 0000000..3e7776c --- /dev/null +++ b/connector/include/connector/ctrl_bluetooth.hpp @@ -0,0 +1,69 @@ +// Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#ifndef CONNECTOR__CTRL_BLUETOOTH_HPP_ +#define CONNECTOR__CTRL_BLUETOOTH_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/wait_set.hpp" +#include "cyberdog_common/cyberdog_log.hpp" +#include "cyberdog_common/cyberdog_toml.hpp" +#include "protocol/msg/audio_play.hpp" +#include "protocol/msg/bluetooth_status.hpp" +#include "protocol/srv/bms_cmd.hpp" +#include "std_srvs/srv/trigger.hpp" + +namespace cyberdog +{ +namespace interaction +{ +class CtrlBluetooth final : public rclcpp::Node +{ +public: + explicit CtrlBluetooth(const std::string name); + ~CtrlBluetooth(); + bool CtrlAdvertising(bool enable); + + bool WriteToFile( + const std::string ssid, + const std::string ip, + const std::string type); + bool IsConnectApp(); + +private: + bool Init(); + std::string runCommand(const std::string & cmd); + std::vector parseConnectionsOutput(const std::string & output); + void BluetoothStatusCallback( + const protocol::msg::BluetoothStatus::SharedPtr msg); + bool DisconnectBluetooth(); + +private: + int bluetooth_connect_status_ = -1; + int bluetooth_advertising_status_ = -1; + + rclcpp::Subscription::SharedPtr bluetooth_status_sub_ {nullptr}; + rclcpp::Client::SharedPtr control_advertise_client_ {nullptr}; + rclcpp::Client::SharedPtr disconnect_app_bt_srv_ {nullptr}; +}; // class CtrlBluetooth +} // namespace interaction +} // namespace cyberdog +#endif // CONNECTOR__CTRL_BLUETOOTH_HPP_ diff --git a/connector/src/connector.cpp b/connector/src/connector.cpp index 9e03a76..e9bc8ac 100644 --- a/connector/src/connector.cpp +++ b/connector/src/connector.cpp @@ -36,7 +36,8 @@ Connector::Connector(const std::string & name) KEY("b1950ca92ccc9ffe"), wifi_name("n"), wifi_password("p"), - ip("i") + ip("i"), + wifi_record_dir_("/wifirecord.toml") { INFO("Creating [Connector] object(node)"); } @@ -51,7 +52,8 @@ bool Connector::Init( const std::function _control_audio, const std::function _control_camera, const std::function _control_led, - const std::function _control_wifi) + const std::function _control_wifi, + const std::function _control_bluetooth) { INFO("Initializing ..."); try { @@ -59,6 +61,7 @@ bool Connector::Init( this->CtrlCamera = _control_camera; this->CtrlLed = _control_led; this->CtrlWifi = _control_wifi; + this->CtrlAdvertising = _control_bluetooth; this->touch_efficient_ = false; this->camer_efficient_ = false; @@ -100,9 +103,12 @@ bool Connector::Init( } std::string original_file = this->params_pkg_dir_ + "/config" + this->wifi_config_dir_; + std::string original_wifi_record_file = this->params_pkg_dir_ + "/config" + + this->wifi_record_dir_; std::string workspace = toml::find( this->params_toml_, "connector", "initialization", "cmd", "workspace"); this->wifi_config_dir_ = workspace + this->wifi_config_dir_; + this->wifi_record_dir_ = workspace + this->wifi_record_dir_; auto make_dir = [&]() -> bool { if (access(workspace.c_str(), F_OK) != 0) { if (mkdir(workspace.c_str(), S_IRWXU | S_IRWXG | S_IRWXO) != 0) { @@ -127,6 +133,12 @@ bool Connector::Init( { return false; } + if (!this->Shell( + std::string("cp " + original_wifi_record_file + " " + this->wifi_record_dir_), code, + message)) + { + return false; + } } this->touch_signal_timeout_s = static_cast(toml::find( @@ -199,12 +211,21 @@ bool Connector::Init( this->params_toml_, "connector", "initialization", "topic", "camera"), 1, std::bind(&Connector::CameraSignalCallback, this, std::placeholders::_1), sub_option); - this->connect_service_ = this->create_service( toml::find( this->params_toml_, "connector", "initialization", "service", "connection"), std::bind(&Connector::Connect, this, std::placeholders::_1, std::placeholders::_2), rclcpp::ServicesQoS().get_rmw_qos_profile(), this->service_cb_group_); + this->bluetooth_status_sub_ = this->create_subscription( + "bluetooth_status", rclcpp::SystemDefaultsQoS(), + std::bind(&Connector::BtStatusCallback, this, std::placeholders::_1), + sub_option); + this->wifi_info_sub_ = this->create_subscription( + "app_send_network_data", rclcpp::SystemDefaultsQoS(), + std::bind(&Connector::APPSendWiFiCallback, this, std::placeholders::_1), + sub_option); + this->notify_to_app_pub_ = this->create_publisher( + "notify_to_app", 1, pub_option); } catch (const std::exception & e) { ERROR("Init data failed: <%s>", e.what()); return false; @@ -325,6 +346,11 @@ void Connector::ResetSignal() INFO("ResetSignal(Exit network mode)."); this->camer_efficient_ = false; this->CtrlCamera(CameraSrv::Request::STOP_IMAGE_PUBLISH); + int count = 0; + while (!this->CtrlAdvertising(false) && count < 3) { + sleep(2); + count++; + } this->touch_efficient_ = false; this->Interaction(AudioMsg::PID_WIFI_EXIT_CONNECTION_MODE_0); } @@ -354,33 +380,45 @@ void Connector::WiFiSignalCallback(const WiFiMsg::SharedPtr msg) { try { std::lock_guard guard(this->state_msg_mutex_); - // INFO( - // "WiFi signal callback:{is_connected=<%s>, ssid=<%s>, ip=<%s>, strength=%d}", - // std::string(msg->is_connected ? "True" : "False").c_str(), - // msg->ssid.c_str(), - // msg->ip.c_str(), - // static_cast(msg->strength)); + if (msg->is_connected) { + this->notify_to_app_msg_.ssid = msg->ssid; + this->notify_to_app_msg_.ip = msg->ip; + + if ((!this->state_msg_.is_connected) && (std::chrono::system_clock::now() > this->touch_signal_timeout_)) { // 网络连接已建立: 开机自动连接网络，自动断网后自动连接网络 + this->notify_to_app_msg_.code = 2000; } if (msg->ssid != this->state_msg_.ssid) { this->state_msg_.is_internet = this->CheckInternet(); if (this->state_msg_.is_internet) { // 当前网络能访问互联网 + this->notify_to_app_msg_.code = 2001; } else { // 当前网络不能访问互联网 + this->notify_to_app_msg_.code = 2002; } } if (this->provider_ip_ == this->initial_ip_) { this->provider_ip_ = this->GetWiFiProvider(msg->ssid); } this->state_msg_.provider_ip = this->provider_ip_; + this->connect_network_status = false; + } else { if (this->state_msg_.is_connected) { // 网络连接已断开 + this->notify_to_app_msg_.code = 2003; + + if (this->connect_network_status) { + // this->Interaction(9); + this->CtrlAudio(17); + this->CtrlLed(9); + this->connect_network_status = true; + } } } this->state_msg_.is_connected = msg->is_connected; @@ -426,10 +464,17 @@ void Connector::TouchSignalCallback(const TouchMsg::SharedPtr msg) this->Interaction(AudioMsg::PID_WIFI_FAILED_PLEASE_RETRY); return; } - this->Interaction(AudioMsg::PID_WIFI_WAIT_FOR_SCAN_CODE_0); + // this->Interaction(AudioMsg::PID_WIFI_WAIT_FOR_SCAN_CODE_0); + this->CtrlAudio(16); + this->CtrlLed(AudioMsg::PID_WIFI_WAIT_FOR_SCAN_CODE_0); this->touch_signal_timeout_ = std::chrono::system_clock::now() + std::chrono::seconds(this->touch_signal_timeout_s); this->touch_efficient_ = true; + int count = 0; + while (!this->CtrlAdvertising(true) && count < 3) { + sleep(3); + count++; + } this->camer_efficient_ = true; INFO( "A touch signal is detected at %s, the signal is valid for %d seconds and expires at %s.", @@ -448,11 +493,14 @@ void Connector::CameraSignalCallback(const CameraMsg::SharedPtr msg) INFO("Identifying current QR code..."); auto return_error = [&](const std::string & msg, const std::string & data) { WARN("%s:\n<%s>.", msg.c_str(), data.c_str()); - this->Interaction(AudioMsg::PID_WIFI_SCAN_CODE_IP_ERROR); + // this->Interaction(AudioMsg::PID_WIFI_SCAN_CODE_IP_ERROR); + this->CtrlLed(AudioMsg::PID_WIFI_SCAN_CODE_IP_ERROR); if (this->touch_efficient_ && (std::chrono::system_clock::now() < this->touch_signal_timeout_)) { - this->Interaction(AudioMsg::PID_WIFI_WAIT_FOR_SCAN_CODE_0); + // this->Interaction(AudioMsg::PID_WIFI_WAIT_FOR_SCAN_CODE_0); + this->CtrlAudio(16); + this->CtrlLed(AudioMsg::PID_WIFI_WAIT_FOR_SCAN_CODE_0); } }; int num_pixels = msg->data.size(); @@ -518,7 +566,9 @@ void Connector::CameraSignalCallback(const CameraMsg::SharedPtr msg) connect_document.HasMember(ip.c_str()) && connect_document[ip.c_str()].IsString()) { - this->Interaction(AudioMsg::PID_WIFI_SCAN_CODE_SUCCEEDED_0); + // this->Interaction(AudioMsg::PID_WIFI_SCAN_CODE_SUCCEEDED_0); + // this->CtrlAudio(15); + // this->CtrlLed(AudioMsg::PID_WIFI_SCAN_CODE_SUCCEEDED_0); if (this->DoConnect( connect_document[wifi_name.c_str()].GetString(), connect_document[wifi_password.c_str()].GetString(), @@ -556,6 +606,8 @@ void Connector::CameraSignalCallback(const CameraMsg::SharedPtr msg) bool Connector::DoConnect(std::string name, std::string password, std::string provider) { + this->CtrlAudio(15); + this->CtrlLed(AudioMsg::PID_WIFI_SCAN_CODE_SUCCEEDED_0); auto return_true = [&](std::string msg, bool same_wifi) -> bool { this->provider_ip_ = provider; this->SaveWiFi(provider, name, password); @@ -570,6 +622,7 @@ bool Connector::DoConnect(std::string name, std::string password, std::string pr // this->Interaction(AudioMsg::PID_WIFI_EXIT_CONNECTION_MODE_0); this->touch_efficient_ = false; this->srv_code_ = ConnectorSrv::Response::CODE_SUCCESS; + this->connect_code = 2000; return true; }; auto return_false = [&](std::string msg) -> bool { @@ -577,8 +630,11 @@ bool Connector::DoConnect(std::string name, std::string password, std::string pr if (this->touch_efficient_ && (std::chrono::system_clock::now() < this->touch_signal_timeout_)) { - this->Interaction(AudioMsg::PID_WIFI_WAIT_FOR_SCAN_CODE_0); + // this->Interaction(AudioMsg::PID_WIFI_WAIT_FOR_SCAN_CODE_0); + this->CtrlAudio(16); + this->CtrlLed(AudioMsg::PID_WIFI_WAIT_FOR_SCAN_CODE_0); } + this->connect_code = 2004; return false; }; auto judge_string = [&](std::string msg) -> bool { @@ -588,17 +644,6 @@ bool Connector::DoConnect(std::string name, std::string password, std::string pr msg.c_str()); return false; } - - // std::regex pattern("^[a-zA-Z0-9_]+$"); // WiFi命名规则 - // std::regex pattern("^[a-zA-Z][a-zA-Z0-9_]*$"); // 变量命名规则 - // return std::regex_match(msg, pattern); - - // std::regex quote("['\"]"); // 包含单引号和双引号 - // if (std::regex_search(input, quoteRegex)) { - // WARN("The current string(%s) is contains single or double quotes, which is illegal.", - // msg.c_str()); - // return false; - // } return true; }; try { @@ -643,7 +688,8 @@ bool Connector::DoConnect(std::string name, std::string password, std::string pr } if (provider.empty()) { // 手机端IP错误，会导致无法与设备通讯 - this->Interaction(AudioMsg::PID_WIFI_SCAN_CODE_INFO_ERROR); + // this->Interaction(AudioMsg::PID_WIFI_SCAN_CODE_INFO_ERROR); + this->CtrlLed(AudioMsg::PID_WIFI_SCAN_CODE_INFO_ERROR); this->srv_code_ = ConnectorSrv::Response::CODE_WIFI_PROVIDER_IP_FAIL; return return_false("Wifi provider is empty."); } @@ -654,18 +700,22 @@ bool Connector::DoConnect(std::string name, std::string password, std::string pr } switch (this->CtrlWifi(name, password)) { case WiFiSrv::Response::RESULT_SUCCESS: + this->connect_code = 2000; return return_true("WiFi connection succeeded.", false); case WiFiSrv::Response::RESULT_NO_SSID: this->Interaction(AudioMsg::PID_WIFI_CONNECTION_FAILED_0); this->srv_code_ = ConnectorSrv::Response::CODE_WIFI_NAME_FAIL; + this->connect_code = 2004; break; case WiFiSrv::Response::RESULT_ERR_PWD: this->Interaction(AudioMsg::PID_WIFI_CONNECTION_FAILED_1); this->srv_code_ = ConnectorSrv::Response::CODE_WIFI_PASSWORD_FAIL; + this->connect_code = 2005; break; default: this->Interaction(AudioMsg::PID_WIFI_CONNECTION_FAILED_2); this->srv_code_ = ConnectorSrv::Response::CODE_CONNECTION_TIMEOUT_FAIL; + this->connect_code = 2001; break; } } catch (const std::exception & e) { @@ -716,8 +766,9 @@ void Connector::SaveWiFi( bool Connector::UserBootsFirstTime() { + this->judge_first_time_ = false; try { - this->judge_first_time_ = false; + // this->judge_first_time_ = false; INFO("Judge user boots first time..."); toml::value wifi_toml; if (cyberdog::common::CyberdogToml::ParseFile( @@ -740,6 +791,30 @@ bool Connector::UserBootsFirstTime() } catch (const std::exception & e) { ERROR("Judge user boots first time is error:%s", e.what()); } + + try { + toml::value wifi_toml1; + INFO("Judge user boots first time : %s", this->wifi_record_dir_.c_str()); + if (cyberdog::common::CyberdogToml::ParseFile( + this->wifi_record_dir_.c_str(), wifi_toml1)) + { + bool first1 = toml::find_or(wifi_toml1, "wifi", "user_boots_first_time", false); + if (first1) { + wifi_toml1["wifi"]["user_boots_first_time"] = false; + if (cyberdog::common::CyberdogToml::WriteFile(this->wifi_record_dir_, wifi_toml1)) { + return true; + } else { + WARN("111WiFi params config file does not have wifi key, write failed"); + } + } + } else { + ERROR( + "111Toml WiFi config file is not in toml format, config file dir:\n%s", + this->wifi_record_dir_.c_str()); + } + } catch (const std::exception & e) { + ERROR("111Judge user boots first time is error:%s", e.what()); + } return false; } @@ -817,5 +892,90 @@ bool Connector::Interaction(const uint16_t & _id) this->CtrlLed(_id); return true; } + +void Connector::APPSendWiFiCallback( + const protocol::msg::WifiInfo::SharedPtr msg) +{ + INFO("receive wifi info from app, connect wifi"); + this->notify_to_app_msg_.ssid = msg->ssid; + this->notify_to_app_msg_.ip = msg->ip; + if (msg->ssid.empty() || msg->ip.empty()) { + INFO("receive wifi info from app, ssid or ip is empty"); + this->notify_to_app_msg_.code = 2001; + this->notify_to_app_pub_->publish(this->notify_to_app_msg_); + return; + } + if (this->DoConnect(msg->ssid, msg->pwd, msg->ip)) { + // 连网成功, 写文件 + this->WriteToFile(msg->ssid, msg->ip, msg->type); + this->notify_to_app_msg_.code = 2000; + } else { + INFO("receive wifi info from app, connect wifi failed"); + this->notify_to_app_msg_.code = 2001; // 连网失败 + } + this->notify_to_app_msg_.code = this->connect_code; + this->notify_to_app_pub_->publish(this->notify_to_app_msg_); +} + +bool Connector::WriteToFile( + const std::string ssid, + const std::string ip, + const std::string type) +{ + // 将ssid，ip，type写入到文件中 + try { + INFO( + "Save WiFi :\n\tprovider = %s\n\tname = %s\n\ttype = %s", + ip.c_str(), + ssid.c_str(), + type.c_str()); + toml::value wifi_toml; + if (cyberdog::common::CyberdogToml::ParseFile( + this->wifi_record_dir_.c_str(), wifi_toml)) + { + std::string old_provider = toml::find_or(wifi_toml, "wifi", ssid, "provider", ""); + if (old_provider.empty()) { + toml::value wifi; + wifi["type"] = type; + wifi["provider"] = ip; + wifi_toml["wifi"][ssid] = wifi; + } else { + wifi_toml["wifi"][ssid]["type"] = type; + wifi_toml["wifi"][ssid]["provider"] = ip; + } + if (!cyberdog::common::CyberdogToml::WriteFile(this->wifi_record_dir_, wifi_toml)) { + WARN("WiFi record file does not have wifi key, write failed"); + return false; + } + } else { + ERROR( + "Toml WiFi config file is not in toml format, config file dir:\n%s", + this->wifi_record_dir_.c_str()); + return false; + } + } catch (const std::exception & e) { + ERROR("WriteToFile()-> Save WiFi is error:%s", e.what()); + return false; + } +} +void Connector::BtStatusCallback( + const protocol::msg::BluetoothStatus::SharedPtr msg) +{ + if (msg->connectable == 1) { + INFO_MILLSECONDS(3000, " bluetooth connect(app and cyberdog)"); + if (this->touch_efficient_ == false) { + // touch没有长按，关闭广播 + int count = 0; + while (!this->CtrlAdvertising(false) && count < 3) { + sleep(3); + count++; + } + } + } else if (msg->connectable == 0) { + // 未连接状态，开起广播 + INFO_MILLSECONDS(3000, " bluetooth is not connect(app and cyberdog)"); + this->CtrlAdvertising(true); + } +} } // namespace interaction } // namespace cyberdog diff --git a/connector/src/ctl_bluetooth.cpp b/connector/src/ctl_bluetooth.cpp new file mode 100644 index 0000000..6b712ab --- /dev/null +++ b/connector/src/ctl_bluetooth.cpp @@ -0,0 +1,132 @@ +// Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include +#include + +#include "connector/ctrl_bluetooth.hpp" + +namespace cyberdog +{ +namespace interaction +{ +CtrlBluetooth::CtrlBluetooth(const std::string name) +: Node(name) +{ + INFO("Creating [CtrlBluetooth] object(node)"); + if (!this->Init()) {exit(-1);} +} + +CtrlBluetooth::~CtrlBluetooth() +{ + INFO("Destroy [CtrlBluetooth] object(node)"); +} + +bool CtrlBluetooth::Init() +{ + INFO("Initializing CtrlBluetooth ..."); + try { + this->bluetooth_status_sub_ = + this->create_subscription( + "bluetooth_status", 1, + std::bind(&CtrlBluetooth::BluetoothStatusCallback, this, std::placeholders::_1)); + + this->control_advertise_client_ = + this->create_client("ctrl_advertising"); + + this->disconnect_app_bt_srv_ = this->create_client( + "disconnect_app_bt", rmw_qos_profile_services_default); + } catch (const std::exception & e) { + ERROR("Init CtrlBluetooth failed: <%s>", e.what()); + return false; + } + return true; +} + +void CtrlBluetooth::BluetoothStatusCallback( + const protocol::msg::BluetoothStatus::SharedPtr msg) +{ + this->bluetooth_connect_status_ = msg->connectable; + this->bluetooth_advertising_status_ = msg->advtiseable; +} + +bool CtrlBluetooth::DisconnectBluetooth() +{ + if (!this->disconnect_app_bt_srv_->wait_for_service(std::chrono::seconds(5))) { + INFO("Failed to call disconnect_app_bt service"); + return false; + } else { + auto request = std::make_shared(); + auto future_result = this->disconnect_app_bt_srv_->async_send_request(request); + std::future_status status = future_result.wait_for(std::chrono::seconds(5)); + if (status == std::future_status::ready) { + auto result = future_result.get(); + if (result->success) { + INFO("success to call disconnect_app_bt service"); + } else { + INFO("success to call disconnect_app_bt service,but failed to disconnect app bluetooth"); + return false; + } + } else { + INFO("Failed to call disconnect_app_bt service"); + return false; + } + } +} + +bool CtrlBluetooth::CtrlAdvertising(bool enable) +{ + std::shared_ptr request = + std::make_shared(); + INFO("enter CtrlAdvertising"); + if (enable) { + if (this->bluetooth_connect_status_ == 1) { + INFO("disconnect app bluetooth"); + if (!this->DisconnectBluetooth()) { + INFO("Failed to disconnect app bluetooth"); + return false; + } + } + if (this->bluetooth_advertising_status_ == 1) { + INFO_MILLSECONDS(3000, "is Advertising"); + return true; // 广播已开启，直接返回 + } + request->electric_machine_command = 1; // 开启广播 + sleep(1); + } else { + if (this->bluetooth_advertising_status_ == 0) { + INFO_MILLSECONDS(3000, "Advertising is closed"); + return true; // 广播已经关闭 + } + request->electric_machine_command = 2; // 关闭广播 + } + + auto future_result = this->control_advertise_client_->async_send_request(request); + std::future_status status = future_result.wait_for(std::chrono::seconds(5)); + if (status == std::future_status::ready) { + auto result = future_result.get(); + if (result->success) { + INFO("success to call CtrlAdvertising service"); + return true; + } else { + INFO("success to call CtrlAdvertising service,but failed to control advertising"); + return false; + } + } else { + INFO("Failed to call CtrlAdvertising service"); + return false; + } + return true; +} +} // namespace interaction +} // namespace cyberdog diff --git a/connector/src/ctrl_audio.cpp b/connector/src/ctrl_audio.cpp index 30503f2..29396f4 100644 --- a/connector/src/ctrl_audio.cpp +++ b/connector/src/ctrl_audio.cpp @@ -90,7 +90,8 @@ bool CtrlAudio::Init() void CtrlAudio::Timer_05hz() { - this->RequestTopic(AudioMsg::PID_WIFI_WAIT_FOR_SCAN_CODE_0); + // this->RequestTopic(AudioMsg::PID_WIFI_WAIT_FOR_SCAN_CODE_0); + this->RequestServer(16); } void CtrlAudio::GoalResponseCallback(GoalHandleAudioAct::SharedPtr goal_handle) @@ -224,7 +225,8 @@ uint CtrlAudio::ControlAudio(uint16_t _id) // * 4: 控制 Audio 失败 // */ INFO("Control audio <%d>", _id); - if (_id == AudioMsg::PID_WIFI_WAIT_FOR_SCAN_CODE_0) { + // if (_id == AudioMsg::PID_WIFI_WAIT_FOR_SCAN_CODE_0) { + if (_id == 16) { this->timer_05hz_->reset(); } else if (!this->timer_05hz_->is_canceled()) { this->timer_05hz_->cancel(); diff --git a/connector/src/ctrl_led.cpp b/connector/src/ctrl_led.cpp index 875cad3..b64c3bf 100644 --- a/connector/src/ctrl_led.cpp +++ b/connector/src/ctrl_led.cpp @@ -196,6 +196,25 @@ uint CtrlLed::ControlLed(const uint16_t & _id) LedSrv::Request::USER_DEFINED, LedSrv::Request::CIRCULAR_BREATH, 255, 165, 0); + } else if (_id == 9) { + this->EnableLed( + true, + LedSrv::Request::HEAD_LED, + LedSrv::Request::USER_DEFINED, + LedSrv::Request::BLINK_FAST, + 255, 140, 0); + this->EnableLed( + true, + LedSrv::Request::TAIL_LED, + LedSrv::Request::USER_DEFINED, + LedSrv::Request::BLINK_FAST, + 255, 140, 0); + this->EnableLed( + true, + LedSrv::Request::MINI_LED, + LedSrv::Request::USER_DEFINED, + LedSrv::Request::CIRCULAR_BREATH, + 255, 140, 0); } else { INFO("Control led : maintain the current state."); } diff --git a/connector/src/main.cpp b/connector/src/main.cpp index 591c011..5bc81c2 100644 --- a/connector/src/main.cpp +++ b/connector/src/main.cpp @@ -19,6 +19,7 @@ #include "connector/ctrl_wifi.hpp" #include "connector/ctrl_camera.hpp" #include "connector/uploader_log.hpp" +#include "connector/ctrl_bluetooth.hpp" int main(int argc, char ** argv) { @@ -31,6 +32,8 @@ int main(int argc, char ** argv) auto ctrl_camera = std::make_shared("connector_ctrl_camera"); auto ctrl_led = std::make_shared("connector_ctrl_led"); auto ctrl_wifi = std::make_shared("connector_ctrl_wifi"); + auto ctrl_bluetooth = std::make_shared( + "connector_ctrl_bluetooth"); auto uploader_log = std::make_shared("connector_uploader_log"); @@ -46,7 +49,10 @@ int main(int argc, char ** argv) ctrl_led, std::placeholders::_1), std::bind( &cyberdog::interaction::CtrlWifi::ControlWifi, - ctrl_wifi, std::placeholders::_1, std::placeholders::_2) + ctrl_wifi, std::placeholders::_1, std::placeholders::_2), + std::bind( + &cyberdog::interaction::CtrlBluetooth::CtrlAdvertising, + ctrl_bluetooth, std::placeholders::_1) )) {exit(-1);} if (!uploader_log->Init(uploader_log)) { ERROR("Init UploaderLog object(node) is failed."); @@ -60,6 +66,7 @@ int main(int argc, char ** argv) exec_.add_node(ctrl_camera->get_node_base_interface()); exec_.add_node(ctrl_led->get_node_base_interface()); exec_.add_node(ctrl_wifi->get_node_base_interface()); + exec_.add_node(ctrl_bluetooth->get_node_base_interface()); exec_.add_node(uploader_log->get_node_base_interface()); exec_.spin(); rclcpp::shutdown(); diff --git a/cyberdog_bluetooth_network/cyberdog_bluetooth_network/__init__.py b/cyberdog_bluetooth_network/cyberdog_bluetooth_network/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/cyberdog_bluetooth_network/cyberdog_bluetooth_network/__pycache__/__init__.cpython-36.pyc b/cyberdog_bluetooth_network/cyberdog_bluetooth_network/__pycache__/__init__.cpython-36.pyc new file mode 100644 index 0000000000000000000000000000000000000000..e1589140a6fe89f7e72d36c25df91516759601e3 GIT binary patch literal 201 zcmXr!<>lJ$o{-7_1dl-k3@`#24nSPY0whuxf*CX!{Z=v*frJsnuQL6N{M=Oil+3*J z;{3dHeIHB1_@tcD)RO%Cl8pGg)ROZ2qHO)*qGbKdypq(S#N?99{5<{S%B0kyl>Bro iO7Td=$7kkcmc+;F6;$5hu*uC&Da}c>1G%Rdh#3Iny*q>e literal 0 HcmV?d00001 diff --git a/cyberdog_bluetooth_network/cyberdog_bluetooth_network/bluetooth_node.py b/cyberdog_bluetooth_network/cyberdog_bluetooth_network/bluetooth_node.py new file mode 100644 index 0000000..0bb21e2 --- /dev/null +++ b/cyberdog_bluetooth_network/cyberdog_bluetooth_network/bluetooth_node.py @@ -0,0 +1,149 @@ +#!/usr/bin/python3 +# +# Copyright (c) 2023 Xiaomi Corporation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import threading +import time +from time import sleep + +from mi.cyberdog_bringup.manual import get_namespace +from protocol.msg import BluetoothStatus +from protocol.msg import NotifyToApp +from protocol.msg import WifiInfo +from protocol.srv import BmsCmd +from rclpy.callback_groups import MutuallyExclusiveCallbackGroup, ReentrantCallbackGroup +from rclpy.node import Node +# from sensor_msgs.msg import BatteryState, Joy +# from std_msgs.msg import Bool, Int8, String +from std_srvs.srv import Trigger + +from . import bt_core + + +class BluetoothNode(Node): + + def __init__(self, node_name: str): + Node.__init__(self, node_name) + self.__logger = self.get_logger() # ROS2 logger + self.__logger.info('[BluetoothCore]: BluetoothNode init') + self.dog_ssif = '' + self.dog_ip = '' + self.__multithread_callback_group = ReentrantCallbackGroup() + self.__singlethread_callback_group = MutuallyExclusiveCallbackGroup() + # 创建ROS服务用于打开和关闭蓝牙广播 + self.__openAdvertisingService = self.create_service( + BmsCmd, 'ctrl_advertising', + self.openAdvertisingCallback, + callback_group=self.__singlethread_callback_group) + self.__openGattService = self.create_service( + BmsCmd, 'ctrl_gatt', + self.openGattCallback, + callback_group=self.__singlethread_callback_group) + self.__disconnect_app_bt_service = self.create_service( + Trigger, 'disconnect_app_bt', self.__disconnect_app_bt_callback, + callback_group=self.__singlethread_callback_group) + # 创建ROS话题用于发布蓝牙广播状态 + self.__bluetoothStatePublisher = self.create_publisher( + BluetoothStatus, 'bluetooth_status', 10) + self.__notify_subscriber = self.create_subscription( + NotifyToApp, 'notify_to_app', self.__notify_to_app_callback, 1, + callback_group=self.__multithread_callback_group) + self.__bt_core_lock = threading.Lock() + self.blue_core = bt_core.BluetoothCore(self.__logger) + # 开启线程,启动蓝牙mainloop的循环 + self.bluetooth_mainloop_thread = threading.Thread(target=self.blue_core.main_bluetooth) + self.bluetooth_mainloop_thread.start() + self.__logger.info('[BluetoothCore]: bluetooth_mainloop_thread start') + time.sleep(1) + self.blue_core.RegADV(True) + sleep(3) + self.blue_core.RegATT(True) + + # 创建一个定时器，定时发布状态（广播、连接、wifi信息） + self.__bluetooth_status_timer = self.create_timer( + 2, self.__bluetooth_status_timer_callback, + callback_group=self.__singlethread_callback_group) + + def __disconnect_app_bt_callback(self, request, response): + self.__logger.info('disconnect_app_bt_callback') + response.success = True + response.message = 'disconnect app bt' + self.blue_core.disconnect_device(self.blue_core.mac_address_get) + return response + + def __notify_to_app_callback(self, msg): + self.__logger.info('notify_to_app_callback') + print(f'ssid:{msg.ssid} ip:{msg.ip} code:{msg.code}') + self.blue_core.doNotify(msg.ssid, msg.ip, msg.code, self.blue_core.dog_sn) + + def openAdvertisingCallback(self, request, response): + with self.__bt_core_lock: + self.__logger.info('openAdvertising service Callback') + response.success = True + response.code = 0 + if request.electric_machine_command == 1: + if self.blue_core.adv_status == 0: + self.__logger.info('openAdvertisingCallback: advertise open') + self.blue_core.RegADV(True) + time.sleep(1) + if self.blue_core.adv_status == 0: + response.success = False + response.code = -1 + pass + else: + self.__logger.info('openAdvertisingCallback: is advertising') + if request.electric_machine_command == 2: + if self.blue_core.adv_status == 1: + self.__logger.info('penAdvertisingCallback: advertise close') + self.blue_core.RegADV(False) + time.sleep(1) + if self.blue_core.adv_status == 1: + response.success = False + response.code = -1 + pass + else: + self.__logger.info('openAdvertisingCallback: is not advertising') + return response + + def openGattCallback(self, request, response): + self.__logger.info('openGattCallback service Callback') + response.success = True + response.code = 0 + if request.electric_machine_command == 1: + self.__logger.info('openGattCallback: openGatt') + self.blue_core.RegATT(True) + elif request.electric_machine_command == 2: + self.__logger.info('openGattCallback: closeGatt') + self.blue_core.RegATT(False) + pass + + def __bluetooth_status_timer_callback(self): + connect_status, adv_status = self.blue_core.bluetooth_status() + msg = BluetoothStatus() + msg.connectable = connect_status + msg.advtiseable = adv_status + self.__bluetoothStatePublisher.publish(msg) + pass + + +class PublisherPhoneIP(Node): + + def __init__(self,): + now_namespace = get_namespace() + super().__init__('phonepublisher', namespace=now_namespace) + self.publisher_ = self.create_publisher(WifiInfo, 'app_send_network_data', 1) + + def publisher(self, msg): + self.publisher_.publish(msg) diff --git a/cyberdog_bluetooth_network/cyberdog_bluetooth_network/bt_core.py b/cyberdog_bluetooth_network/cyberdog_bluetooth_network/bt_core.py new file mode 100644 index 0000000..1e58197 --- /dev/null +++ b/cyberdog_bluetooth_network/cyberdog_bluetooth_network/bt_core.py @@ -0,0 +1,848 @@ +#!/usr/bin/python3 +# +# Copyright (c) 2023 Xiaomi Corporation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import json +import re +import subprocess +import threading +import time + +import dbus +import dbus.exceptions +import dbus.mainloop.glib +import dbus.service + +# import rclpy +# from rclpy.node import Node +# from std_msgs.msg import Bool, Int8, String +from protocol.msg import WifiInfo + +from . import bluetooth_node + +try: + from gi.repository import GObject +except ImportError: + import gobject as GObject + +ad_manager = None +service_manager = None +robotAdv = None +robotApp = None +mainloop = None + + +notifyChar = None +# gnotifystr = {'ssid': None, 'bssid': None, 'ip': None, 'status': 0} + +# 反馈给APP的错误码 +code = 0 +g_current_mac = '' +connected = 0 +g_connect_status = 0 +adv_status = 0 + +app_send_mac = '' +app_send_ssid = '' +app_send_ip = '' +app_send_pwd = '' +app_send_type = '' +mac_address_get = '' + +# 获取wifi信息的状态反馈 +WIFI_INFO_GET_SUCCESS = 1000 # 获取成功 +WIFI_INFO_SSID_ERROR = 1001 # 获取ssid失败 +WIFI_INFO_PWD_ERROR = 1002 # 获取pwd失败 +WIFI_INFO_IP_ERROR = 1003 # 获取ip失败 +WIFI_INFO_MAC_ERROR = 1004 # 获取mac失败 + +# 网络状态反馈 +NETWORK_CONNECT_SUCCESS = 2000 # 连接成功，且能访问外网 +NETWORK_CONNECT_FAILED = 2001 # 连接失败，连不上这个网络 +NETWORK_NOT_CONNECT_INTERNET = 2002 # 连接失败，连上了这个网络，但是没有连上互联网 +NETWORK_DISCONNECT = 2003 # 断开连接 + + +NETWORK_PATH = '/etc/NetworkManager/system-connections/' + +BLUEZ_SERVICE_NAME = 'org.bluez' +ADAPTER_INTERFACE = BLUEZ_SERVICE_NAME + '.Adapter1' +DEVICE_INTERFACE = BLUEZ_SERVICE_NAME + '.Device1' +GATT_MANAGER_IFACE = 'org.bluez.GattManager1' +DBUS_OM_IFACE = 'org.freedesktop.DBus.ObjectManager' +DBUS_PROP_IFACE = 'org.freedesktop.DBus.Properties' +LE_ADVERTISING_MANAGER_IFACE = 'org.bluez.LEAdvertisingManager1' +LE_ADVERTISEMENT_IFACE = 'org.bluez.LEAdvertisement1' +GATT_SERVICE_IFACE = 'org.bluez.GattService1' +GATT_CHRC_IFACE = 'org.bluez.GattCharacteristic1' +GATT_DESC_IFACE = 'org.bluez.GattDescriptor1' + + +class InvalidArgsException(dbus.exceptions.DBusException): + _dbus_error_name = 'org.freedesktop.DBus.Error.InvalidArgs' + + +class NotSupportedException(dbus.exceptions.DBusException): + _dbus_error_name = 'org.bluez.Error.NotSupported' + + +class NotPermittedException(dbus.exceptions.DBusException): + _dbus_error_name = 'org.bluez.Error.NotPermitted' + + +class InvalidValueLengthException(dbus.exceptions.DBusException): + _dbus_error_name = 'org.bluez.Error.InvalidValueLength' + + +class FailedException(dbus.exceptions.DBusException): + _dbus_error_name = 'org.bluez.Error.Failed' + + +class Advertisement(dbus.service.Object): + PATH_BASE = '/org/bluez/example/advertisement' + + def __init__(self, bus, index, advertising_type): + self.path = self.PATH_BASE + str(index) + self.bus = bus + self.ad_type = advertising_type + self.service_uuids = None + self.manufacturer_data = None + self.solicit_uuids = None + self.service_data = None + self.local_name = '铁蛋' + self.include_tx_power = None + self.discoverable = True + dbus.service.Object.__init__(self, bus, self.path) + + def get_properties(self): + properties = {} + properties['Type'] = self.ad_type + if self.service_uuids is not None: + properties['ServiceUUIDs'] = dbus.Array(self.service_uuids, + signature='s') + if self.solicit_uuids is not None: + properties['SolicitUUIDs'] = dbus.Array(self.solicit_uuids, + signature='s') + if self.manufacturer_data is not None: + properties['ManufacturerData'] = dbus.Dictionary( + self.manufacturer_data, signature='qv') + if self.service_data is not None: + properties['ServiceData'] = dbus.Dictionary(self.service_data, + signature='sv') + if self.local_name is not None: + properties['LocalName'] = dbus.String(self.local_name) + if self.include_tx_power is not None: + properties['IncludeTxPower'] = dbus.Boolean(self.include_tx_power) + return {LE_ADVERTISEMENT_IFACE: properties} + + def get_path(self): + return dbus.ObjectPath(self.path) + + def add_service_uuid(self, uuid): + if not self.service_uuids: + self.service_uuids = [] + self.service_uuids.append(uuid) + + def add_solicit_uuid(self, uuid): + if not self.solicit_uuids: + self.solicit_uuids = [] + self.solicit_uuids.append(uuid) + + def add_manufacturer_data(self, manuf_code, data): + if not self.manufacturer_data: + self.manufacturer_data = dbus.Dictionary({}, signature='qv') + self.manufacturer_data[manuf_code] = dbus.Array(data, signature='y') + + def add_service_data(self, uuid, data): + if not self.service_data: + self.service_data = dbus.Dictionary({}, signature='sv') + self.service_data[uuid] = dbus.Array(data, signature='y') + + def add_local_name(self, name): + if not self.local_name: + self.local_name = '' + self.local_name = dbus.String(name) + + @dbus.service.method(DBUS_PROP_IFACE, + in_signature='s', + out_signature='a{sv}') + def GetAll(self, interface): + print('GetAll') + if interface != LE_ADVERTISEMENT_IFACE: + raise InvalidArgsException() + print('returning props') + return self.get_properties()[LE_ADVERTISEMENT_IFACE] + + @dbus.service.method(LE_ADVERTISEMENT_IFACE, + in_signature='', + out_signature='') + def Release(self): + print('%s: Released!' % self.path) + + +class RobotAdvertisement(Advertisement): + + def __init__(self, bus, index, g_bt_local_name): + Advertisement.__init__(self, bus, index, 'peripheral') + self.add_service_uuid('C420') # 添加广播的服务,UUID + self.add_manufacturer_data(0xffff, [0x00, 0x01, 0x02, 0x03, 0x04]) + self.add_service_data('9999', [0x00, 0x01, 0x02, 0x03, 0x04]) + self.add_local_name('铁蛋' + g_bt_local_name) # 设置蓝牙广播名称 + self.include_tx_power = True + + +class RobotApplication(dbus.service.Object): + # org.bluez.GattApplication1 interface implementation + + def __init__(self, bus, bluetoothCore): + self.path = '/' + self.services = [] + dbus.service.Object.__init__(self, bus, + self.path) + print('add service: ' + str(bus)) + self.add_service(RobotService(bus, 0, bluetoothCore)) # 将服务添加到服务列表中 + + def get_path(self): + print('get_path: ' + str(self)) + return dbus.ObjectPath(self.path) + + def add_service(self, service): + print('add_service: ' + str(service)) + self.services.append(service) + + # 用于获取应用程序及其包含的服务、特征和描述符的对象信息。 + @dbus.service.method(DBUS_OM_IFACE, out_signature='a{oa{sa{sv}}}') + def GetManagedObjects(self): + response = {} + for service in self.services: + response[service.get_path()] = service.get_properties() + chrcs = service.get_characteristics() + for chrc in chrcs: + response[chrc.get_path()] = chrc.get_properties() + descs = chrc.get_descriptors() + for desc in descs: + response[desc.get_path()] = desc.get_properties() + + return response + + +class Service(dbus.service.Object): + # org.bluez.GattService1 interface implementation + + PATH_BASE = '/org/bluez/example/service' + + def __init__(self, bus, index, uuid, primary): + self.path = self.PATH_BASE + str(index) + self.bus = bus + self.uuid = uuid + self.primary = primary + self.characteristics = [] + dbus.service.Object.__init__(self, bus, self.path) + + def get_properties(self): + return { + GATT_SERVICE_IFACE: { + 'UUID': self.uuid, + 'Primary': self.primary, + 'Characteristics': dbus.Array( + self.get_characteristic_paths(), + signature='o') + } + } + + def get_path(self): + return dbus.ObjectPath(self.path) + + def add_characteristic(self, characteristic): + self.characteristics.append(characteristic) + + def get_characteristic_paths(self): + result = [] + for chrc in self.characteristics: + result.append(chrc.get_path()) + return result + + def get_characteristics(self): + return self.characteristics + + @dbus.service.method(DBUS_PROP_IFACE, + in_signature='s', + out_signature='a{sv}') + def GetAll(self, interface): + if interface != GATT_SERVICE_IFACE: + raise InvalidArgsException() + + return self.get_properties()[GATT_SERVICE_IFACE] + + +class Characteristic(dbus.service.Object): + # org.bluez.GattCharacteristic1 interface implementation + + def __init__(self, bus, index, uuid, flags, service): + self.path = service.path + '/char' + str(index) + self.bus = bus + self.uuid = uuid + self.service = service + self.flags = flags + self.descriptors = [] + dbus.service.Object.__init__(self, bus, self.path) + + def get_properties(self): + return { + GATT_CHRC_IFACE: { + 'Service': self.service.get_path(), + 'UUID': self.uuid, + 'Flags': self.flags, + 'Descriptors': dbus.Array( + self.get_descriptor_paths(), + signature='o') + } + } + + def get_path(self): + return dbus.ObjectPath(self.path) + + def add_descriptor(self, descriptor): + self.descriptors.append(descriptor) + + def get_descriptor_paths(self): + result = [] + for desc in self.descriptors: + result.append(desc.get_path()) + return result + + def get_descriptors(self): + return self.descriptors + + @dbus.service.method(DBUS_PROP_IFACE, + in_signature='s', + out_signature='a{sv}') + def GetAll(self, interface): + if interface != GATT_CHRC_IFACE: + raise InvalidArgsException() + + return self.get_properties()[GATT_CHRC_IFACE] + + @dbus.service.method(GATT_CHRC_IFACE, + in_signature='a{sv}', + out_signature='ay') + def ReadValue(self, options): + global log + print('Default ReadValue called, returning error') + raise NotSupportedException() + + @dbus.service.method(GATT_CHRC_IFACE, in_signature='aya{sv}') + def WriteValue(self, value, options): + global log + print('Default WriteValue called, returning error') + raise NotSupportedException() + + @dbus.service.method(GATT_CHRC_IFACE) + def StartNotify(self): + global log + print('Default StartNotify called, returning error') + raise NotSupportedException() + + @dbus.service.method(GATT_CHRC_IFACE) + def StopNotify(self): + global log + print('Default StopNotify called, returning error') + raise NotSupportedException() + + @dbus.service.signal(DBUS_PROP_IFACE, + signature='sa{sv}as') + def PropertiesChanged(self, interface, changed, invalidated): + pass + + +class Descriptor(dbus.service.Object): + + def __init__(self, bus, index, uuid, flags, characteristic): + self.path = characteristic.path + '/desc' + str(index) + self.bus = bus + self.uuid = uuid + self.flags = flags + self.chrc = characteristic + dbus.service.Object.__init__(self, bus, self.path) + + def get_properties(self): + return { + GATT_DESC_IFACE: { + 'Characteristic': self.chrc.get_path(), + 'UUID': self.uuid, + 'Flags': self.flags, + } + } + + def get_path(self): + return dbus.ObjectPath(self.path) + + @dbus.service.method(DBUS_PROP_IFACE, + in_signature='s', + out_signature='a{sv}') + def GetAll(self, interface): + if interface != GATT_DESC_IFACE: + raise InvalidArgsException() + + return self.get_properties()[GATT_DESC_IFACE] + + @dbus.service.method(GATT_DESC_IFACE, + in_signature='a{sv}', + out_signature='ay') + def ReadValue(self, options): + global log + print('Default ReadValue called, returning error') + raise NotSupportedException() + + @dbus.service.method(GATT_DESC_IFACE, in_signature='aya{sv}') + def WriteValue(self, value, options): + global log + print('Default WriteValue called, returning error') + raise NotSupportedException() + + +# RobotService for ATT +class RobotService(Service): + ROBOT_SVC_UUID = '0000C420-0000-1000-8000-00805F9B34FB' + + def __init__(self, bus, index, bluetoothCore): + Service.__init__(self, bus, index, self.ROBOT_SVC_UUID, True) + self.add_characteristic(writeWifiParameterFromPhone(bus, 0, self, bluetoothCore)) + self.add_characteristic(notifyWifiStatusToPhone(bus, 1, self)) + + +class writeWifiParameterFromPhone(Characteristic): + WIFI_CHRC_UUID_WRITE = '0000C421-0000-1000-8000-00805F9B34FB' + + def __init__(self, bus, index, service, bluetoothCore): + Characteristic.__init__( + self, bus, index, + self.WIFI_CHRC_UUID_WRITE, + ['write-without-response'], + service) + self.pub_info = bluetooth_node.PublisherPhoneIP() + self.logger = bluetoothCore.get_logger() + self.bluetooth_core = bluetoothCore + self.sn = bluetoothCore.dog_sn + + def WriteValue(self, value, options): + global code, app_send_ssid, app_send_pwd, app_send_ip, app_send_mac, app_send_type + try: + format_value = bytes(value).decode('utf-8') + self.logger.info('WriteValue format all value :%s' % format_value) + text = json.loads(format_value) + except UnicodeDecodeError: + self.logger.info('WriteValue UnicodeDecodeError') + except json.JSONDecodeError: + self.logger.info('WriteValue JSONDecodeError') + + ssid = text.get('ssid') + pwd = text.get('pwd') + ip = text.get('ip') + mac = text.get('mac') + type_receive = text.get('type') + + code = WIFI_INFO_GET_SUCCESS + if ssid is not None: + self.logger.info('WriteValue ssid %s' % repr(text['ssid'])) + app_send_ssid = ssid + else: + self.logger.info('WriteValue ssid is none!!!') + app_send_ssid = '' + code = WIFI_INFO_SSID_ERROR + + if pwd is not None: + self.logger.info('WriteValue pwd %s' % repr(text['pwd'])) + app_send_pwd = pwd + else: + self.logger.info('WriteValue pwd is none!!!') + app_send_pwd = '' + # code = WIFI_INFO_PWD_ERROR + + if ip is not None: + self.logger.info('WriteValue ip %s' % repr(text['ip'])) + app_send_ip = ip + else: + self.logger.info('WriteValue ip is none!!!') + app_send_ip = '' + code = WIFI_INFO_IP_ERROR + + if mac is not None: + self.logger.info('WriteValue mac %s' % repr(text['mac'])) + app_send_mac = mac + else: + self.logger.info('WriteValue ip is none!!!') + app_send_mac = '' + code = WIFI_INFO_MAC_ERROR + + if type_receive is not None: + # self.logger.info('WriteValue type %s' % repr(text['type'])) + app_send_type = type_receive + else: + self.logger.info('WriteValue ip is none!!!') + app_send_type = '' + code = 1005 + + if(code == WIFI_INFO_GET_SUCCESS): + if(app_send_type == 'wifi' or app_send_type == 'hotspot'): + msg = WifiInfo() + msg.ssid = app_send_ssid + msg.ip = app_send_ip + msg.pwd = app_send_pwd + msg.mac = app_send_mac + msg.type = app_send_type + self.pub_info.publisher(msg) + if(app_send_mac != '' and app_send_type != 'heartBeat' and + (app_send_ip == '' or app_send_ssid == '' or app_send_pwd == '')): + code = 3000 + self.doNotifyOnce(app_send_ssid, app_send_ip, code, self.sn) + # 蓝牙心跳 + if(app_send_type == 'heartBeat'): + self.logger.info('send heartBeat') + code = 5000 + self.doNotifyOnce(app_send_ssid, app_send_ip, code, self.sn) + self.bluetooth_core.GetAppData( + app_send_ssid, app_send_pwd, app_send_ip, app_send_mac, app_send_type) + return + + def doNotifyOnce(self, ssid, ip, code, sn): + gnotifystr = { + 'ssid': ssid, + 'ip': ip, + 'code': code, + 'sn': sn} + self.logger.info(' doNotifyOnce %s' % repr(gnotifystr)) + arraySend = convert_to_dbus_array(json.dumps(gnotifystr)) + # 发送Dbus通知 + notifyChar.PropertiesChanged(GATT_CHRC_IFACE, {'Value': arraySend}, []) + + +# notify wlan state +class notifyWifiStatusToPhone(Characteristic): + WIFI_CHRC_UUID_NOTIFY = '0000C422-0000-1000-8000-00805F9B34FB' + + def __init__(self, bus, index, service): + Characteristic.__init__( + self, bus, index, + self.WIFI_CHRC_UUID_NOTIFY, + ['notify'], + service) + + global notifyChar + notifyChar = self + + # phone start notify + def StartNotify(self): + # RegADV(False) # unregister adv and update led + # notify once immediately when phone connected + gnotifystr = { + 'ssid': 'strat notify', + 'ip': 'strat notify', + 'code': 4000, + 'sn': 'strat notify'} + arraySend = convert_to_dbus_array(json.dumps(gnotifystr)) + # 发送Dbus通知 + notifyChar.PropertiesChanged(GATT_CHRC_IFACE, {'Value': arraySend}, []) + + # phone stop notify + def StopNotify(self): + pass + # global bleStatus + # if BLE_STATUS_DISCONNECTED != bleStatus: + # bleStatus = BLE_STATUS_DISCONNECTED + # print(' StopNotify: set bleStatus ') + # # RegATT(False) + # else: + # print(' StopNotify: Current bleStatus is: ' + repr(bleStatus)) + + +class TimeoutTimer: + + def __init__(self, _timeout): + self.userTimeoutFunc = None + self.userTimeoutFunc_args = None + self.timeoutAlarm: bool = False + self.timeoutHandle = threading.Timer(_timeout, self.timeoutFunc) + + def addUserTimeoutFunc(self, _user_timeout_func, *_args): + self.userTimeoutFunc = _user_timeout_func + self.userTimeoutFunc_args = _args + + def startTimer(self): + self.timeoutHandle.start() + + def timeoutFunc(self): + self.timeoutAlarm = True + if self.userTimeoutFunc is not None: + self.userTimeoutFunc(*self.userTimeoutFunc_args) + + def getAlarm(self): + return self.timeoutAlarm + + def stopTimer(self): + if self.timeoutHandle is not None: + self.timeoutHandle.cancel() + self.timeoutAlarm = False + + def resetTimer(self): + if self.timeoutHandle and self.timeoutHandle.is_alive(): + self.timeoutHandle.cancel() + + +def convert_to_dbus_array(string): + value = [] + for c in string: + value.append(dbus.Byte(c.encode())) + return value + + +class BluetoothCore: + + def __init__(self, logger): + self.__logger = logger + self.__logger.info('BluetoothCore: __init__') + self.g_bt_local_name = '铁蛋' + self.adv_status = 0 + self.g_current_mac = '' + self.mac_address_get = '' + self.receive_mac = '' + self.receive_ssid = '' + self.receive_pwd = '' + self.receive_ip = '' + self.dog_sn = '' + self.connected = 0 + self.masters = [] + self.slavers = [] + command = 'factory-tool -f /usr/share/factory_cmd_config/system.xml -i SN' + self.g_bt_local_name = self.runCommand(command) + self.dog_sn = self.g_bt_local_name.rstrip('\n') + self.g_bt_local_name = (self.g_bt_local_name[4] + + self.g_bt_local_name[5] + + self.g_bt_local_name[9] + + self.g_bt_local_name[10] + + self.g_bt_local_name[11]) + self.__logger.info('g_bt_local_name: %s' % self.g_bt_local_name) + dbus.mainloop.glib.DBusGMainLoop(set_as_default=True) + self.bus = dbus.SystemBus() + self.remote_om = dbus.Interface(self.bus.get_object( + BLUEZ_SERVICE_NAME, '/'), DBUS_OM_IFACE) + self.adapter = self.find_adapter(self.bus) + if not self.adapter: + self.__logger.info('GattManager1 interface not found') + return + + self.service_manager = dbus.Interface( + self.bus.get_object(BLUEZ_SERVICE_NAME, self.adapter), + GATT_MANAGER_IFACE) + self.robotApp = RobotApplication(self.bus, self) + self.adapter_props = dbus.Interface(self.bus.get_object(BLUEZ_SERVICE_NAME, self.adapter), + 'org.freedesktop.DBus.Properties') + self.adapter_props.Set('org.bluez.Adapter1', 'Powered', dbus.Boolean(1)) + self.ad_manager = dbus.Interface(self.bus.get_object(BLUEZ_SERVICE_NAME, self.adapter), + LE_ADVERTISING_MANAGER_IFACE) + self.bus.add_signal_receiver(self.properties_changed, + dbus_interface='org.freedesktop.DBus.Properties', + signal_name='PropertiesChanged', + path_keyword='path') + + self.bus.add_signal_receiver(self.interfaces_added, + dbus_interface='org.freedesktop.DBus.ObjectManager', + signal_name='InterfacesAdded') + self.robotAdv = RobotAdvertisement(self.bus, 0, self.g_bt_local_name) + self.connected_timer = None + + def find_adapter(self, bus): + # remote_om = dbus.Interface(bus.get_object(BLUEZ_SERVICE_NAME, '/'), + # DBUS_OM_IFACE) + objects = self.remote_om.GetManagedObjects() + + for o, props in objects.items(): + if GATT_MANAGER_IFACE in props.keys(): + if LE_ADVERTISING_MANAGER_IFACE in props: + return o + return None + + def runCommand(self, cmd: str): + self.__logger.info(' run command:%s' % cmd) + output = subprocess.Popen( + cmd, shell=True, stdout=subprocess.PIPE, stderr=subprocess.STDOUT) + output.wait() # wait for cmd done + tmp = str(output.stdout.read(), encoding='utf-8') + return tmp + + def extract_mac_address_from_path(self, path): + parts = path.split('/') + if len(parts) == 5: + return parts[-1].replace('_', ':').upper() + return None + + def parse_connections_output(self, output): + masters_connect = re.findall(r'< LE (\S+) handle \d+ state \d+ lm MASTER', output) + slaves_connect = re.findall(r'> LE (\S+) handle \d+ state \d+ lm SLAVE', output) + return masters_connect, slaves_connect + + def disconnect_device(self, mac_address): + script = f""" + bluetoothctl << EOF + disconnect {mac_address} + exit + EOF + """ + self.__logger.info('disconnect_device: %s' % mac_address) + self.runCommand(script) + + def check_connected(self): + mac_address = '02:00:00:00:00:00' + self.connected_timer.stopTimer() + self.__logger.info('receive_mac: %s' % self.receive_mac) + if self.receive_mac == mac_address: + self.__logger.info('app connect success ') + self.connected = 1 + self.__logger.info('device is connected, close advertise ') + else: + self.disconnect_device(self.mac_address_get) + self.__logger.info(' disconnect :%s' % self.mac_address_get) + self.connected = 0 + self.__logger.info('receive_mac: %s' % self.receive_mac) + pass + + def set_connected_status(self, status): + self.masters, self.slavers = self.parse_connections_output( + self.runCommand('hcitool con')) + if status == 1: + self.__logger.info('connected') + if len(self.slavers) > 0: + self.__logger.info(' APP is connecting') + self.connected = 1 + self.connected_timer = TimeoutTimer(10) + self.connected_timer.addUserTimeoutFunc(self.check_connected) + self.connected_timer.startTimer() + else: + self.__logger.info('disconnected') + if len(self.slavers) == 0: + self.connected = 0 + self.receive_mac = '' + self.__logger.info('device is disconnected, open advertise ') + pass + + def properties_changed(self, interface, changed, invalidated, path): + if (interface == DEVICE_INTERFACE): + if ('Connected' in changed): + self.mac_address_get = self.extract_mac_address_from_path(path) + self.mac_address_get = self.splite_mac_address_get(self.mac_address_get) + self.__logger.info('connect_change,mac:%s' % self.mac_address_get) + self.set_connected_status(changed['Connected']) + + def interfaces_added(self, path, interfaces): + if DEVICE_INTERFACE in interfaces: + properties = interfaces[DEVICE_INTERFACE] + if ('Connected' in properties): + self.mac_address_get = self.extract_mac_address_from_path(path) + self.mac_address_get = self.splite_mac_address_get(self.mac_address_get) + self.__logger.info('interface add,mac_addess: %s' % self.mac_address_get) + self.set_connected_status(properties['Connected']) + + def register_app_cb(self): + self.__logger.info('register_app_cb: GATT application registered!!!') + + def register_app_error_cb(self, error): + self.__logger.info('Failed to unregister application: %s' % str(error)) + + def unregister_app_cb(self): + self.__logger.info('register_app_cb: GATT application unregistered!!!') + + def unregister_app_error_cb(self, error): + self.__logger.info('Failed to unregister application: %s' % str(error)) + + def register_ad_cb(self): + self.__logger.info('register_ad_cb: Advertisement registered!!!') + self.adv_status = 1 + + def register_ad_error_cb(self, error): + self.__logger.info(' Failed to register advertisement: %s' % str(error)) + self.adv_status = 0 + + def unregister_ad_cb(self): + self.__logger.info('register_ad_cb: Advertisement Unregistered!!!') + self.adv_status = 0 + + def unregister_ad_error_cb(self, error): + self.__logger.info('Failed to Unregister advertisement: %s' % str(error)) + self.adv_status = 1 + + def RegADV(self, state): + if state is True: + self.__logger.info('RegADV->RegisterAdvertisement') + self.ad_manager.RegisterAdvertisement(self.robotAdv.get_path(), {}, + reply_handler=self.register_ad_cb, + error_handler=self.register_ad_error_cb) + else: + self.__logger.info('RegADV->UnregisterAdvertisement') + self.ad_manager.UnregisterAdvertisement(self.robotAdv.get_path(), + reply_handler=self.unregister_ad_cb, + error_handler=self.unregister_ad_error_cb) + time.sleep(3) + + # 功能：注册或销毁GATT服务 + def RegATT(self, state): + if state is True: + self.__logger.info('RegATT->RegisterApplication') + self.service_manager.RegisterApplication(self.robotApp.get_path(), {}, + reply_handler=self.register_app_cb, + error_handler=self.register_app_error_cb) + else: + self.__logger.info('RegATT->UnregisterApplication') + self.service_manager.UnregisterApplication(self.robotApp.get_path(), + reply_handler=self.unregister_app_cb, + error_handler=self.unregister_app_error_cb) + + def bluetooth_status(self): + return self.connected, self.adv_status + + def main_bluetooth(self): + mainloop = GObject.MainLoop() + mainloop.run() + + def splite_mac_address_get(self, address): + self.__logger.info('splite_mac_address_get %s' % repr(address)) + match = re.match(r'DEV:(\w+)$', address) + if match: + target_str = match.group(1) + return target_str + else: + self.__logger.info('not search match mac address') + return '' + + def get_logger(self): + return self.__logger + + def doNotify(self, ssid, ip, code, sn): + gnotifystr = { + 'ssid': ssid, + 'ip': ip, + 'code': code, + 'sn': sn} + self.__logger.info('doNotifyOnce %s' % repr(gnotifystr)) + arraySend = convert_to_dbus_array(json.dumps(gnotifystr)) + notifyChar.PropertiesChanged(GATT_CHRC_IFACE, {'Value': arraySend}, []) + + def GetAppData(self, ssid, pwd, ip, mac, type_receive): + self.receive_ssid = ssid + self.receive_pwd = pwd + self.receive_ip = ip + self.receive_mac = mac + self.receive_type = type_receive + self.__logger.info('GetAppData: %s %s %s %s %s' % + (ssid, pwd, ip, mac, type_receive)) diff --git a/cyberdog_bluetooth_network/cyberdog_bluetooth_network/cyberdog_bluetooth_main.py b/cyberdog_bluetooth_network/cyberdog_bluetooth_network/cyberdog_bluetooth_main.py new file mode 100644 index 0000000..7f5d7ec --- /dev/null +++ b/cyberdog_bluetooth_network/cyberdog_bluetooth_network/cyberdog_bluetooth_main.py @@ -0,0 +1,38 @@ +#!/usr/bin/python3 +# +# Copyright (c) 2023 Xiaomi Corporation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import rclpy +from rclpy.executors import MultiThreadedExecutor + +# from . import bt_core +from . import bluetooth_node + + +def main(args=None): + rclpy.init(args=args) + bt_node = bluetooth_node.BluetoothNode('cyberdog_bluetooth_network') + print('[BluetoothCore]: Hi from cyberdog_bluetooth.') + executor = MultiThreadedExecutor() + executor.add_node(bt_node) + try: + executor.spin() + except KeyboardInterrupt: + bt_node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/cyberdog_bluetooth_network/package.xml b/cyberdog_bluetooth_network/package.xml new file mode 100644 index 0000000..257f249 --- /dev/null +++ b/cyberdog_bluetooth_network/package.xml @@ -0,0 +1,24 @@ + + + + cyberdog_bluetooth_network + 0.0.0 + TODO: Package description + dingsong1 + TODO: License declaration + + rclpy + std_msgs + sensor_msgs + protocol + + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/cyberdog_bluetooth_network/resource/cyberdog_bluetooth_network b/cyberdog_bluetooth_network/resource/cyberdog_bluetooth_network new file mode 100644 index 0000000..e69de29 diff --git a/cyberdog_bluetooth_network/setup.cfg b/cyberdog_bluetooth_network/setup.cfg new file mode 100644 index 0000000..663da2b --- /dev/null +++ b/cyberdog_bluetooth_network/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/cyberdog_bluetooth_network +[install] +install_scripts=$base/lib/cyberdog_bluetooth_network diff --git a/cyberdog_bluetooth_network/setup.py b/cyberdog_bluetooth_network/setup.py new file mode 100644 index 0000000..f84a261 --- /dev/null +++ b/cyberdog_bluetooth_network/setup.py @@ -0,0 +1,26 @@ +from setuptools import setup + +package_name = 'cyberdog_bluetooth_network' + +setup( + name=package_name, + version='0.0.0', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='dingsong1', + maintainer_email='dingsong1@xiaomi.com', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'cyberdog_bluetooth_main = cyberdog_bluetooth_network.cyberdog_bluetooth_main:main' + ], + }, +) diff --git a/cyberdog_bluetooth_network/test/__pycache__/test_copyright.cpython-36-pytest-6.2.5.pyc b/cyberdog_bluetooth_network/test/__pycache__/test_copyright.cpython-36-pytest-6.2.5.pyc new file mode 100644 index 0000000000000000000000000000000000000000..0a08f66197dddeaf75fed5701d6ec8ebf224f327 GIT binary patch literal 964 zcmZuwPjAyO6t|r;P18RNhPrXv1&Lf5#uyS3orZwH0qp>{TtXVVY1$;NY?slgy-*HZ zxN+nQ@Co=XzH;I#?8I}z#*nb&_viPX-~0JFIp}u1A1~jrzJ<_FbZyySAH$ZQ%B$Eoy&48@$0`hvK)$Z~THv=+wvBF5;|A zjAvE@wgdYaZ21+0f^L+ggj!$l8c}!?1 znIm68ys?75;tg1iu}T7QV*?lJej|Ofw!m-F7Wm!{qV1KHVL)x=0^VBLhV@6dt&P@X z4I0?@PBk8ATQRB34PueL7Z}K1km2y+*4=ya<$Yi74TpQiJn*F^BJqi~01rVNoOe$6 zqNICF2rlILmUk3O$%Lx%4+3 z3#K~|jz)_tS6L~wlXJ+BdN-02zQ}2n8l^cO#d%c51=Foa#$#qGK(#R!MGTcIE`>~W z?`dUbynJDp>g~%E_FQ=k8s!V7I9HRXWa^BISs+CcWTj#vPE6k*S&o>XJpE6q8BkD7DdF>_ z$kK@#%$K_NFSHH4unErG*8urrY< z7~>VBn}0CThBM)97;>qizckF(UcaPMS2I6=nCaG4O6cU(H7gd?fUb!hL&SYjaJtCZ R(GBpV2f{@i)WNRh;NJ@57*YTL literal 0 HcmV?d00001 diff --git a/cyberdog_bluetooth_network/test/__pycache__/test_flake8.cpython-36-pytest-6.2.5.pyc b/cyberdog_bluetooth_network/test/__pycache__/test_flake8.cpython-36-pytest-6.2.5.pyc new file mode 100644 index 0000000000000000000000000000000000000000..9137c4ac335522a199f0ca0b5a128386f55507cb GIT binary patch literal 1022 zcmZuw&2G~`5Z+xoj_V|)rBVw5q+BZSrKXf3K%^=H3J1giZV{4kyqnaC?TvOfgjDv0 zaODLMXC$70ciAf^UZE#u?H1*LCC`7pnfb;!=ybeaFF!D4A@m!qJT}4 zr6wZwiMAka#KGC^6F$pncb9f!PFYu~MasH0C*7bsk3^p2Bl+0V*gMSFx`f9IG81=N^+^4ltYEo z*Ft&9XDO{p!%@lyQ5xn^#`Ib!qaia0fHveJivYQrC6gmm?ybje_BNop1&}M z)mxX6(cZ4~!cIrKK%M4Sa-CK3`Yo~5^f7N;L6NwuDaUKV!JYxYhB=LL{ zyneJF4$>J@oWr-3Gd1U85=apTNv@cPV$(l}7Xv0JAN{A)a28a=4by1C9_~#Sy0zkO zL07o}0$J*uI9|0{)xRQTMcgzaxKA=pXDK_n0-p3hxTuZV I*tHz|7qu2E5C8xG literal 0 HcmV?d00001 diff --git a/cyberdog_bluetooth_network/test/__pycache__/test_pep257.cpython-36-pytest-6.2.5.pyc b/cyberdog_bluetooth_network/test/__pycache__/test_pep257.cpython-36-pytest-6.2.5.pyc new file mode 100644 index 0000000000000000000000000000000000000000..82ad9b59bac2a598211de90b3ec5fa4ca6de4c4c GIT binary patch literal 974 zcmZuwO>fgM7`B}>O+U9WX)w4{AS7~W*v3FebQ%H%2ebp+dI@RlW@(c+vRy`}_Ch&u z;l`0az%Ssx_{xdDuoJJ74fTK}zu$h}$B&bPPN(_n^#^v(Lg+VITQ=Cou;oh-6j7X_ zAuctUSQC3#1Dm8yRv*@Jm3N0Ob0>{Svy?4rQTq#8;T0aXDSnT<+8^kIPCcycEKKsq zcxKgLJFs8Cmft}rm_|uVsPzpm5w(|iffjg5mSjQXSA{2pwijfKX~)FJWM$L30!%A3 zb>th!*A_5WyaUTIR!$&qY`{X@A7lqDE%58K0erQFlE?C5vw0Ls$%HDq96}d3dgx(c_yJWnswoI4-8?Sl z22BtOy%j`Zng)UuBH~#Q3Z~nT4o0&iRY@+jlX9q#`c@#%_$;MmX%MG;6sAEQW=uB% z8IGCh0Nuu1WFd4ev1BsV-RGs5@cfklt+%gB*ry^5^H4#?s|nWtYnjQ_(fxI2uX3v1 zkJg>XKatplqnD9?#xv$qLk7=d|LxPgV3f|7;#{2tIaB9cOnoULKgksnVPxj|(R{=N zu3KyVHcXY9AduC?Hge&`PtXqPB5?;|UPrsv6pAhc^lng* z-mmTl)BA#*i$no||3bQc6%%bZGu#4ta}`_xFrR+)fKJuQ{0(9XFH~M>=u~y({F0P4 baoa#}pJklRQg*Zfo@|0}Q5&_fYdQEYg5(+u literal 0 HcmV?d00001 diff --git a/cyberdog_bluetooth_network/test/test_copyright.py b/cyberdog_bluetooth_network/test/test_copyright.py new file mode 100644 index 0000000..cc8ff03 --- /dev/null +++ b/cyberdog_bluetooth_network/test/test_copyright.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/cyberdog_bluetooth_network/test/test_flake8.py b/cyberdog_bluetooth_network/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/cyberdog_bluetooth_network/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/cyberdog_bluetooth_network/test/test_pep257.py b/cyberdog_bluetooth_network/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/cyberdog_bluetooth_network/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' From d6f5af5cbf84d2afb87783a891b454278f1843ac Mon Sep 17 00:00:00 2001 From: dingdingsong Date: Fri, 24 Nov 2023 20:36:04 +0800 Subject: [PATCH 02/12] [Modify]: Modify audio file MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 1.修改配网的语音文件； Signed-off-by: dingdingsong --- cyberdog_audio/include/cyberdog_audio/speech_handler.hpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/cyberdog_audio/include/cyberdog_audio/speech_handler.hpp b/cyberdog_audio/include/cyberdog_audio/speech_handler.hpp index 5095a42..ec2e35e 100644 --- a/cyberdog_audio/include/cyberdog_audio/speech_handler.hpp +++ b/cyberdog_audio/include/cyberdog_audio/speech_handler.hpp @@ -280,9 +280,12 @@ class SpeechHandler final {12, "wifi_communication"}, {13, "wifi_scan_code_ip_error"}, {14, "wifi_scan_code_info_error"}, - {15, "wifi_request_open_camera_success"}, - {16, "wifi_request_open_camera_fail"}, - {17, "wifi_request_close_camera_success"}, + // {15, "wifi_request_open_camera_success"}, + // {16, "wifi_request_open_camera_fail"}, + // {17, "wifi_request_close_camera_success"}, + {15, "is_connecting"}, + {16, "wait_connect_network"}, + {17, "connect_network_failed_retry"}, {18, "wifi_request_close_camera_fail"}, {21, "face_entry_add_face"}, {22, "face_entry_cancel_add_face"}, From 3dfa9aadf4451d44bf0632b0daa645690953e692 Mon Sep 17 00:00:00 2001 From: dingdingsong Date: Sat, 25 Nov 2023 17:57:32 +0800 Subject: [PATCH 03/12] [New]: Add connector config toml file and wait services function MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 1.增加了记录wifi类型的toml文件； 2.修改了蓝牙配网节点，等待蓝牙和Dbus服务准备好，且sleep3秒再进行后续步骤； Signed-off-by: dingdingsong --- connector/config/wifirecord.toml | 5 +++++ .../bluetooth_node.py | 20 ++++++++++++++++++- 2 files changed, 24 insertions(+), 1 deletion(-) create mode 100644 connector/config/wifirecord.toml diff --git a/connector/config/wifirecord.toml b/connector/config/wifirecord.toml new file mode 100644 index 0000000..245ac70 --- /dev/null +++ b/connector/config/wifirecord.toml @@ -0,0 +1,5 @@ +[wifi] +user_boots_first_time = false +[wifi.ssid] +type = "type" +provider = "ip" diff --git a/cyberdog_bluetooth_network/cyberdog_bluetooth_network/bluetooth_node.py b/cyberdog_bluetooth_network/cyberdog_bluetooth_network/bluetooth_node.py index 0bb21e2..328e088 100644 --- a/cyberdog_bluetooth_network/cyberdog_bluetooth_network/bluetooth_node.py +++ b/cyberdog_bluetooth_network/cyberdog_bluetooth_network/bluetooth_node.py @@ -13,7 +13,7 @@ # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. - +import subprocess import threading import time from time import sleep @@ -38,6 +38,7 @@ def __init__(self, node_name: str): Node.__init__(self, node_name) self.__logger = self.get_logger() # ROS2 logger self.__logger.info('[BluetoothCore]: BluetoothNode init') + self.check_system_service() self.dog_ssif = '' self.dog_ip = '' self.__multithread_callback_group = ReentrantCallbackGroup() @@ -137,6 +138,23 @@ def __bluetooth_status_timer_callback(self): self.__bluetoothStatePublisher.publish(msg) pass + def check_system_service(self): + self.__logger.info('check system bluetooth and dbus service') + command_bluetooth = ( + 'sudo systemctl status bluetooth.service ' + '| grep running > /dev/null 2>&1') + command_dbus = 'sudo systemctl status dbus.service | grep running > /dev/null 2>&1' + while True: + result_bluetooth = subprocess.call(command_bluetooth, shell=True) + result_dbus = subprocess.call(command_dbus, shell=True) + if result_bluetooth == 0 and result_dbus == 0: + self.__logger.info('Both services are running. Continuing with further actions.') + time.sleep(3) + break + else: + self.__logger.error('Services are not both running. Waiting for 100ms...') + time.sleep(0.1) + class PublisherPhoneIP(Node): From 51527f107606ee379f75ac95d65d4f1dee44153b Mon Sep 17 00:00:00 2001 From: dingdingsong Date: Wed, 29 Nov 2023 11:53:51 +0800 Subject: [PATCH 04/12] [Modify]: modify toml config file and fix bug MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 1.修改connector模块toml文件初时值; 2.修复正常连网流程时灯效错误问题; 3.修复wifi密码错误时反馈信息错误问题; Signed-off-by: dingdingsong --- connector/config/wifi.toml | 1 - connector/config/wifirecord.toml | 1 - connector/src/connector.cpp | 21 ++++++++++----------- 3 files changed, 10 insertions(+), 13 deletions(-) diff --git a/connector/config/wifi.toml b/connector/config/wifi.toml index 16300f3..b58ef1a 100644 --- a/connector/config/wifi.toml +++ b/connector/config/wifi.toml @@ -1,5 +1,4 @@ [wifi] -user_boots_first_time = false [wifi.ssid] password = "password" provider = "provider" diff --git a/connector/config/wifirecord.toml b/connector/config/wifirecord.toml index 245ac70..4234a38 100644 --- a/connector/config/wifirecord.toml +++ b/connector/config/wifirecord.toml @@ -1,5 +1,4 @@ [wifi] -user_boots_first_time = false [wifi.ssid] type = "type" provider = "ip" diff --git a/connector/src/connector.cpp b/connector/src/connector.cpp index e9bc8ac..4be4692 100644 --- a/connector/src/connector.cpp +++ b/connector/src/connector.cpp @@ -382,10 +382,8 @@ void Connector::WiFiSignalCallback(const WiFiMsg::SharedPtr msg) std::lock_guard guard(this->state_msg_mutex_); if (msg->is_connected) { - this->notify_to_app_msg_.ssid = msg->ssid; - this->notify_to_app_msg_.ip = msg->ip; - - + // this->notify_to_app_msg_.ssid = msg->ssid; + // this->notify_to_app_msg_.ip = msg->ip; if ((!this->state_msg_.is_connected) && (std::chrono::system_clock::now() > this->touch_signal_timeout_)) { @@ -396,7 +394,7 @@ void Connector::WiFiSignalCallback(const WiFiMsg::SharedPtr msg) this->state_msg_.is_internet = this->CheckInternet(); if (this->state_msg_.is_internet) { // 当前网络能访问互联网 - this->notify_to_app_msg_.code = 2001; + // this->notify_to_app_msg_.code = 2001; } else { // 当前网络不能访问互联网 this->notify_to_app_msg_.code = 2002; @@ -412,11 +410,9 @@ void Connector::WiFiSignalCallback(const WiFiMsg::SharedPtr msg) if (this->state_msg_.is_connected) { // 网络连接已断开 this->notify_to_app_msg_.code = 2003; - if (this->connect_network_status) { - // this->Interaction(9); this->CtrlAudio(17); - this->CtrlLed(9); + this->CtrlLed(AudioMsg::PID_WIFI_ENTER_CONNECTION_MODE_0); this->connect_network_status = true; } } @@ -607,7 +603,7 @@ void Connector::CameraSignalCallback(const CameraMsg::SharedPtr msg) bool Connector::DoConnect(std::string name, std::string password, std::string provider) { this->CtrlAudio(15); - this->CtrlLed(AudioMsg::PID_WIFI_SCAN_CODE_SUCCEEDED_0); + this->CtrlLed(AudioMsg::PID_WIFI_ENTER_CONNECTION_MODE_0); auto return_true = [&](std::string msg, bool same_wifi) -> bool { this->provider_ip_ = provider; this->SaveWiFi(provider, name, password); @@ -622,7 +618,7 @@ bool Connector::DoConnect(std::string name, std::string password, std::string pr // this->Interaction(AudioMsg::PID_WIFI_EXIT_CONNECTION_MODE_0); this->touch_efficient_ = false; this->srv_code_ = ConnectorSrv::Response::CODE_SUCCESS; - this->connect_code = 2000; + // this->connect_code = 2000; return true; }; auto return_false = [&](std::string msg) -> bool { @@ -634,7 +630,7 @@ bool Connector::DoConnect(std::string name, std::string password, std::string pr this->CtrlAudio(16); this->CtrlLed(AudioMsg::PID_WIFI_WAIT_FOR_SCAN_CODE_0); } - this->connect_code = 2004; + // this->connect_code = 2004; return false; }; auto judge_string = [&](std::string msg) -> bool { @@ -656,6 +652,7 @@ bool Connector::DoConnect(std::string name, std::string password, std::string pr name.c_str()); this->Interaction(AudioMsg::PID_WIFI_CONNECTION_FAILED_0); this->srv_code_ = ConnectorSrv::Response::CODE_WIFI_NAME_FAIL; + this->connect_code = 2004; return return_false(" Wifi name is empty."); } else { if (!judge_string(name)) { @@ -665,6 +662,7 @@ bool Connector::DoConnect(std::string name, std::string password, std::string pr name.c_str()); this->Interaction(AudioMsg::PID_WIFI_CONNECTION_FAILED_0); this->srv_code_ = ConnectorSrv::Response::CODE_WIFI_NAME_FAIL; + this->connect_code = 2004; return return_false(" Wifi name is invalid."); } } @@ -683,6 +681,7 @@ bool Connector::DoConnect(std::string name, std::string password, std::string pr name.c_str(), password.c_str()); this->Interaction(AudioMsg::PID_WIFI_CONNECTION_FAILED_1); + this->connect_code = 2005; return return_false("Wifi password is invalid."); } } From 614b02fc1cc56007e13dfce857683baa0f2101a4 Mon Sep 17 00:00:00 2001 From: dingdingsong Date: Wed, 29 Nov 2023 13:11:27 +0800 Subject: [PATCH 05/12] [Modify]: fix bug MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 1.修复连网失败后灯效不恢复正常的bug; Signed-off-by: dingdingsong --- connector/src/connector.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/connector/src/connector.cpp b/connector/src/connector.cpp index 4be4692..b4817a2 100644 --- a/connector/src/connector.cpp +++ b/connector/src/connector.cpp @@ -412,7 +412,7 @@ void Connector::WiFiSignalCallback(const WiFiMsg::SharedPtr msg) this->notify_to_app_msg_.code = 2003; if (this->connect_network_status) { this->CtrlAudio(17); - this->CtrlLed(AudioMsg::PID_WIFI_ENTER_CONNECTION_MODE_0); + this->CtrlLed(AudioMsg::PID_WIFI_FAILED_PLEASE_RETRY); this->connect_network_status = true; } } @@ -630,7 +630,8 @@ bool Connector::DoConnect(std::string name, std::string password, std::string pr this->CtrlAudio(16); this->CtrlLed(AudioMsg::PID_WIFI_WAIT_FOR_SCAN_CODE_0); } - // this->connect_code = 2004; + // 释放灯效 + this->CtrlLed(AudioMsg::PID_WIFI_EXIT_CONNECTION_MODE_0); return false; }; auto judge_string = [&](std::string msg) -> bool { From 3d690d30ab620201fb19d0c99de2821f2eebc758 Mon Sep 17 00:00:00 2001 From: dingdingsong Date: Thu, 30 Nov 2023 21:12:52 +0800 Subject: [PATCH 06/12] [Modify]: Fix bug MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 1.修复wifi断开后不播报语音的bug; Signed-off-by: dingdingsong --- connector/src/connector.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/connector/src/connector.cpp b/connector/src/connector.cpp index b4817a2..4d4b7ee 100644 --- a/connector/src/connector.cpp +++ b/connector/src/connector.cpp @@ -404,7 +404,7 @@ void Connector::WiFiSignalCallback(const WiFiMsg::SharedPtr msg) this->provider_ip_ = this->GetWiFiProvider(msg->ssid); } this->state_msg_.provider_ip = this->provider_ip_; - this->connect_network_status = false; + this->connect_network_status = true; } else { if (this->state_msg_.is_connected) { @@ -413,7 +413,7 @@ void Connector::WiFiSignalCallback(const WiFiMsg::SharedPtr msg) if (this->connect_network_status) { this->CtrlAudio(17); this->CtrlLed(AudioMsg::PID_WIFI_FAILED_PLEASE_RETRY); - this->connect_network_status = true; + this->connect_network_status = false; } } } From 9ee29233518f16f4d4b1c0514482e1eedef02a8c Mon Sep 17 00:00:00 2001 From: dingdingsong Date: Mon, 4 Dec 2023 14:51:46 +0800 Subject: [PATCH 07/12] [Modify]: Modify find_adapter MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 1、取消蓝牙配网节点启动时对服务的判断； 2、修改find_adapter为:返回为空则重试120次； Signed-off-by: dingdingsong --- .../bluetooth_node.py | 18 ------------------ .../cyberdog_bluetooth_network/bt_core.py | 9 ++++++++- 2 files changed, 8 insertions(+), 19 deletions(-) diff --git a/cyberdog_bluetooth_network/cyberdog_bluetooth_network/bluetooth_node.py b/cyberdog_bluetooth_network/cyberdog_bluetooth_network/bluetooth_node.py index 328e088..170c2d6 100644 --- a/cyberdog_bluetooth_network/cyberdog_bluetooth_network/bluetooth_node.py +++ b/cyberdog_bluetooth_network/cyberdog_bluetooth_network/bluetooth_node.py @@ -38,7 +38,6 @@ def __init__(self, node_name: str): Node.__init__(self, node_name) self.__logger = self.get_logger() # ROS2 logger self.__logger.info('[BluetoothCore]: BluetoothNode init') - self.check_system_service() self.dog_ssif = '' self.dog_ip = '' self.__multithread_callback_group = ReentrantCallbackGroup() @@ -138,23 +137,6 @@ def __bluetooth_status_timer_callback(self): self.__bluetoothStatePublisher.publish(msg) pass - def check_system_service(self): - self.__logger.info('check system bluetooth and dbus service') - command_bluetooth = ( - 'sudo systemctl status bluetooth.service ' - '| grep running > /dev/null 2>&1') - command_dbus = 'sudo systemctl status dbus.service | grep running > /dev/null 2>&1' - while True: - result_bluetooth = subprocess.call(command_bluetooth, shell=True) - result_dbus = subprocess.call(command_dbus, shell=True) - if result_bluetooth == 0 and result_dbus == 0: - self.__logger.info('Both services are running. Continuing with further actions.') - time.sleep(3) - break - else: - self.__logger.error('Services are not both running. Waiting for 100ms...') - time.sleep(0.1) - class PublisherPhoneIP(Node): diff --git a/cyberdog_bluetooth_network/cyberdog_bluetooth_network/bt_core.py b/cyberdog_bluetooth_network/cyberdog_bluetooth_network/bt_core.py index 1e58197..5373af2 100644 --- a/cyberdog_bluetooth_network/cyberdog_bluetooth_network/bt_core.py +++ b/cyberdog_bluetooth_network/cyberdog_bluetooth_network/bt_core.py @@ -639,10 +639,17 @@ def __init__(self, logger): self.remote_om = dbus.Interface(self.bus.get_object( BLUEZ_SERVICE_NAME, '/'), DBUS_OM_IFACE) self.adapter = self.find_adapter(self.bus) + self.counter = 0 + self.__logger.info('start find Gattmanager1 interface') + while not self.adapter and self.counter < 120: + self.__logger.info('GattManager1 interface not found') + self.__logger.info('sleep 500ms, find adapter again') + time.sleep(0.5) + self.adapter = self.find_adapter(self.bus) + self.counter = self.counter + 1 if not self.adapter: self.__logger.info('GattManager1 interface not found') return - self.service_manager = dbus.Interface( self.bus.get_object(BLUEZ_SERVICE_NAME, self.adapter), GATT_MANAGER_IFACE) From 18e79ee36404dea6876adcb5dc8d2754337c46d5 Mon Sep 17 00:00:00 2001 From: dingdingsong Date: Mon, 4 Dec 2023 15:10:56 +0800 Subject: [PATCH 08/12] [Modify]: Fix bug MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 1、修复连网返回值为-1的bug(请求连接的wifi与已连接的wifi是同一个wifi的情形); Signed-off-by: dingdingsong --- connector/src/connector.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/connector/src/connector.cpp b/connector/src/connector.cpp index 4d4b7ee..dcaa4d2 100644 --- a/connector/src/connector.cpp +++ b/connector/src/connector.cpp @@ -908,10 +908,12 @@ void Connector::APPSendWiFiCallback( if (this->DoConnect(msg->ssid, msg->pwd, msg->ip)) { // 连网成功, 写文件 this->WriteToFile(msg->ssid, msg->ip, msg->type); - this->notify_to_app_msg_.code = 2000; + // this->notify_to_app_msg_.code = 2000; + this->connect_code = 2000; } else { INFO("receive wifi info from app, connect wifi failed"); - this->notify_to_app_msg_.code = 2001; // 连网失败 + // this->notify_to_app_msg_.code = 2001; // 连网失败 + this->connect_code = 2001; } this->notify_to_app_msg_.code = this->connect_code; this->notify_to_app_pub_->publish(this->notify_to_app_msg_); From cb16334030a45f2ed41df3b30308d9ec22ae512a Mon Sep 17 00:00:00 2001 From: dingdingsong Date: Fri, 8 Dec 2023 10:47:37 +0800 Subject: [PATCH 09/12] [Modify]: Fix bug MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 1、蓝牙从机模块监控快连模块初始化状态，快连未初始化完成时接收了连网请求，则给app返回6000的错误码。 Signed-off-by: dingdingsong --- connector/include/connector/connector.hpp | 4 +- connector/src/connector.cpp | 34 ++++++----------- connector/src/ctl_bluetooth.cpp | 1 - .../bluetooth_node.py | 38 +++++++++++-------- .../cyberdog_bluetooth_network/bt_core.py | 22 ++++++++--- 5 files changed, 53 insertions(+), 46 deletions(-) diff --git a/connector/include/connector/connector.hpp b/connector/include/connector/connector.hpp index e4f318b..df9f72d 100644 --- a/connector/include/connector/connector.hpp +++ b/connector/include/connector/connector.hpp @@ -101,6 +101,7 @@ class Connector final : public rclcpp::Node using NotifyToAppMsg = protocol::msg::NotifyToApp; /*!< [topic 类型]通知APP连接状态消息 */ using WIFIINFOMSG = protocol::msg::WifiInfo; using BLUETOOTHSTATUSMSG = protocol::msg::BluetoothStatus; + using ConnectorStatus = std_msgs::msg::String; using TimeType = std::chrono::time_point; /*!< 超时 */ enum ShellEnum @@ -201,10 +202,11 @@ class Connector final : public rclcpp::Node const std::string type); NotifyToAppMsg notify_to_app_msg_; + ConnectorStatus connector_status_msg_; rclcpp::Subscription::SharedPtr wifi_info_sub_ {nullptr}; rclcpp::Publisher::SharedPtr notify_to_app_pub_ {nullptr}; rclcpp::Subscription::SharedPtr bluetooth_status_sub_ {nullptr}; - + rclcpp::Publisher::SharedPtr connector_init_pub_ {nullptr}; std::string wifi_record_dir_ {""}; /*!< wifi 类型记录文件路径 */ bool connect_network_status = true; int connect_code = -1; diff --git a/connector/src/connector.cpp b/connector/src/connector.cpp index dcaa4d2..9b1cc47 100644 --- a/connector/src/connector.cpp +++ b/connector/src/connector.cpp @@ -185,7 +185,8 @@ bool Connector::Init( toml::find( this->params_toml_, "connector", "initialization", "topic", "connector"), 1, pub_option); - + this->connector_init_pub_ = this->create_publisher( + "connector_init", 1, pub_option); rclcpp::SubscriptionOptions sub_option; sub_option.callback_group = this->wifi_cb_group_; this->wifi_sub_ = this->create_subscription( @@ -332,6 +333,7 @@ void Connector::UpdateStatus() { try { std::lock_guard guard(this->state_msg_mutex_); + INFO("UpdateStatus"); this->status_pub_->publish(this->state_msg_); } catch (const std::exception & e) { WARN("Update status failed: <%s>", e.what()); @@ -361,6 +363,8 @@ void Connector::ResetSignal() this->TouchSignalCallback(touch_ptr); } } + this->connector_status_msg_.data = "connector_init"; + this->connector_init_pub_->publish(connector_status_msg_); } void Connector::DisconnectAppCallback(const DisConMsg::SharedPtr msg) @@ -382,8 +386,6 @@ void Connector::WiFiSignalCallback(const WiFiMsg::SharedPtr msg) std::lock_guard guard(this->state_msg_mutex_); if (msg->is_connected) { - // this->notify_to_app_msg_.ssid = msg->ssid; - // this->notify_to_app_msg_.ip = msg->ip; if ((!this->state_msg_.is_connected) && (std::chrono::system_clock::now() > this->touch_signal_timeout_)) { @@ -394,7 +396,6 @@ void Connector::WiFiSignalCallback(const WiFiMsg::SharedPtr msg) this->state_msg_.is_internet = this->CheckInternet(); if (this->state_msg_.is_internet) { // 当前网络能访问互联网 - // this->notify_to_app_msg_.code = 2001; } else { // 当前网络不能访问互联网 this->notify_to_app_msg_.code = 2002; @@ -405,7 +406,6 @@ void Connector::WiFiSignalCallback(const WiFiMsg::SharedPtr msg) } this->state_msg_.provider_ip = this->provider_ip_; this->connect_network_status = true; - } else { if (this->state_msg_.is_connected) { // 网络连接已断开 @@ -460,7 +460,6 @@ void Connector::TouchSignalCallback(const TouchMsg::SharedPtr msg) this->Interaction(AudioMsg::PID_WIFI_FAILED_PLEASE_RETRY); return; } - // this->Interaction(AudioMsg::PID_WIFI_WAIT_FOR_SCAN_CODE_0); this->CtrlAudio(16); this->CtrlLed(AudioMsg::PID_WIFI_WAIT_FOR_SCAN_CODE_0); this->touch_signal_timeout_ = std::chrono::system_clock::now() + @@ -489,12 +488,10 @@ void Connector::CameraSignalCallback(const CameraMsg::SharedPtr msg) INFO("Identifying current QR code..."); auto return_error = [&](const std::string & msg, const std::string & data) { WARN("%s:\n<%s>.", msg.c_str(), data.c_str()); - // this->Interaction(AudioMsg::PID_WIFI_SCAN_CODE_IP_ERROR); this->CtrlLed(AudioMsg::PID_WIFI_SCAN_CODE_IP_ERROR); if (this->touch_efficient_ && (std::chrono::system_clock::now() < this->touch_signal_timeout_)) { - // this->Interaction(AudioMsg::PID_WIFI_WAIT_FOR_SCAN_CODE_0); this->CtrlAudio(16); this->CtrlLed(AudioMsg::PID_WIFI_WAIT_FOR_SCAN_CODE_0); } @@ -562,9 +559,6 @@ void Connector::CameraSignalCallback(const CameraMsg::SharedPtr msg) connect_document.HasMember(ip.c_str()) && connect_document[ip.c_str()].IsString()) { - // this->Interaction(AudioMsg::PID_WIFI_SCAN_CODE_SUCCEEDED_0); - // this->CtrlAudio(15); - // this->CtrlLed(AudioMsg::PID_WIFI_SCAN_CODE_SUCCEEDED_0); if (this->DoConnect( connect_document[wifi_name.c_str()].GetString(), connect_document[wifi_password.c_str()].GetString(), @@ -615,10 +609,8 @@ bool Connector::DoConnect(std::string name, std::string password, std::string pr } this->camer_efficient_ = false; this->CtrlCamera(CameraSrv::Request::STOP_IMAGE_PUBLISH); - // this->Interaction(AudioMsg::PID_WIFI_EXIT_CONNECTION_MODE_0); this->touch_efficient_ = false; this->srv_code_ = ConnectorSrv::Response::CODE_SUCCESS; - // this->connect_code = 2000; return true; }; auto return_false = [&](std::string msg) -> bool { @@ -626,7 +618,6 @@ bool Connector::DoConnect(std::string name, std::string password, std::string pr if (this->touch_efficient_ && (std::chrono::system_clock::now() < this->touch_signal_timeout_)) { - // this->Interaction(AudioMsg::PID_WIFI_WAIT_FOR_SCAN_CODE_0); this->CtrlAudio(16); this->CtrlLed(AudioMsg::PID_WIFI_WAIT_FOR_SCAN_CODE_0); } @@ -688,7 +679,6 @@ bool Connector::DoConnect(std::string name, std::string password, std::string pr } if (provider.empty()) { // 手机端IP错误，会导致无法与设备通讯 - // this->Interaction(AudioMsg::PID_WIFI_SCAN_CODE_INFO_ERROR); this->CtrlLed(AudioMsg::PID_WIFI_SCAN_CODE_INFO_ERROR); this->srv_code_ = ConnectorSrv::Response::CODE_WIFI_PROVIDER_IP_FAIL; return return_false("Wifi provider is empty."); @@ -768,7 +758,6 @@ bool Connector::UserBootsFirstTime() { this->judge_first_time_ = false; try { - // this->judge_first_time_ = false; INFO("Judge user boots first time..."); toml::value wifi_toml; if (cyberdog::common::CyberdogToml::ParseFile( @@ -794,7 +783,9 @@ bool Connector::UserBootsFirstTime() try { toml::value wifi_toml1; - INFO("Judge user boots first time : %s", this->wifi_record_dir_.c_str()); + INFO( + "(wifi_record)Judge user boots first time : %s", + this->wifi_record_dir_.c_str()); if (cyberdog::common::CyberdogToml::ParseFile( this->wifi_record_dir_.c_str(), wifi_toml1)) { @@ -804,16 +795,16 @@ bool Connector::UserBootsFirstTime() if (cyberdog::common::CyberdogToml::WriteFile(this->wifi_record_dir_, wifi_toml1)) { return true; } else { - WARN("111WiFi params config file does not have wifi key, write failed"); + WARN("(wifi_record)WiFi params config file does not have wifi key, write failed"); } } } else { ERROR( - "111Toml WiFi config file is not in toml format, config file dir:\n%s", + "(wifi_record)Toml WiFi config file is not in toml format, config file dir:\n%s", this->wifi_record_dir_.c_str()); } } catch (const std::exception & e) { - ERROR("111Judge user boots first time is error:%s", e.what()); + ERROR("(wifi_record)Judge user boots first time is error:%s", e.what()); } return false; } @@ -906,13 +897,10 @@ void Connector::APPSendWiFiCallback( return; } if (this->DoConnect(msg->ssid, msg->pwd, msg->ip)) { - // 连网成功, 写文件 this->WriteToFile(msg->ssid, msg->ip, msg->type); - // this->notify_to_app_msg_.code = 2000; this->connect_code = 2000; } else { INFO("receive wifi info from app, connect wifi failed"); - // this->notify_to_app_msg_.code = 2001; // 连网失败 this->connect_code = 2001; } this->notify_to_app_msg_.code = this->connect_code; diff --git a/connector/src/ctl_bluetooth.cpp b/connector/src/ctl_bluetooth.cpp index 6b712ab..a5b5923 100644 --- a/connector/src/ctl_bluetooth.cpp +++ b/connector/src/ctl_bluetooth.cpp @@ -88,7 +88,6 @@ bool CtrlBluetooth::CtrlAdvertising(bool enable) { std::shared_ptr request = std::make_shared(); - INFO("enter CtrlAdvertising"); if (enable) { if (this->bluetooth_connect_status_ == 1) { INFO("disconnect app bluetooth"); diff --git a/cyberdog_bluetooth_network/cyberdog_bluetooth_network/bluetooth_node.py b/cyberdog_bluetooth_network/cyberdog_bluetooth_network/bluetooth_node.py index 170c2d6..87f607c 100644 --- a/cyberdog_bluetooth_network/cyberdog_bluetooth_network/bluetooth_node.py +++ b/cyberdog_bluetooth_network/cyberdog_bluetooth_network/bluetooth_node.py @@ -13,7 +13,6 @@ # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. -import subprocess import threading import time from time import sleep @@ -26,7 +25,7 @@ from rclpy.callback_groups import MutuallyExclusiveCallbackGroup, ReentrantCallbackGroup from rclpy.node import Node # from sensor_msgs.msg import BatteryState, Joy -# from std_msgs.msg import Bool, Int8, String +from std_msgs.msg import String from std_srvs.srv import Trigger from . import bt_core @@ -40,26 +39,30 @@ def __init__(self, node_name: str): self.__logger.info('[BluetoothCore]: BluetoothNode init') self.dog_ssif = '' self.dog_ip = '' + # self.connect_init_status = False self.__multithread_callback_group = ReentrantCallbackGroup() self.__singlethread_callback_group = MutuallyExclusiveCallbackGroup() # 创建ROS服务用于打开和关闭蓝牙广播 self.__openAdvertisingService = self.create_service( - BmsCmd, 'ctrl_advertising', - self.openAdvertisingCallback, - callback_group=self.__singlethread_callback_group) + BmsCmd, 'ctrl_advertising', + self.openAdvertisingCallback, + callback_group=self.__singlethread_callback_group) self.__openGattService = self.create_service( - BmsCmd, 'ctrl_gatt', - self.openGattCallback, - callback_group=self.__singlethread_callback_group) + BmsCmd, 'ctrl_gatt', + self.openGattCallback, + callback_group=self.__singlethread_callback_group) self.__disconnect_app_bt_service = self.create_service( - Trigger, 'disconnect_app_bt', self.__disconnect_app_bt_callback, - callback_group=self.__singlethread_callback_group) + Trigger, 'disconnect_app_bt', self.__disconnect_app_bt_callback, + callback_group=self.__singlethread_callback_group) # 创建ROS话题用于发布蓝牙广播状态 self.__bluetoothStatePublisher = self.create_publisher( - BluetoothStatus, 'bluetooth_status', 10) + BluetoothStatus, 'bluetooth_status', 10) self.__notify_subscriber = self.create_subscription( - NotifyToApp, 'notify_to_app', self.__notify_to_app_callback, 1, - callback_group=self.__multithread_callback_group) + NotifyToApp, 'notify_to_app', self.__notify_to_app_callback, 1, + callback_group=self.__multithread_callback_group) + self.__connect_status_subscriber = self.create_subscription( + String, 'connector_init', self.__connect_status_callback, 1, + callback_group=self.__multithread_callback_group) self.__bt_core_lock = threading.Lock() self.blue_core = bt_core.BluetoothCore(self.__logger) # 开启线程,启动蓝牙mainloop的循环 @@ -73,8 +76,8 @@ def __init__(self, node_name: str): # 创建一个定时器，定时发布状态（广播、连接、wifi信息） self.__bluetooth_status_timer = self.create_timer( - 2, self.__bluetooth_status_timer_callback, - callback_group=self.__singlethread_callback_group) + 2, self.__bluetooth_status_timer_callback, + callback_group=self.__singlethread_callback_group) def __disconnect_app_bt_callback(self, request, response): self.__logger.info('disconnect_app_bt_callback') @@ -83,6 +86,11 @@ def __disconnect_app_bt_callback(self, request, response): self.blue_core.disconnect_device(self.blue_core.mac_address_get) return response + def __connect_status_callback(self, msg): + # self.connect_init_status = True + self.__logger.info('connect_status_callback') + self.blue_core.GetConnectStatus(True) + def __notify_to_app_callback(self, msg): self.__logger.info('notify_to_app_callback') print(f'ssid:{msg.ssid} ip:{msg.ip} code:{msg.code}') diff --git a/cyberdog_bluetooth_network/cyberdog_bluetooth_network/bt_core.py b/cyberdog_bluetooth_network/cyberdog_bluetooth_network/bt_core.py index 5373af2..3946152 100644 --- a/cyberdog_bluetooth_network/cyberdog_bluetooth_network/bt_core.py +++ b/cyberdog_bluetooth_network/cyberdog_bluetooth_network/bt_core.py @@ -494,9 +494,14 @@ def WriteValue(self, value, options): self.logger.info('WriteValue ip is none!!!') app_send_type = '' code = 1005 - + # 如果接收的数据都不为空，且type为wifi或者hotspot，就发送给app if(code == WIFI_INFO_GET_SUCCESS): if(app_send_type == 'wifi' or app_send_type == 'hotspot'): + # 如果快连模块没有初始化，通过蓝牙返回6000错误码 + if self.bluetooth_core.get_connect_init_status is False: + self.logger.info('connector is not init') + self.doNotifyOnce('', '', 6000, self.sn) + return msg = WifiInfo() msg.ssid = app_send_ssid msg.ip = app_send_ip @@ -523,7 +528,7 @@ def doNotifyOnce(self, ssid, ip, code, sn): 'ip': ip, 'code': code, 'sn': sn} - self.logger.info(' doNotifyOnce %s' % repr(gnotifystr)) + self.logger.info('doNotifyOnce %s' % repr(gnotifystr)) arraySend = convert_to_dbus_array(json.dumps(gnotifystr)) # 发送Dbus通知 notifyChar.PropertiesChanged(GATT_CHRC_IFACE, {'Value': arraySend}, []) @@ -625,6 +630,7 @@ def __init__(self, logger): self.connected = 0 self.masters = [] self.slavers = [] + self.get_connect_init_status = False command = 'factory-tool -f /usr/share/factory_cmd_config/system.xml -i SN' self.g_bt_local_name = self.runCommand(command) self.dog_sn = self.g_bt_local_name.rstrip('\n') @@ -717,7 +723,7 @@ def check_connected(self): if self.receive_mac == mac_address: self.__logger.info('app connect success ') self.connected = 1 - self.__logger.info('device is connected, close advertise ') + self.__logger.info('App is connected, close advertise ') else: self.disconnect_device(self.mac_address_get) self.__logger.info(' disconnect :%s' % self.mac_address_get) @@ -741,7 +747,7 @@ def set_connected_status(self, status): if len(self.slavers) == 0: self.connected = 0 self.receive_mac = '' - self.__logger.info('device is disconnected, open advertise ') + self.__logger.info('App is disconnected, open advertise ') pass def properties_changed(self, interface, changed, invalidated, path): @@ -749,7 +755,7 @@ def properties_changed(self, interface, changed, invalidated, path): if ('Connected' in changed): self.mac_address_get = self.extract_mac_address_from_path(path) self.mac_address_get = self.splite_mac_address_get(self.mac_address_get) - self.__logger.info('connect_change,mac:%s' % self.mac_address_get) + self.__logger.info('connect_change') self.set_connected_status(changed['Connected']) def interfaces_added(self, path, interfaces): @@ -758,7 +764,7 @@ def interfaces_added(self, path, interfaces): if ('Connected' in properties): self.mac_address_get = self.extract_mac_address_from_path(path) self.mac_address_get = self.splite_mac_address_get(self.mac_address_get) - self.__logger.info('interface add,mac_addess: %s' % self.mac_address_get) + self.__logger.info('interface add') self.set_connected_status(properties['Connected']) def register_app_cb(self): @@ -853,3 +859,7 @@ def GetAppData(self, ssid, pwd, ip, mac, type_receive): self.receive_type = type_receive self.__logger.info('GetAppData: %s %s %s %s %s' % (ssid, pwd, ip, mac, type_receive)) + + def GetConnectStatus(self, status): + self.get_connect_init_status = status + self.__logger.info('GetConnectStatus: %s' % status) From c919586c0433cf449ee2e6c899be1693bac74948 Mon Sep 17 00:00:00 2001 From: dingdingsong Date: Thu, 14 Dec 2023 11:04:42 +0800 Subject: [PATCH 10/12] [Modify]: Modify connecte wifi return code bug MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 1.修复连网请求返回值错误的bug; 2.注释调试日志打印; Signed-off-by: dingdingsong --- connector/src/connector.cpp | 2 +- .../cyberdog_bluetooth_network/bluetooth_node.py | 3 +-- .../cyberdog_bluetooth_network/bt_core.py | 16 ++++++++-------- 3 files changed, 10 insertions(+), 11 deletions(-) diff --git a/connector/src/connector.cpp b/connector/src/connector.cpp index 9b1cc47..2624248 100644 --- a/connector/src/connector.cpp +++ b/connector/src/connector.cpp @@ -901,7 +901,7 @@ void Connector::APPSendWiFiCallback( this->connect_code = 2000; } else { INFO("receive wifi info from app, connect wifi failed"); - this->connect_code = 2001; + // this->connect_code = 2001; } this->notify_to_app_msg_.code = this->connect_code; this->notify_to_app_pub_->publish(this->notify_to_app_msg_); diff --git a/cyberdog_bluetooth_network/cyberdog_bluetooth_network/bluetooth_node.py b/cyberdog_bluetooth_network/cyberdog_bluetooth_network/bluetooth_node.py index 87f607c..adab7f2 100644 --- a/cyberdog_bluetooth_network/cyberdog_bluetooth_network/bluetooth_node.py +++ b/cyberdog_bluetooth_network/cyberdog_bluetooth_network/bluetooth_node.py @@ -87,8 +87,7 @@ def __disconnect_app_bt_callback(self, request, response): return response def __connect_status_callback(self, msg): - # self.connect_init_status = True - self.__logger.info('connect_status_callback') + # self.__logger.info('connect_status_callback') self.blue_core.GetConnectStatus(True) def __notify_to_app_callback(self, msg): diff --git a/cyberdog_bluetooth_network/cyberdog_bluetooth_network/bt_core.py b/cyberdog_bluetooth_network/cyberdog_bluetooth_network/bt_core.py index 3946152..183686c 100644 --- a/cyberdog_bluetooth_network/cyberdog_bluetooth_network/bt_core.py +++ b/cyberdog_bluetooth_network/cyberdog_bluetooth_network/bt_core.py @@ -456,7 +456,7 @@ def WriteValue(self, value, options): code = WIFI_INFO_GET_SUCCESS if ssid is not None: - self.logger.info('WriteValue ssid %s' % repr(text['ssid'])) + # self.logger.info('WriteValue ssid %s' % repr(text['ssid'])) app_send_ssid = ssid else: self.logger.info('WriteValue ssid is none!!!') @@ -464,7 +464,7 @@ def WriteValue(self, value, options): code = WIFI_INFO_SSID_ERROR if pwd is not None: - self.logger.info('WriteValue pwd %s' % repr(text['pwd'])) + # self.logger.info('WriteValue pwd %s' % repr(text['pwd'])) app_send_pwd = pwd else: self.logger.info('WriteValue pwd is none!!!') @@ -472,7 +472,7 @@ def WriteValue(self, value, options): # code = WIFI_INFO_PWD_ERROR if ip is not None: - self.logger.info('WriteValue ip %s' % repr(text['ip'])) + # self.logger.info('WriteValue ip %s' % repr(text['ip'])) app_send_ip = ip else: self.logger.info('WriteValue ip is none!!!') @@ -480,7 +480,7 @@ def WriteValue(self, value, options): code = WIFI_INFO_IP_ERROR if mac is not None: - self.logger.info('WriteValue mac %s' % repr(text['mac'])) + # self.logger.info('WriteValue mac %s' % repr(text['mac'])) app_send_mac = mac else: self.logger.info('WriteValue ip is none!!!') @@ -515,7 +515,7 @@ def WriteValue(self, value, options): self.doNotifyOnce(app_send_ssid, app_send_ip, code, self.sn) # 蓝牙心跳 if(app_send_type == 'heartBeat'): - self.logger.info('send heartBeat') + # self.logger.info('send heartBeat') code = 5000 self.doNotifyOnce(app_send_ssid, app_send_ip, code, self.sn) self.bluetooth_core.GetAppData( @@ -857,9 +857,9 @@ def GetAppData(self, ssid, pwd, ip, mac, type_receive): self.receive_ip = ip self.receive_mac = mac self.receive_type = type_receive - self.__logger.info('GetAppData: %s %s %s %s %s' % - (ssid, pwd, ip, mac, type_receive)) + # self.__logger.info('GetAppData: %s %s %s %s %s' % + # (ssid, pwd, ip, mac, type_receive)) def GetConnectStatus(self, status): self.get_connect_init_status = status - self.__logger.info('GetConnectStatus: %s' % status) + # self.__logger.info('GetConnectStatus: %s' % status) From 4a49d4613b35da4206613479d19830382ffaa19d Mon Sep 17 00:00:00 2001 From: dingdingsong Date: Tue, 19 Dec 2023 11:44:54 +0800 Subject: [PATCH 11/12] [Modify]:Fix Uwb tag upgrade failed MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 1.增加对标签升级过程的监控,在标签升级期间不开启广播; 2.调整语音播放,APP下发的wifi信息和此时连接的wifi信息相同则不播报语音; Signed-off-by: dingdingsong --- connector/include/connector/connector.hpp | 5 +++ connector/src/connector.cpp | 38 +++++++++++++++---- .../bluetooth_node.py | 20 ++++++++++ .../cyberdog_bluetooth_network/bt_core.py | 6 +++ 4 files changed, 62 insertions(+), 7 deletions(-) diff --git a/connector/include/connector/connector.hpp b/connector/include/connector/connector.hpp index df9f72d..edb273e 100644 --- a/connector/include/connector/connector.hpp +++ b/connector/include/connector/connector.hpp @@ -45,6 +45,7 @@ #include #include #include +#include #include #include @@ -102,6 +103,7 @@ class Connector final : public rclcpp::Node using WIFIINFOMSG = protocol::msg::WifiInfo; using BLUETOOTHSTATUSMSG = protocol::msg::BluetoothStatus; using ConnectorStatus = std_msgs::msg::String; + using BLEDfuProgressMsg = protocol::msg::BLEDFUProgress; using TimeType = std::chrono::time_point; /*!< 超时 */ enum ShellEnum @@ -196,6 +198,7 @@ class Connector final : public rclcpp::Node void APPSendWiFiCallback(const protocol::msg::WifiInfo::SharedPtr msg); void AppConnectState(const std_msgs::msg::Bool msg); void BtStatusCallback(const protocol::msg::BluetoothStatus::SharedPtr msg); + void BledfuProgressCallback(const BLEDfuProgressMsg::SharedPtr msg); bool WriteToFile( const std::string ssid, const std::string ip, @@ -206,10 +209,12 @@ class Connector final : public rclcpp::Node rclcpp::Subscription::SharedPtr wifi_info_sub_ {nullptr}; rclcpp::Publisher::SharedPtr notify_to_app_pub_ {nullptr}; rclcpp::Subscription::SharedPtr bluetooth_status_sub_ {nullptr}; + rclcpp::Subscription::SharedPtr bledfu_progress_sub_ {nullptr}; rclcpp::Publisher::SharedPtr connector_init_pub_ {nullptr}; std::string wifi_record_dir_ {""}; /*!< wifi 类型记录文件路径 */ bool connect_network_status = true; int connect_code = -1; + bool bledfu_progress_status = true; }; // class Connector } // namespace interaction } // namespace cyberdog diff --git a/connector/src/connector.cpp b/connector/src/connector.cpp index 2624248..3ea9b90 100644 --- a/connector/src/connector.cpp +++ b/connector/src/connector.cpp @@ -227,6 +227,10 @@ bool Connector::Init( sub_option); this->notify_to_app_pub_ = this->create_publisher( "notify_to_app", 1, pub_option); + this->bledfu_progress_sub_ = this->create_subscription( + "ble_dfu_progress", rclcpp::SystemDefaultsQoS(), + std::bind(&Connector::BledfuProgressCallback, this, std::placeholders::_1), + sub_option); } catch (const std::exception & e) { ERROR("Init data failed: <%s>", e.what()); return false; @@ -333,7 +337,7 @@ void Connector::UpdateStatus() { try { std::lock_guard guard(this->state_msg_mutex_); - INFO("UpdateStatus"); + // INFO("UpdateStatus"); this->status_pub_->publish(this->state_msg_); } catch (const std::exception & e) { WARN("Update status failed: <%s>", e.what()); @@ -596,8 +600,6 @@ void Connector::CameraSignalCallback(const CameraMsg::SharedPtr msg) bool Connector::DoConnect(std::string name, std::string password, std::string provider) { - this->CtrlAudio(15); - this->CtrlLed(AudioMsg::PID_WIFI_ENTER_CONNECTION_MODE_0); auto return_true = [&](std::string msg, bool same_wifi) -> bool { this->provider_ip_ = provider; this->SaveWiFi(provider, name, password); @@ -605,7 +607,8 @@ bool Connector::DoConnect(std::string name, std::string password, std::string pr if (password.empty()) { this->Interaction(AudioMsg::PID_WIFI_CONNECTED_UNKNOWN_NET); } else { - this->Interaction(AudioMsg::PID_WIFI_CONNECTION_SUCCEEDED_0); + INFO("connect wifi success"); + // this->Interaction(AudioMsg::PID_WIFI_CONNECTION_SUCCEEDED_0); } this->camer_efficient_ = false; this->CtrlCamera(CameraSrv::Request::STOP_IMAGE_PUBLISH); @@ -688,9 +691,12 @@ bool Connector::DoConnect(std::string name, std::string password, std::string pr "The target wifi and the currently connected wifi are the same wifi.", true); } + this->CtrlAudio(15); + this->CtrlLed(AudioMsg::PID_WIFI_ENTER_CONNECTION_MODE_0); switch (this->CtrlWifi(name, password)) { case WiFiSrv::Response::RESULT_SUCCESS: this->connect_code = 2000; + this->Interaction(AudioMsg::PID_WIFI_CONNECTION_SUCCEEDED_0); return return_true("WiFi connection succeeded.", false); case WiFiSrv::Response::RESULT_NO_SSID: this->Interaction(AudioMsg::PID_WIFI_CONNECTION_FAILED_0); @@ -952,7 +958,7 @@ void Connector::BtStatusCallback( const protocol::msg::BluetoothStatus::SharedPtr msg) { if (msg->connectable == 1) { - INFO_MILLSECONDS(3000, " bluetooth connect(app and cyberdog)"); + INFO_MILLSECONDS(4000, " bluetooth connect(app and cyberdog)"); if (this->touch_efficient_ == false) { // touch没有长按，关闭广播 int count = 0; @@ -961,11 +967,29 @@ void Connector::BtStatusCallback( count++; } } - } else if (msg->connectable == 0) { + } else if (msg->connectable == 0 && this->bledfu_progress_status) { // 未连接状态，开起广播 - INFO_MILLSECONDS(3000, " bluetooth is not connect(app and cyberdog)"); + INFO_MILLSECONDS(4000, " bluetooth is not connect(app and cyberdog)"); this->CtrlAdvertising(true); } } +void Connector::BledfuProgressCallback( + const BLEDfuProgressMsg::SharedPtr msg) +{ + if (msg->status == 0 || msg->status == 3 || + msg->status == 5 || msg->status == 7 || + msg->status == 9) + { + this->bledfu_progress_status = false; + INFO("DFU processing"); + } + if (msg->status == 1 || msg->status == 2 || + msg->status == 4 || msg->status == 6 || + msg->status == 8 || msg->status == 10) + { + this->bledfu_progress_status = true; + INFO("DFU peocessing end"); + } +} } // namespace interaction } // namespace cyberdog diff --git a/cyberdog_bluetooth_network/cyberdog_bluetooth_network/bluetooth_node.py b/cyberdog_bluetooth_network/cyberdog_bluetooth_network/bluetooth_node.py index adab7f2..93f6344 100644 --- a/cyberdog_bluetooth_network/cyberdog_bluetooth_network/bluetooth_node.py +++ b/cyberdog_bluetooth_network/cyberdog_bluetooth_network/bluetooth_node.py @@ -18,6 +18,7 @@ from time import sleep from mi.cyberdog_bringup.manual import get_namespace +from protocol.msg import BLEDFUProgress from protocol.msg import BluetoothStatus from protocol.msg import NotifyToApp from protocol.msg import WifiInfo @@ -63,6 +64,9 @@ def __init__(self, node_name: str): self.__connect_status_subscriber = self.create_subscription( String, 'connector_init', self.__connect_status_callback, 1, callback_group=self.__multithread_callback_group) + self.__uwb_update_subscriber = self.create_subscription( + BLEDFUProgress, 'ble_dfu_progress', self.__uwb_update_callback, 1, + callback_group=self.__multithread_callback_group) self.__bt_core_lock = threading.Lock() self.blue_core = bt_core.BluetoothCore(self.__logger) # 开启线程,启动蓝牙mainloop的循环 @@ -90,6 +94,22 @@ def __connect_status_callback(self, msg): # self.__logger.info('connect_status_callback') self.blue_core.GetConnectStatus(True) + def __uwb_update_callback(self, msg): + self.__logger.info('uwb_update_callback, status: %d' % msg.status) + self.__logger.info('uwb_update_callback, progress: %d' % msg.progress) + # self.__logger.info('uwb_update_callback, error: %s' % msg.message) + if(msg.status == 0 or msg.status == 3 or + msg.status == 5 or msg.status == 7 or msg.status == 9): + self.__logger.info('uwb_update_callback, -5000') + self.blue_core.GetUwbUpdataStatus(-5000) + pass + if(msg.status == 1 or msg.status == 2 or msg.status == 4 or + msg.status == 6 or msg.status == 8 or msg.status == 10): + self.__logger.info('uwb_update_callback, 5000') + self.blue_core.GetUwbUpdataStatus(5000) + pass + pass + def __notify_to_app_callback(self, msg): self.__logger.info('notify_to_app_callback') print(f'ssid:{msg.ssid} ip:{msg.ip} code:{msg.code}') diff --git a/cyberdog_bluetooth_network/cyberdog_bluetooth_network/bt_core.py b/cyberdog_bluetooth_network/cyberdog_bluetooth_network/bt_core.py index 183686c..96286e6 100644 --- a/cyberdog_bluetooth_network/cyberdog_bluetooth_network/bt_core.py +++ b/cyberdog_bluetooth_network/cyberdog_bluetooth_network/bt_core.py @@ -631,6 +631,7 @@ def __init__(self, logger): self.masters = [] self.slavers = [] self.get_connect_init_status = False + self.get_uwb_updata_status = 5000 command = 'factory-tool -f /usr/share/factory_cmd_config/system.xml -i SN' self.g_bt_local_name = self.runCommand(command) self.dog_sn = self.g_bt_local_name.rstrip('\n') @@ -863,3 +864,8 @@ def GetAppData(self, ssid, pwd, ip, mac, type_receive): def GetConnectStatus(self, status): self.get_connect_init_status = status # self.__logger.info('GetConnectStatus: %s' % status) + + def GetUwbUpdataStatus(self, status): + self.get_uwb_updata_status = status + self.__logger.info('GetUwbUpdataStatus, adv_status: %d' % self.adv_status) + pass From 334a177f64dcd19dc5496a70e757a3aea3fabc98 Mon Sep 17 00:00:00 2001 From: dingdingsong Date: Mon, 25 Dec 2023 11:28:45 +0800 Subject: [PATCH 12/12] [Modify]: Fixed aiting for voice to connect to the network cannot be stopped MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 1.扫码连接下发相同网络,连网成功则播报连网成功打断按touch产生的"等待连网"语音播报; 2.蓝牙连网下发相同网络时,连网成功下发9999打断语音播报; Signed-off-by: dingdingsong --- connector/include/connector/connector.hpp | 1 + connector/src/connector.cpp | 8 ++++++++ 2 files changed, 9 insertions(+) diff --git a/connector/include/connector/connector.hpp b/connector/include/connector/connector.hpp index edb273e..47c879f 100644 --- a/connector/include/connector/connector.hpp +++ b/connector/include/connector/connector.hpp @@ -215,6 +215,7 @@ class Connector final : public rclcpp::Node bool connect_network_status = true; int connect_code = -1; bool bledfu_progress_status = true; + bool camera_callback_flag = false; }; // class Connector } // namespace interaction } // namespace cyberdog diff --git a/connector/src/connector.cpp b/connector/src/connector.cpp index 3ea9b90..3500eb4 100644 --- a/connector/src/connector.cpp +++ b/connector/src/connector.cpp @@ -563,6 +563,7 @@ void Connector::CameraSignalCallback(const CameraMsg::SharedPtr msg) connect_document.HasMember(ip.c_str()) && connect_document[ip.c_str()].IsString()) { + this->camera_callback_flag = true; if (this->DoConnect( connect_document[wifi_name.c_str()].GetString(), connect_document[wifi_password.c_str()].GetString(), @@ -687,6 +688,13 @@ bool Connector::DoConnect(std::string name, std::string password, std::string pr return return_false("Wifi provider is empty."); } if (name == this->state_msg_.ssid) { + if (this->camera_callback_flag) { + this->camera_callback_flag = false; + this->Interaction(AudioMsg::PID_WIFI_CONNECTION_SUCCEEDED_0); + } else { + this->CtrlAudio(9999); + this->CtrlLed(AudioMsg::PID_WIFI_CONNECTION_SUCCEEDED_0); + } return return_true( "The target wifi and the currently connected wifi are the same wifi.", true);