diff --git a/system/bluetooth_monitor/CMakeLists.txt b/system/bluetooth_monitor/CMakeLists.txt new file mode 100644 index 0000000000000..ea91d77abeeb8 --- /dev/null +++ b/system/bluetooth_monitor/CMakeLists.txt @@ -0,0 +1,35 @@ +cmake_minimum_required(VERSION 3.5) +project(bluetooth_monitor) + +### Dependencies +find_package(autoware_cmake REQUIRED) +autoware_package() + +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 + service/l2ping.cpp +) + +find_package(Boost REQUIRED COMPONENTS + serialization +) + +## Specify libraries to link a library or executable target against +target_link_libraries(bluetooth_monitor_lib ${Boost_LIBRARIES}) +target_link_libraries(l2ping_service ${Boost_LIBRARIES}) + +rclcpp_components_register_node(bluetooth_monitor_lib + PLUGIN "BluetoothMonitor" + EXECUTABLE bluetooth_monitor +) + +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..54d59fd18f739 --- /dev/null +++ b/system/bluetooth_monitor/README.md @@ -0,0 +1,84 @@ +# 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. + +![block_diagram](docs/block_diagram.drawio.svg) + +## Output + +### bluetooth_monitor: bluetooth_connection + +[summary] + +| level | message | +| ----- | -------------- | +| OK | OK | +| WARN | RTT warning | +| ERROR | Lost | +| | 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 | + +- 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. | +| `timeout` | int | 5 | Wait timeout seconds for the response. | +| `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..fd4c9eaa40d21 --- /dev/null +++ b/system/bluetooth_monitor/config/bluetooth_monitor.param.yaml @@ -0,0 +1,7 @@ +--- +/**: + ros__parameters: + port: 7640 + timeout: 5 + 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..6ebca9b4189d5 --- /dev/null +++ b/system/bluetooth_monitor/include/bluetooth_monitor/bluetooth_monitor.hpp @@ -0,0 +1,112 @@ +// 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. +// 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 + * @param [in] options Options associated with this node + */ + explicit BluetoothMonitor(const rclcpp::NodeOptions & options); + +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::LOST, "Lost"}, + {StatusCode::FUNCTION_ERROR, FUNCTION_ERROR_STR}}; + + const std::map status_error_list_ = { + {StatusCode::OK, DiagStatus::OK}, + {StatusCode::RTT_WARNING, DiagStatus::WARN}, + {StatusCode::LOST, 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..79940329336c4 --- /dev/null +++ b/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping.hpp @@ -0,0 +1,87 @@ +// 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. +// 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__SERVICE__L2PING_HPP_ +#define BLUETOOTH_MONITOR__SERVICE__L2PING_HPP_ + +#include "bluetooth_monitor/service/l2ping_interface.hpp" + +#include +#include +#include + +class L2ping +{ +public: + /** + * @brief Constructor + * @param [in] address Bluetooth address of remote device + * @param [in] config Configuration of L2ping + */ + L2ping(const std::string & address, const L2pingConfig & config); + + /** + * @brief Start ping thread + */ + void run(); + + /** + * @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 Get information from remote device + * @return true on success, false on error + */ + bool getDeviceInformation(); + + /** + * @brief Thread loop + */ + void thread(); + + /** + * @brief Ping to remote device + * @return true on success, false on error + */ + bool ping(); + + /** + * @brief Set error data to inform ros2 node + * @param [in] function_name Function name which error occurred + * @param [in] error_message Error message to display + */ + void setFunctionError(const std::string & function_name, const std::string & error_message); + + /** + * @brief Set status code + * @param [in] code Status code + */ + void setStatusCode(StatusCode code); + + L2pingConfig config_; //!< @brief Configuration of L2ping + std::thread thread_; //!< @brief Thread to L2ping + L2pingStatus status_; //!< @brief L2ping status +}; + +#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 new file mode 100644 index 0000000000000..f04dccb94f429 --- /dev/null +++ b/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping_interface.hpp @@ -0,0 +1,127 @@ +// 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. +// 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__SERVICE__L2PING_INTERFACE_HPP_ +#define BLUETOOTH_MONITOR__SERVICE__L2PING_INTERFACE_HPP_ + +#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 timeout{DEFAULT_TIMEOUT}; //!< @brief Wait timeout seconds for the response + 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 & timeout; + 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, + LOST = 2, + FUNCTION_ERROR = 3, +}; + +/** + * @brief L2ping status + */ +struct L2pingStatus +{ + StatusCode status_code; //!< @brief Status code of a device + std::string function_name; //!< @brief Function name which error occurred + 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 + + /** + * @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_message; + ar & name; + ar & manufacturer; + ar & address; + ar & time_difference; + } +}; + +/** + * @brief List of L2ping status + */ +typedef std::vector L2pingStatusList; + +#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 new file mode 100644 index 0000000000000..fa325ca733196 --- /dev/null +++ b/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping_service.hpp @@ -0,0 +1,83 @@ +// 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. +// 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__SERVICE__L2PING_SERVICE_HPP_ +#define BLUETOOTH_MONITOR__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_message Error message to display + */ + void setFunctionError(const std::string & function_name, const std::string & error_message); + + /** + * @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 // BLUETOOTH_MONITOR__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..1b96451044631 --- /dev/null +++ b/system/bluetooth_monitor/package.xml @@ -0,0 +1,29 @@ + + + + bluetooth_monitor + 0.1.0 + Bluetooth alive monitoring + Fumihito Ito + Apache License 2.0 + + ament_cmake_auto + + autoware_cmake + + diagnostic_msgs + diagnostic_updater + fmt + libboost-dev + rclcpp + rclcpp_components + + bluez + + ament_lint_auto + autoware_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..03e806408c655 --- /dev/null +++ b/system/bluetooth_monitor/service/l2ping.cpp @@ -0,0 +1,165 @@ +// 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. +// 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.hpp" + +#include + +#include + +#include +#include + +#define FMT_HEADER_ONLY +#include + +namespace bp = boost::process; + +L2ping::L2ping(const std::string & address, const L2pingConfig & config) +: config_(config), status_{} +{ + status_.address = address; +} + +bool L2ping::getDeviceInformation() +{ + 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; + } + + std::string line; + std::cmatch match; + const std::regex filter_device_name("\tDevice Name: (.*)"); + const std::regex filter_manufacturer("\tManufacturer: (.*)"); + + 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(); + } + } + + return true; +} + +void L2ping::run() +{ + // Start thread loop + thread_ = std::thread(&L2ping::thread, this); +} + +void L2ping::thread() +{ + while (true) { + // Get device information if not provided + if (status_.name.empty() || status_.manufacturer.empty()) { + getDeviceInformation(); + } + // Ping to remote device in loop + ping(); + } +} + +bool L2ping::ping() +{ + 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); + } + } + + // No response + const std::regex filter_no_response("^no response from .*"); + + if (std::regex_match(line.c_str(), match, filter_no_response)) { + setStatusCode(StatusCode::LOST); + } + + 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, const std::string & error_message) +{ + // Build error data + status_.status_code = StatusCode::FUNCTION_ERROR; + status_.function_name = function_name; + status_.error_message = error_message; +} + +void L2ping::setStatusCode(StatusCode code) +{ + // Build error data + status_.status_code = code; + status_.function_name = {}; + status_.error_message = {}; +} + +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..1cc1d81a31f12 --- /dev/null +++ b/system/bluetooth_monitor/service/l2ping_service.cpp @@ -0,0 +1,230 @@ +// 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. +// 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_service.hpp" + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + +#define FMT_HEADER_ONLY +#include + +namespace bp = boost::process; + +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, const std::string & error_message) +{ + // Clear list + status_list_.clear(); + + // Set error data + L2pingStatus status{}; + status.status_code = StatusCode::FUNCTION_ERROR; + status.function_name = function_name; + status.error_message = error_message; + + status_list_.emplace_back(status); +} + +bool L2pingService::buildDeviceList() +{ + 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; + } + } + + // 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; + } + + // 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) { + found = true; + break; + } + } + + if (!found) { + // Start ping thread + objects_.emplace_back(std::make_unique(address, config_.l2ping)); + objects_.back().get()->run(); + } + } + + return true; +} diff --git a/system/bluetooth_monitor/service/main.cpp b/system/bluetooth_monitor/service/main.cpp new file mode 100644 index 0000000000000..a3ccb971a5eb7 --- /dev/null +++ b/system/bluetooth_monitor/service/main.cpp @@ -0,0 +1,97 @@ +// 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. +// 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 Main 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..f8414c6353cfd --- /dev/null +++ b/system/bluetooth_monitor/src/bluetooth_monitor.cpp @@ -0,0 +1,201 @@ +// 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. +// 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 + +#include +#include +#include + +#define FMT_HEADER_ONLY +#include + +BluetoothMonitor::BluetoothMonitor(const rclcpp::NodeOptions & options) +: Node("bluetooth_monitor", options), + 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.timeout = declare_parameter("timeout", DEFAULT_TIMEOUT); + 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); + + if (status.status_code == StatusCode::FUNCTION_ERROR) { + stat.add(fmt::format("Device {}: {}", index, status.function_name), status.error_message); + } + + 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); +} + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(BluetoothMonitor)