Calibrating the vl5310x TOF laser distance sensor.

Here is a good starting point, however, it might need a polynomial regression.

#include <Wire.h>

#include <VL53L0X.h>

#include <Preferences.h>

VL53L0X sensor;

Preferences laserPrefs;

const int numMeasurements = 100;  // Number of measurements to average

const int numCalPoints = 7;  // Number of calibration points

uint16_t measuredDistances[numCalPoints];

uint16_t actualDistances[numCalPoints] = {10, 100, 300, 600, 900, 1000, 1050}; // Replace with your actual distances in mm

// Calibration parameters

float offset = 0.0;

float scale = 1.0;

void setup()

{

  Serial.begin(9600);

  delay(500);

  Wire.begin();

  // Initialize Preferences for laser calibration data (read-write)

  laserPrefs.begin("laser_calibration", false); // Initialize for writing

  if (!laserPrefs.isKey("offset")) {

    Serial.println("No laser calibration data found. Please calibrate and store offsets.");

    // Perform laser sensor calibration and store offsets if necessary

  } else {

    Serial.println("Laser calibration data loaded.");

    // Load laser calibration offsets if they exist

  }

  sensor.setTimeout(500);

  if (!sensor.init())

  {

    Serial.println("Failed to detect and initialize sensor!");

    while (1) {}

  }

  // Start continuous back-to-back mode (take readings as

  // fast as possible).  To use continuous timed mode

  // instead, provide a desired inter-measurement period in

  // ms (e.g. sensor.startContinuous(100)).

  sensor.startContinuous();

  // Perform calibration if no stored data found

  for (int i = 0; i < numCalPoints; i++) {

    Serial.print("Place the sensor at ");

    Serial.print(actualDistances[i]);

    Serial.println(" mm");

    delay(5000); // Wait for user to place the sensor

    measuredDistances[i] = measureDistance();

    Serial.print("Measured distance: ");

    Serial.println(measuredDistances[i]);

    // Prompt for user confirmation of measured distance

    if (!confirmDistance(actualDistances[i], measuredDistances[i])) {

      Serial.println("Measurement rejected. Please adjust sensor position and retry.");

      i--; // Retry measurement for the same distance

    } else {

      Serial.println("Measurement accepted.");

    }

  }

  // Calculate calibration parameters

  offset = calculateOffset();

  scale = calculateScale();

  // Store calibration data

  laserPrefs.putFloat("offset", offset);

  laserPrefs.putFloat("scale", scale);

  Serial.println("Calibration data stored in laserPrefs");

 

  Serial.print("Calibration offset: ");

  Serial.println(offset);

  Serial.print("Calibration scale: ");

  Serial.println(scale);

  Serial.println("Entering main loop...");

}

void loop()

{

  int tofDist = sensor.readRangeContinuousMillimeters();

  if(tofDist < 2000 && tofDist != 8191){

    float calibratedDist = tofDist * scale + offset;

    Serial.print("Raw Distance: ");

    Serial.print(tofDist);

    Serial.print(" mm, Calibrated Distance: ");

    Serial.print(calibratedDist);

    Serial.println(" mm");

    delay(1000); // Adjust delay as necessary

  }

 

  //if (sensor.timeoutOccurred()) { Serial.print(" TIMEOUT"); }

 

}

uint16_t measureDistance() {

  uint32_t sum = 0;  // Use uint32_t to avoid overflow if the sum gets large

  int validCount = 0;

  for (int i = 0; i < numMeasurements; i++) {

    uint16_t distance = sensor.readRangeContinuousMillimeters();

   

    if (distance < 4000) {  // Check if the distance is valid (assuming valid distances are < 4000 mm)

      sum += distance;

      validCount++;

      Serial.print("VM: ");

      Serial.print(validCount);

      Serial.print(" ");

      Serial.println(distance);

    } else {

      // Handle invalid measurements if necessary

      Serial.print("VM: ");

      Serial.print(validCount);

      Serial.print(" Invalid measurement: ");

      Serial.println(distance);

    }

   

    delay(50); // Small delay between measurements

  }

  Serial.print("SUM: ");

  Serial.println(sum);

  Serial.print("Valid measurements: ");

  Serial.println(validCount);

  if (validCount > 0) {

    return sum / validCount;  // Return the average of valid measurements

  } else {

    return 0;  // Return 0 or handle this case appropriately if no valid measurements were recorded

  }

}

float calculateOffset() {

  float sum = 0;

  for (int i = 0; i < numCalPoints; i++) {

    sum += (actualDistances[i] - measuredDistances[i]);

  }

  return sum / numCalPoints;

}

