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 new file mode 100644 index 0000000..4234a38 --- /dev/null +++ b/connector/config/wifirecord.toml @@ -0,0 +1,4 @@ +[wifi] +[wifi.ssid] +type = "type" +provider = "ip" diff --git a/connector/include/connector/connector.hpp b/connector/include/connector/connector.hpp index 55ca3d7..47c879f 100644 --- a/connector/include/connector/connector.hpp +++ b/connector/include/connector/connector.hpp @@ -42,6 +42,11 @@ #include #include +#include +#include +#include +#include + #include #include #include @@ -68,7 +73,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 +99,13 @@ 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 ConnectorStatus = std_msgs::msg::String; + using BLEDfuProgressMsg = protocol::msg::BLEDFUProgress; + + using TimeType = std::chrono::time_point; /*!< 超时 */ enum ShellEnum { shell = 1993, /*!< 异常结束, 执行shell命令错误 */ @@ -110,9 +121,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 +130,7 @@ class Connector final : public rclcpp::Node std::function CtrlLed; /*!< [客户端]控制led */ std::function CtrlWifi; /*!< [客户端]控制wifi */ + std::function CtrlAdvertising; /*!< [客户端]控制蓝牙 */ private: bool InitPythonInterpreter(); /*!< 初始化 python 解释器 */ @@ -183,6 +194,28 @@ 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); + void BledfuProgressCallback(const BLEDfuProgressMsg::SharedPtr msg); + bool WriteToFile( + const std::string ssid, + const std::string ip, + 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::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; + bool camera_callback_flag = false; }; // 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..3500eb4 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( @@ -173,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( @@ -199,12 +212,25 @@ 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); + 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; @@ -311,6 +337,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()); @@ -325,6 +352,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); } @@ -335,6 +367,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) @@ -354,17 +388,13 @@ 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) { 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(); @@ -372,15 +402,23 @@ void Connector::WiFiSignalCallback(const WiFiMsg::SharedPtr msg) // 当前网络能访问互联网 } 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 = true; } else { if (this->state_msg_.is_connected) { // 网络连接已断开 + this->notify_to_app_msg_.code = 2003; + if (this->connect_network_status) { + this->CtrlAudio(17); + this->CtrlLed(AudioMsg::PID_WIFI_FAILED_PLEASE_RETRY); + this->connect_network_status = false; + } } } this->state_msg_.is_connected = msg->is_connected; @@ -426,10 +464,16 @@ 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() + 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 +492,12 @@ 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); } }; int num_pixels = msg->data.size(); @@ -518,7 +563,7 @@ 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->camera_callback_flag = true; if (this->DoConnect( connect_document[wifi_name.c_str()].GetString(), connect_document[wifi_password.c_str()].GetString(), @@ -563,11 +608,11 @@ 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); - // this->Interaction(AudioMsg::PID_WIFI_EXIT_CONNECTION_MODE_0); this->touch_efficient_ = false; this->srv_code_ = ConnectorSrv::Response::CODE_SUCCESS; return true; @@ -577,8 +622,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->CtrlAudio(16); + this->CtrlLed(AudioMsg::PID_WIFI_WAIT_FOR_SCAN_CODE_0); } + // 释放灯效 + this->CtrlLed(AudioMsg::PID_WIFI_EXIT_CONNECTION_MODE_0); return false; }; auto judge_string = [&](std::string msg) -> bool { @@ -588,17 +636,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 { @@ -611,6 +648,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)) { @@ -620,6 +658,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."); } } @@ -638,34 +677,49 @@ 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."); } } 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."); } 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); } + 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); 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 +770,8 @@ void Connector::SaveWiFi( 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( @@ -740,6 +794,32 @@ 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( + "(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)) + { + 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("(wifi_record)WiFi params config file does not have wifi key, write failed"); + } + } + } else { + ERROR( + "(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("(wifi_record)Judge user boots first time is error:%s", e.what()); + } return false; } @@ -817,5 +897,107 @@ 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->connect_code = 2000; + } else { + INFO("receive wifi info from app, connect wifi failed"); + // this->connect_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(4000, " 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 && this->bledfu_progress_status) { + // 未连接状态，开起广播 + 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/connector/src/ctl_bluetooth.cpp b/connector/src/ctl_bluetooth.cpp new file mode 100644 index 0000000..a5b5923 --- /dev/null +++ b/connector/src/ctl_bluetooth.cpp @@ -0,0 +1,131 @@ +// 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(); + 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_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"}, 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 0000000..e158914 Binary files /dev/null and b/cyberdog_bluetooth_network/cyberdog_bluetooth_network/__pycache__/__init__.cpython-36.pyc differ 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..93f6344 --- /dev/null +++ b/cyberdog_bluetooth_network/cyberdog_bluetooth_network/bluetooth_node.py @@ -0,0 +1,176 @@ +#!/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 BLEDFUProgress +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 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.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) + 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.__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的循环 + 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 __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}') + 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..96286e6 --- /dev/null +++ b/cyberdog_bluetooth_network/cyberdog_bluetooth_network/bt_core.py @@ -0,0 +1,871 @@ +#!/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 + # 如果接收的数据都不为空，且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 + 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 = [] + 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') + 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) + 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) + 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('App 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('App 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') + 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') + 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)) + + 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 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 0000000..0a08f66 Binary files /dev/null and b/cyberdog_bluetooth_network/test/__pycache__/test_copyright.cpython-36-pytest-6.2.5.pyc differ 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 0000000..9137c4a Binary files /dev/null and b/cyberdog_bluetooth_network/test/__pycache__/test_flake8.cpython-36-pytest-6.2.5.pyc differ 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 0000000..82ad9b5 Binary files /dev/null and b/cyberdog_bluetooth_network/test/__pycache__/test_pep257.cpython-36-pytest-6.2.5.pyc differ 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'