From 2a6fb1c6ea58bc576a66157921a2ea0e2768e2c2 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Fri, 17 Sep 2021 22:47:31 +0900 Subject: [PATCH 1/4] 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 79013f0d1..d31395ada 100644 --- a/rosserial_arduino/src/ros_lib/ros.h +++ b/rosserial_arduino/src/ros_lib/ros.h @@ -39,6 +39,8 @@ #if (defined(ESP8266) or defined(ESP32) or defined(ROSSERIAL_ARDUINO_TCP)) and not defined(ESP_SERIAL) #include "ArduinoTcpHardware.h" +#elif defined(ROSSERIAL_ARDUINO_BLUETOOTH) + #include "ArduinoBluetoothHardware.h" #else #include "ArduinoHardware.h" #endif From ff13d09edbd7b2109efe0cf320455f5b298dc941 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Sat, 18 Sep 2021 23:59:40 +0900 Subject: [PATCH 2/4] 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 e0d84e70008a65e4c20b076e35d9dc201858e5a2 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Mon, 20 Sep 2021 13:14:21 +0900 Subject: [PATCH 3/4] 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 7b1bdfe6ee7fd14c0a0685cdbc674275f3aeb53b Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Mon, 20 Sep 2021 14:40:44 +0900 Subject: [PATCH 4/4] [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