float calculateScale() {

  float sum = 0;

  for (int i = 0; i < numCalPoints; i++) {

    sum += (actualDistances[i] / measuredDistances[i]);

  }

  return sum / numCalPoints;

}

bool confirmDistance(uint16_t actual, uint16_t measured) {

  Serial.print("Measured distance is ");

  Serial.print(measured);

  Serial.print(" mm. Is this correct for ");

  Serial.print(actual);

  Serial.println(" mm? (y/n)");

  while (true) {

    if (Serial.available() > 0) {

      char response = Serial.read();

      if (response == 'y' || response == 'Y') {

        return true;

      } else if (response == 'n' || response == 'N') {

        return false;

      }

    }

    delay(100); // Wait for user input

  }

}

Pinky's Lighthouse

 After getting communication set up with the Raspberry Pi controller in this post, we are off to send and interpret sensor data with ROS2.

Cartographer is a good starting point. In my ROS2 docker container:
sudo apt install ros-$ROS_DISTRO-cartographer

It installs, but it looks like CARTOGRAPHER IS NOT SUPPORTED ON IRON...    I don't know why this package is available...

Moving on to nav2.

From inside the container, I can't launch GUIs. My working container must be committed to create a new image and re-run with the following:

sudo docker run -it \

    --privileged \

    -p 8888:8888/udp \

    --name c5cont \

    --env DISPLAY=$DISPLAY \

    --env QT_X11_NO_MITSHM=1 \

    --volume /tmp/.X11-unix:/tmp/.X11-unix \

    --cpus="3" \

    c5 /bin/bash

Then create a run file that has the following:

#/bin/bash

sudo docker start <container name>

xhost +local:docker

sudo docker exec -it <container name> /bin/bash

Also look into creating a dockerfile:

I'll have to reformat the data sent from the lighthouse IMU and laser distance sensor to mimic a lidar array.

RL training using Nvidia Omniverse

 How to run Isaac gym on my computer:

C:\Users\jynx4\AppData\Local\ov\pkg\isaac_sim-2023.1.1\isaac-sim.gym.bat --ext-folder D:\isaac\

This includes default robots. In order to run training on Pinky, many files must be created along with importing a .xacro or .urdf robot description into a USD stage.

Modify this file:

by adding "omni.isaac.sensor" = {} at the end.

Gait phases:

1. Stance: the foot is in contact with the ground
1. Loading: the foot transitions from non-contact to contact
2. Travel: foot follows the opposite vector of the command
3. Pre-swing: as the foot nears the end of its range of motion, it waits for 4 feet to be in contact with the ground
2. Swing: the food is in the air
1. Unloading: the foot transitions from non-contact to contact
2. Position: the foot lifts to a minimum height for clearance while maintaining minimal distance from the center of the robot.
3. Travel: moves to maximize its stride
4. Pre-stance: waits until only 3 feet are in contact with the ground

Micro-ROS adventure

 PS C:\Program Files\uagent_superbuild\bin> .\MicroXRCEAgent.exe udp4 -p 2018 -v 6

Pinky: ROSberry pi controller

 

Raspberry Pi 4 setup:

sudo apt update

sudo apt upgrade

sudo raspi-config

    enable VNC 1080p resolution

    expand filesystem

    enable SPI

#Increasing Swap because this RPI has only 2gb of memory\

sudo dphys-swapfile swapoff

sudo nano /etc/dphys-swapfile

CONF_SWAPSIZE=2048

sudo dphys-swapfile setup

sudo dphys-swapfile swapon

sudo reboot

Setting up Docker:

curl -fsSL https://get.docker.com -o get-docker.sh

sudo sh ./get-docker.sh 

sudo docker run -it  ros:iron-ros-core #use --privileged to use spi and gpio

# UPDATE ^ to add port forwarding for micro ros

sudo docker run -it --privileged -p 8888:8888/udp --name c4 control4 /bin/bash

sudo docker rename e8b9c16b0544 ROS-TEST

sudo docker exec -ti ROS-TEST /bin/bash

apt update

apt install apt-utils nano whiptail apt install python3-colcon-common-extensions python3-rosdep git

python3 -m pip install gpiozero

apt upgrade

mkdir -p ~/colcon_venv/src

cd ~/colcon_venv/

# Make a virtual env and activate it

#apt install python3.10-venv #from raspberry pi

#python3 -m venv env #from raspberry pi

#Modified from ROS

virtualenv -p python3 --system-site-packages ./rosvenv

