From 50b9644efb833249cc399046bcebf20f4e85a7c0 Mon Sep 17 00:00:00 2001 From: ito-san Date: Fri, 6 May 2022 14:46:33 +0900 Subject: [PATCH 01/17] feat(bluetooth_monitor): add functionality to monitor Bluetooth connection --- system/bluetooth_monitor/CMakeLists.txt | 46 + system/bluetooth_monitor/README.md | 89 + .../config/bluetooth_monitor.param.yaml | 9 + .../docs/block_diagram.drawio.svg | 3441 +++++++++++++++++ .../bluetooth_monitor/bluetooth_monitor.hpp | 115 + .../bluetooth_monitor/service/l2ping.hpp | 134 + .../service/l2ping_interface.hpp | 139 + .../service/l2ping_service.hpp | 83 + .../launch/bluetooth_monitor.launch.xml | 6 + system/bluetooth_monitor/package.xml | 24 + system/bluetooth_monitor/service/l2ping.cpp | 354 ++ .../service/l2ping_service.cpp | 281 ++ system/bluetooth_monitor/service/main.cpp | 97 + .../src/bluetooth_monitor.cpp | 197 + .../src/bluetooth_monitor_node.cpp | 29 + 15 files changed, 5044 insertions(+) create mode 100644 system/bluetooth_monitor/CMakeLists.txt create mode 100644 system/bluetooth_monitor/README.md create mode 100644 system/bluetooth_monitor/config/bluetooth_monitor.param.yaml create mode 100644 system/bluetooth_monitor/docs/block_diagram.drawio.svg create mode 100644 system/bluetooth_monitor/include/bluetooth_monitor/bluetooth_monitor.hpp create mode 100644 system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping.hpp create mode 100644 system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping_interface.hpp create mode 100644 system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping_service.hpp create mode 100644 system/bluetooth_monitor/launch/bluetooth_monitor.launch.xml create mode 100644 system/bluetooth_monitor/package.xml create mode 100644 system/bluetooth_monitor/service/l2ping.cpp create mode 100644 system/bluetooth_monitor/service/l2ping_service.cpp create mode 100644 system/bluetooth_monitor/service/main.cpp create mode 100644 system/bluetooth_monitor/src/bluetooth_monitor.cpp create mode 100644 system/bluetooth_monitor/src/bluetooth_monitor_node.cpp diff --git a/system/bluetooth_monitor/CMakeLists.txt b/system/bluetooth_monitor/CMakeLists.txt new file mode 100644 index 0000000000000..6abf843eff9fe --- /dev/null +++ b/system/bluetooth_monitor/CMakeLists.txt @@ -0,0 +1,46 @@ +cmake_minimum_required(VERSION 3.5) +project(bluetooth_monitor) + +### Compile options +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +### Dependencies +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +### Target executable +ament_auto_add_executable(bluetooth_monitor + src/bluetooth_monitor_node.cpp + src/bluetooth_monitor.cpp +) + +ament_auto_add_executable(l2ping_service + service/main.cpp + service/l2ping_service.cpp + service/l2ping.cpp +) + +find_package(Boost REQUIRED COMPONENTS + serialization +) + +## Specify libraries to link a library or executable target against +target_link_libraries(bluetooth_monitor bluetooth ${Boost_LIBRARIES}) +target_link_libraries(l2ping_service bluetooth ${Boost_LIBRARIES}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package(INSTALL_TO_SHARE + config + launch +) diff --git a/system/bluetooth_monitor/README.md b/system/bluetooth_monitor/README.md new file mode 100644 index 0000000000000..e79f9ff4c3043 --- /dev/null +++ b/system/bluetooth_monitor/README.md @@ -0,0 +1,89 @@ +# bluetooth_monitor + +## Description + +This node monitors a Bluetooth connection to a wireless device by using L2ping.
+L2ping generates PING echo command on Bluetooth L2CAP layer, and it is able to receive and check echo response from a wireless device. + +## Block diagram + +L2ping is only allowed for root by default, so this package provides the following approach to minimize security risks as much as possible: + +- Provide a small program named `l2ping_service` which performs L2ping and provides wireless device information to `bluetooth_monitor` by using socket programming. +- `bluetooth_monitor` is able to know wireless device information and L2ping status as an unprivileged user since those information are sent by socket communication. + +![a](docs/block_diagram.drawio.svg) + +## Output + +### bluetooth_monitor: bluetooth_connection + +[summary] + +| level | message | +| ----- | -------------- | +| OK | OK | +| WARN | RTT warning | +| | Verify error | +| ERROR | Lost | +| | Ping rejected | +| | Function error | + +[values] + +| key | value (example) | +| ------------------------------ | ----------------------------------------------------------------------- | +| Device [0-9]: Status | OK / RTT warning / Verify error / Lost / Ping rejected / Function error | +| Device [0-9]: Name | Wireless Controller | +| Device [0-9]: Manufacturer | MediaTek, Inc. | +| Device [0-9]: Address | AA:BB:CC:DD:EE:FF | +| Device [0-9]: RTT | 0.00ms | +| Device [0-9]: Sent packets | 1000 | +| Device [0-9]: Received packets | 1000 | + +- The following key will be added when `bluetooth_monitor` reports `Function error`.
+ ex.) The `connect` system call failed. + +| key (example) | value (example) | +| --------------------- | ------------------------- | +| Device [0-9]: connect | No such file or directory | + +## Parameters + +| Name | Type | Default Value | Explanation | +| ------------- | ------- | ------------- | --------------------------------------------------------- | +| `port` | int | 7640 | Port number to connect to L2ping service. | +| `delay` | int | 1 | Wait seconds between sending each packet. | +| `timeout` | int | 5 | Wait timeout seconds for the response. | +| `verify` | bool | false | Verify request and response payload. | +| `rtt_warn` | float | 0.00 | RTT(Round-Trip Time) to generate warn. | +| `addresses` | string | * | List of bluetooth address of wireless devices to monitor. | + +- `rtt_warn` + - **0.00(zero)**: Disable checking RTT + - **otherwise**: Check RTT with specified seconds + +- `addresses` + - **\***: All connected devices + - **AA:BB:CC:DD:EE:FF**: You can specify a device to monitor by setting a Bluetooth address + +## Instructions before starting + +- You can skip this instructions if you run `l2ping_service` as root user. + +1. Assign capability to `l2ping_service` since L2ping requires `cap_net_raw+eip` capability. + + ```sh + sudo setcap 'cap_net_raw+eip' ./build/bluetooth_monitor/l2ping_service + ``` + +2. Run `l2ping_service` and `bluetooth_monitor`. + + ```sh + ./build/bluetooth_monitor/l2ping_service + ros2 launch bluetooth_monitor bluetooth_monitor.launch.xml + ``` + +## Known limitations and issues + +None. diff --git a/system/bluetooth_monitor/config/bluetooth_monitor.param.yaml b/system/bluetooth_monitor/config/bluetooth_monitor.param.yaml new file mode 100644 index 0000000000000..5a8d6ce575110 --- /dev/null +++ b/system/bluetooth_monitor/config/bluetooth_monitor.param.yaml @@ -0,0 +1,9 @@ +--- +/**: + ros__parameters: + port: 7640 + delay: 1 + timeout: 5 + verify: false + rtt_warn: 0.00 + addresses: ["4C:B9:9B:6E:7F:9A"] diff --git a/system/bluetooth_monitor/docs/block_diagram.drawio.svg b/system/bluetooth_monitor/docs/block_diagram.drawio.svg new file mode 100644 index 0000000000000..1087283100417 --- /dev/null +++ b/system/bluetooth_monitor/docs/block_diagram.drawio.svg @@ -0,0 +1,3441 @@ + + + + + + + + + +
+
+
+ Bluez +
+
+
+ + + Bluez + + + + + + + +
+
+
+ Autoware +
+
+
+ + + Autoware + + + + + + + + + + + +
+
+
+ Socket +
+ communication +
+
+
+ + + Socket... + + + + + + + +
+
+
+ + <<node>> +
+ + bluetooth_monitor +
+
+
+ + + <<node>>... + + + + + + + + + +
+
+
+ Get device information +
+
+
+ + + Get device information + + + + + + + + + +
+
+
+ Ping +
+
+
+ + + Ping + + + + + + + +
+
+
+ + <<daemon>> +
+ + l2ping_service +
+
+
+ + + <<daemon>>... + + + + + + + + + + +
+
+
+ + <<library>> +
+ + Bluez +
+ (libbluetooth) +
+
+
+ + + <<library>>... + + + + + + + + + + +
+
+
+ + USB/UART drivers + +
+
+
+ + + USB/UART drivers + + + + + + + + + + +
+
+
+ + Bluetooth hardware + +
+
+
+ + + Bluetooth hardware + + + + + + + Bluetooth device + + + + + + +
+
+
+ + Failsafe Architecture + +
+
+
+ + + Failsafe Architect... + + + + + + + + + + +
+
+
+ + L2CAP socket interface + +
+
+
+ + + L2CAP socket inter... + + + + + + + + + + +
+
+
+ + HCI socket interface + +
+
+
+ + + HCI socket interfa... + + + + + + + + + + +
+
+
+ + Bluez Core + +
+
+
+ + + Bluez Core + + + + + + + + + + +
+
+
+ + Host Controller Interface +
+ (HCI) + +
+
+
+ + + Host Controller Interface... + + + + + + + +
+
+
+

+ User Space +
+ Kernel Space +

