From 7b1cce6d7cb832bf3d109b2d5b34bf5fdf750ec7 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Fri, 17 Sep 2021 22:47:31 +0900 Subject: [PATCH 1/5] Add ArduinoBluetoothHardware and example --- .../src/ros_lib/ArduinoBluetoothHardware.h | 79 +++++++++++++++++++ .../BluetoothHelloworld.ino | 41 ++++++++++ rosserial_arduino/src/ros_lib/ros.h | 2 + 3 files changed, 122 insertions(+) create mode 100644 rosserial_arduino/src/ros_lib/ArduinoBluetoothHardware.h create mode 100644 rosserial_arduino/src/ros_lib/examples/BluetoothHelloWorld/BluetoothHelloworld.ino diff --git a/rosserial_arduino/src/ros_lib/ArduinoBluetoothHardware.h b/rosserial_arduino/src/ros_lib/ArduinoBluetoothHardware.h new file mode 100644 index 000000000..5caaf3738 --- /dev/null +++ b/rosserial_arduino/src/ros_lib/ArduinoBluetoothHardware.h @@ -0,0 +1,79 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote prducts derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef ROS_ARDUINO_BLUETOOTH_HARDWARE_H_ +#define ROS_ARDUINO_BLUETOOTH_HARDWARE_H_ + +#if not defined(ESP32) + #error ARDUINO BLUETOOTH HARDWARE CURRENTLY SUPPORT ONLY ESP32 +#endif + +#include +#include "BluetoothSerial.h" + +class ArduinoHardware { +public: + ArduinoHardware() + { + } + + void init() + { + bluetooth_serial.begin("ROSSERIAL_BLUETOOTH"); + } + + void init(char *name) + { + bluetooth_serial.begin(name); + } + + int read(){ + return bluetooth_serial.read(); + } + + void write(const uint8_t* data, int length) + { + bluetooth_serial.write(data, length); + } + + unsigned long time() + { + return millis(); + } + +protected: + BluetoothSerial bluetooth_serial; +}; + +#endif diff --git a/rosserial_arduino/src/ros_lib/examples/BluetoothHelloWorld/BluetoothHelloworld.ino b/rosserial_arduino/src/ros_lib/examples/BluetoothHelloWorld/BluetoothHelloworld.ino new file mode 100644 index 000000000..dd3437e1d --- /dev/null +++ b/rosserial_arduino/src/ros_lib/examples/BluetoothHelloWorld/BluetoothHelloworld.ino @@ -0,0 +1,41 @@ +/** + * rosserial Publisher Example with ArduinoBluetoothHardware + * Prints "hello world!" + * This intends to be connected from bluetooth interface of host PC. + * After pairing and trusting this device from PC, bind your device to serial device. + * sudo rfcomm bind 1 + * sudo stty -F /dev/rfcomm1 57600 cs8 + * then you can now connect rosserial host node to serial device + * rosrun rosserial_python serial_node.py _port:=/dev/rfcomm1 _baud:=57600 + */ + +#define ROSSERIAL_ARDUINO_BLUETOOTH + +#include +#include +#include + +std_msgs::String str_msg; +ros::Publisher chatter("chatter", &str_msg); + +char hello[14] = "hello world!"; + +void setup() +{ + nh.initNode("BluetoothHelloworld"); + nh.advertise(chatter); + + // wait for bluetooth and rosserial host connection. + while (not nh.connected()) { + nh.spinOnce(); + delay(100); + } +} + +void loop() +{ + str_msg.data = hello; + chatter.publish( &str_msg ); + nh.spinOnce(); + delay(1000); +} diff --git a/rosserial_arduino/src/ros_lib/ros.h b/rosserial_arduino/src/ros_lib/ros.h index cd4a6d114..77e2b80a9 100644 --- a/rosserial_arduino/src/ros_lib/ros.h +++ b/rosserial_arduino/src/ros_lib/ros.h @@ -39,6 +39,8 @@ #if defined(ROSSERIAL_ARDUINO_TCP) #include "ArduinoTcpHardware.h" +#elif defined(ROSSERIAL_ARDUINO_BLUETOOTH) + #include "ArduinoBluetoothHardware.h" #else #include "ArduinoHardware.h" #endif From 5f536ec466e847e05249158b980e5a08f4a5a372 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Sat, 18 Sep 2021 23:59:40 +0900 Subject: [PATCH 2/5] renamed exmaple ino --- .../{BluetoothHelloworld.ino => BluetoothHelloWorld.ino} | 1 + 1 file changed, 1 insertion(+) rename rosserial_arduino/src/ros_lib/examples/BluetoothHelloWorld/{BluetoothHelloworld.ino => BluetoothHelloWorld.ino} (98%) diff --git a/rosserial_arduino/src/ros_lib/examples/BluetoothHelloWorld/BluetoothHelloworld.ino b/rosserial_arduino/src/ros_lib/examples/BluetoothHelloWorld/BluetoothHelloWorld.ino similarity index 98% rename from rosserial_arduino/src/ros_lib/examples/BluetoothHelloWorld/BluetoothHelloworld.ino rename to rosserial_arduino/src/ros_lib/examples/BluetoothHelloWorld/BluetoothHelloWorld.ino index dd3437e1d..9670be725 100644 --- a/rosserial_arduino/src/ros_lib/examples/BluetoothHelloWorld/BluetoothHelloworld.ino +++ b/rosserial_arduino/src/ros_lib/examples/BluetoothHelloWorld/BluetoothHelloWorld.ino @@ -15,6 +15,7 @@ #include #include +ros::NodeHandle nh; std_msgs::String str_msg; ros::Publisher chatter("chatter", &str_msg); From e6056812bf796cd41f2886ea27c57edf55149f97 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Mon, 20 Sep 2021 13:14:21 +0900 Subject: [PATCH 3/5] add comment to BluetoothHelloworld example --- .../examples/BluetoothHelloWorld/BluetoothHelloWorld.ino | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/rosserial_arduino/src/ros_lib/examples/BluetoothHelloWorld/BluetoothHelloWorld.ino b/rosserial_arduino/src/ros_lib/examples/BluetoothHelloWorld/BluetoothHelloWorld.ino index 9670be725..04ba3aad7 100644 --- a/rosserial_arduino/src/ros_lib/examples/BluetoothHelloWorld/BluetoothHelloWorld.ino +++ b/rosserial_arduino/src/ros_lib/examples/BluetoothHelloWorld/BluetoothHelloWorld.ino @@ -2,7 +2,14 @@ * rosserial Publisher Example with ArduinoBluetoothHardware * Prints "hello world!" * This intends to be connected from bluetooth interface of host PC. - * After pairing and trusting this device from PC, bind your device to serial device. + * First pair and connect to your esp32 after burning firmware. + * bluetoothctl + * scan on # check MAC address + * scan off + * pair + * trust + * connect + * After connect this device from PC, bind your device to serial device. * sudo rfcomm bind 1 * sudo stty -F /dev/rfcomm1 57600 cs8 * then you can now connect rosserial host node to serial device From d825fec8c7bd07bb8555eaed75cc4780e61cfb32 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Mon, 20 Sep 2021 14:40:44 +0900 Subject: [PATCH 4/5] [rosserial_arduino] add comment to BluetoothHelloworld --- .../examples/BluetoothHelloWorld/BluetoothHelloWorld.ino | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/rosserial_arduino/src/ros_lib/examples/BluetoothHelloWorld/BluetoothHelloWorld.ino b/rosserial_arduino/src/ros_lib/examples/BluetoothHelloWorld/BluetoothHelloWorld.ino index 04ba3aad7..6074300fb 100644 --- a/rosserial_arduino/src/ros_lib/examples/BluetoothHelloWorld/BluetoothHelloWorld.ino +++ b/rosserial_arduino/src/ros_lib/examples/BluetoothHelloWorld/BluetoothHelloWorld.ino @@ -9,6 +9,7 @@ * pair * trust * connect + * info # check if successfully connected * After connect this device from PC, bind your device to serial device. * sudo rfcomm bind 1 * sudo stty -F /dev/rfcomm1 57600 cs8 @@ -16,7 +17,7 @@ * rosrun rosserial_python serial_node.py _port:=/dev/rfcomm1 _baud:=57600 */ -#define ROSSERIAL_ARDUINO_BLUETOOTH +#define ROSSERIAL_ARDUINO_BLUETOOTH // set this if you use rosserial over bluetooth #include #include From 8b0c4c10f45251580fc1abbf3b590cc2f384442a Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Mon, 10 Jul 2023 12:46:30 +0900 Subject: [PATCH 5/5] add ArduinoBluetoothBLEHardware.h --- .../src/ros_lib/ArduinoBluetoothBLEHardware.h | 192 ++++++++++++++++++ 1 file changed, 192 insertions(+) create mode 100644 rosserial_arduino/src/ros_lib/ArduinoBluetoothBLEHardware.h diff --git a/rosserial_arduino/src/ros_lib/ArduinoBluetoothBLEHardware.h b/rosserial_arduino/src/ros_lib/ArduinoBluetoothBLEHardware.h new file mode 100644 index 000000000..db99c7d1c --- /dev/null +++ b/rosserial_arduino/src/ros_lib/ArduinoBluetoothBLEHardware.h @@ -0,0 +1,192 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Kei Okada + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of copyright holder nor the names of its + * contributors may be used to endorse or promote prducts derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +// Copied from esp32/hardware/esp32/2.0.9/libraries/BLE/examples/BLE_uart/BLE_uart.ino + +#ifndef ROS_ARDUINO_BLUETOOTH_BLE_HARDWARE_H_ +#define ROS_ARDUINO_BLUETOOTH_BLE_HARDWARE_H_ + +#if not defined(ESP32) + #error ARDUINO BLUETOOTH HARDWARE CURRENTLY SUPPORT ONLY ESP32 +#endif + +#include +#include +#include +#include + +// See the following for generating UUIDs: +// https://www.uuidgenerator.net/ + +#define SERVICE_UUID "6E400001-B5A3-F393-E0A9-E50E24DCCA9E" // UART service UUID +#define CHARACTERISTIC_UUID_RX "6E400002-B5A3-F393-E0A9-E50E24DCCA9E" +#define CHARACTERISTIC_UUID_TX "6E400003-B5A3-F393-E0A9-E50E24DCCA9E" + +bool deviceConnected = false; + +class MyServerCallbacks: public BLEServerCallbacks { + void onConnect(BLEServer* pServer) { + deviceConnected = true; + }; + + void onDisconnect(BLEServer* pServer) { + deviceConnected = false; + } +}; + +#include // Need RingBuffer by Jean-Luc Locoduino + +RingBuf rx_buffer; +class MyCallbacks: public BLECharacteristicCallbacks { + void onWrite(BLECharacteristic *pCharacteristic) { + std::string rxValue = pCharacteristic->getValue(); + + // Serial.print("onWrite : "); + // Serial.println(rxValue.length()); + if (rxValue.length() > 0) { + for (int i = 0; i < rxValue.length(); i++) { + rx_buffer.push(rxValue[i]); + } + } + } +}; + +class ArduinoHardware { +public: + ArduinoHardware() + { + } + + void init() + { + init("rosserial BLE UART Service"); + } + + void init(char *name) + { + // Create the BLE Device + BLEDevice::init(name); + + // Create the BLE Server + pServer = BLEDevice::createServer(); + pServer->setCallbacks(new MyServerCallbacks()); + + // Create the BLE Service + BLEService *pService = pServer->createService(SERVICE_UUID); + + // Create a BLE Characteristic + pTxCharacteristic = pService->createCharacteristic( + CHARACTERISTIC_UUID_TX, + BLECharacteristic::PROPERTY_NOTIFY | + BLECharacteristic::PROPERTY_READ + ); + pTxCharacteristic->addDescriptor(new BLE2902()); + + BLECharacteristic * pRxCharacteristic = pService->createCharacteristic( + CHARACTERISTIC_UUID_RX, + BLECharacteristic::PROPERTY_WRITE + ); + + pRxCharacteristic->setCallbacks(new MyCallbacks()); + + // Start the service + pService->start(); + + // Start advertising + pServer->getAdvertising()->start(); + //Serial.println("Waiting a client connection to notify..."); + } + + int read(){ + int data; + // Serial.print("read : "); + // Serial.println(rx_buffer.size()); + if (rx_buffer.lockedPop(data)) { + return data; + } + return -1; + } + + void write(uint8_t* data, int length) + { + // Serial.print("write : "); + // Serial.println(length); + // Serial.println(ESP_GATT_MAX_ATTR_LEN); + size_t max_length = ESP_GATT_MAX_ATTR_LEN-100; + if (deviceConnected) { + for(size_t i = 0; i < length; i += max_length) { + // Serial.print("i = "); + // Serial.print(i); + // Serial.print(" / "); + // Serial.println(min(length-i, max_length)); + pTxCharacteristic->setValue(data+i, (size_t)(min(length-i, max_length))); + pTxCharacteristic->notify(); + delay(10); // bluetooth stack will go into congestion, if too many packets are sent + } + delay(20); + } + loop(); + } + + void loop() { + // disconnecting + if (!deviceConnected && oldDeviceConnected) { + delay(500); // give the bluetooth stack the chance to get things ready + pServer->startAdvertising(); // restart advertising + Serial.println("start advertising"); + oldDeviceConnected = deviceConnected; + } + // connecting + if (deviceConnected && !oldDeviceConnected) { + // do stuff here on connecting + oldDeviceConnected = deviceConnected; + } + } + + unsigned long time() + { + return millis(); + } + + +protected: + int rxvalue_length = 0; + + BLEServer *pServer = NULL; + BLECharacteristic * pTxCharacteristic; + bool oldDeviceConnected = false; + uint8_t txValue = 0; +}; + +#endif