Micro-ROS Teensy non blocking HC-SR04 implementation?
Freuqent fella Pointcloud here again!
Guys, it's cloudy here and I'm need of someone pointing me towards the light :-)
Been working on getting the HC-SR04 Ultra-sonic sensor working on my Teensy 4.1 with the goal of having another publisher. Here is what I have gotten so far.
Hardware set-up and Development Environment:
Teensy 4.1
-> micro-ROS
Ubuntu 22.04.1
ROS2 Humble
Visual Studio Code Version: 1.72.2
PlatformIO: Core 6.1.4 Home 3.4.3
NewPing Library
Compiling .pio/build/teensy41/lib3d4/NewPing/NewPing.cpp.o .pio/libdeps/teensy41/NewPing/src/NewPing.cpp: In constructor 'NewPing::NewPing(uint32t, uint32t, unsigned int)': .pio/libdeps/teensy41/NewPing/src/NewPing.cpp:19:17: error: cannot convert 'volatile uint32t* {aka volatile long unsigned int*}' to 'volatile uint8t* {aka volatile unsigned char}' in assignment triggerOutput = portOutputRegister(digitalPinToPort(triggerpin)); // Get the output port register for the trigger pin. ^ .pio/libdeps/teensy41/NewPing/src/NewPing.cpp:20:13: error: cannot convert 'volatile uint32_t {aka volatile long unsigned int}' to 'volatile uint8_t {aka volatile unsigned char}' in assignment echoInput = portInputRegister(digitalPinToPort(echopin)); // Get the input port register for the echo pin. ^ src/main.cpp:115:29: warning: ISO C++ forbids converting a string constant to 'char' [-Wwrite-strings] char *sonarLabel[] = {"Rear"};
To solve this I have modified the following:
NewPing.cpp
From: NewPing::NewPing(uint8_t trigger_pin, uint8_t echo_pin, unsigned int max_cm_distance) {
To: NewPing::NewPing(uint32_t trigger_pin, uint32_t echo_pin, unsigned int max_cm_distance) {
NewPing.h
From:
// volatile uint8_t *_triggerOutput;
// volatile uint8_t *_echoInput;
// volatile uint8_t *_triggerMode;
To:
volatile uint32_t *_triggerOutput;
volatile uint32_t *_echoInput;
volatile uint32_t *_triggerMode;
After that it compiled and the program executed. I removed any "delay" functions and rather did it the usual "non-blocking" way inside my main loop, where i would check if it is time to execute the sonar logic again.
if ((t-tTime[2]) >= (1000 / US_PULSE))
{ ...
tTime[2] = t;
}
US_PULSE I adjusted from 20Hz (50ms), to 10Hz (100ms), 5Hz (200ms) and 4Hz (250ms). But no matter how often I executed this logic, I could witness some spikes in the reported distance in the serial monitor, without me blocking it and background static wall.
44cm
44cm
34cm
12cm
44cm
44cm
44cm
44cm
44cm
27cm
0cm
44cm
44cm
44cm
44cm
41cm
14cm
0cm
44cm
44cm
44cm
44cm
31cm
4cm
44cm
44cm
44cm
44cm
44cm
20cm
2cm
44cm
44cm
44cm
44cm
37cm
11cm
44cm
44cm
44cm
44cm
44cm
28cm
2cm
44cm
44cm
44cm
44cm
44cm
17cm
0cm
44cm
44cm
44cm
- Researched the web for other examples: Found https://ubuntu.com/blog/hc-sr04-with-the-raspberry-pi-pico-and-micro-ros However I noticed the source code is using while loops and sleep_us and I got nauseous immediately. Having a while loop in the call back that could potentially lead to hang-up. Furhter there were some other more raspberry pi specific commands and functions that wouldn't port over nicely anyways.
Therefore my question is, if there is anyone out there having used an ultrasonic sensor with teensy / micro-ros before and how that was implemented. Or if anyone knows of better examples I could check out. I have done some web-research however, neither the duck, nor the google find suitable implementations.
Of course the entire source code should be also provided. (Well I stripped that gnarly HC-SR04 out again, so not sure if that's going to be helpful?
#include <Arduino.h>
#include <Wire.h>
#include <micro_ros_platformio.h>
#include <example_interfaces/srv/add_two_ints.h>
#include <example_interfaces/srv/set_bool.h>
#include <teensy_interfaces/srv/neo_pixel_control.h>
#include <stdio.h>
#include <global.h>
#include <config.h>
#include <setup.h>
// #include <NewPing.h>
#include <rcl/error_handling.h>
#include <rcl/rcl.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <std_msgs/msg/int32.h>
#include <std_msgs/msg/int64.h>
#include <std_msgs/msg/bool.h>
#include <sensor_msgs/msg/range.h>
#include <WS2812Serial.h>
#if !defined(MICRO_ROS_TRANSPORT_ARDUINO_SERIAL)
#error This example is only avaliable for Arduino framework with serial transport.
#endif
volatile int TFMiniVal = 0;
volatile int TFMiniStrength = 0;
int TFMini_FloorDistance = 30;
int TFMini_FloorDistanceTolerance = 12;
// ============================================================================
// INSTANCES, OBJECTS
// ============================================================================
// publisher and subscriber common
rcl_node_t node;
rclc_support_t support;
rcl_allocator_t allocator;
rclc_executor_t executor;
rcl_timer_t timer;
unsigned int num_handles = 7; // 1 subscribers, 2 publishers, 4 services
// service addTwoInts
// rcl_service_t service_addTwoInts;
// services
rcl_service_t service_FrontLights;
rcl_service_t service_RearLights;
rcl_service_t service_OdriveFans;
rcl_service_t service_NeoPixel;
rcl_wait_set_t wait_set;
// msg_pub publisher
rcl_publisher_t publisher;
std_msgs__msg__Int32 msg_pub;
// msg_pub_range publisher
rcl_publisher_t publisher_range;
sensor_msgs__msg__Range msg_pub_range;
// subscriber
rcl_subscription_t subscriber;
std_msgs__msg__Int32 msg_sub;
// example_interfaces__srv__AddTwoInts_Response res;
// example_interfaces__srv__AddTwoInts_Request req;
example_interfaces__srv__SetBool_Request req_FrontLights;
example_interfaces__srv__SetBool_Response res_FrontLights;
example_interfaces__srv__SetBool_Request req_RearLights;
example_interfaces__srv__SetBool_Response res_RearLights;
example_interfaces__srv__SetBool_Request req_OdriveFans;
example_interfaces__srv__SetBool_Response res_OdriveFans;
teensy_interfaces__srv__NeoPixelControl_Request req_NeoPixel;
teensy_interfaces__srv__NeoPixelControl_Response res_NeoPixel;
// Left NeoPixel
byte dwgMem_NeoPixel_L[numNeoPixel_L*3]; // 3 bytes per LED
DMAMEM byte dispMem_NeoPixel_L[numNeoPixel_L*12]; // 12 bytes per LED
WS2812Serial objNeoPixel_L(numNeoPixel_L, dispMem_NeoPixel_L, dwgMem_NeoPixel_L, pinNeoPixel_L, WS2812_GRB);
// Right NeoPixel
byte dwgMem_NeoPixel_R[numNeoPixel_R*3]; // 3 bytes per LED
DMAMEM byte dispMem_NeoPixel_R[numNeoPixel_R*12]; // 12 bytes per LED
WS2812Serial objNeoPixel_R(numNeoPixel_R, dispMem_NeoPixel_R, dwgMem_NeoPixel_R, pinNeoPixel_R, WS2812_GRB);
// ============================================================================
// DEFINES
// ============================================================================
// I/O - Definitions
#define LED_PIN 13
// cleartext substitutions
// debug
#ifndef DebugMonitor
#define DebugMonitor Serial6
#endif
#define debug(x) DebugMonitor.print(x)
#define debugln(x) DebugMonitor.println(x)
// HC-SR04
// #define SONAR_NUM 1 // Number of sensors.
// #define MAX_DISTANCE 200 // Maximum distance (in cm) to ping.
// int loopcount = 0;
// unsigned long pingTimer[SONAR_NUM]; // Holds the times when the next ping should happen for each sensor.
// unsigned int cm[SONAR_NUM]; // Where the ping distances are stored.
// uint8_t currentSensor = 0; // Keeps track of which sensor is active.
// char *sonarLabel[] = {"Rear"};
// NewPing sonar[SONAR_NUM] = { // Sensor object array.
// //NewPing(10, 9, MAX_DISTANCE), // Front left. Each sensor's trigger pinNeoPixel_L, echo pinNeoPixel_L, and max distance to ping.
// //NewPing(12, 11, MAX_DISTANCE), // Front right
// NewPing(2, 3, MAX_DISTANCE) // Back
// };
// NewPing sonar(2, 3, MAX_DISTANCE);
// ============================================================================
// FUNCTION PROTOTYPES
// ============================================================================
#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){error_loop();}}
#define RCSOFTCHECK(fn) {rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){}}
// --------------------------------------------------------
// NeoPixel.colorWipe
void colorWipe(int, int, WS2812Serial);
// TFMini-Plus
void readTFMiniPlus(void);
// Error handle loop
void error_loop() {
while(1) {
digitalWrite(LED_PIN, !digitalRead(LED_PIN));
delay(500);
}
}
// --------------------------------------------------------
// timer callback (currently just increments msg_pub.data)
void timer_callback(rcl_timer_t * timer, int64_t last_call_time) {
RCLC_UNUSED(last_call_time);
if (timer != NULL) {
// increments msg_pub.data and publishes on the topic
RCSOFTCHECK(rcl_publish(&publisher, &msg_pub, NULL));
msg_pub.data++;
// publishes the latest value from TFMini-Plus
msg_pub_range.range = TFMiniVal;
msg_pub_range.min_range = 0;
msg_pub_range.max_range = 1000;
msg_pub_range.radiation_type = sensor_msgs__msg__Range__INFRARED;
msg_pub_range.field_of_view = 0.04;
msg_pub_range.header.frame_id.data = "TFMini-Plus";
// fill_msg_stamp(range_msg.header.stamp);
RCSOFTCHECK(rcl_publish(&publisher_range, &msg_pub_range, NULL));
}
}
// --------------------------------------------------------
// subscriber callback
void subscription_callback(const void * msgin) {
const std_msgs__msg__Int32 * msg = (const std_msgs__msg__Int32 *)msgin;
digitalWrite(LED_PIN, (msg->data == 0) ? LOW : HIGH);
}
// --------------------------------------------------------
// srv.addTwoInts callback
// void service_addTwoInts_callback(const void * req, void * res) {
// example_interfaces__srv__AddTwoInts_Request * req_in = (example_interfaces__srv__AddTwoInts_Request *) req;
// example_interfaces__srv__AddTwoInts_Response * res_in = (example_interfaces__srv__AddTwoInts_Response *) res;
// //printf("Service request value: %d + %d.\n", (int) req_in->a, (int) req_in->b);
// res_in->sum = req_in->a + req_in->b;
// }
// --------------------------------------------------------
// srv.FrontLights callback
void service_FrontLights_callback(const void * req_FrontLights, void * res_FrontLights) {
bool previousState = digitalRead(FRONT_LIGHT);
example_interfaces__srv__SetBool_Request * req_in = (example_interfaces__srv__SetBool_Request *) req_FrontLights;
example_interfaces__srv__SetBool_Response * res_in = (example_interfaces__srv__SetBool_Response *) res_FrontLights;
digitalWrite(FRONT_LIGHT, (req_in->data == 0) ? LOW:HIGH);
if (previousState != digitalRead(FRONT_LIGHT)) {
res_in->success = true;
res_in->message.data = const_cast<char*>("Front Lights succeeded to update");
}
else {
res_in->success = false;
res_in->message.data = const_cast<char*>("Front Lights FAILED to update");
}
}
// --------------------------------------------------------
// srv.RearLights callback
void service_RearLights_callback(const void * req_RearLights, void * res_RearLights) {
bool previousState = digitalRead(REAR_LIGHT);
example_interfaces__srv__SetBool_Request * req_in = (example_interfaces__srv__SetBool_Request *) req_RearLights;
example_interfaces__srv__SetBool_Response * res_in = (example_interfaces__srv__SetBool_Response *) res_RearLights;
digitalWrite(REAR_LIGHT, (req_in->data == 0) ? LOW:HIGH);
if (previousState != digitalRead(REAR_LIGHT)) {
res_in->success = true;
res_in->message.data = const_cast<char*>("Rear Lights succeeded to update");
}
else {
res_in->success = false;
res_in->message.data = const_cast<char*>("Rear Lights FAILED to update");
}
}
// --------------------------------------------------------
// srv.OdriveFans callback
void service_OdriveFans_callback(const void * req_OdriveFans, void * res_OdriveFans) {
bool previousState = digitalRead(ODRIVE_FAN);
example_interfaces__srv__SetBool_Request * req_in = (example_interfaces__srv__SetBool_Request *) req_OdriveFans;
example_interfaces__srv__SetBool_Response * res_in = (example_interfaces__srv__SetBool_Response *) res_OdriveFans;
digitalWrite(ODRIVE_FAN, (req_in->data == 0) ? LOW:HIGH);
if (previousState != digitalRead(ODRIVE_FAN)) {
res_in->success = true;
res_in->message.data = const_cast<char*>("Odrive Fans succeeded to update");
}
else {
res_in->success = false;
res_in->message.data = const_cast<char*>("Odrive Fans FAILED to update");
}
}
// --------------------------------------------------------
// srv.NeoPixel callback
void service_NeoPixel_callback(const void * req_NeoPixel, void * res_NeoPixel) {
teensy_interfaces__srv__NeoPixelControl_Request * req_in = (teensy_interfaces__srv__NeoPixelControl_Request *) req_NeoPixel;
teensy_interfaces__srv__NeoPixelControl_Response * res_in = (teensy_interfaces__srv__NeoPixelControl_Response *) res_NeoPixel;
switch(req_in->status) {
case 1:
colorWipe(ORANGE, 10, objNeoPixel_L);
colorWipe(ORANGE, 10, objNeoPixel_R);
break;
case 2:
colorWipe(RED, 10, objNeoPixel_L);
colorWipe(RED, 10, objNeoPixel_R);
break;
case 3:
colorWipe(GREEN, 10, objNeoPixel_L);
colorWipe(GREEN, 10, objNeoPixel_R);
break;
case 4:
colorWipe(BLUE, 10, objNeoPixel_L);
colorWipe(BLUE, 10, objNeoPixel_R);
break;
case 5:
colorWipe(YELLOW, 10, objNeoPixel_L);
colorWipe(YELLOW, 10, objNeoPixel_R);
break;
case 6:
colorWipe(PINK, 10, objNeoPixel_L);
colorWipe(PINK, 10, objNeoPixel_R);
break;
case 7:
colorWipe(ORANGE, 10, objNeoPixel_L);
colorWipe(ORANGE, 10, objNeoPixel_R);
break;
case 8:
colorWipe(WHT, 10, objNeoPixel_L);
colorWipe(WHT, 10, objNeoPixel_R);
break;
default:
colorWipe(BLANK, 10, objNeoPixel_L);
colorWipe(BLANK, 10, objNeoPixel_R);
break;
}
res_in->success = true;
res_in->message.data = const_cast<char*>("Neopixel changed");
// res_in->success = true;
// res_in->message.data = const_cast<char*>("Neopixel blanked");
}
// ============================================================================
// SETUP
// ============================================================================
void setup() {
set_microros_serial_transports(Serial);
// Debug
DebugMonitor.begin(115200);
// I/O setup
pinMode(LED_PIN, OUTPUT);
digitalWrite(LED_PIN, HIGH);
pinMode(FRONT_LIGHT, OUTPUT); digitalWrite(FRONT_LIGHT, LOW);
pinMode(REAR_LIGHT, OUTPUT); digitalWrite(REAR_LIGHT, LOW);
pinMode(ODRIVE_FAN, OUTPUT); digitalWrite(ODRIVE_FAN, LOW);
// Neopixel
objNeoPixel_L.begin();
objNeoPixel_R.begin();
// TFMini Plus
TFMiniPlus.begin(115200); // HW Serial for TFmini
delay (200); // Give a little time for things to start
// Set TFMini Plus to Standard Output mode
TFMiniPlus.write(0x42);
TFMiniPlus.write(0x57);
TFMiniPlus.write(0x02);
TFMiniPlus.write(0x00);
TFMiniPlus.write(0x00);
TFMiniPlus.write(0x00);
TFMiniPlus.write(0x01);
TFMiniPlus.write(0x06);
delay(1000);
colorWipe(ORANGE, 10, objNeoPixel_L);
colorWipe(ORANGE, 10, objNeoPixel_R);
allocator = rcl_get_default_allocator();
// create init_options
RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));
// create node
RCCHECK(rclc_node_init_default(&node, "uros_platformio_node", "", &support));
// create service addTwoInts
// RCCHECK(rclc_service_init_default(&service_addTwoInts, &node, ROSIDL_GET_SRV_TYPE_SUPPORT(example_interfaces, srv, AddTwoInts), "/addtwoints"));
// create service FrontLights
RCCHECK(rclc_service_init_default(&service_FrontLights, &node, ROSIDL_GET_SRV_TYPE_SUPPORT(example_interfaces, srv, SetBool), "/frontlights"));
// create service RearLights
RCCHECK(rclc_service_init_default(&service_RearLights, &node, ROSIDL_GET_SRV_TYPE_SUPPORT(example_interfaces, srv, SetBool), "/rearlights"));
// create service OdriveFans
RCCHECK(rclc_service_init_default(&service_OdriveFans, &node, ROSIDL_GET_SRV_TYPE_SUPPORT(example_interfaces, srv, SetBool), "/odrivefans"));
// create service NeoPixel
RCCHECK(rclc_service_init_default(&service_NeoPixel, &node, ROSIDL_GET_SRV_TYPE_SUPPORT(teensy_interfaces, srv, NeoPixelControl), "/neopixel"));
// create msg_pub publisher
RCCHECK(rclc_publisher_init_default(&publisher, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32), "micro_ros_platformio_node_publisher"));
// create msg_pub_range publisher
// RCCHECK(rclc_publisher_init_default(&publisher_range, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32), "micro_ros_platformio_node_publisher_range"));
RCCHECK(rclc_publisher_init_default(&publisher_range, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, Range), "micro_ros_platformio_node_publisher_range"));
// create subscriber
RCCHECK(rclc_subscription_init_default(&subscriber, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32), "micro_ros_platformio_node_subscriber"));
// create timer,
// const unsigned int timer_timeout = 1000;
const unsigned int timer_timeout = 50;
RCCHECK(rclc_timer_init_default(&timer, &support, RCL_MS_TO_NS(timer_timeout), timer_callback));
// create executor
RCCHECK(rclc_executor_init(&executor, &support.context, num_handles, &allocator));
// RCCHECK(rclc_executor_add_service(&executor, &service_addTwoInts, &req, &res, service_addTwoInts_callback));
RCCHECK(rclc_executor_add_service(&executor, &service_FrontLights, &req_FrontLights, &res_FrontLights, service_FrontLights_callback));
RCCHECK(rclc_executor_add_service(&executor, &service_RearLights, &req_RearLights, &res_RearLights, service_RearLights_callback));
RCCHECK(rclc_executor_add_service(&executor, &service_OdriveFans, &req_OdriveFans, &res_OdriveFans, service_OdriveFans_callback));
RCCHECK(rclc_executor_add_service(&executor, &service_NeoPixel, &req_NeoPixel, &res_NeoPixel, service_NeoPixel_callback));
RCCHECK(rclc_executor_add_subscription(&executor, &subscriber, &msg_sub, &subscription_callback, ON_NEW_DATA));
RCCHECK(rclc_executor_add_timer(&executor, &timer));
msg_pub.data = 0;
} // end setup
// ============================================================================
// MAIN LOOP
// ============================================================================
void loop() {
// variable declarations
uint32_t t = millis();
if (bFirstScan) {
bFirstScan = false;
debugln("First Scan");
}
// ------------------------------------------------------
// Service Routine[0]: DEBUG
#if SR00_DEBUG_EN == 1
if ((t-tTime[0]) >= (1000 / DEBUG_PULSE))
{
// debugln(msg_pub.data);
tTime[0] = t;
}
#endif
#if SR01_TFMINI_EN == 1
if ((t-tTime[1]) >= (1000 / TFM_PULSE))
{
readTFMiniPlus();
debug("Distance: "); debugln(TFMiniVal);
debug("Strength: "); debugln(TFMiniStrength);
debugln("--------------------");
tTime[1] = t;
}
#endif
#if SR01_HC_SR04_EN == 1
if ((t-tTime[2]) >= (1000 / US_PULSE))
{
// debug(sonar[0].ping_cm());
// debug(sonar.ping_cm());
// debugln("cm ");
tTime[2] = t;
}
#endif
// EXECUTOR HANDLE SPIN
if ((t-tTime[10]) >= (1000 / SPIN_PULSE))
{
RCSOFTCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)));
tTime[10] = t;
}
}
// --------------------------------------------------------
// FUNCTIONS
// --------------------------------------------------------
// colorWipe
void colorWipe(int color, int wait, WS2812Serial refObject) {
for (int i=0; i < refObject.numPixels(); i++) {
refObject.setPixel(i, color);
refObject.show();
delayMicroseconds(wait);
}
}
// ----------------------------------------------------------------------------
void readTFMiniPlus(){
// Data Format for Benewake TFmini
// ===============================
// 9 bytes total per message:
// 1) 0x59
// 2) 0x59
// 3) Dist_L (low 8bit)
// 4) Dist_H (high 8bit)
// 5) Strength_L (low 8bit)
// 6) Strength_H (high 8bit)
// 7) Reserved bytes
// 8) Original signal quality degree
// 9) Checksum parity bit (low 8bit), Checksum = Byte1 + Byte2 +…+Byte8. This is only a low 8bit though
while(TFMiniPlus.available()>=9) // When at least 9 bytes of data available (expected number of bytes for 1 signal), then read
{
// debugln("Check2");
if((0x59 == TFMiniPlus.read()) && (0x59 == TFMiniPlus.read())) // byte 1 and byte 2
{
unsigned int t1 = TFMiniPlus.read(); // byte 3 = Dist_L
unsigned int t2 = TFMiniPlus.read(); // byte 4 = Dist_H
t2 <<= 8;
t2 += t1;
TFMiniVal = t2*10;
t1 = TFMiniPlus.read(); // byte 5 = Strength_L
t2 = TFMiniPlus.read(); // byte 6 = Strength_H
t2 <<= 8;
t2 += t1;
TFMiniStrength = t2;
for(int i=0; i<3; i++)TFMiniPlus.read(); // byte 7, 8, 9 are ignored
}
}
}
// ----------------------------------------------------------------------------
Asked by PointCloud on 2022-10-31 03:03:11 UTC
Comments