+
+
+
+ + + User Space... + + + + + + + + + Viewer does not support full SVG 1.1 + + + + \ No newline at end of file diff --git a/system/bluetooth_monitor/include/bluetooth_monitor/bluetooth_monitor.hpp b/system/bluetooth_monitor/include/bluetooth_monitor/bluetooth_monitor.hpp new file mode 100644 index 0000000000000..9f464b70d120c --- /dev/null +++ b/system/bluetooth_monitor/include/bluetooth_monitor/bluetooth_monitor.hpp @@ -0,0 +1,115 @@ +// Copyright 2022 Autoware Foundation +// +// 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 BLUETOOTH_MONITOR__BLUETOOTH_MONITOR_HPP_ +#define BLUETOOTH_MONITOR__BLUETOOTH_MONITOR_HPP_ + +#include "bluetooth_monitor/service/l2ping_interface.hpp" + +#include +#include + +#include +#include +#include + +class BluetoothMonitor : public rclcpp::Node +{ +public: + /** + * @brief Constructor + */ + BluetoothMonitor(); + +protected: + /** + * @brief Connect to L2ping service + * @param [out] stat Diagnostic message passed directly to diagnostic publish calls + * @return true on success, false on error + * @note NOLINT syntax is needed since this function asks for a non-const reference + * to pass diagnostic message updated in this function to diagnostic publish calls. + */ + bool connectService( + diagnostic_updater::DiagnosticStatusWrapper & stat); // NOLINT(runtime/references) + + /** + * @brief Send L2ping configuration to L2ping service + * @param [out] stat Diagnostic message passed directly to diagnostic publish calls + * @return true on success, false on error + * @note NOLINT syntax is needed since this function asks for a non-const reference + * to pass diagnostic message updated in this function to diagnostic publish calls. + */ + bool sendConfig( + diagnostic_updater::DiagnosticStatusWrapper & stat); // NOLINT(runtime/references) + + /** + * @brief Receive data from L2ping service + * @param [out] stat Diagnostic message passed directly to diagnostic publish calls + * @return true on success, false on error + * @note NOLINT syntax is needed since this function asks for a non-const reference + * to pass diagnostic message updated in this function to diagnostic publish calls. + */ + bool receiveData(diagnostic_updater::DiagnosticStatusWrapper & stat); + + /** + * @brief Close connection with L2ping service + */ + void closeConnection(); + + /** + * @brief Set error level of diagnostic status + * @param [out] stat Diagnostic message passed directly to diagnostic publish calls + * @note NOLINT syntax is needed since diagnostic_updater asks for a non-const reference + * to pass diagnostic message updated in this function to diagnostic publish calls. + */ + void setErrorLevel( + diagnostic_updater::DiagnosticStatusWrapper & stat); // NOLINT(runtime/references) + + /** + * @brief Obtain diagnostic status and check connection + * @param [out] stat Diagnostic message passed directly to diagnostic publish calls + * @note NOLINT syntax is needed since diagnostic_updater asks for a non-const reference + * to pass diagnostic message updated in this function to diagnostic publish calls. + */ + void checkConnection( + diagnostic_updater::DiagnosticStatusWrapper & stat); // NOLINT(runtime/references) + + diagnostic_updater::Updater updater_; //!< @brief Updater class which advertises to /diagnostics + int socket_; //!< @brief Socket to communicate with L2ping service + int port_; //!< @brief Port number to connect with L2ping service + L2pingServiceConfig config_; //!< @brief Configuration of L2ping service + L2pingStatusList status_list_; //!< @brief Device status list + + using DiagStatus = diagnostic_msgs::msg::DiagnosticStatus; + + static constexpr const char * FUNCTION_ERROR_STR = "Function error"; + + const std::map status_string_list_ = { + {StatusCode::OK, "OK"}, + {StatusCode::RTT_WARNING, "RTT warning"}, + {StatusCode::VERIFY_ERROR, "Verify error"}, + {StatusCode::LOST, "Lost"}, + {StatusCode::REJECTED, "Ping rejected"}, + {StatusCode::FUNCTION_ERROR, FUNCTION_ERROR_STR}}; + + const std::map status_error_list_ = { + {StatusCode::OK, DiagStatus::OK}, + {StatusCode::RTT_WARNING, DiagStatus::WARN}, + {StatusCode::VERIFY_ERROR, DiagStatus::WARN}, + {StatusCode::LOST, DiagStatus::ERROR}, + {StatusCode::REJECTED, DiagStatus::ERROR}, + {StatusCode::FUNCTION_ERROR, DiagStatus::ERROR}}; +}; + +#endif // BLUETOOTH_MONITOR__BLUETOOTH_MONITOR_HPP_ diff --git a/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping.hpp b/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping.hpp new file mode 100644 index 0000000000000..f1b55a18cb5ea --- /dev/null +++ b/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping.hpp @@ -0,0 +1,134 @@ +// Copyright 2022 Autoware Foundation +// +// 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 SERVICE__L2PING_HPP_ +#define SERVICE__L2PING_HPP_ + +#include "bluetooth_monitor/service/l2ping_interface.hpp" + +#include +#include +#include + +#include +#include +#include + +inline float tv2fl(const timeval & tv) +{ + return static_cast(tv.tv_sec * 1000.0) + static_cast(tv.tv_sec / 1000.0); +} + +class L2ping +{ +public: + /** + * @brief Constructor + * @param [in] address Bluetooth address of remote device + * @param [in] config Configuration of L2ping + */ + L2ping(const bdaddr_t & address, const L2pingConfig & config); + + /** + * @brief Start ping thread + */ + void run(); + + /** + * @brief Get information from remote device + * @param [in] handle Handle to connection info + * @return true on success, false on error + */ + bool getDeviceInformation(uint16_t handle); + + /** + * @brief Get status + * @return Status + */ + L2pingStatus getStatus() const; + + /** + * @brief Get address of remote device + * @return address of remote device + */ + const std::string & getAddress() const; + +protected: + /** + * @brief Thread loop + */ + void thread(); + + /** + * @brief Connect to remote device + * @return true on success, false on error + */ + bool initialize(); + + /** + * @brief Shutdown + */ + void shutdown(); + + /** + * @brief Ping to remote device + * @return true on success, false on error + */ + bool ping(); + + /** + * @brief Send echo command to remote device + * @param [in] id id + * @return true on success, false on error + */ + bool sendEchoCommand(int id); + + /** + * @brief Receive echo command from remote device + * @return true on success, false on error + */ + bool receiveEchoComand(); + + /** + * @brief Verify received command + * @param [in] length Command length + * @return true on success, false on error + */ + bool verifyCommand(uint16_t length); + + /** + * @brief Set function error data to inform ros2 node + * @param [in] function_name Function name which error occurred + * @param [in] error_code number which is set by system calls + */ + void setFunctionError(const std::string & function_name, int error_code); + + /** + * @brief Set status code + * @param [in] code Status code + */ + void setStatusCode(StatusCode code); + + bdaddr_t address_; //!< @brief Bluetooth address of remote device + L2pingConfig config_; //!< @brief Configuration of L2ping + int socket_; //!< @brief Socket to L2CAP protocol + std::thread thread_; //!< @brief Thread to L2ping + L2pingStatus status_; //!< @brief L2ping status + + static constexpr int SIZE = 44; //!< @brief The size of the data packets to be sent + unsigned char send_buffer_[L2CAP_CMD_HDR_SIZE + SIZE]; //!< @brief buffer to send + unsigned char receive_buffer_[L2CAP_CMD_HDR_SIZE + SIZE]; //!< @brief buffer to receive +}; + +#endif // SERVICE__L2PING_HPP_ diff --git a/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping_interface.hpp b/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping_interface.hpp new file mode 100644 index 0000000000000..1dbf84e4540bd --- /dev/null +++ b/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping_interface.hpp @@ -0,0 +1,139 @@ +// Copyright 2022 Autoware Foundation +// +// 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 SERVICE__L2PING_INTERFACE_HPP_ +#define SERVICE__L2PING_INTERFACE_HPP_ + +#include +#include +#include + +#include + +#include +#include + +// 7634-7647 Unassigned +static constexpr int DEFAULT_PORT = 7640; +static constexpr int DEFAULT_DELAY = 1; +static constexpr int DEFAULT_TIMEOUT = 5; +static constexpr bool DEFAULT_VERIFY = false; +static constexpr float RTT_NO_WARN = 0.0f; + +/** + * @brief Configuration of L2ping + */ +struct L2pingConfig +{ + int delay{DEFAULT_DELAY}; //!< @brief Wait seconds between sending each packet + int timeout{DEFAULT_TIMEOUT}; //!< @brief Wait timeout seconds for the response + bool verify{DEFAULT_VERIFY}; //!< @brief Verify request and response payload + float rtt_warn{RTT_NO_WARN}; //!< @brief RTT warning time + + /** + * @brief Load or save data members. + * @param [inout] ar Archive reference to load or save the serialized data members + * @param [in] version Version for the archive + * @note NOLINT syntax is needed since this is an interface to serialization and + * used inside boost serialization. + */ + template + void serialize(archive & ar, const unsigned /*version*/) // NOLINT(runtime/references) + { + ar & delay; + ar & timeout; + ar & verify; + ar & rtt_warn; + } +}; + +/** + * @brief Configuration of L2ping service + */ +struct L2pingServiceConfig +{ + L2pingConfig l2ping{}; //!< @brief Configuration of L2ping + std::vector addresses; //!< @brief List of bluetooth address + + /** + * @brief Load or save data members. + * @param [inout] ar Archive reference to load or save the serialized data members + * @param [in] version Version for the archive + * @note NOLINT syntax is needed since this is an interface to serialization and + * used inside boost serialization. + */ + template + void serialize(archive & ar, const unsigned /*version*/) // NOLINT(runtime/references) + { + ar & l2ping; + ar & addresses; + } +}; + +/** + * @brief Status code of a device + */ +enum class StatusCode { + OK = 0, + RTT_WARNING = 1, + VERIFY_ERROR = 2, + LOST = 3, + REJECTED = 4, + FUNCTION_ERROR = 5, +}; + +/** + * @brief L2ping status + */ +struct L2pingStatus +{ + StatusCode status_code; //!< @brief Status code of a device + std::string function_name; //!< @brief Function name which error occurred + int error_code; //!< @brief Error number which is set by system calls + + std::string name; //!< @brief Name of remote device + std::string manufacturer; //!< @brief Manufacturer name of remote device + std::string address; //!< @brief Bluetooth address + float time_difference; //!< @brief Time difference between sent and received + int sent_packets; //!< @brief Number of sent packets to remote device + int received_packets; //!< @brief Number of received packets from remote device + + /** + * @brief Load or save data members. + * @param [inout] ar Archive reference to load or save the serialized data members + * @param [in] version Version for the archive + * @note NOLINT syntax is needed since this is an interface to serialization and + * used inside boost serialization. + */ + template + void serialize(archive & ar, const unsigned /*version*/) // NOLINT(runtime/references) + { + ar & status_code; + ar & function_name; + ar & error_code; + ar & name; + ar & manufacturer; + ar & address; + ar & time_difference; + ar & sent_packets; + ar & received_packets; + } +}; + +/** + * @brief List of L2ping status + */ +typedef std::vector L2pingStatusList; + +#endif // SERVICE__L2PING_INTERFACE_HPP_ diff --git a/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping_service.hpp b/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping_service.hpp new file mode 100644 index 0000000000000..25c7847f8a0fb --- /dev/null +++ b/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping_service.hpp @@ -0,0 +1,83 @@ +// Copyright 2022 Autoware Foundation +// +// 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 SERVICE__L2PING_SERVICE_HPP_ +#define SERVICE__L2PING_SERVICE_HPP_ + +#include "bluetooth_monitor/service/l2ping.hpp" +#include "bluetooth_monitor/service/l2ping_interface.hpp" + +#include +#include +#include +#include +#include + +class L2pingService +{ +public: + /** + * @brief Constructor + * @param [in] port Port number to access l2ping service + */ + explicit L2pingService(const int port); + + /** + * @brief Initialization + * @return true on success, false on error + */ + bool initialize(); + + /** + * @brief Shutdown + */ + void shutdown(); + + /** + * @brief Main loop + */ + void run(); + +protected: + /** + * @brief Set error data to inform ros2 node + * @param [in] function_name Function name which error occurred + * @param [in] error_code number which is set by system calls + */ + void setFunctionError(const std::string & function_name, int error_code); + + /** + * @brief Build device list to ping + * @param [in] addresses List of bluetooth address + * @return true on success, false on error + */ + bool buildDeviceList(); + + /** + * @brief Build device list to ping from connected devices + * @param [in] sock socket to bluetooth host controller interface(HCI) + * @param [in] device_id Device ID + * @param [in] addresses List of bluetooth address + * @return true on success, false on error + */ + bool buildDeviceListFromConnectedDevices(int sock, uint16_t device_id); + + int port_; //!< @brief Port number to access l2ping service + int socket_; //!< @brief Socket to communicate with ros2 node + L2pingServiceConfig config_; //!< @brief Configuration of L2ping service + std::vector> objects_; //!< @brief List of l2ping object + L2pingStatusList status_list_; //!< @brief List of l2ping status +}; + +#endif // SERVICE__L2PING_SERVICE_HPP_ diff --git a/system/bluetooth_monitor/launch/bluetooth_monitor.launch.xml b/system/bluetooth_monitor/launch/bluetooth_monitor.launch.xml new file mode 100644 index 0000000000000..7dd98572e544b --- /dev/null +++ b/system/bluetooth_monitor/launch/bluetooth_monitor.launch.xml @@ -0,0 +1,6 @@ + + + + + + diff --git a/system/bluetooth_monitor/package.xml b/system/bluetooth_monitor/package.xml new file mode 100644 index 0000000000000..e497b02c1d62c --- /dev/null +++ b/system/bluetooth_monitor/package.xml @@ -0,0 +1,24 @@ + + + + bluetooth_monitor + 0.1.0 + Bluetooth alive monitoring + Fumihito Ito + Apache License 2.0 + + ament_cmake_auto + + bluez + diagnostic_msgs + diagnostic_updater + fmt + rclcpp + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/system/bluetooth_monitor/service/l2ping.cpp b/system/bluetooth_monitor/service/l2ping.cpp new file mode 100644 index 0000000000000..b05270cc90227 --- /dev/null +++ b/system/bluetooth_monitor/service/l2ping.cpp @@ -0,0 +1,354 @@ +// Copyright 2022 Autoware Foundation +// +// 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. +// +// This code is derived from BlueZ with following copyrights. + +// SPDX-License-Identifier: GPL-2.0-or-later +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2000-2001 Qualcomm Incorporated + * Copyright (C) 2002-2003 Maxim Krasnyansky + * Copyright (C) 2002-2010 Marcel Holtmann + * + * + */ + +#include "bluetooth_monitor/service/l2ping.hpp" + +#include +#include +#include +#include +#include + +static constexpr int IDENTIFIER = 200; //!< @brief Identifier to match responses with requests + +L2ping::L2ping(const bdaddr_t & address, const L2pingConfig & config) +: config_(config), socket_(-1), status_{} +{ + bacpy(&address_, &address); + + // Prepare string value fot log output + char address_str[18]{}; + ba2str(&address_, address_str); + status_.address = address_str; + + // Initialize buffer + memset(send_buffer_, 0, sizeof(send_buffer_)); + for (int i = 0; i < SIZE; ++i) send_buffer_[L2CAP_CMD_HDR_SIZE + i] = (i % 40) + 'A'; +} + +bool L2ping::getDeviceInformation(uint16_t handle) +{ + // Retrieve the resource number of the Bluetooth adapter + int dev_id = hci_get_route(&address_); + if (dev_id < 0) { + setFunctionError("hci_get_route", errno); + syslog( + LOG_ERR, "Device is not available. address = %s, %s", status_.address.c_str(), + strerror(errno)); + return false; + } + + // Opens a Bluetooth socket with the specified resource number + int dev_sock = hci_open_dev(dev_id); + if (dev_sock < 0) { + setFunctionError("hci_open_dev", errno); + syslog( + LOG_ERR, "Failed to open HCI device. address = %s, %s", status_.address.c_str(), + strerror(errno)); + return false; + } + + // Get name of remote device + char name[HCI_MAX_NAME_LENGTH]{}; + + if (hci_read_remote_name(dev_sock, &address_, sizeof(name), name, 25000) == 0) { + status_.name = name; + } + + // Get version of remote device + hci_version version{}; + + if (hci_read_remote_version(dev_sock, handle, &version, 20000) == 0) { + char * version_str = lmp_vertostr(version.lmp_ver); + status_.manufacturer = bt_compidtostr(version.manufacturer); + syslog( + LOG_INFO, + "Device Name: %s LMP Version: %s (0x%x) LMP Subversion: 0x%x Manufacturer: %s (%d)\n", name, + version_str ? version_str : "n/a", version.lmp_ver, version.lmp_subver, + bt_compidtostr(version.manufacturer), version.manufacturer); + if (version_str) bt_free(version_str); + } + + hci_close_dev(dev_sock); + + return true; +} + +void L2ping::run() +{ + // Start thread loop + thread_ = std::thread(&L2ping::thread, this); +} + +void L2ping::thread() +{ + while (true) { + // Connect to remote device + if (initialize()) { + // Ping to remote device in loop + ping(); + } + + // Retry create socket + shutdown(); + sleep(DEFAULT_DELAY); + } + + shutdown(); +} + +bool L2ping::initialize() +{ + // Create socket to L2CAP protocol + socket_ = socket(PF_BLUETOOTH, SOCK_RAW, BTPROTO_L2CAP); + + if (socket_ < 0) { + setFunctionError("socket", errno); + syslog( + LOG_ERR, "Failed to create socket for '%s'. %s\n", status_.address.c_str(), strerror(errno)); + return false; + } + + // Register local address to connect to L2CAP + sockaddr_l2 local_address{}; + local_address.l2_family = AF_BLUETOOTH; + + if (bind(socket_, (struct sockaddr *)&local_address, sizeof(local_address)) < 0) { + setFunctionError("bind", errno); + syslog( + LOG_ERR, "Failed to bind socket for '%s'. %s\n", status_.address.c_str(), strerror(errno)); + return false; + } + + // Connect to remote device + sockaddr_l2 remote_address{}; + remote_address.l2_family = AF_BLUETOOTH; + remote_address.l2_bdaddr = address_; + + if (connect(socket_, (struct sockaddr *)&remote_address, sizeof(remote_address)) < 0) { + setFunctionError("connect", errno); + syslog(LOG_ERR, "Failed to connect for '%s'. %s\n", status_.address.c_str(), strerror(errno)); + return false; + } + + return true; +} + +void L2ping::shutdown() { close(socket_); } + +bool L2ping::ping() +{ + // Get local address + sockaddr_l2 local_address{}; + socklen_t length = sizeof(local_address); + + if (getsockname(socket_, (struct sockaddr *)&local_address, &length) < 0) { + setFunctionError("getsockname", errno); + syslog(LOG_ERR, "Failed to get address. %s\n", strerror(errno)); + return false; + } + + char host_address[18]{}; + ba2str(&local_address.l2_bdaddr, host_address); + syslog( + LOG_INFO, "Ping: %s from %s (data size %d) ...\n", status_.address.c_str(), host_address, SIZE); + + int id = IDENTIFIER; + + while (true) { + timeval time_send{}; + timeval time_receive{}; + timeval time_diff{}; + + gettimeofday(&time_send, NULL); + + // Send echo command + if (!sendEchoCommand(id)) { + return false; + } + + // Send completed + ++status_.sent_packets; + + l2cap_cmd_hdr * receive_command = reinterpret_cast(receive_buffer_); + + // Wait for echo response + while (true) { + if (!receiveEchoComand()) { + return false; + } + + // Check received command + receive_command->len = btohs(receive_command->len); + + // Check for our id + if (receive_command->ident != id) { + // Retry to receive our id + continue; + } + // Check type + if (receive_command->code == L2CAP_ECHO_RSP) { + // Receive complete + break; + } + // Echo command rejected + if (receive_command->code == L2CAP_COMMAND_REJ) { + setStatusCode(StatusCode::REJECTED); + syslog( + LOG_ERR, "Peer doesn't support echo packets for '%s'. %s\n", status_.address.c_str(), + strerror(errno)); + return false; + } + } + + // Remote device responds correctly + ++status_.received_packets; + + gettimeofday(&time_receive, NULL); + timersub(&time_receive, &time_send, &time_diff); + status_.time_difference = tv2fl(time_diff); + + bool failed = false; + // If verify request and response payload + if (config_.verify) { + // Verify received command + if (!verifyCommand(receive_command->len)) { + failed = true; + setStatusCode(StatusCode::VERIFY_ERROR); + } + } + + // If check RTT + if (!failed && config_.rtt_warn > RTT_NO_WARN) { + if (status_.time_difference >= config_.rtt_warn) { + setStatusCode(StatusCode::RTT_WARNING); + } + } + + if (!failed) setStatusCode(StatusCode::OK); + + // Try next send + sleep(config_.delay); + if (++id > 254) id = IDENTIFIER; + } +} + +bool L2ping::sendEchoCommand(int id) +{ + l2cap_cmd_hdr * send_command = reinterpret_cast(send_buffer_); + + // Build command header + send_command->ident = id; + send_command->len = htobs(SIZE); + send_command->code = L2CAP_ECHO_REQ; + + // Send echo command + if (send(socket_, send_buffer_, sizeof(send_buffer_), 0) <= 0) { + // Remote device is down + setStatusCode(StatusCode::LOST); + syslog(LOG_ERR, "Failed to send for '%s'. %s\n", status_.address.c_str(), strerror(errno)); + return false; + } + + return true; +} + +bool L2ping::receiveEchoComand() +{ + pollfd pf[1]{}; + int ret; + + pf[0].fd = socket_; + pf[0].events = POLLIN; + + if ((ret = poll(pf, 1, config_.timeout * 1000)) < 0) { + setFunctionError("poll", errno); + syslog(LOG_ERR, "Failed to poll for '%s'. %s\n", status_.address.c_str(), strerror(errno)); + return false; + } + + // Timeout + if (!ret) { + setStatusCode(StatusCode::LOST); + syslog(LOG_ERR, "Polling timeout for '%s'.\n", status_.address.c_str()); + return false; + } + + memset(receive_buffer_, 0, sizeof(receive_buffer_)); + + if ((ret = recv(socket_, receive_buffer_, sizeof(receive_buffer_), 0)) < 0) { + setStatusCode(StatusCode::LOST); + syslog(LOG_ERR, "Failed to receive for '%s'. %s\n", status_.address.c_str(), strerror(errno)); + return false; + } + + // Shutdown connection from device + if (!ret) { + setStatusCode(StatusCode::LOST); + syslog(LOG_ERR, "Disconnected from '%s'. %s\n", status_.address.c_str(), strerror(errno)); + return false; + } + + return true; +} + +bool L2ping::verifyCommand(uint16_t length) +{ + // Check payload length + if (length != SIZE) { + syslog(LOG_ERR, "Received %d bytes, expected %d for %s", length, SIZE, status_.address.c_str()); + return false; + } + // Check payload + if (memcmp(&send_buffer_[L2CAP_CMD_HDR_SIZE], &receive_buffer_[L2CAP_CMD_HDR_SIZE], SIZE)) { + syslog(LOG_ERR, "Response payload different for %s.", status_.address.c_str()); + return false; + } + + return true; +} + +void L2ping::setFunctionError(const std::string & function_name, int error_code) +{ + // Build error data + status_.status_code = StatusCode::FUNCTION_ERROR; + status_.function_name = function_name; + status_.error_code = error_code; +} + +void L2ping::setStatusCode(StatusCode code) +{ + // Build error data + status_.status_code = code; + status_.function_name = {}; + status_.error_code = 0; +} + +L2pingStatus L2ping::getStatus() const { return status_; } + +const std::string & L2ping::getAddress() const { return status_.address; } diff --git a/system/bluetooth_monitor/service/l2ping_service.cpp b/system/bluetooth_monitor/service/l2ping_service.cpp new file mode 100644 index 0000000000000..9eaa2fa13eb8e --- /dev/null +++ b/system/bluetooth_monitor/service/l2ping_service.cpp @@ -0,0 +1,281 @@ +// Copyright 2022 Autoware Foundation +// +// 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. +// +// This code is derived from BlueZ with following copyrights. + +// SPDX-License-Identifier: GPL-2.0-or-later +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2000-2001 Qualcomm Incorporated + * Copyright (C) 2002-2003 Maxim Krasnyansky + * Copyright (C) 2002-2010 Marcel Holtmann + * + * + */ + +#include "bluetooth_monitor/service/l2ping_service.hpp" + +#include +#include + +#include +#include +#include +#include + +#include + +static const bdaddr_t ANY_ADDRESS = {{0, 0, 0, 0, 0, 0}}; + +L2pingService::L2pingService(const int port) : port_(port), socket_(-1) {} + +bool L2pingService::initialize() +{ + // Create a new socket + socket_ = socket(AF_INET, SOCK_STREAM, 0); + if (socket_ < 0) { + syslog(LOG_ERR, "Failed to create a new socket. %s\n", strerror(errno)); + return false; + } + + // Allow address reuse + int ret = 0; + int opt = 1; + ret = setsockopt( + socket_, SOL_SOCKET, SO_REUSEADDR, reinterpret_cast(&opt), (socklen_t)sizeof(opt)); + if (ret < 0) { + syslog(LOG_ERR, "Failed to set socket FD's option. %s\n", strerror(errno)); + return false; + } + + // Give the socket FD the local address ADDR + sockaddr_in address; + memset(&address, 0, sizeof(sockaddr_in)); + address.sin_family = AF_INET; + address.sin_port = htons(port_); + address.sin_addr.s_addr = htonl(INADDR_ANY); + ret = bind(socket_, (struct sockaddr *)&address, sizeof(address)); + if (ret < 0) { + syslog(LOG_ERR, "Failed to give the socket FD the local address ADDR. %s\n", strerror(errno)); + return false; + } + + // Prepare to accept connections on socket FD + ret = listen(socket_, 5); + if (ret < 0) { + syslog(LOG_ERR, "Failed to prepare to accept connections on socket FD. %s\n", strerror(errno)); + return false; + } + + return true; +} + +void L2pingService::shutdown() { close(socket_); } + +void L2pingService::run() +{ + sockaddr_in client; + socklen_t len = sizeof(client); + + while (true) { + // Await a connection on socket FD + int new_sock = accept(socket_, reinterpret_cast(&client), &len); + if (new_sock < 0) { + syslog( + LOG_ERR, "Failed to prepare to accept connections on socket FD. %s\n", strerror(errno)); + continue; + } + + // Receive configuration of L2ping + char buf[1024]{}; + int ret = recv(new_sock, buf, sizeof(buf) - 1, 0); + if (ret < 0) { + syslog(LOG_ERR, "Failed to receive. %s\n", strerror(errno)); + close(new_sock); + continue; + } + // No data received + if (ret == 0) { + syslog(LOG_ERR, "No data received. %s\n", strerror(errno)); + close(new_sock); + continue; + } + + // Restore configuration of L2ping + try { + std::istringstream iss(buf); + boost::archive::text_iarchive oa(iss); + oa & config_; + } catch (const std::exception & e) { + syslog(LOG_ERR, "Exception occurred. %s\n", e.what()); + close(new_sock); + continue; + } + + // Now communication with ros2 node successful + + // Build device list to ping + if (buildDeviceList()) { + // Clear list + status_list_.clear(); + // Build status list + for (const auto & object : objects_) { + status_list_.emplace_back(object->getStatus()); + } + } + + // Inform ros2 node if error occurred + std::ostringstream out_stream; + boost::archive::text_oarchive archive(out_stream); + archive << status_list_; + // Write N bytes of BUF to FD + ret = write(new_sock, out_stream.str().c_str(), out_stream.str().length()); + if (ret < 0) { + syslog(LOG_ERR, "Failed to write N bytes of BUF to FD. %s\n", strerror(errno)); + } + + close(new_sock); + } +} + +void L2pingService::setFunctionError(const std::string & function_name, int error_code) +{ + // Clear list + status_list_.clear(); + + // Set error data + L2pingStatus status; + status.status_code = StatusCode::FUNCTION_ERROR; + status.function_name = function_name; + status.error_code = error_code; + + status_list_.emplace_back(status); +} + +bool L2pingService::buildDeviceList() +{ + // Create socket to bluetooth host controller interface(HCI) + int sock = socket(AF_BLUETOOTH, SOCK_RAW, BTPROTO_HCI); + + if (sock < 0) { + setFunctionError("socket", errno); + syslog(LOG_ERR, "Failed to create socket. %s", strerror(errno)); + return false; + } + + // Register local address to connect to HCI + sockaddr_rc local_address{}; + + local_address.rc_family = AF_BLUETOOTH; // Bluetooth family + local_address.rc_bdaddr = ANY_ADDRESS; // Any address + local_address.rc_channel = 1; // Single channel + + if (bind(sock, (struct sockaddr *)&local_address, sizeof(local_address)) < 0) { + setFunctionError("bind", errno); + syslog(LOG_ERR, "Failed to bind socket. %s", strerror(errno)); + return false; + } + + // Get list of HCI devices + hci_dev_list_req hci_device_list{}; + hci_device_list.dev_num = HCI_MAX_DEV; + + if (ioctl(sock, HCIGETDEVLIST, &hci_device_list) < 0) { + setFunctionError("HCIGETDEVLIST", errno); + syslog(LOG_ERR, "Failed to get list of HCI devices. %s", strerror(errno)); + close(sock); + return false; + } + + // Get information of each HCI device + hci_dev_req * hci_device = hci_device_list.dev_req; + + // Loop for HCI devices + for (int i = 0; i < hci_device_list.dev_num; ++i, ++hci_device) { + // Build device list to ping from connected devices + if (!buildDeviceListFromConnectedDevices(sock, hci_device->dev_id)) return false; + } + + close(sock); + + return true; +} + +bool L2pingService::buildDeviceListFromConnectedDevices(int sock, uint16_t device_id) +{ + // Get informaton of HCI device + hci_dev_info hci_device_info{}; + hci_device_info.dev_id = device_id; + if (ioctl(sock, HCIGETDEVINFO, &hci_device_info) < 0) { + setFunctionError("HCIGETDEVINFO", errno); + syslog( + LOG_ERR, "Failed to get informaton of HCI device. id = %d, %s", hci_device_info.dev_id, + strerror(errno)); + return false; + } + + // Get list of connected devices + hci_conn_list_req connection_list_request{}; + connection_list_request.dev_id = hci_device_info.dev_id; + connection_list_request.conn_num = HCI_MAX_DEV; + + if (ioctl(sock, HCIGETCONNLIST, &connection_list_request)) { + setFunctionError("HCIGETCONNLIST", errno); + syslog( + LOG_ERR, "Failed to get connection list of HCI device. id = %d, %s", hci_device_info.dev_id, + strerror(errno)); + return false; + } + + // Allocate space for list of connected devices + hci_conn_info * devices = new hci_conn_info[connection_list_request.conn_num]; + + // Get information of each connected device + hci_conn_info * connection_info = connection_list_request.conn_info; + + // Loop for connected devices + for (int i = 0; i < connection_list_request.conn_num; ++i, ++connection_info) { + char address_str[18]{}; + ba2str(&connection_info->bdaddr, address_str); + + // Skip if device not found and wild card not specified + if ( + std::count(config_.addresses.begin(), config_.addresses.end(), address_str) == 0 && + std::count(config_.addresses.begin(), config_.addresses.end(), "*") == 0) { + continue; + } + + bool found = false; + for (const auto & object : objects_) { + // If device not registered + if (object->getAddress() == address_str) { + found = true; + break; + } + } + + if (!found) { + // Start ping thread + objects_.emplace_back(std::make_unique(connection_info->bdaddr, config_.l2ping)); + objects_.back().get()->getDeviceInformation(connection_info->handle); + objects_.back().get()->run(); + } + } + + delete[] devices; + + return true; +} diff --git a/system/bluetooth_monitor/service/main.cpp b/system/bluetooth_monitor/service/main.cpp new file mode 100644 index 0000000000000..2c459237510ee --- /dev/null +++ b/system/bluetooth_monitor/service/main.cpp @@ -0,0 +1,97 @@ +// Copyright 2022 Autoware Foundation +// +// 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 "bluetooth_monitor/service/l2ping_interface.hpp" +#include "bluetooth_monitor/service/l2ping_service.hpp" + +#include + +#include +#include + +/** + * @brief print usage + */ +void usage() +{ + printf("Usage: l2ping_service [options]\n"); + printf(" -h --help : Display help\n"); + printf(" -p --port # : Port number to listen to.\n"); + printf("\n"); +} + +/** + * @brief Maln loop + * @param [in] argc Number of arguments + * @param [in] argv Command line arguments + * @return 0 on success, 1 on error + */ +int main(int argc, char ** argv) +{ + static struct option long_options[] = { + {"help", no_argument, 0, 'h'}, {"port", required_argument, 0, 'p'}, {0, 0, 0, 0}}; + + // Parse command-line options + int c = 0; + int option_index = 0; + int port = DEFAULT_PORT; + while ((c = getopt_long(argc, argv, "hp:", long_options, &option_index)) != -1) { + switch (c) { + case 'h': + usage(); + return EXIT_SUCCESS; + + case 'p': + try { + port = boost::lexical_cast(optarg); + } catch (const boost::bad_lexical_cast & e) { + printf("Error: %s\n", e.what()); + return EXIT_FAILURE; + } + break; + + default: + break; + } + } + + // Put the program in the background + if (daemon(0, 0) < 0) { + printf("Failed to put the program in the background. %s\n", strerror(errno)); + return errno; + } + + // Open connection to system logger + openlog(nullptr, LOG_PID, LOG_DAEMON); + + // Initialize l2ping service + L2pingService service(port); + + if (!service.initialize()) { + service.shutdown(); + closelog(); + return EXIT_FAILURE; + } + + // Run main loop + service.run(); + + // Shutdown l2ping service + service.shutdown(); + + // Close descriptor used to write to system logger + closelog(); + + return EXIT_SUCCESS; +} diff --git a/system/bluetooth_monitor/src/bluetooth_monitor.cpp b/system/bluetooth_monitor/src/bluetooth_monitor.cpp new file mode 100644 index 0000000000000..764af952ff013 --- /dev/null +++ b/system/bluetooth_monitor/src/bluetooth_monitor.cpp @@ -0,0 +1,197 @@ +// Copyright 2022 Autoware Foundation +// +// 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 "bluetooth_monitor/bluetooth_monitor.hpp" + +#include +#include + +#include +#include +#include + +#define FMT_HEADER_ONLY +#include + +BluetoothMonitor::BluetoothMonitor() +: Node("bluetooth_monitor"), + updater_(this), + socket_(-1), + port_(declare_parameter("port", DEFAULT_PORT)) +{ + // Get host name + char host_name[HOST_NAME_MAX + 1]; + gethostname(host_name, sizeof(host_name)); + + // Build L2ping configuration + config_.l2ping.delay = declare_parameter("delay", DEFAULT_DELAY); + config_.l2ping.timeout = declare_parameter("timeout", DEFAULT_TIMEOUT); + config_.l2ping.verify = declare_parameter("verify", DEFAULT_VERIFY); + config_.l2ping.rtt_warn = declare_parameter("rtt_warn", RTT_NO_WARN); + config_.addresses = declare_parameter("addresses", std::vector()); + + updater_.add("bluetooth_connection", this, &BluetoothMonitor::checkConnection); + updater_.setHardwareID(host_name); +} + +bool BluetoothMonitor::connectService(diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + // Create a new socket + socket_ = socket(AF_INET, SOCK_STREAM, 0); + if (socket_ < 0) { + stat.summary(DiagStatus::ERROR, FUNCTION_ERROR_STR); + stat.add("socket", strerror(errno)); + return false; + } + + // Specify the receiving timeouts until reporting an error + timeval tv{10, 0}; + int ret = setsockopt(socket_, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv)); + if (ret < 0) { + stat.summary(DiagStatus::ERROR, FUNCTION_ERROR_STR); + stat.add("setsockopt", strerror(errno)); + return false; + } + + // Connect the socket referred to by the file descriptor + sockaddr_in address{}; + address.sin_family = AF_INET; + address.sin_port = htons(port_); + address.sin_addr.s_addr = htonl(INADDR_ANY); + + ret = connect(socket_, reinterpret_cast(&address), sizeof(address)); + if (ret < 0) { + stat.summary(DiagStatus::ERROR, FUNCTION_ERROR_STR); + stat.add("connect", strerror(errno)); + return false; + } + + return true; +} + +bool BluetoothMonitor::sendConfig(diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + std::ostringstream out_stream; + boost::archive::text_oarchive archive(out_stream); + archive & config_; + + // Write list of devices to FD + int ret = write(socket_, out_stream.str().c_str(), out_stream.str().length()); + if (ret < 0) { + stat.summary(DiagStatus::ERROR, FUNCTION_ERROR_STR); + stat.add("write", strerror(errno)); + return false; + } + + return true; +} + +bool BluetoothMonitor::receiveData(diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + char buffer[1024]{}; + + int ret = recv(socket_, buffer, sizeof(buffer) - 1, 0); + if (ret < 0) { + stat.summary(DiagStatus::ERROR, FUNCTION_ERROR_STR); + stat.add("recv", strerror(errno)); + return false; + } + // No data received + if (ret == 0) { + stat.summary(DiagStatus::ERROR, FUNCTION_ERROR_STR); + stat.add("recv", "No data received"); + return false; + } + + // Restore device status list + try { + std::istringstream in_stream(buffer); + boost::archive::text_iarchive archive(in_stream); + archive >> status_list_; + } catch (const std::exception & e) { + stat.summary(DiagStatus::ERROR, "Exception occurred"); + stat.add("Exception", e.what()); + return false; + } + + return true; +} + +void BluetoothMonitor::closeConnection() { close(socket_); } + +void BluetoothMonitor::setErrorLevel(diagnostic_updater::DiagnosticStatusWrapper & stat) +{ // No device + if (status_list_.size() == 0) { + stat.summary(DiagStatus::OK, "No device connected"); + return; + } + + int level = DiagStatus::OK; + int whole_level = DiagStatus::OK; + StatusCode whole_status_code = StatusCode::OK; + int index = 0; + for (const auto & status : status_list_) { + stat.add(fmt::format("Device {}: Status", index), status_string_list_.at(status.status_code)); + stat.add(fmt::format("Device {}: Name", index), status.name); + stat.add(fmt::format("Device {}: Manufacturer", index), status.manufacturer); + stat.add(fmt::format("Device {}: Address", index), status.address); + stat.addf(fmt::format("Device {}: RTT", index), "%.2f ms", status.time_difference); + stat.addf(fmt::format("Device {} Sent packets", index), "%d", status.sent_packets); + stat.addf(fmt::format("Device {} Received packets", index), "%d", status.received_packets); + + if (status.status_code == StatusCode::FUNCTION_ERROR) { + stat.add(fmt::format("Device {}: {}", index, status.function_name), strerror(errno)); + } + + level = status_error_list_.at(status.status_code); + whole_level = std::max(whole_level, level); + whole_status_code = std::max(whole_status_code, status.status_code); + ++index; + } + + stat.summary(whole_level, status_string_list_.at(whole_status_code)); +} + +void BluetoothMonitor::checkConnection(diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + if (config_.addresses.empty()) { + RCLCPP_ERROR(get_logger(), "Invalid device parameter."); + stat.summary(DiagStatus::ERROR, "Invalid device parameter"); + return; + } + + // Connect to L2ping service + if (!connectService(stat)) { + closeConnection(); + return; + } + + // Send L2ping configuration to L2ping service + if (!sendConfig(stat)) { + closeConnection(); + return; + } + + // Receive data from L2ping service + if (!receiveData(stat)) { + closeConnection(); + return; + } + + // Close connection with L2ping service + closeConnection(); + + // Set error level of diagnostic status + setErrorLevel(stat); +} diff --git a/system/bluetooth_monitor/src/bluetooth_monitor_node.cpp b/system/bluetooth_monitor/src/bluetooth_monitor_node.cpp new file mode 100644 index 0000000000000..f4134c09277aa --- /dev/null +++ b/system/bluetooth_monitor/src/bluetooth_monitor_node.cpp @@ -0,0 +1,29 @@ +// Copyright 2022 Autoware Foundation +// +// 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 "bluetooth_monitor/bluetooth_monitor.hpp" + +#include + +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + + return 0; +} From b379dd4bb07cc6bc7c4360278d5f5eb7ca32891a Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 6 May 2022 06:14:35 +0000 Subject: [PATCH 02/17] ci(pre-commit): autofix --- system/bluetooth_monitor/README.md | 19 ++++++++++--------- .../bluetooth_monitor/service/l2ping.hpp | 6 +++--- .../service/l2ping_interface.hpp | 6 +++--- .../service/l2ping_service.hpp | 6 +++--- 4 files changed, 19 insertions(+), 18 deletions(-) diff --git a/system/bluetooth_monitor/README.md b/system/bluetooth_monitor/README.md index e79f9ff4c3043..5666258f8e9d8 100644 --- a/system/bluetooth_monitor/README.md +++ b/system/bluetooth_monitor/README.md @@ -44,22 +44,23 @@ L2ping is only allowed for root by default, so this package provides the followi - The following key will be added when `bluetooth_monitor` reports `Function error`.
ex.) The `connect` system call failed. -| key (example) | value (example) | +| key (example) | value (example) | | --------------------- | ------------------------- | | Device [0-9]: connect | No such file or directory | ## Parameters -| Name | Type | Default Value | Explanation | -| ------------- | ------- | ------------- | --------------------------------------------------------- | -| `port` | int | 7640 | Port number to connect to L2ping service. | -| `delay` | int | 1 | Wait seconds between sending each packet. | -| `timeout` | int | 5 | Wait timeout seconds for the response. | -| `verify` | bool | false | Verify request and response payload. | -| `rtt_warn` | float | 0.00 | RTT(Round-Trip Time) to generate warn. | -| `addresses` | string | * | List of bluetooth address of wireless devices to monitor. | +| Name | Type | Default Value | Explanation | +| ----------- | ------ | ------------- | --------------------------------------------------------- | +| `port` | int | 7640 | Port number to connect to L2ping service. | +| `delay` | int | 1 | Wait seconds between sending each packet. | +| `timeout` | int | 5 | Wait timeout seconds for the response. | +| `verify` | bool | false | Verify request and response payload. | +| `rtt_warn` | float | 0.00 | RTT(Round-Trip Time) to generate warn. | +| `addresses` | string | \* | List of bluetooth address of wireless devices to monitor. | - `rtt_warn` + - **0.00(zero)**: Disable checking RTT - **otherwise**: Check RTT with specified seconds diff --git a/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping.hpp b/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping.hpp index f1b55a18cb5ea..c7fb87a08f6ec 100644 --- a/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping.hpp +++ b/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SERVICE__L2PING_HPP_ -#define SERVICE__L2PING_HPP_ +#ifndef BLUETOOTH_MONITOR__SERVICE__L2PING_HPP_ +#define BLUETOOTH_MONITOR__SERVICE__L2PING_HPP_ #include "bluetooth_monitor/service/l2ping_interface.hpp" @@ -131,4 +131,4 @@ class L2ping unsigned char receive_buffer_[L2CAP_CMD_HDR_SIZE + SIZE]; //!< @brief buffer to receive }; -#endif // SERVICE__L2PING_HPP_ +#endif // BLUETOOTH_MONITOR__SERVICE__L2PING_HPP_ diff --git a/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping_interface.hpp b/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping_interface.hpp index 1dbf84e4540bd..b93d08ebc5e3f 100644 --- a/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping_interface.hpp +++ b/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping_interface.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SERVICE__L2PING_INTERFACE_HPP_ -#define SERVICE__L2PING_INTERFACE_HPP_ +#ifndef BLUETOOTH_MONITOR__SERVICE__L2PING_INTERFACE_HPP_ +#define BLUETOOTH_MONITOR__SERVICE__L2PING_INTERFACE_HPP_ #include #include @@ -136,4 +136,4 @@ struct L2pingStatus */ typedef std::vector L2pingStatusList; -#endif // SERVICE__L2PING_INTERFACE_HPP_ +#endif // BLUETOOTH_MONITOR__SERVICE__L2PING_INTERFACE_HPP_ diff --git a/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping_service.hpp b/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping_service.hpp index 25c7847f8a0fb..ce30460dcfa31 100644 --- a/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping_service.hpp +++ b/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping_service.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SERVICE__L2PING_SERVICE_HPP_ -#define SERVICE__L2PING_SERVICE_HPP_ +#ifndef BLUETOOTH_MONITOR__SERVICE__L2PING_SERVICE_HPP_ +#define BLUETOOTH_MONITOR__SERVICE__L2PING_SERVICE_HPP_ #include "bluetooth_monitor/service/l2ping.hpp" #include "bluetooth_monitor/service/l2ping_interface.hpp" @@ -80,4 +80,4 @@ class L2pingService L2pingStatusList status_list_; //!< @brief List of l2ping status }; -#endif // SERVICE__L2PING_SERVICE_HPP_ +#endif // BLUETOOTH_MONITOR__SERVICE__L2PING_SERVICE_HPP_ From ee73bcfc438097fe5a0b04cd8ceb8c9ff022c314 Mon Sep 17 00:00:00 2001 From: ito-san Date: Fri, 6 May 2022 15:15:37 +0900 Subject: [PATCH 03/17] Fixed a typo --- system/bluetooth_monitor/service/main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/system/bluetooth_monitor/service/main.cpp b/system/bluetooth_monitor/service/main.cpp index 2c459237510ee..a5fa89c7dce05 100644 --- a/system/bluetooth_monitor/service/main.cpp +++ b/system/bluetooth_monitor/service/main.cpp @@ -32,7 +32,7 @@ void usage() } /** - * @brief Maln loop + * @brief Main loop * @param [in] argc Number of arguments * @param [in] argv Command line arguments * @return 0 on success, 1 on error From 5a82aa118485bcf8b25d2c8b2a798f3a485c992f Mon Sep 17 00:00:00 2001 From: ito-san Date: Fri, 6 May 2022 15:28:47 +0900 Subject: [PATCH 04/17] Add a dependency --- system/bluetooth_monitor/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/system/bluetooth_monitor/package.xml b/system/bluetooth_monitor/package.xml index e497b02c1d62c..6d630a4ddade1 100644 --- a/system/bluetooth_monitor/package.xml +++ b/system/bluetooth_monitor/package.xml @@ -13,6 +13,7 @@ diagnostic_msgs diagnostic_updater fmt + libbluetooth-dev rclcpp ament_lint_auto From aac01cfd343945321863dea2c6d1e9650ce19365 Mon Sep 17 00:00:00 2001 From: ito-san Date: Fri, 6 May 2022 16:14:51 +0900 Subject: [PATCH 05/17] Fixed pre-commit errors --- .../service/l2ping_interface.hpp | 9 +++++---- system/bluetooth_monitor/service/l2ping.cpp | 18 +++++++++++------- .../service/l2ping_service.cpp | 11 +++++++---- .../src/bluetooth_monitor.cpp | 6 +++++- 4 files changed, 28 insertions(+), 16 deletions(-) diff --git a/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping_interface.hpp b/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping_interface.hpp index b93d08ebc5e3f..e4429aa4ea7f1 100644 --- a/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping_interface.hpp +++ b/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping_interface.hpp @@ -48,7 +48,7 @@ struct L2pingConfig * @note NOLINT syntax is needed since this is an interface to serialization and * used inside boost serialization. */ - template + template void serialize(archive & ar, const unsigned /*version*/) // NOLINT(runtime/references) { ar & delay; @@ -73,7 +73,7 @@ struct L2pingServiceConfig * @note NOLINT syntax is needed since this is an interface to serialization and * used inside boost serialization. */ - template + template void serialize(archive & ar, const unsigned /*version*/) // NOLINT(runtime/references) { ar & l2ping; @@ -84,7 +84,8 @@ struct L2pingServiceConfig /** * @brief Status code of a device */ -enum class StatusCode { +enum class StatusCode +{ OK = 0, RTT_WARNING = 1, VERIFY_ERROR = 2, @@ -116,7 +117,7 @@ struct L2pingStatus * @note NOLINT syntax is needed since this is an interface to serialization and * used inside boost serialization. */ - template + template void serialize(archive & ar, const unsigned /*version*/) // NOLINT(runtime/references) { ar & status_code; diff --git a/system/bluetooth_monitor/service/l2ping.cpp b/system/bluetooth_monitor/service/l2ping.cpp index b05270cc90227..91b96bfbe50ed 100644 --- a/system/bluetooth_monitor/service/l2ping.cpp +++ b/system/bluetooth_monitor/service/l2ping.cpp @@ -34,6 +34,8 @@ #include #include +#include + static constexpr int IDENTIFIER = 200; //!< @brief Identifier to match responses with requests L2ping::L2ping(const bdaddr_t & address, const L2pingConfig & config) @@ -48,7 +50,9 @@ L2ping::L2ping(const bdaddr_t & address, const L2pingConfig & config) // Initialize buffer memset(send_buffer_, 0, sizeof(send_buffer_)); - for (int i = 0; i < SIZE; ++i) send_buffer_[L2CAP_CMD_HDR_SIZE + i] = (i % 40) + 'A'; + for (int i = 0; i < SIZE; ++i) { + send_buffer_[L2CAP_CMD_HDR_SIZE + i] = (i % 40) + 'A'; + } } bool L2ping::getDeviceInformation(uint16_t handle) @@ -91,7 +95,7 @@ bool L2ping::getDeviceInformation(uint16_t handle) "Device Name: %s LMP Version: %s (0x%x) LMP Subversion: 0x%x Manufacturer: %s (%d)\n", name, version_str ? version_str : "n/a", version.lmp_ver, version.lmp_subver, bt_compidtostr(version.manufacturer), version.manufacturer); - if (version_str) bt_free(version_str); + if (version_str) {bt_free(version_str);} } hci_close_dev(dev_sock); @@ -159,7 +163,7 @@ bool L2ping::initialize() return true; } -void L2ping::shutdown() { close(socket_); } +void L2ping::shutdown() {close(socket_);} bool L2ping::ping() { @@ -250,11 +254,11 @@ bool L2ping::ping() } } - if (!failed) setStatusCode(StatusCode::OK); + if (!failed) {setStatusCode(StatusCode::OK);} // Try next send sleep(config_.delay); - if (++id > 254) id = IDENTIFIER; + if (++id > 254) {id = IDENTIFIER}; } } @@ -349,6 +353,6 @@ void L2ping::setStatusCode(StatusCode code) status_.error_code = 0; } -L2pingStatus L2ping::getStatus() const { return status_; } +L2pingStatus L2ping::getStatus() const {return status_;} -const std::string & L2ping::getAddress() const { return status_.address; } +const std::string & L2ping::getAddress() const {return status_.address;} diff --git a/system/bluetooth_monitor/service/l2ping_service.cpp b/system/bluetooth_monitor/service/l2ping_service.cpp index 9eaa2fa13eb8e..9ccf873065611 100644 --- a/system/bluetooth_monitor/service/l2ping_service.cpp +++ b/system/bluetooth_monitor/service/l2ping_service.cpp @@ -36,7 +36,9 @@ #include #include +#include #include +#include static const bdaddr_t ANY_ADDRESS = {{0, 0, 0, 0, 0, 0}}; @@ -83,7 +85,7 @@ bool L2pingService::initialize() return true; } -void L2pingService::shutdown() { close(socket_); } +void L2pingService::shutdown() {close(socket_);} void L2pingService::run() { @@ -157,7 +159,7 @@ void L2pingService::setFunctionError(const std::string & function_name, int erro status_list_.clear(); // Set error data - L2pingStatus status; + L2pingStatus status{}; status.status_code = StatusCode::FUNCTION_ERROR; status.function_name = function_name; status.error_code = error_code; @@ -206,7 +208,7 @@ bool L2pingService::buildDeviceList() // Loop for HCI devices for (int i = 0; i < hci_device_list.dev_num; ++i, ++hci_device) { // Build device list to ping from connected devices - if (!buildDeviceListFromConnectedDevices(sock, hci_device->dev_id)) return false; + if (!buildDeviceListFromConnectedDevices(sock, hci_device->dev_id)) {return false}; } close(sock); @@ -254,7 +256,8 @@ bool L2pingService::buildDeviceListFromConnectedDevices(int sock, uint16_t devic // Skip if device not found and wild card not specified if ( std::count(config_.addresses.begin(), config_.addresses.end(), address_str) == 0 && - std::count(config_.addresses.begin(), config_.addresses.end(), "*") == 0) { + std::count(config_.addresses.begin(), config_.addresses.end(), "*") == 0) + { continue; } diff --git a/system/bluetooth_monitor/src/bluetooth_monitor.cpp b/system/bluetooth_monitor/src/bluetooth_monitor.cpp index 764af952ff013..7e20fe23e55d8 100644 --- a/system/bluetooth_monitor/src/bluetooth_monitor.cpp +++ b/system/bluetooth_monitor/src/bluetooth_monitor.cpp @@ -21,6 +21,10 @@ #include #include +#include +#include +#include + #define FMT_HEADER_ONLY #include @@ -128,7 +132,7 @@ bool BluetoothMonitor::receiveData(diagnostic_updater::DiagnosticStatusWrapper & return true; } -void BluetoothMonitor::closeConnection() { close(socket_); } +void BluetoothMonitor::closeConnection() {close(socket_);} void BluetoothMonitor::setErrorLevel(diagnostic_updater::DiagnosticStatusWrapper & stat) { // No device From 0ccd81091a18cc610223245a63aaf607b53e7fa6 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 6 May 2022 07:16:12 +0000 Subject: [PATCH 06/17] ci(pre-commit): autofix --- .../service/l2ping_interface.hpp | 9 ++++----- system/bluetooth_monitor/service/l2ping.cpp | 18 ++++++++++++------ .../service/l2ping_service.cpp | 9 +++++---- .../src/bluetooth_monitor.cpp | 2 +- 4 files changed, 22 insertions(+), 16 deletions(-) diff --git a/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping_interface.hpp b/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping_interface.hpp index e4429aa4ea7f1..b93d08ebc5e3f 100644 --- a/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping_interface.hpp +++ b/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping_interface.hpp @@ -48,7 +48,7 @@ struct L2pingConfig * @note NOLINT syntax is needed since this is an interface to serialization and * used inside boost serialization. */ - template + template void serialize(archive & ar, const unsigned /*version*/) // NOLINT(runtime/references) { ar & delay; @@ -73,7 +73,7 @@ struct L2pingServiceConfig * @note NOLINT syntax is needed since this is an interface to serialization and * used inside boost serialization. */ - template + template void serialize(archive & ar, const unsigned /*version*/) // NOLINT(runtime/references) { ar & l2ping; @@ -84,8 +84,7 @@ struct L2pingServiceConfig /** * @brief Status code of a device */ -enum class StatusCode -{ +enum class StatusCode { OK = 0, RTT_WARNING = 1, VERIFY_ERROR = 2, @@ -117,7 +116,7 @@ struct L2pingStatus * @note NOLINT syntax is needed since this is an interface to serialization and * used inside boost serialization. */ - template + template void serialize(archive & ar, const unsigned /*version*/) // NOLINT(runtime/references) { ar & status_code; diff --git a/system/bluetooth_monitor/service/l2ping.cpp b/system/bluetooth_monitor/service/l2ping.cpp index 91b96bfbe50ed..65d8fe956c367 100644 --- a/system/bluetooth_monitor/service/l2ping.cpp +++ b/system/bluetooth_monitor/service/l2ping.cpp @@ -95,7 +95,9 @@ bool L2ping::getDeviceInformation(uint16_t handle) "Device Name: %s LMP Version: %s (0x%x) LMP Subversion: 0x%x Manufacturer: %s (%d)\n", name, version_str ? version_str : "n/a", version.lmp_ver, version.lmp_subver, bt_compidtostr(version.manufacturer), version.manufacturer); - if (version_str) {bt_free(version_str);} + if (version_str) { + bt_free(version_str); + } } hci_close_dev(dev_sock); @@ -163,7 +165,7 @@ bool L2ping::initialize() return true; } -void L2ping::shutdown() {close(socket_);} +void L2ping::shutdown() { close(socket_); } bool L2ping::ping() { @@ -254,11 +256,15 @@ bool L2ping::ping() } } - if (!failed) {setStatusCode(StatusCode::OK);} + if (!failed) { + setStatusCode(StatusCode::OK); + } // Try next send sleep(config_.delay); - if (++id > 254) {id = IDENTIFIER}; + if (++id > 254) { + id = IDENTIFIER + }; } } @@ -353,6 +359,6 @@ void L2ping::setStatusCode(StatusCode code) status_.error_code = 0; } -L2pingStatus L2ping::getStatus() const {return status_;} +L2pingStatus L2ping::getStatus() const { return status_; } -const std::string & L2ping::getAddress() const {return status_.address;} +const std::string & L2ping::getAddress() const { return status_.address; } diff --git a/system/bluetooth_monitor/service/l2ping_service.cpp b/system/bluetooth_monitor/service/l2ping_service.cpp index 9ccf873065611..854be4f45db1e 100644 --- a/system/bluetooth_monitor/service/l2ping_service.cpp +++ b/system/bluetooth_monitor/service/l2ping_service.cpp @@ -85,7 +85,7 @@ bool L2pingService::initialize() return true; } -void L2pingService::shutdown() {close(socket_);} +void L2pingService::shutdown() { close(socket_); } void L2pingService::run() { @@ -208,7 +208,9 @@ bool L2pingService::buildDeviceList() // Loop for HCI devices for (int i = 0; i < hci_device_list.dev_num; ++i, ++hci_device) { // Build device list to ping from connected devices - if (!buildDeviceListFromConnectedDevices(sock, hci_device->dev_id)) {return false}; + if (!buildDeviceListFromConnectedDevices(sock, hci_device->dev_id)) { + return false + }; } close(sock); @@ -256,8 +258,7 @@ bool L2pingService::buildDeviceListFromConnectedDevices(int sock, uint16_t devic // Skip if device not found and wild card not specified if ( std::count(config_.addresses.begin(), config_.addresses.end(), address_str) == 0 && - std::count(config_.addresses.begin(), config_.addresses.end(), "*") == 0) - { + std::count(config_.addresses.begin(), config_.addresses.end(), "*") == 0) { continue; } diff --git a/system/bluetooth_monitor/src/bluetooth_monitor.cpp b/system/bluetooth_monitor/src/bluetooth_monitor.cpp index 7e20fe23e55d8..b8e60f51e1263 100644 --- a/system/bluetooth_monitor/src/bluetooth_monitor.cpp +++ b/system/bluetooth_monitor/src/bluetooth_monitor.cpp @@ -132,7 +132,7 @@ bool BluetoothMonitor::receiveData(diagnostic_updater::DiagnosticStatusWrapper & return true; } -void BluetoothMonitor::closeConnection() {close(socket_);} +void BluetoothMonitor::closeConnection() { close(socket_); } void BluetoothMonitor::setErrorLevel(diagnostic_updater::DiagnosticStatusWrapper & stat) { // No device From 49e77c566802e10a6b5068300b80c84e1cf12fb3 Mon Sep 17 00:00:00 2001 From: ito-san Date: Fri, 6 May 2022 16:22:31 +0900 Subject: [PATCH 07/17] Fixed pre-commit errors --- system/bluetooth_monitor/service/l2ping.cpp | 4 ++-- system/bluetooth_monitor/service/l2ping_service.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/system/bluetooth_monitor/service/l2ping.cpp b/system/bluetooth_monitor/service/l2ping.cpp index 65d8fe956c367..7fedb02bba2bd 100644 --- a/system/bluetooth_monitor/service/l2ping.cpp +++ b/system/bluetooth_monitor/service/l2ping.cpp @@ -263,8 +263,8 @@ bool L2ping::ping() // Try next send sleep(config_.delay); if (++id > 254) { - id = IDENTIFIER - }; + id = IDENTIFIER; + } } } diff --git a/system/bluetooth_monitor/service/l2ping_service.cpp b/system/bluetooth_monitor/service/l2ping_service.cpp index 854be4f45db1e..a4ba04ddec035 100644 --- a/system/bluetooth_monitor/service/l2ping_service.cpp +++ b/system/bluetooth_monitor/service/l2ping_service.cpp @@ -209,8 +209,8 @@ bool L2pingService::buildDeviceList() for (int i = 0; i < hci_device_list.dev_num; ++i, ++hci_device) { // Build device list to ping from connected devices if (!buildDeviceListFromConnectedDevices(sock, hci_device->dev_id)) { - return false - }; + return false; + } } close(sock); From 2b87cc46368ee6d8ef92643efd7c828357bbf4b0 Mon Sep 17 00:00:00 2001 From: ito-san Date: Fri, 6 May 2022 16:49:37 +0900 Subject: [PATCH 08/17] Fixed uncrustify errors --- .../bluetooth_monitor/service/l2ping_interface.hpp | 9 +++++---- system/bluetooth_monitor/service/l2ping.cpp | 6 +++--- system/bluetooth_monitor/src/bluetooth_monitor.cpp | 2 +- 3 files changed, 9 insertions(+), 8 deletions(-) diff --git a/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping_interface.hpp b/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping_interface.hpp index b93d08ebc5e3f..e4429aa4ea7f1 100644 --- a/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping_interface.hpp +++ b/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping_interface.hpp @@ -48,7 +48,7 @@ struct L2pingConfig * @note NOLINT syntax is needed since this is an interface to serialization and * used inside boost serialization. */ - template + template void serialize(archive & ar, const unsigned /*version*/) // NOLINT(runtime/references) { ar & delay; @@ -73,7 +73,7 @@ struct L2pingServiceConfig * @note NOLINT syntax is needed since this is an interface to serialization and * used inside boost serialization. */ - template + template void serialize(archive & ar, const unsigned /*version*/) // NOLINT(runtime/references) { ar & l2ping; @@ -84,7 +84,8 @@ struct L2pingServiceConfig /** * @brief Status code of a device */ -enum class StatusCode { +enum class StatusCode +{ OK = 0, RTT_WARNING = 1, VERIFY_ERROR = 2, @@ -116,7 +117,7 @@ struct L2pingStatus * @note NOLINT syntax is needed since this is an interface to serialization and * used inside boost serialization. */ - template + template void serialize(archive & ar, const unsigned /*version*/) // NOLINT(runtime/references) { ar & status_code; diff --git a/system/bluetooth_monitor/service/l2ping.cpp b/system/bluetooth_monitor/service/l2ping.cpp index 7fedb02bba2bd..3fd983b04a66c 100644 --- a/system/bluetooth_monitor/service/l2ping.cpp +++ b/system/bluetooth_monitor/service/l2ping.cpp @@ -165,7 +165,7 @@ bool L2ping::initialize() return true; } -void L2ping::shutdown() { close(socket_); } +void L2ping::shutdown() {close(socket_);} bool L2ping::ping() { @@ -359,6 +359,6 @@ void L2ping::setStatusCode(StatusCode code) status_.error_code = 0; } -L2pingStatus L2ping::getStatus() const { return status_; } +L2pingStatus L2ping::getStatus() const {return status_;} -const std::string & L2ping::getAddress() const { return status_.address; } +const std::string & L2ping::getAddress() const {return status_.address;} diff --git a/system/bluetooth_monitor/src/bluetooth_monitor.cpp b/system/bluetooth_monitor/src/bluetooth_monitor.cpp index b8e60f51e1263..7e20fe23e55d8 100644 --- a/system/bluetooth_monitor/src/bluetooth_monitor.cpp +++ b/system/bluetooth_monitor/src/bluetooth_monitor.cpp @@ -132,7 +132,7 @@ bool BluetoothMonitor::receiveData(diagnostic_updater::DiagnosticStatusWrapper & return true; } -void BluetoothMonitor::closeConnection() { close(socket_); } +void BluetoothMonitor::closeConnection() {close(socket_);} void BluetoothMonitor::setErrorLevel(diagnostic_updater::DiagnosticStatusWrapper & stat) { // No device From 742d1744a446f702bda126ef0bfbddba3186b255 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 6 May 2022 07:50:56 +0000 Subject: [PATCH 09/17] ci(pre-commit): autofix --- .../bluetooth_monitor/service/l2ping_interface.hpp | 9 ++++----- system/bluetooth_monitor/service/l2ping.cpp | 6 +++--- system/bluetooth_monitor/src/bluetooth_monitor.cpp | 2 +- 3 files changed, 8 insertions(+), 9 deletions(-) diff --git a/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping_interface.hpp b/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping_interface.hpp index e4429aa4ea7f1..b93d08ebc5e3f 100644 --- a/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping_interface.hpp +++ b/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping_interface.hpp @@ -48,7 +48,7 @@ struct L2pingConfig * @note NOLINT syntax is needed since this is an interface to serialization and * used inside boost serialization. */ - template + template void serialize(archive & ar, const unsigned /*version*/) // NOLINT(runtime/references) { ar & delay; @@ -73,7 +73,7 @@ struct L2pingServiceConfig * @note NOLINT syntax is needed since this is an interface to serialization and * used inside boost serialization. */ - template + template void serialize(archive & ar, const unsigned /*version*/) // NOLINT(runtime/references) { ar & l2ping; @@ -84,8 +84,7 @@ struct L2pingServiceConfig /** * @brief Status code of a device */ -enum class StatusCode -{ +enum class StatusCode { OK = 0, RTT_WARNING = 1, VERIFY_ERROR = 2, @@ -117,7 +116,7 @@ struct L2pingStatus * @note NOLINT syntax is needed since this is an interface to serialization and * used inside boost serialization. */ - template + template void serialize(archive & ar, const unsigned /*version*/) // NOLINT(runtime/references) { ar & status_code; diff --git a/system/bluetooth_monitor/service/l2ping.cpp b/system/bluetooth_monitor/service/l2ping.cpp index 3fd983b04a66c..7fedb02bba2bd 100644 --- a/system/bluetooth_monitor/service/l2ping.cpp +++ b/system/bluetooth_monitor/service/l2ping.cpp @@ -165,7 +165,7 @@ bool L2ping::initialize() return true; } -void L2ping::shutdown() {close(socket_);} +void L2ping::shutdown() { close(socket_); } bool L2ping::ping() { @@ -359,6 +359,6 @@ void L2ping::setStatusCode(StatusCode code) status_.error_code = 0; } -L2pingStatus L2ping::getStatus() const {return status_;} +L2pingStatus L2ping::getStatus() const { return status_; } -const std::string & L2ping::getAddress() const {return status_.address;} +const std::string & L2ping::getAddress() const { return status_.address; } diff --git a/system/bluetooth_monitor/src/bluetooth_monitor.cpp b/system/bluetooth_monitor/src/bluetooth_monitor.cpp index 7e20fe23e55d8..b8e60f51e1263 100644 --- a/system/bluetooth_monitor/src/bluetooth_monitor.cpp +++ b/system/bluetooth_monitor/src/bluetooth_monitor.cpp @@ -132,7 +132,7 @@ bool BluetoothMonitor::receiveData(diagnostic_updater::DiagnosticStatusWrapper & return true; } -void BluetoothMonitor::closeConnection() {close(socket_);} +void BluetoothMonitor::closeConnection() { close(socket_); } void BluetoothMonitor::setErrorLevel(diagnostic_updater::DiagnosticStatusWrapper & stat) { // No device From 98879925cfe727d78fd7be3ea10be0a6eddfb813 Mon Sep 17 00:00:00 2001 From: ito-san Date: Fri, 6 May 2022 17:10:50 +0900 Subject: [PATCH 10/17] use autoware_cmake --- system/bluetooth_monitor/CMakeLists.txt | 14 ++------------ system/bluetooth_monitor/package.xml | 4 +++- 2 files changed, 5 insertions(+), 13 deletions(-) diff --git a/system/bluetooth_monitor/CMakeLists.txt b/system/bluetooth_monitor/CMakeLists.txt index 6abf843eff9fe..ff9dc49547886 100644 --- a/system/bluetooth_monitor/CMakeLists.txt +++ b/system/bluetooth_monitor/CMakeLists.txt @@ -1,19 +1,9 @@ cmake_minimum_required(VERSION 3.5) project(bluetooth_monitor) -### Compile options -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) - set(CMAKE_CXX_STANDARD_REQUIRED ON) - set(CMAKE_CXX_EXTENSIONS OFF) -endif() -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - ### Dependencies -find_package(ament_cmake_auto REQUIRED) -ament_auto_find_build_dependencies() +find_package(autoware_cmake REQUIRED) +autoware_package() ### Target executable ament_auto_add_executable(bluetooth_monitor diff --git a/system/bluetooth_monitor/package.xml b/system/bluetooth_monitor/package.xml index 6d630a4ddade1..34ce95105c105 100644 --- a/system/bluetooth_monitor/package.xml +++ b/system/bluetooth_monitor/package.xml @@ -9,6 +9,8 @@ ament_cmake_auto + autoware_cmake + bluez diagnostic_msgs diagnostic_updater @@ -17,7 +19,7 @@ rclcpp ament_lint_auto - ament_lint_common + autoware_lint_common ament_cmake From 32bf4c90df9ef9289ea53e045c56db4ea97c924c Mon Sep 17 00:00:00 2001 From: ito-san Date: Fri, 6 May 2022 17:46:58 +0900 Subject: [PATCH 11/17] Fixed license, Fixed CMakeLists.txt, and Use register_node_macro --- system/bluetooth_monitor/CMakeLists.txt | 15 +++++----- .../bluetooth_monitor/bluetooth_monitor.hpp | 5 ++-- .../bluetooth_monitor/service/l2ping.hpp | 2 +- .../service/l2ping_interface.hpp | 2 +- .../service/l2ping_service.hpp | 2 +- system/bluetooth_monitor/package.xml | 1 + system/bluetooth_monitor/service/l2ping.cpp | 2 +- .../service/l2ping_service.cpp | 2 +- system/bluetooth_monitor/service/main.cpp | 2 +- .../src/bluetooth_monitor.cpp | 7 +++-- .../src/bluetooth_monitor_node.cpp | 29 ------------------- 11 files changed, 22 insertions(+), 47 deletions(-) delete mode 100644 system/bluetooth_monitor/src/bluetooth_monitor_node.cpp diff --git a/system/bluetooth_monitor/CMakeLists.txt b/system/bluetooth_monitor/CMakeLists.txt index ff9dc49547886..ccb27848e8857 100644 --- a/system/bluetooth_monitor/CMakeLists.txt +++ b/system/bluetooth_monitor/CMakeLists.txt @@ -5,12 +5,11 @@ project(bluetooth_monitor) find_package(autoware_cmake REQUIRED) autoware_package() -### Target executable -ament_auto_add_executable(bluetooth_monitor - src/bluetooth_monitor_node.cpp +ament_auto_add_library(bluetooth_monitor_lib SHARED src/bluetooth_monitor.cpp ) +### Target executable ament_auto_add_executable(l2ping_service service/main.cpp service/l2ping_service.cpp @@ -22,13 +21,13 @@ find_package(Boost REQUIRED COMPONENTS ) ## Specify libraries to link a library or executable target against -target_link_libraries(bluetooth_monitor bluetooth ${Boost_LIBRARIES}) +target_link_libraries(bluetooth_monitor_lib bluetooth ${Boost_LIBRARIES}) target_link_libraries(l2ping_service bluetooth ${Boost_LIBRARIES}) -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() +rclcpp_components_register_node(bluetooth_monitor_lib + PLUGIN "BluetoothMonitor" + EXECUTABLE bluetooth_monitor +) ament_auto_package(INSTALL_TO_SHARE config diff --git a/system/bluetooth_monitor/include/bluetooth_monitor/bluetooth_monitor.hpp b/system/bluetooth_monitor/include/bluetooth_monitor/bluetooth_monitor.hpp index 9f464b70d120c..de855d7c3aa34 100644 --- a/system/bluetooth_monitor/include/bluetooth_monitor/bluetooth_monitor.hpp +++ b/system/bluetooth_monitor/include/bluetooth_monitor/bluetooth_monitor.hpp @@ -1,4 +1,4 @@ -// Copyright 2022 Autoware Foundation +// Copyright 2022 The Autoware Contributors // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -29,8 +29,9 @@ class BluetoothMonitor : public rclcpp::Node public: /** * @brief Constructor + * @param [in] options Options associated with this node */ - BluetoothMonitor(); + explicit BluetoothMonitor(const rclcpp::NodeOptions & options); protected: /** diff --git a/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping.hpp b/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping.hpp index c7fb87a08f6ec..1458eeac89f82 100644 --- a/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping.hpp +++ b/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping.hpp @@ -1,4 +1,4 @@ -// Copyright 2022 Autoware Foundation +// Copyright 2022 The Autoware Contributors // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping_interface.hpp b/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping_interface.hpp index b93d08ebc5e3f..e5953dfc3dc2a 100644 --- a/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping_interface.hpp +++ b/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping_interface.hpp @@ -1,4 +1,4 @@ -// Copyright 2022 Autoware Foundation +// Copyright 2022 The Autoware Contributors // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping_service.hpp b/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping_service.hpp index ce30460dcfa31..4ba48afed410d 100644 --- a/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping_service.hpp +++ b/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping_service.hpp @@ -1,4 +1,4 @@ -// Copyright 2022 Autoware Foundation +// Copyright 2022 The Autoware Contributors // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/system/bluetooth_monitor/package.xml b/system/bluetooth_monitor/package.xml index 34ce95105c105..12957b4c90d58 100644 --- a/system/bluetooth_monitor/package.xml +++ b/system/bluetooth_monitor/package.xml @@ -17,6 +17,7 @@ fmt libbluetooth-dev rclcpp + rclcpp_components ament_lint_auto autoware_lint_common diff --git a/system/bluetooth_monitor/service/l2ping.cpp b/system/bluetooth_monitor/service/l2ping.cpp index 7fedb02bba2bd..36d7c9a446f6b 100644 --- a/system/bluetooth_monitor/service/l2ping.cpp +++ b/system/bluetooth_monitor/service/l2ping.cpp @@ -1,4 +1,4 @@ -// Copyright 2022 Autoware Foundation +// Copyright 2022 The Autoware Contributors // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/system/bluetooth_monitor/service/l2ping_service.cpp b/system/bluetooth_monitor/service/l2ping_service.cpp index a4ba04ddec035..3a930a2a5f65f 100644 --- a/system/bluetooth_monitor/service/l2ping_service.cpp +++ b/system/bluetooth_monitor/service/l2ping_service.cpp @@ -1,4 +1,4 @@ -// Copyright 2022 Autoware Foundation +// Copyright 2022 The Autoware Contributors // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/system/bluetooth_monitor/service/main.cpp b/system/bluetooth_monitor/service/main.cpp index a5fa89c7dce05..a3ccb971a5eb7 100644 --- a/system/bluetooth_monitor/service/main.cpp +++ b/system/bluetooth_monitor/service/main.cpp @@ -1,4 +1,4 @@ -// Copyright 2022 Autoware Foundation +// Copyright 2022 The Autoware Contributors // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/system/bluetooth_monitor/src/bluetooth_monitor.cpp b/system/bluetooth_monitor/src/bluetooth_monitor.cpp index b8e60f51e1263..6931591e2b476 100644 --- a/system/bluetooth_monitor/src/bluetooth_monitor.cpp +++ b/system/bluetooth_monitor/src/bluetooth_monitor.cpp @@ -28,8 +28,8 @@ #define FMT_HEADER_ONLY #include -BluetoothMonitor::BluetoothMonitor() -: Node("bluetooth_monitor"), +BluetoothMonitor::BluetoothMonitor(const rclcpp::NodeOptions & options) +: Node("bluetooth_monitor", options), updater_(this), socket_(-1), port_(declare_parameter("port", DEFAULT_PORT)) @@ -199,3 +199,6 @@ void BluetoothMonitor::checkConnection(diagnostic_updater::DiagnosticStatusWrapp // Set error level of diagnostic status setErrorLevel(stat); } + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(BluetoothMonitor) diff --git a/system/bluetooth_monitor/src/bluetooth_monitor_node.cpp b/system/bluetooth_monitor/src/bluetooth_monitor_node.cpp deleted file mode 100644 index f4134c09277aa..0000000000000 --- a/system/bluetooth_monitor/src/bluetooth_monitor_node.cpp +++ /dev/null @@ -1,29 +0,0 @@ -// Copyright 2022 Autoware Foundation -// -// 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 "bluetooth_monitor/bluetooth_monitor.hpp" - -#include - -#include - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - auto node = std::make_shared(); - rclcpp::spin(node); - rclcpp::shutdown(); - - return 0; -} From cfe04cd81dd265d0193df75f837710d0aab0ddf0 Mon Sep 17 00:00:00 2001 From: ito-san Date: Fri, 6 May 2022 17:48:33 +0900 Subject: [PATCH 12/17] Fixed license --- system/bluetooth_monitor/src/bluetooth_monitor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/system/bluetooth_monitor/src/bluetooth_monitor.cpp b/system/bluetooth_monitor/src/bluetooth_monitor.cpp index 6931591e2b476..38acdca8b632c 100644 --- a/system/bluetooth_monitor/src/bluetooth_monitor.cpp +++ b/system/bluetooth_monitor/src/bluetooth_monitor.cpp @@ -1,4 +1,4 @@ -// Copyright 2022 Autoware Foundation +// Copyright 2022 The Autoware Contributors // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. From ff8ac40bf56ae3b445693f30960930f5c00a39a5 Mon Sep 17 00:00:00 2001 From: ito-san Date: Fri, 6 May 2022 18:35:17 +0900 Subject: [PATCH 13/17] Fixed link title --- system/bluetooth_monitor/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/system/bluetooth_monitor/README.md b/system/bluetooth_monitor/README.md index 5666258f8e9d8..a8b25394e93b6 100644 --- a/system/bluetooth_monitor/README.md +++ b/system/bluetooth_monitor/README.md @@ -12,7 +12,7 @@ L2ping is only allowed for root by default, so this package provides the followi - Provide a small program named `l2ping_service` which performs L2ping and provides wireless device information to `bluetooth_monitor` by using socket programming. - `bluetooth_monitor` is able to know wireless device information and L2ping status as an unprivileged user since those information are sent by socket communication. -![a](docs/block_diagram.drawio.svg) +![block_diagram](docs/block_diagram.drawio.svg) ## Output From 80ad518c57f77625307c72385bc66ce1761a88c1 Mon Sep 17 00:00:00 2001 From: ito-san Date: Tue, 24 May 2022 10:27:45 +0900 Subject: [PATCH 14/17] changed the way to run l2ping Signed-off-by: ito-san --- system/bluetooth_monitor/README.md | 6 - .../config/bluetooth_monitor.param.yaml | 2 - .../bluetooth_monitor/bluetooth_monitor.hpp | 4 - .../bluetooth_monitor/service/l2ping.hpp | 63 +-- .../service/l2ping_interface.hpp | 20 +- .../service/l2ping_service.hpp | 4 +- system/bluetooth_monitor/service/l2ping.cpp | 367 ++++-------------- .../service/l2ping_service.cpp | 167 +++----- .../src/bluetooth_monitor.cpp | 9 +- 9 files changed, 157 insertions(+), 485 deletions(-) diff --git a/system/bluetooth_monitor/README.md b/system/bluetooth_monitor/README.md index a8b25394e93b6..0e84b90238507 100644 --- a/system/bluetooth_monitor/README.md +++ b/system/bluetooth_monitor/README.md @@ -24,9 +24,7 @@ L2ping is only allowed for root by default, so this package provides the followi | ----- | -------------- | | OK | OK | | WARN | RTT warning | -| | Verify error | | ERROR | Lost | -| | Ping rejected | | | Function error | [values] @@ -38,8 +36,6 @@ L2ping is only allowed for root by default, so this package provides the followi | Device [0-9]: Manufacturer | MediaTek, Inc. | | Device [0-9]: Address | AA:BB:CC:DD:EE:FF | | Device [0-9]: RTT | 0.00ms | -| Device [0-9]: Sent packets | 1000 | -| Device [0-9]: Received packets | 1000 | - The following key will be added when `bluetooth_monitor` reports `Function error`.
ex.) The `connect` system call failed. @@ -53,9 +49,7 @@ L2ping is only allowed for root by default, so this package provides the followi | Name | Type | Default Value | Explanation | | ----------- | ------ | ------------- | --------------------------------------------------------- | | `port` | int | 7640 | Port number to connect to L2ping service. | -| `delay` | int | 1 | Wait seconds between sending each packet. | | `timeout` | int | 5 | Wait timeout seconds for the response. | -| `verify` | bool | false | Verify request and response payload. | | `rtt_warn` | float | 0.00 | RTT(Round-Trip Time) to generate warn. | | `addresses` | string | \* | List of bluetooth address of wireless devices to monitor. | diff --git a/system/bluetooth_monitor/config/bluetooth_monitor.param.yaml b/system/bluetooth_monitor/config/bluetooth_monitor.param.yaml index 5a8d6ce575110..fd4c9eaa40d21 100644 --- a/system/bluetooth_monitor/config/bluetooth_monitor.param.yaml +++ b/system/bluetooth_monitor/config/bluetooth_monitor.param.yaml @@ -2,8 +2,6 @@ /**: ros__parameters: port: 7640 - delay: 1 timeout: 5 - verify: false rtt_warn: 0.00 addresses: ["4C:B9:9B:6E:7F:9A"] diff --git a/system/bluetooth_monitor/include/bluetooth_monitor/bluetooth_monitor.hpp b/system/bluetooth_monitor/include/bluetooth_monitor/bluetooth_monitor.hpp index de855d7c3aa34..6ebca9b4189d5 100644 --- a/system/bluetooth_monitor/include/bluetooth_monitor/bluetooth_monitor.hpp +++ b/system/bluetooth_monitor/include/bluetooth_monitor/bluetooth_monitor.hpp @@ -99,17 +99,13 @@ class BluetoothMonitor : public rclcpp::Node const std::map status_string_list_ = { {StatusCode::OK, "OK"}, {StatusCode::RTT_WARNING, "RTT warning"}, - {StatusCode::VERIFY_ERROR, "Verify error"}, {StatusCode::LOST, "Lost"}, - {StatusCode::REJECTED, "Ping rejected"}, {StatusCode::FUNCTION_ERROR, FUNCTION_ERROR_STR}}; const std::map status_error_list_ = { {StatusCode::OK, DiagStatus::OK}, {StatusCode::RTT_WARNING, DiagStatus::WARN}, - {StatusCode::VERIFY_ERROR, DiagStatus::WARN}, {StatusCode::LOST, DiagStatus::ERROR}, - {StatusCode::REJECTED, DiagStatus::ERROR}, {StatusCode::FUNCTION_ERROR, DiagStatus::ERROR}}; }; diff --git a/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping.hpp b/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping.hpp index 1458eeac89f82..79940329336c4 100644 --- a/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping.hpp +++ b/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping.hpp @@ -17,19 +17,10 @@ #include "bluetooth_monitor/service/l2ping_interface.hpp" -#include -#include -#include - #include #include #include -inline float tv2fl(const timeval & tv) -{ - return static_cast(tv.tv_sec * 1000.0) + static_cast(tv.tv_sec / 1000.0); -} - class L2ping { public: @@ -38,20 +29,13 @@ class L2ping * @param [in] address Bluetooth address of remote device * @param [in] config Configuration of L2ping */ - L2ping(const bdaddr_t & address, const L2pingConfig & config); + L2ping(const std::string & address, const L2pingConfig & config); /** * @brief Start ping thread */ void run(); - /** - * @brief Get information from remote device - * @param [in] handle Handle to connection info - * @return true on success, false on error - */ - bool getDeviceInformation(uint16_t handle); - /** * @brief Get status * @return Status @@ -66,20 +50,15 @@ class L2ping protected: /** - * @brief Thread loop - */ - void thread(); - - /** - * @brief Connect to remote device + * @brief Get information from remote device * @return true on success, false on error */ - bool initialize(); + bool getDeviceInformation(); /** - * @brief Shutdown + * @brief Thread loop */ - void shutdown(); + void thread(); /** * @brief Ping to remote device @@ -88,31 +67,11 @@ class L2ping bool ping(); /** - * @brief Send echo command to remote device - * @param [in] id id - * @return true on success, false on error - */ - bool sendEchoCommand(int id); - - /** - * @brief Receive echo command from remote device - * @return true on success, false on error - */ - bool receiveEchoComand(); - - /** - * @brief Verify received command - * @param [in] length Command length - * @return true on success, false on error - */ - bool verifyCommand(uint16_t length); - - /** - * @brief Set function error data to inform ros2 node + * @brief Set error data to inform ros2 node * @param [in] function_name Function name which error occurred - * @param [in] error_code number which is set by system calls + * @param [in] error_message Error message to display */ - void setFunctionError(const std::string & function_name, int error_code); + void setFunctionError(const std::string & function_name, const std::string & error_message); /** * @brief Set status code @@ -120,15 +79,9 @@ class L2ping */ void setStatusCode(StatusCode code); - bdaddr_t address_; //!< @brief Bluetooth address of remote device L2pingConfig config_; //!< @brief Configuration of L2ping - int socket_; //!< @brief Socket to L2CAP protocol std::thread thread_; //!< @brief Thread to L2ping L2pingStatus status_; //!< @brief L2ping status - - static constexpr int SIZE = 44; //!< @brief The size of the data packets to be sent - unsigned char send_buffer_[L2CAP_CMD_HDR_SIZE + SIZE]; //!< @brief buffer to send - unsigned char receive_buffer_[L2CAP_CMD_HDR_SIZE + SIZE]; //!< @brief buffer to receive }; #endif // BLUETOOTH_MONITOR__SERVICE__L2PING_HPP_ diff --git a/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping_interface.hpp b/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping_interface.hpp index e5953dfc3dc2a..f04dccb94f429 100644 --- a/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping_interface.hpp +++ b/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping_interface.hpp @@ -19,8 +19,6 @@ #include #include -#include - #include #include @@ -36,9 +34,7 @@ static constexpr float RTT_NO_WARN = 0.0f; */ struct L2pingConfig { - int delay{DEFAULT_DELAY}; //!< @brief Wait seconds between sending each packet int timeout{DEFAULT_TIMEOUT}; //!< @brief Wait timeout seconds for the response - bool verify{DEFAULT_VERIFY}; //!< @brief Verify request and response payload float rtt_warn{RTT_NO_WARN}; //!< @brief RTT warning time /** @@ -51,9 +47,7 @@ struct L2pingConfig template void serialize(archive & ar, const unsigned /*version*/) // NOLINT(runtime/references) { - ar & delay; ar & timeout; - ar & verify; ar & rtt_warn; } }; @@ -87,10 +81,8 @@ struct L2pingServiceConfig enum class StatusCode { OK = 0, RTT_WARNING = 1, - VERIFY_ERROR = 2, - LOST = 3, - REJECTED = 4, - FUNCTION_ERROR = 5, + LOST = 2, + FUNCTION_ERROR = 3, }; /** @@ -100,14 +92,12 @@ struct L2pingStatus { StatusCode status_code; //!< @brief Status code of a device std::string function_name; //!< @brief Function name which error occurred - int error_code; //!< @brief Error number which is set by system calls + std::string error_message; //!< @brief Error message to display std::string name; //!< @brief Name of remote device std::string manufacturer; //!< @brief Manufacturer name of remote device std::string address; //!< @brief Bluetooth address float time_difference; //!< @brief Time difference between sent and received - int sent_packets; //!< @brief Number of sent packets to remote device - int received_packets; //!< @brief Number of received packets from remote device /** * @brief Load or save data members. @@ -121,13 +111,11 @@ struct L2pingStatus { ar & status_code; ar & function_name; - ar & error_code; + ar & error_message; ar & name; ar & manufacturer; ar & address; ar & time_difference; - ar & sent_packets; - ar & received_packets; } }; diff --git a/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping_service.hpp b/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping_service.hpp index 4ba48afed410d..fa325ca733196 100644 --- a/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping_service.hpp +++ b/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping_service.hpp @@ -53,9 +53,9 @@ class L2pingService /** * @brief Set error data to inform ros2 node * @param [in] function_name Function name which error occurred - * @param [in] error_code number which is set by system calls + * @param [in] error_message Error message to display */ - void setFunctionError(const std::string & function_name, int error_code); + void setFunctionError(const std::string & function_name, const std::string & error_message); /** * @brief Build device list to ping diff --git a/system/bluetooth_monitor/service/l2ping.cpp b/system/bluetooth_monitor/service/l2ping.cpp index 36d7c9a446f6b..03e806408c655 100644 --- a/system/bluetooth_monitor/service/l2ping.cpp +++ b/system/bluetooth_monitor/service/l2ping.cpp @@ -12,96 +12,54 @@ // See the License for the specific language governing permissions and // limitations under the License. // -// This code is derived from BlueZ with following copyrights. - -// SPDX-License-Identifier: GPL-2.0-or-later -/* - * - * BlueZ - Bluetooth protocol stack for Linux - * - * Copyright (C) 2000-2001 Qualcomm Incorporated - * Copyright (C) 2002-2003 Maxim Krasnyansky - * Copyright (C) 2002-2010 Marcel Holtmann - * - * - */ #include "bluetooth_monitor/service/l2ping.hpp" -#include -#include -#include -#include +#include + #include +#include #include -static constexpr int IDENTIFIER = 200; //!< @brief Identifier to match responses with requests - -L2ping::L2ping(const bdaddr_t & address, const L2pingConfig & config) -: config_(config), socket_(-1), status_{} -{ - bacpy(&address_, &address); +#define FMT_HEADER_ONLY +#include - // Prepare string value fot log output - char address_str[18]{}; - ba2str(&address_, address_str); - status_.address = address_str; +namespace bp = boost::process; - // Initialize buffer - memset(send_buffer_, 0, sizeof(send_buffer_)); - for (int i = 0; i < SIZE; ++i) { - send_buffer_[L2CAP_CMD_HDR_SIZE + i] = (i % 40) + 'A'; - } +L2ping::L2ping(const std::string & address, const L2pingConfig & config) +: config_(config), status_{} +{ + status_.address = address; } -bool L2ping::getDeviceInformation(uint16_t handle) +bool L2ping::getDeviceInformation() { - // Retrieve the resource number of the Bluetooth adapter - int dev_id = hci_get_route(&address_); - if (dev_id < 0) { - setFunctionError("hci_get_route", errno); - syslog( - LOG_ERR, "Device is not available. address = %s, %s", status_.address.c_str(), - strerror(errno)); + std::ostringstream os; + bp::ipstream is_out; + bp::ipstream is_err; + bp::child c( + fmt::format("hcitool info {}", status_.address), bp::std_out > is_out, bp::std_err > is_err); + c.wait(); + if (c.exit_code() != 0) { + is_err >> os.rdbuf(); + syslog(LOG_ERR, "hcitool info: %s\n", os.str().c_str()); return false; } - // Opens a Bluetooth socket with the specified resource number - int dev_sock = hci_open_dev(dev_id); - if (dev_sock < 0) { - setFunctionError("hci_open_dev", errno); - syslog( - LOG_ERR, "Failed to open HCI device. address = %s, %s", status_.address.c_str(), - strerror(errno)); - return false; - } - - // Get name of remote device - char name[HCI_MAX_NAME_LENGTH]{}; - - if (hci_read_remote_name(dev_sock, &address_, sizeof(name), name, 25000) == 0) { - status_.name = name; - } - - // Get version of remote device - hci_version version{}; + std::string line; + std::cmatch match; + const std::regex filter_device_name("\tDevice Name: (.*)"); + const std::regex filter_manufacturer("\tManufacturer: (.*)"); - if (hci_read_remote_version(dev_sock, handle, &version, 20000) == 0) { - char * version_str = lmp_vertostr(version.lmp_ver); - status_.manufacturer = bt_compidtostr(version.manufacturer); - syslog( - LOG_INFO, - "Device Name: %s LMP Version: %s (0x%x) LMP Subversion: 0x%x Manufacturer: %s (%d)\n", name, - version_str ? version_str : "n/a", version.lmp_ver, version.lmp_subver, - bt_compidtostr(version.manufacturer), version.manufacturer); - if (version_str) { - bt_free(version_str); + while (std::getline(is_out, line) && !line.empty()) { + if (std::regex_match(line.c_str(), match, filter_device_name)) { + status_.name = match[1].str(); + } else if (std::regex_match(line.c_str(), match, filter_manufacturer)) { + status_.manufacturer = match[1].str(); } } - hci_close_dev(dev_sock); - return true; } @@ -114,241 +72,84 @@ void L2ping::run() void L2ping::thread() { while (true) { - // Connect to remote device - if (initialize()) { - // Ping to remote device in loop - ping(); + // Get device information if not provided + if (status_.name.empty() || status_.manufacturer.empty()) { + getDeviceInformation(); } - - // Retry create socket - shutdown(); - sleep(DEFAULT_DELAY); + // Ping to remote device in loop + ping(); } - - shutdown(); } -bool L2ping::initialize() -{ - // Create socket to L2CAP protocol - socket_ = socket(PF_BLUETOOTH, SOCK_RAW, BTPROTO_L2CAP); - - if (socket_ < 0) { - setFunctionError("socket", errno); - syslog( - LOG_ERR, "Failed to create socket for '%s'. %s\n", status_.address.c_str(), strerror(errno)); - return false; - } - - // Register local address to connect to L2CAP - sockaddr_l2 local_address{}; - local_address.l2_family = AF_BLUETOOTH; - - if (bind(socket_, (struct sockaddr *)&local_address, sizeof(local_address)) < 0) { - setFunctionError("bind", errno); - syslog( - LOG_ERR, "Failed to bind socket for '%s'. %s\n", status_.address.c_str(), strerror(errno)); - return false; - } - - // Connect to remote device - sockaddr_l2 remote_address{}; - remote_address.l2_family = AF_BLUETOOTH; - remote_address.l2_bdaddr = address_; - - if (connect(socket_, (struct sockaddr *)&remote_address, sizeof(remote_address)) < 0) { - setFunctionError("connect", errno); - syslog(LOG_ERR, "Failed to connect for '%s'. %s\n", status_.address.c_str(), strerror(errno)); - return false; - } - - return true; -} - -void L2ping::shutdown() { close(socket_); } - bool L2ping::ping() { - // Get local address - sockaddr_l2 local_address{}; - socklen_t length = sizeof(local_address); - - if (getsockname(socket_, (struct sockaddr *)&local_address, &length) < 0) { - setFunctionError("getsockname", errno); - syslog(LOG_ERR, "Failed to get address. %s\n", strerror(errno)); - return false; - } - - char host_address[18]{}; - ba2str(&local_address.l2_bdaddr, host_address); - syslog( - LOG_INFO, "Ping: %s from %s (data size %d) ...\n", status_.address.c_str(), host_address, SIZE); - - int id = IDENTIFIER; - - while (true) { - timeval time_send{}; - timeval time_receive{}; - timeval time_diff{}; - - gettimeofday(&time_send, NULL); - - // Send echo command - if (!sendEchoCommand(id)) { - return false; - } - - // Send completed - ++status_.sent_packets; - - l2cap_cmd_hdr * receive_command = reinterpret_cast(receive_buffer_); - - // Wait for echo response - while (true) { - if (!receiveEchoComand()) { - return false; - } - - // Check received command - receive_command->len = btohs(receive_command->len); - - // Check for our id - if (receive_command->ident != id) { - // Retry to receive our id - continue; - } - // Check type - if (receive_command->code == L2CAP_ECHO_RSP) { - // Receive complete - break; - } - // Echo command rejected - if (receive_command->code == L2CAP_COMMAND_REJ) { - setStatusCode(StatusCode::REJECTED); - syslog( - LOG_ERR, "Peer doesn't support echo packets for '%s'. %s\n", status_.address.c_str(), - strerror(errno)); - return false; - } - } - - // Remote device responds correctly - ++status_.received_packets; - - gettimeofday(&time_receive, NULL); - timersub(&time_receive, &time_send, &time_diff); - status_.time_difference = tv2fl(time_diff); - - bool failed = false; - // If verify request and response payload - if (config_.verify) { - // Verify received command - if (!verifyCommand(receive_command->len)) { - failed = true; - setStatusCode(StatusCode::VERIFY_ERROR); - } - } - - // If check RTT - if (!failed && config_.rtt_warn > RTT_NO_WARN) { - if (status_.time_difference >= config_.rtt_warn) { - setStatusCode(StatusCode::RTT_WARNING); - } - } - - if (!failed) { + std::ostringstream os; + bp::ipstream is_out; + bp::ipstream is_err; + + bp::child c( + fmt::format("l2ping -c 1 -t {} {}", config_.timeout, status_.address), bp::std_out > is_out, + bp::std_err > is_err); + + /* + Output example of `l2ping` + + Ping: AA:BB:CC:DD:EE:FF from 01:23:45:67:89:01 (data size 44) ... + 44 bytes from AA:BB:CC:DD:EE:FF id 0 time 23.08ms + 1 sent, 1 received, 0% loss + */ + std::string line; + // Skip header + std::getline(is_out, line); + // Get 2nd line + std::getline(is_out, line); + + std::cmatch match; + + // Echo response received + const std::regex filter_time(".*time (\\d+\\.\\d+)ms"); + + if (std::regex_match(line.c_str(), match, filter_time)) { + status_.time_difference = std::stof(match[1].str()); + // Check RTT if configured + if (config_.rtt_warn != RTT_NO_WARN && status_.time_difference > config_.rtt_warn) { + setStatusCode(StatusCode::RTT_WARNING); + // Otherwise, ok + } else { setStatusCode(StatusCode::OK); } - - // Try next send - sleep(config_.delay); - if (++id > 254) { - id = IDENTIFIER; - } } -} - -bool L2ping::sendEchoCommand(int id) -{ - l2cap_cmd_hdr * send_command = reinterpret_cast(send_buffer_); - // Build command header - send_command->ident = id; - send_command->len = htobs(SIZE); - send_command->code = L2CAP_ECHO_REQ; + // No response + const std::regex filter_no_response("^no response from .*"); - // Send echo command - if (send(socket_, send_buffer_, sizeof(send_buffer_), 0) <= 0) { - // Remote device is down + if (std::regex_match(line.c_str(), match, filter_no_response)) { setStatusCode(StatusCode::LOST); - syslog(LOG_ERR, "Failed to send for '%s'. %s\n", status_.address.c_str(), strerror(errno)); - return false; } - return true; -} - -bool L2ping::receiveEchoComand() -{ - pollfd pf[1]{}; - int ret; - - pf[0].fd = socket_; - pf[0].events = POLLIN; - - if ((ret = poll(pf, 1, config_.timeout * 1000)) < 0) { - setFunctionError("poll", errno); - syslog(LOG_ERR, "Failed to poll for '%s'. %s\n", status_.address.c_str(), strerror(errno)); - return false; - } - - // Timeout - if (!ret) { - setStatusCode(StatusCode::LOST); - syslog(LOG_ERR, "Polling timeout for '%s'.\n", status_.address.c_str()); - return false; - } - - memset(receive_buffer_, 0, sizeof(receive_buffer_)); - - if ((ret = recv(socket_, receive_buffer_, sizeof(receive_buffer_), 0)) < 0) { - setStatusCode(StatusCode::LOST); - syslog(LOG_ERR, "Failed to receive for '%s'. %s\n", status_.address.c_str(), strerror(errno)); - return false; - } - - // Shutdown connection from device - if (!ret) { - setStatusCode(StatusCode::LOST); - syslog(LOG_ERR, "Disconnected from '%s'. %s\n", status_.address.c_str(), strerror(errno)); - return false; - } - - return true; -} - -bool L2ping::verifyCommand(uint16_t length) -{ - // Check payload length - if (length != SIZE) { - syslog(LOG_ERR, "Received %d bytes, expected %d for %s", length, SIZE, status_.address.c_str()); - return false; - } - // Check payload - if (memcmp(&send_buffer_[L2CAP_CMD_HDR_SIZE], &receive_buffer_[L2CAP_CMD_HDR_SIZE], SIZE)) { - syslog(LOG_ERR, "Response payload different for %s.", status_.address.c_str()); + c.wait(); + if (c.exit_code() != 0) { + is_err >> os.rdbuf(); + // Remove return code + std::string message = std::regex_replace(os.str(), std::regex("\n"), ""); + // Get stdout if stderr is empty + if (message.empty()) { + message = std::regex_replace(line, std::regex("\n"), ""); + } + syslog(LOG_ERR, "l2ping: %s\n", message.c_str()); + setFunctionError("l2ping", message); return false; } return true; } -void L2ping::setFunctionError(const std::string & function_name, int error_code) +void L2ping::setFunctionError(const std::string & function_name, const std::string & error_message) { // Build error data status_.status_code = StatusCode::FUNCTION_ERROR; status_.function_name = function_name; - status_.error_code = error_code; + status_.error_message = error_message; } void L2ping::setStatusCode(StatusCode code) @@ -356,7 +157,7 @@ void L2ping::setStatusCode(StatusCode code) // Build error data status_.status_code = code; status_.function_name = {}; - status_.error_code = 0; + status_.error_message = {}; } L2pingStatus L2ping::getStatus() const { return status_; } diff --git a/system/bluetooth_monitor/service/l2ping_service.cpp b/system/bluetooth_monitor/service/l2ping_service.cpp index 3a930a2a5f65f..1cc1d81a31f12 100644 --- a/system/bluetooth_monitor/service/l2ping_service.cpp +++ b/system/bluetooth_monitor/service/l2ping_service.cpp @@ -11,36 +11,25 @@ // 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. -// -// This code is derived from BlueZ with following copyrights. - -// SPDX-License-Identifier: GPL-2.0-or-later -/* - * - * BlueZ - Bluetooth protocol stack for Linux - * - * Copyright (C) 2000-2001 Qualcomm Incorporated - * Copyright (C) 2002-2003 Maxim Krasnyansky - * Copyright (C) 2002-2010 Marcel Holtmann - * - * - */ #include "bluetooth_monitor/service/l2ping_service.hpp" #include #include +#include -#include -#include #include #include #include +#include #include #include -static const bdaddr_t ANY_ADDRESS = {{0, 0, 0, 0, 0, 0}}; +#define FMT_HEADER_ONLY +#include + +namespace bp = boost::process; L2pingService::L2pingService(const int port) : port_(port), socket_(-1) {} @@ -122,7 +111,7 @@ void L2pingService::run() boost::archive::text_iarchive oa(iss); oa & config_; } catch (const std::exception & e) { - syslog(LOG_ERR, "Exception occurred. %s\n", e.what()); + syslog(LOG_ERR, "Exception occurred. ! %s\n", e.what()); close(new_sock); continue; } @@ -153,7 +142,8 @@ void L2pingService::run() } } -void L2pingService::setFunctionError(const std::string & function_name, int error_code) +void L2pingService::setFunctionError( + const std::string & function_name, const std::string & error_message) { // Clear list status_list_.clear(); @@ -162,110 +152,68 @@ void L2pingService::setFunctionError(const std::string & function_name, int erro L2pingStatus status{}; status.status_code = StatusCode::FUNCTION_ERROR; status.function_name = function_name; - status.error_code = error_code; + status.error_message = error_message; status_list_.emplace_back(status); } bool L2pingService::buildDeviceList() { - // Create socket to bluetooth host controller interface(HCI) - int sock = socket(AF_BLUETOOTH, SOCK_RAW, BTPROTO_HCI); - - if (sock < 0) { - setFunctionError("socket", errno); - syslog(LOG_ERR, "Failed to create socket. %s", strerror(errno)); - return false; - } - - // Register local address to connect to HCI - sockaddr_rc local_address{}; - - local_address.rc_family = AF_BLUETOOTH; // Bluetooth family - local_address.rc_bdaddr = ANY_ADDRESS; // Any address - local_address.rc_channel = 1; // Single channel - - if (bind(sock, (struct sockaddr *)&local_address, sizeof(local_address)) < 0) { - setFunctionError("bind", errno); - syslog(LOG_ERR, "Failed to bind socket. %s", strerror(errno)); - return false; - } - - // Get list of HCI devices - hci_dev_list_req hci_device_list{}; - hci_device_list.dev_num = HCI_MAX_DEV; - - if (ioctl(sock, HCIGETDEVLIST, &hci_device_list) < 0) { - setFunctionError("HCIGETDEVLIST", errno); - syslog(LOG_ERR, "Failed to get list of HCI devices. %s", strerror(errno)); - close(sock); - return false; - } - - // Get information of each HCI device - hci_dev_req * hci_device = hci_device_list.dev_req; - - // Loop for HCI devices - for (int i = 0; i < hci_device_list.dev_num; ++i, ++hci_device) { - // Build device list to ping from connected devices - if (!buildDeviceListFromConnectedDevices(sock, hci_device->dev_id)) { + bp::pipe pipe; + std::ostringstream os; + + // Get MAC address list of paired devices + /* + Output example of `bluetoothctl paired-devices` + + Device 01:02:03:04:05:06 Wireless Controller + Device AA:BB:CC:DD:EE:FF bluetooth mouse4.0 + */ + { + bp::ipstream is_err; + bp::child c("bluetoothctl paired-devices", bp::std_out > pipe, bp::std_err > is_err); + c.wait(); + if (c.exit_code() != 0) { + is_err >> os.rdbuf(); + setFunctionError("bluetoothctl", os.str()); + syslog(LOG_ERR, "%s\n", os.str().c_str()); return false; } } - close(sock); - - return true; -} - -bool L2pingService::buildDeviceListFromConnectedDevices(int sock, uint16_t device_id) -{ - // Get informaton of HCI device - hci_dev_info hci_device_info{}; - hci_device_info.dev_id = device_id; - if (ioctl(sock, HCIGETDEVINFO, &hci_device_info) < 0) { - setFunctionError("HCIGETDEVINFO", errno); - syslog( - LOG_ERR, "Failed to get informaton of HCI device. id = %d, %s", hci_device_info.dev_id, - strerror(errno)); - return false; - } - - // Get list of connected devices - hci_conn_list_req connection_list_request{}; - connection_list_request.dev_id = hci_device_info.dev_id; - connection_list_request.conn_num = HCI_MAX_DEV; - - if (ioctl(sock, HCIGETCONNLIST, &connection_list_request)) { - setFunctionError("HCIGETCONNLIST", errno); - syslog( - LOG_ERR, "Failed to get connection list of HCI device. id = %d, %s", hci_device_info.dev_id, - strerror(errno)); - return false; - } - - // Allocate space for list of connected devices - hci_conn_info * devices = new hci_conn_info[connection_list_request.conn_num]; - - // Get information of each connected device - hci_conn_info * connection_info = connection_list_request.conn_info; - - // Loop for connected devices - for (int i = 0; i < connection_list_request.conn_num; ++i, ++connection_info) { - char address_str[18]{}; - ba2str(&connection_info->bdaddr, address_str); + // Pick up MAC address + std::vector address_list; + { + bp::ipstream is_out; + bp::ipstream is_err; + bp::child c("cut -f 2 -d \" \"", bp::std_out > is_out, bp::std_err > is_err, bp::std_in < pipe); + c.wait(); + if (c.exit_code() != 0) { + is_err >> os.rdbuf(); + setFunctionError("cut", os.str()); + syslog(LOG_ERR, "%s\n", os.str().c_str()); + return false; + } - // Skip if device not found and wild card not specified - if ( - std::count(config_.addresses.begin(), config_.addresses.end(), address_str) == 0 && - std::count(config_.addresses.begin(), config_.addresses.end(), "*") == 0) { - continue; + // Register device + std::string line; + while (std::getline(is_out, line) && !line.empty()) { + // Skip if device not found and wild card not specified + if ( + std::count(config_.addresses.begin(), config_.addresses.end(), line) == 0 && + std::count(config_.addresses.begin(), config_.addresses.end(), "*") == 0) { + continue; + } + address_list.push_back(line); } + } + // Loop for registered devices + for (const auto & address : address_list) { bool found = false; for (const auto & object : objects_) { // If device not registered - if (object->getAddress() == address_str) { + if (object->getAddress() == address) { found = true; break; } @@ -273,13 +221,10 @@ bool L2pingService::buildDeviceListFromConnectedDevices(int sock, uint16_t devic if (!found) { // Start ping thread - objects_.emplace_back(std::make_unique(connection_info->bdaddr, config_.l2ping)); - objects_.back().get()->getDeviceInformation(connection_info->handle); + objects_.emplace_back(std::make_unique(address, config_.l2ping)); objects_.back().get()->run(); } } - delete[] devices; - return true; } diff --git a/system/bluetooth_monitor/src/bluetooth_monitor.cpp b/system/bluetooth_monitor/src/bluetooth_monitor.cpp index 38acdca8b632c..f8414c6353cfd 100644 --- a/system/bluetooth_monitor/src/bluetooth_monitor.cpp +++ b/system/bluetooth_monitor/src/bluetooth_monitor.cpp @@ -39,9 +39,7 @@ BluetoothMonitor::BluetoothMonitor(const rclcpp::NodeOptions & options) gethostname(host_name, sizeof(host_name)); // Build L2ping configuration - config_.l2ping.delay = declare_parameter("delay", DEFAULT_DELAY); config_.l2ping.timeout = declare_parameter("timeout", DEFAULT_TIMEOUT); - config_.l2ping.verify = declare_parameter("verify", DEFAULT_VERIFY); config_.l2ping.rtt_warn = declare_parameter("rtt_warn", RTT_NO_WARN); config_.addresses = declare_parameter("addresses", std::vector()); @@ -135,7 +133,8 @@ bool BluetoothMonitor::receiveData(diagnostic_updater::DiagnosticStatusWrapper & void BluetoothMonitor::closeConnection() { close(socket_); } void BluetoothMonitor::setErrorLevel(diagnostic_updater::DiagnosticStatusWrapper & stat) -{ // No device +{ + // No device if (status_list_.size() == 0) { stat.summary(DiagStatus::OK, "No device connected"); return; @@ -151,11 +150,9 @@ void BluetoothMonitor::setErrorLevel(diagnostic_updater::DiagnosticStatusWrapper stat.add(fmt::format("Device {}: Manufacturer", index), status.manufacturer); stat.add(fmt::format("Device {}: Address", index), status.address); stat.addf(fmt::format("Device {}: RTT", index), "%.2f ms", status.time_difference); - stat.addf(fmt::format("Device {} Sent packets", index), "%d", status.sent_packets); - stat.addf(fmt::format("Device {} Received packets", index), "%d", status.received_packets); if (status.status_code == StatusCode::FUNCTION_ERROR) { - stat.add(fmt::format("Device {}: {}", index, status.function_name), strerror(errno)); + stat.add(fmt::format("Device {}: {}", index, status.function_name), status.error_message); } level = status_error_list_.at(status.status_code); From 71c975126410840472b8128dc2cae7bb5881fc36 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 24 May 2022 01:29:10 +0000 Subject: [PATCH 15/17] ci(pre-commit): autofix --- system/bluetooth_monitor/README.md | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/system/bluetooth_monitor/README.md b/system/bluetooth_monitor/README.md index 0e84b90238507..54d59fd18f739 100644 --- a/system/bluetooth_monitor/README.md +++ b/system/bluetooth_monitor/README.md @@ -29,13 +29,13 @@ L2ping is only allowed for root by default, so this package provides the followi [values] -| key | value (example) | -| ------------------------------ | ----------------------------------------------------------------------- | -| Device [0-9]: Status | OK / RTT warning / Verify error / Lost / Ping rejected / Function error | -| Device [0-9]: Name | Wireless Controller | -| Device [0-9]: Manufacturer | MediaTek, Inc. | -| Device [0-9]: Address | AA:BB:CC:DD:EE:FF | -| Device [0-9]: RTT | 0.00ms | +| key | value (example) | +| -------------------------- | ----------------------------------------------------------------------- | +| Device [0-9]: Status | OK / RTT warning / Verify error / Lost / Ping rejected / Function error | +| Device [0-9]: Name | Wireless Controller | +| Device [0-9]: Manufacturer | MediaTek, Inc. | +| Device [0-9]: Address | AA:BB:CC:DD:EE:FF | +| Device [0-9]: RTT | 0.00ms | - The following key will be added when `bluetooth_monitor` reports `Function error`.
ex.) The `connect` system call failed. From 84ff2165e366d341da45e3571e848b329afdc9eb Mon Sep 17 00:00:00 2001 From: ito-san Date: Tue, 31 May 2022 13:44:22 +0900 Subject: [PATCH 16/17] fixed clang tidy error and removed unnecessary dependencies in CMakeLists.txt Signed-off-by: ito-san --- system/bluetooth_monitor/CMakeLists.txt | 4 ++-- system/bluetooth_monitor/package.xml | 3 +-- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/system/bluetooth_monitor/CMakeLists.txt b/system/bluetooth_monitor/CMakeLists.txt index ccb27848e8857..ea91d77abeeb8 100644 --- a/system/bluetooth_monitor/CMakeLists.txt +++ b/system/bluetooth_monitor/CMakeLists.txt @@ -21,8 +21,8 @@ find_package(Boost REQUIRED COMPONENTS ) ## Specify libraries to link a library or executable target against -target_link_libraries(bluetooth_monitor_lib bluetooth ${Boost_LIBRARIES}) -target_link_libraries(l2ping_service bluetooth ${Boost_LIBRARIES}) +target_link_libraries(bluetooth_monitor_lib ${Boost_LIBRARIES}) +target_link_libraries(l2ping_service ${Boost_LIBRARIES}) rclcpp_components_register_node(bluetooth_monitor_lib PLUGIN "BluetoothMonitor" diff --git a/system/bluetooth_monitor/package.xml b/system/bluetooth_monitor/package.xml index 12957b4c90d58..34f1c556bf2a9 100644 --- a/system/bluetooth_monitor/package.xml +++ b/system/bluetooth_monitor/package.xml @@ -11,11 +11,10 @@ autoware_cmake - bluez diagnostic_msgs diagnostic_updater fmt - libbluetooth-dev + libboost-dev rclcpp rclcpp_components From 6ed2530f55bc2bfdd8b9e6c5485bf1d7640f4ec0 Mon Sep 17 00:00:00 2001 From: ito-san Date: Tue, 31 May 2022 13:50:26 +0900 Subject: [PATCH 17/17] corrected dependency in package.xml Signed-off-by: ito-san --- system/bluetooth_monitor/package.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/system/bluetooth_monitor/package.xml b/system/bluetooth_monitor/package.xml index 34f1c556bf2a9..1b96451044631 100644 --- a/system/bluetooth_monitor/package.xml +++ b/system/bluetooth_monitor/package.xml @@ -18,6 +18,8 @@ rclcpp rclcpp_components + bluez + ament_lint_auto autoware_lint_common