feat(bluetooth_monitor): add functionality to monitor Bluetooth conne…

…ction (#862)

* feat(bluetooth_monitor): add functionality to monitor Bluetooth connection

* ci(pre-commit): autofix

* Fixed a typo

* Add a dependency

* Fixed pre-commit errors

* ci(pre-commit): autofix

* Fixed pre-commit errors

* Fixed uncrustify errors

* ci(pre-commit): autofix

* use autoware_cmake

* Fixed license, Fixed CMakeLists.txt, and Use register_node_macro

* Fixed license

* Fixed link title

* changed the way to run l2ping

Signed-off-by: ito-san <fumihito.ito@tier4.jp>

* ci(pre-commit): autofix

* fixed clang tidy error and  removed unnecessary dependencies in CMakeLists.txt

Signed-off-by: ito-san <fumihito.ito@tier4.jp>

* corrected dependency in package.xml

Signed-off-by: ito-san <fumihito.ito@tier4.jp>

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

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
+)

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.<br>
+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
+
+### <u>bluetooth_monitor: bluetooth_connection</u>
+
+<b>[summary]</b>
+
+| level | message        |
+| ----- | -------------- |
+| OK    | OK             |
+| WARN  | RTT warning    |
+| ERROR | Lost           |
+|       | Function error |
+
+<b>[values]</b>
+
+| 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`.<br>
+  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.

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"]

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 <diagnostic_updater/diagnostic_updater.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <map>
+#include <string>
+#include <vector>
+
+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<StatusCode, const char *> status_string_list_ = {
+    {StatusCode::OK, "OK"},
+    {StatusCode::RTT_WARNING, "RTT warning"},
+    {StatusCode::LOST, "Lost"},
+    {StatusCode::FUNCTION_ERROR, FUNCTION_ERROR_STR}};
+
+  const std::map<StatusCode, unsigned char> 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_

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 <string>
+#include <thread>
+#include <vector>
+
+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_