source ./rosvenv/bin/activate

#Make sure that colcon doesn’t try to build the venv

touch ./rosvenv/COLCON_IGNORE

pip install gpiozero adafruit-circuitpython-mcp3xxx

Building a package:

~/colcon_venv/src# ros2 pkg create --build-type ament_python --license Apache-2.0 CustomController

#Warnings thrown:/usr/lib/python3/dist-packages/setuptools/command/install.py:34: SetuptoolsDeprecationWarning: setup.py install is deprecated. Use build and pip and other standards-based tools.

#setuptools version 59.6.0

pip install setuptools==58.2.0

ROS Build

#Modify /root/colcon_venv/src/control_package/control_package/control_node.py with codes below.

colcon build --packages-select control_package

source install/local_setup.bash

ros2 run control_package control_node

Docker reentry

1. sudo docker ps -a
2. sudo docker start existing_container_ID_or_name
1. sudo docker start c4
3. sudo docker exec -it existing_container_ID_or_name /bin/bash
1. sudo docker exec -it c4 /bin/bash
4. check version with "printenv ROS_DISTRO"

ROS2 reentry:

1. cd ~/colcon_venv/
2. source ./rosvenv/bin/activate
3. MicroRos
1. source install/local_setup.bash
2. ros2 run micro_ros_agent micro_ros_agent udp4 --port 8888
3. 
   

3D printed Controller on the workbench:

Custom Pi Hat:

The joystick has 3 analog outputs. These run through an MCP3008. 

ANALOG:

• Up/down is chan1, 
• Left/Right is chan2
• Twist is chan0. 

#!/usr/bin/env python3

#ROS2

import rclpy

from rclpy.node import Node

from sensor_msgs.msg import Joy

from std_msgs.msg import String

#RaspberryPi

import RPi.GPIO

#from gpiozero import MCP3008, Button

import adafruit_mcp3xxx.mcp3008 as MCP

from adafruit_mcp3xxx.analog_in import AnalogIn

import busio

import digitalio

import board

import time

def gpio_callback(data, channels, pub):

    # Process GPIO inputs here

    analog_values = [mcp[i].value for i in range(3)]

    # Display the input values

    print(f"Analog Inputs (MCP3008): {analog_values}")

def main():

    rclpy.init()

    # Set up MCP3008 on SPI0 with CE0 as the chip select

    # create the spi bus

    spi = busio.SPI(clock=board.SCK, MISO=board.MISO, MOSI=board.MOSI)

    # create the cs (chip select)

    cs = digitalio.DigitalInOut(board.D24)

    # create the mcp object

    mcp = MCP.MCP3008(spi, cs)

    #gpio0 mcp = [MCP3008(channel=i) for i in range(3)]

    # define the channel pins

    channel_pins = [MCP.P0, MCP.P1, MCP.P2]

    # create an array of analog input channels

    channels = [AnalogIn(mcp, pin) for pin in channel_pins]

    # iterate over the channels

    for channel, chan in enumerate(channels):

        print(f'Raw ADC Value (Channel {channel}): ', chan.value)

        print(f'ADC Voltage (Channel {channel}): {chan.voltage}V')

        print()

    node = rclpy.create_node('joy_publisher')

    # create a Joy publisher

    joy_publisher = node.create_publisher(Joy, 'joy', 10)

    # set the publishing rate

    publish_rate = 1  # 1 Hz

    timer_period = 1.0 / publish_rate

    def publish_joy():

        # create a Joy message

        joy_msg = Joy()

        # iterate over the channels and add readings to the axes

        for channel, chan in enumerate(channels):

            joy_msg.axes.append(chan.value)

        # publish the Joy message

        joy_publisher.publish(joy_msg)

    timer = node.create_timer(timer_period, publish_joy)

    rclpy.spin(node)

    # shutdown when done

    node.destroy_node()

    rclpy.shutdown()

    print("DONE!")

if __name__ == '__main__':

    main()

This was the first input challenge. It took all day to get the RPI/Docker/ROS to cooperate. It looks like my twist is wired backward and it tampers with other readings when i bring it high.

DIGITAL:

• Joystick button
• GPIO 12
• D-Pad
• Up
• Down
• Left
• Right
• Center
• Rotary Encoder

import RPi.GPIO as GPIO

import rclpy

from rclpy.node import Node

from sensor_msgs.msg import Joy

from std_msgs.msg import Header

from builtin_interfaces.msg import Time

from adafruit_mcp3xxx.analog_in import AnalogIn

import adafruit_mcp3xxx.mcp3008 as MCP

