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 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..6074300fb --- /dev/null +++ b/rosserial_arduino/src/ros_lib/examples/BluetoothHelloWorld/BluetoothHelloWorld.ino @@ -0,0 +1,50 @@ +/** + * rosserial Publisher Example with ArduinoBluetoothHardware + * Prints "hello world!" + * This intends to be connected from bluetooth interface of host PC. + * First pair and connect to your esp32 after burning firmware. + * bluetoothctl + * scan on # check MAC address + * scan off + * 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 + * 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 // set this if you use rosserial over bluetooth + +#include +#include +#include + +ros::NodeHandle nh; +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