import busio

import digitalio

import board

import time

class JoyPublisherNode(Node):

    def __init__(self):

        super().__init__('joy_publisher')

        # Set up MCP3008 on SPI0 with CE0 as the chip select

        spi = busio.SPI(clock=board.SCK, MISO=board.MISO, MOSI=board.MOSI)

        cs = digitalio.DigitalInOut(board.D24)

        mcp = MCP.MCP3008(spi, cs)

        # Define the channel pins

        channel_pins = [MCP.P0, MCP.P1, MCP.P2]

        self.channels = [AnalogIn(mcp, pin) for pin in channel_pins]

        # Define GPIO pins for buttons

        self.button_pins = [12, 26, 17, 27, 22, 4]

        GPIO.setmode(GPIO.BCM)

        GPIO.setup(self.button_pins, GPIO.IN, pull_up_down=GPIO.PUD_UP)

        # Define GPIO pins for rotary encoder

        self.encoder_pin_a = 19  # CW

        self.encoder_pin_b = 13  # DT - these might need to be switched

        GPIO.setup(self.encoder_pin_a, GPIO.IN, pull_up_down=GPIO.PUD_UP)

        GPIO.setup(self.encoder_pin_b, GPIO.IN, pull_up_down=GPIO.PUD_UP)

        GPIO.add_event_detect(self.encoder_pin_a, GPIO.BOTH, callback=self.encoder_callback)

        # Create a Joy publisher

        self.joy_publisher = self.create_publisher(Joy, 'joy', 10)

        # Set the publishing rate

        publish_rate = 1  # 1 Hz

        timer_period = 1.0 / publish_rate

        self.timer = self.create_timer(timer_period, self.publish_joy)

        self.last_encoder_state = GPIO.input(self.encoder_pin_a)

        self.encoder_counter = 0 # Initialize the encoder counter

    def publish_joy(self):

        # Create a Joy message

        joy_msg = Joy()

        joy_msg.header = Header()

        joy_msg.header.stamp = self.get_clock().now().to_msg()

        joy_msg.header.frame_id = 'base_link'

        # Read analog inputs

        joy_msg.axes.extend([chan.value for chan in self.channels])

        # Read button states

        joy_msg.buttons.extend(GPIO.input(button_pin) for button_pin in self.button_pins)

        # No rotation initially

        joy_msg.axes.append(self.encoder_counter)

        # Publish the Joy message

        self.joy_publisher.publish(joy_msg)

    def encoder_callback(self, channel):

        # Callback function for rotary encoder

        a_state = GPIO.input(self.encoder_pin_a)

        b_state = GPIO.input(self.encoder_pin_b)

        if a_state != self.last_encoder_state:

            if b_state != a_state:

                self.last_encoder_state = a_state

                # Counter-clockwise rotation

                self.publish_encoder_rotation(-1.0)

            else:

                self.last_encoder_state = a_state

                # Clockwise rotation

                self.publish_encoder_rotation(1.0)

    def publish_encoder_rotation(self, rotation):

        # Create a Joy message for rotary encoder

        joy_msg = Joy()

        joy_msg.header = Header()

        joy_msg.header.stamp = self.get_clock().now().to_msg()

        joy_msg.header.frame_id = 'base_link'

        # Read analog inputs

        joy_msg.axes.extend([chan.value for chan in self.channels])

        # Read button states

        joy_msg.buttons.extend(GPIO.input(button_pin) for button_pin in self.button_pins)

       # Update the encoder counter based on rotation direction

        if rotation > 0:

            self.encoder_counter += 15

        elif rotation < 0:

            self.encoder_counter -= 15

        # Set axes for rotary encoder

        joy_msg.axes.append(self.encoder_counter)

        # Publish the Joy message

        self.joy_publisher.publish(joy_msg)

def main():

    rclpy.init()

    joy_publisher_node = JoyPublisherNode()

    rclpy.spin(joy_publisher_node)

    joy_publisher_node.destroy_node()

    rclpy.shutdown()

if __name__ == '__main__':

    main()

I had problems with the twist joystick impacting the other potentiometers. This was because my particular stick had the colors off from what I expected.

As you can see, the resistance across the red-black was 5k, not 10k.

Below you can see, that I found the full resistance. I used red as 3.3V, white to ground, and black as the signal.

Setting up a second network adapter as a wifi access point for Pinky's onboard microcontrollers to connect to:

I plugged in a TP-link TL-WN725N v2 USB dongle. Some problems I ran into:

• It is coming up as WLAN0 after reboots, this is unwanted behavior. So let's make it WLAN1 and the onboard should be WLAN0. 

• Persistent Interface Naming

  If your built-in WiFi adapter is being labeled wlan1 instead of wlan0 and you want to ensure consistent naming, you can create a custom udev rule to assign the names based on the MAC address.

  1. Find the MAC Address:

     Identify the MAC address of your built-in WiFi adapter using the steps above.

  2. Create a udev Rule:

     Create a new udev rule file:

     bash
     sudo nano /etc/udev/rules.d/70-persistent-net.rules
     

     Add a rule to assign a specific name based on the MAC address. Replace xx:xx:xx:xx:xx:xx with the MAC address of your built-in WiFi adapter:

     plaintext
     SUBSYSTEM=="net", ACTION=="add", ATTR{address}=="xx:xx:xx:xx:xx:xx", NAME="wlan0"
     

     You can also add a rule for your USB adapter if needed:

     plaintext
     SUBSYSTEM=="net", ACTION=="add", ATTR{address}=="yy:yy:yy:yy:yy:yy", NAME="wlan1"
     

  3. Reload udev Rules and Reboot:

     Reload the udev rules and reboot the system:

     bash
     sudo udevadm control --reload-rules
     sudo reboot
     

  After rebooting, your built-in WiFi adapter should be consistently named wlan0.

• The desktop network manager is telling me that AP mode is not possible with the new dongle. I may have to switch to having the onboard be the AP and the new adapter be my internet connection.
• I switched to an edimax wifi dongle and then did this:
  

  Step-by-Step Setup

  1. Update and Install Required Packages:

     bash
     sudo apt-get update
     sudo apt-get upgrade
     sudo apt-get install hostapd dnsmasq iptables
     

  2. Configure Static IP for wlan1:

     Edit /etc/dhcpcd.conf:

     bash
     sudo nano /etc/dhcpcd.conf
     

     Add:

     plaintext
     interface wlan1
     static ip_address=192.168.4.1/24
     nohook wpa_supplicant
     

  3. Configure dnsmasq:

     Backup existing configuration and create a new one:

     bash
     sudo mv /etc/dnsmasq.conf /etc/dnsmasq.conf.orig
     sudo nano /etc/dnsmasq.conf
     

     Add:

     plaintext
     interface=wlan1
     dhcp-range=192.168.4.2,192.168.4.20,255.255.255.0,24h
     

  4. Configure hostapd:

     Create or edit the configuration file:

     bash
     sudo nano /etc/hostapd/hostapd.conf
     

     Add:

     plaintext
     interface=wlan1
     driver=nl80211
     ssid=MyNetwork
     hw_mode=g
     channel=7
     wmm_enabled=0
     macaddr_acl=0
     auth_algs=1
     ignore_broadcast_ssid=0
     wpa=2
     wpa_passphrase=MyPassword
     wpa_key_mgmt=WPA-PSK
     rsn_pairwise=CCMP
     

     Edit /etc/default/hostapd to set the configuration file path:

     plaintext
     DAEMON_CONF="/etc/hostapd/hostapd.conf"
     

  5. Enable and Start Services:

     bash
     sudo systemctl unmask hostapd
     sudo systemctl enable hostapd
     sudo systemctl start hostapd
     sudo systemctl restart dnsmasq
     

  6. Enable IP Forwarding:

     Edit /etc/sysctl.conf:

     bash
     sudo nano /etc/sysctl.conf
     

     Uncomment or add:

     plaintext
     net.ipv4.ip_forward=1
     

     Apply the changes:

     bash
     sudo sysctl -p
     

  7. Configure NAT with iptables:

     bash
     sudo iptables -t nat -A POSTROUTING -o wlan0 -j MASQUERADE
     sudo sh -c "iptables-save > /etc/iptables.ipv4.nat"
     

     Add to /etc/rc.local to restore iptables rules on boot:

     plaintext
     iptables-restore < /etc/iptables.ipv4.nat

• Now for testing with my ros2 in docker and micropython on my microcontroller...
• Build micro-ros for this install
• I can see my ESP32 connected with: 
  • cat /var/lib/misc/dnsmasq.leases
• I can see packets over port 8888 with:
• sudo tcpdump -v -i wlan1 udp port 8888
• ESP32 devkit and esp32CAM enter error loop after the memory allocator is initialized.
• Find port conflicts with netstat and lsof
• SOLVED:The micropython agent on the controller must be started before the microcontrollers are powered on. Otherwise, they will go into an error loop when initializing support.
• Getting display to work from a container
• export DISPLAY=:0