Intro

Implemented a Python ROS node to process PointCloud2 data from a forward-facing sonar and republish in a suitable format for web-based visualization. The node subscribes to the raw sonar Point Cloud, extracts the (x, y) coordinates, and publishes them on a separate topic.

On the frontend, roslib.js is used to establish a WebSocket connection to the ROS backend via the Rosbridge server, allowing real-time data streaming directly into the browser. Plotly.js is used to render and visualize the sonar returns within the GUI.

V1

onboard_forward_sonar.py

import rclpy # Python library for ROS 2
from rclpy.node import Node # Handles the creation of nodes
from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy # sets quality of service for nodes
from sensor_msgs.msg import PointCloud2 # PointCloud2 is the message type
import cv2 # OpenCV library
from cv_bridge import CvBridge # Package to convert between ROS and OpenCV Images
from std_msgs.msg import Float32MultiArray
import numpy as np
 
# TODO: come up with a better name for this
class OnboardForwardSonar(Node):
  
    # Create an ImageSubscriber class, which is a subclass of the Node class.
    def __init__(self):
        
        # Initiate the Node class's constructor and give it a name
        super().__init__('onboard_forward_sonar')
 
        # Cameras require quality of service to be set to best effort so they don't 
        # build up a huge backlog of frames to process and get too far behind
        qos_profile = QoSProfile(
                          reliability=QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT,
                          history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST,
                          depth=1
                      )
 
 
        # Create the subscriber. This subscriber will receive the point cloud
        # from the forward_sonar topic. The queue size is 10 messages.
        self.subscription = self.create_subscription(
          PointCloud2, 
          '/nekton/sonar_forward',
          self.listener_callback, 
          qos_profile=qos_profile
        )
        
        
        # Create publisher
        self.p = self.create_publisher(
            Float32MultiArray,
            '/nekton/sonar_forward/sonar_points',
            qos_profile=qos_profile
            )
   
    def listener_callback(self, data):
        # Display the message on the console
        points = data.width # number of points in the pointcloud
        self.get_logger().info('Receiving sonar frame with ' + str(points) + ' points')
 
        # Cast bytes to numpy array
        points = np.ndarray(
            shape=(cloud.width * cloud.height, ),
            dtype=dtype_from_fields(cloud.fields, point_step=cloud.point_step),
            buffer=cloud.data)
 
        my_msg = Float32MultiArray()
        for p in points:
            my_msg.data.append(p[0])
            my_msg.data.append(p[1])
 
        self.p.publish(my_msg)
 
  
    
 
def main(args=None):
  
  # Initialize the rclpy library
  rclpy.init(args=args)
  
  # Create the node
  ForwardSonarProcessor = OnboardForwardSonar()
  
  # Spin the node so the callback function is called.
  rclpy.spin(ForwardSonarProcessor)
  
  # Destroy the node explicitly
  # (optional - otherwise it will be done automatically
  # when the garbage collector destroys the node object)
  ForwardSonarProcessor.destroy_node()
  
  # Shutdown the ROS client library for Python
  rclpy.shutdown()
  
if __name__ == '__main__':
  main()

index.html

<!DOCTYPE html>
<html>
  <head>
    <meta charset="utf-8" />
    <link rel="stylesheet" href="styles.css" />
    <script src='https://cdn.plot.ly/plotly-2.16.1.min.js'>
    <script
      type="text/javascript"
      src="http://static.robotwebtools.org/EventEmitter2/current/eventemitter2.min.js"></script>
    <script
      type="text/javascript"
      src="http://static.robotwebtools.org/roslibjs/current/roslib.min.js"></script>
    <script 
      type="text/javascript"
      src="https://static.robotwebtools.org/roslibjs/current/roslib.js"></script>
    <script src="https://static.robotwebtools.org/threejs/current/three.js"></script>
    <script 
      type="text/javascript"
      src="https://cdn.jsdelivr.net/npm/ros3d@1/build/ros3d.min.js"></script>  
  </head>
  <!-- <body onload="init()"> -->
  <body >
    <div class="center">
      <div class="box">
        <h1>Nekton GUI roslib Example</h1>
        <p>Check your web console for an output.</p>
        <hr />
        <div class="btn-group">
          <button id="pause_physics" type="button" onclick="pausePhysics()">
            Pause Physics
          </button>
          <button id="unpause_physics" type="button" onclick="unpausePhysics()">
            Unpause Physics
          </button>
        </div>
        <img id="my_image" style='height: 100%; width: 100%; object-fit: contain' src="hzttps://user-images.githubusercontent.com/33184844/207543214-2cd65dfa-8473-4bc8-94ac-f6c2d934cd48.png">
      </div>
    </div>
    <div class="center"id='myDiv'>
    <!-- Plotly chart will be drawn inside this DIV -->
    </div>
    <script type="text/javascript" src="index.js"></script>
  <!--     </div>  
    <div class="center">
      <h1>Simple PointCloud2 Example</h1>
      <p>Run the following commands in the terminal then refresh the page.</p>
      <ol>
        <li><tt>roscore</tt></li>
        <li><tt>roslaunch rosbridge_server rosbridge_websocket.launch</tt></li>
        <li><tt>rosrun tf2_web_republisher tf2_web_republisher</tt></li>
        <li><tt>roslaunch openni_launch openni.launch depth_registration:=true</tt></li>
      </ol>
      <div id="viewer"></div>  
    </div> -->  
  </body>
</html>

index.js

/**
* Setup all visualization elements when the page is loaded.
*/
var ros = new ROSLIB.Ros({
  url : 'ws://localhost:9090'
});
 
 
ros.on("connection", function () {
  console.log("Connected to websocket server.");
});
 
ros.on("error", function (error) {
  console.log("Error connecting to websocket server: ", error);
});
 
ros.on("close", function () {
  console.log("Connection to websocket server closed.");
});
 
// Subscribing to a Topic
var listener = new ROSLIB.Topic({
  ros: ros,
  name: "/listener",
  messageType: "std_msgs/String",
});
 
listener.subscribe(function (message) {
  console.log("Received message on " + listener.name + ": " + message.data);
  listener.unsubscribe();
});
 
// Creating a Service
var pausePhysicsService = new ROSLIB.Service({
  ros: ros,
  name: "/pause_physics",
  serviceType: "std_srvs/srv/Empty",
});
 
var unpausePhysicsService = new ROSLIB.Service({
  ros: ros,
  name: "/unpause_physics",
  serviceType: "std_srvs/srv/Empty",
});
 
// Function that calls a Service
function pausePhysics() {
  pausePhysicsService.callService(new ROSLIB.ServiceRequest({}));
  console.log('Physics is now paused.')
}
 
function unpausePhysics() {
  unpausePhysicsService.callService(new ROSLIB.ServiceRequest({}));
  console.log('Physics is now unpaused.')
}
 
// Set up Plotly chart for sonar point cloud
let x_coords = [];
let y_coords = [];
 
var trace1 = {
  type: "pointcloud",
  mode: "markers",
  marker: {
	sizemin: 0.5,
	sizemax: 45,
	arearatio: 0,
	color: "rgba(0, 0, 255, 0.8)"
  },
   x: x_coords,
   y: y_coords
}
 
var data = [trace1];
 
var layout = {
		plot_bgcolor:"black",
		paper_bgcolor:"black",
	title: {
	  text: 'Forward Sonar Point Cloud',
	  font: {
		family: 'Courier New, monospace',
		size: 30,
		color: 'Red'
	  }
	},
  xaxis: {
	type: "linear",
	range: [
	  0,
	  91],
	autorange: false
  },
  yaxis: {
	type: "linear",
	range: [0.131,0.146],
	autorange: false
  },
  height: 598,
  width: 1080,
  autosize: true,
  showlegend: false
}
 
Plotly.newPlot('myDiv', data, layout);
 
// Subscribing to parsed point cloud topic
var sonar_xy = new ROSLIB.Topic({
  ros: ros,
  name: "/onboard/test_topic",
  messageType: "std_msgs/msg/Float64MultiArray",
});
 
sonar_xy.subscribe(function (message) {
  console.log('sonar_xy');
  console.log(message.data);
  let x_coords = [];
  let y_coords = [];
  
  for (let i = 0; i < message.data.length; i++){
	  // console.log(message.data[i]);
	  if (i % 2 === 0){
		  x_coords.push(message.data[i]);
	  } else {
		  y_coords.push(message.data[i]);
	  }
  }
		
  var data_update = {
	x: [x_coords],
	y: [y_coords]
  };
  
  console.log(data_update);
  Plotly.update('myDiv', data_update);
  // sonar_xy.unsubscribe();
});

style.css

h1 {
  text-align: center;
  padding-left: 20px;
  padding-right: 20px;
  margin-bottom: 0px;
}
 
body {
  background-color: cadetblue;
}
 
h2 {
  margin-bottom: 0px;
}
 
.center {
  margin: auto;
  width: fit-content;
  padding: 10px;
}
 
.box {
  text-align: center;
  width: fit-content;
  margin-left: 10px;
  margin-top: 10px;
  padding: 5px;
  background-color: white;
  border-radius: 15px;
}
 
.btn-group button {
  background-color: grey;
  border: 1px solid grey;
  color: white;
  padding: 10px 24px;
  cursor: pointer;
  float: center;
  border-radius: 10px;
}
 
.btn-group:after {
  content: "";
  clear: both;
  display: table;
}
 
.btn-group button:not(:last-child) {
  border-right: none;
}
 
.btn-group button:hover {
  background-color: lightgray;
}

V2

onboard_forward_sonar.py v2

import rclpy # Python library for ROS 2
from rclpy.node import Node # Handles the creation of nodes
from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy # sets quality of service for nodes
from sensor_msgs.msg import PointCloud2, PointField # PointCloud2 is the message type
import cv2 # OpenCV library
from cv_bridge import CvBridge # Package to convert between ROS and OpenCV Images
from std_msgs.msg import Float32MultiArray
import numpy as np
import ctypes
import math
import struct
 
_DATATYPES = {}
_DATATYPES[PointField.INT8]    = ('b', 1)
_DATATYPES[PointField.UINT8]   = ('B', 1)
_DATATYPES[PointField.INT16]   = ('h', 2)
_DATATYPES[PointField.UINT16]  = ('H', 2)
_DATATYPES[PointField.INT32]   = ('i', 4)
_DATATYPES[PointField.UINT32]  = ('I', 4)
_DATATYPES[PointField.FLOAT32] = ('f', 4)
_DATATYPES[PointField.FLOAT64] = ('d', 8)
 
# TODO: come up with a better name for this
class OnboardForwardSonar(Node):
  
    # Create an ImageSubscriber class, which is a subclass of the Node class.
    def __init__(self):
        # Initiate the Node class's constructor and give it a name
        super().__init__('onboard_forward_sonar')
 
        # Cameras require quality of service to be set to best effort so they don't 
        # build up a huge backlog of frames to process and get too far behind
        qos_profile = QoSProfile(
                          reliability=QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT,
                          history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST,
                          depth=1
                      )
 
 
        # Create the subscriber. This subscriber will receive the point cloud
        # from the forward_sonar topic. The queue size is 10 messages.
        self.subscription = self.create_subscription(
          PointCloud2, 
          '/nekton/sonar_forward',
          self.listener_callback, 
          qos_profile=qos_profile
        )
        
        
        # Create publisher
        self.p = self.create_publisher(
            Float32MultiArray,
            '/nekton/sonar_forward/sonar_points',
            qos_profile=qos_profile
            )
   
    def _get_struct_fmt(self, is_bigendian, fields, field_names=None):
        fmt = '>' if is_bigendian else '<'
        offset = 0
        for field in (f for f in sorted(fields, key=lambda f: f.offset) if field_names is None or f.name in field_names):
            if offset < field.offset:
                fmt += 'x' * (field.offset - offset)
                offset = field.offset
            if field.datatype not in _DATATYPES:
                print('Skipping unknown PointField datatype [%d]' % field.datatype, file=sys.stderr)
            else:
                datatype_fmt, datatype_length = _DATATYPES[field.datatype]
                fmt    += field.count * datatype_fmt
                offset += field.count * datatype_length
        return fmt
 
    def read_points(self, cloud, field_names=None, skip_nans = True):
        fmt = self._get_struct_fmt(cloud.is_bigendian, cloud.fields, field_names)
        unpack_from = struct.Struct(fmt).unpack_from
width, height, point_step, row_step, data, isnan = cloud.width, cloud.height, cloud.point_step, cloud.row_step, cloud.data, math.isnan
        my_msg = Float32MultiArray()
        if skip_nans:
            for v in range(height):
                offset = row_step * v
                for u in range(width):
                    p = unpack_from(data, offset)
                    has_nan = False
                    for pv in p:
                        if isnan(pv):
                            has_nan = True
                            break
                    if not has_nan:
                        for v in p:
                            my_msg.data.append(v)
                    offset += point_step
        else:
            for v in range(height):
                offset = row_step * v
                for u in range(width):
                    p = unpack_from(data, offset)
                    for v in p:
                        my_msg.data.append(v)
                    offset += point_step
        return my_msg
 
    def listener_callback(self, cloud):
        msg = self.read_points(cloud, field_names = ['x', 'y', 'z', 'intensity'])
        self.p.publish(msg)
 
def main(args=None):
 
    # Initialize the rclpy library
    rclpy.init(args=args)
    
    # Create the node
    ForwardSonarProcessor = OnboardForwardSonar()
    
    # Spin the node so the callback function is called.
    rclpy.spin(ForwardSonarProcessor)
    
    # Destroy the node explicitly
    # (optional - otherwise it will be done automatically
    # when the garbage collector destroys the node object)
    ForwardSonarProcessor.destroy_node()
    
    # Shutdown the ROS client library for Python
    rclpy.shutdown()
    
    if __name__ == '__main__':
        main()

index.js v2

// Connecting to ROS
var ros = new ROSLIB.Ros({
  url: "ws://localhost:9090",
});
 
ros.on("connection", function () {
  console.log("Connected to websocket server.");
});
 
ros.on("error", function (error) {
  console.log("Error connecting to websocket server: ", error);
});
 
ros.on("close", function () {
  console.log("Connection to websocket server closed.");
});
 
// Subscribing to a Topic
var listener = new ROSLIB.Topic({
  ros: ros,
  name: "/nekton/pressure",
  messageType: "sensor_msgs/FluidPressure",
});
 
listener.subscribe(function (message) {
    const x = message.fluid_pressure / 10051.8; //not sure which formula is correct
    // const x = message.fluid_pressure / (1000 * 9.81);
    const myElement = document.getElementById('my-element');
    myElement.innerHTML = "Depth: " + x;
    // console.log("Received message on " + x);
  // listener.unsubscribe();
});
 
// Subscribe to sonar topic
var sonar_listener = new ROSLIB.Topic({
  ros: ros,
  name: "/nekton/sonar_forward/sonar_points",
  messageType: "std_msgs/Float32MultiArray",
});
 
sonar_listener.subscribe(function (sonar_message) {
  // sonar_message holds x,y,z,i values in the following format:
  // [x0, y0, z0, i0, x1, y1, z1, i1, x2, y2, z2, i2 ..... xn, yn, zn, in], where i = intensity
  
  x = []
  y= []
  z = []
  intensity = []
 
  for(i = 0; i < sonar_message.data.length; i++) {
    if (i%4 == 0) {
      x.push(sonar_message.data[i])
    } else if (i%4 == 1) {
      y.push(sonar_message.data[i])
    } else if (i%4 == 2){
      z.push(sonar_message.data[i])
    } else {
      intensity.push(sonar_message.data[i])
    }
  }
 
  var trace1 = {
    type: "pointcloud",
    mode: "markers",
    marker: {
      sizemin: 0.5,
      sizemax: 35,
      arearatio: 0,
      color: "rgba(0, 255 0, 0.8)"
    },
    x:x,
    y:y
  }
  
  var data = [trace1];
  
  var layout = {
      plot_bgcolor:"black",
      paper_bgcolor:"black",
      title: {
        text: 'Forward Sonar Point Cloud',
        font: {
          family: 'Courier New, monospace',
          size: 30,
          color: 'Red'
        }
      },
    xaxis: {
      type: "linear",
      range: [
        -2.501411175139456, 4],
      autorange: true
    },
    yaxis: {
      type: "linear",
      range: [-1.5,.25],
      autorange: true
    },
    height: 598,
    width: 1080,
    autosize: true,
    showlegend: false
  }
  Plotly.newPlot('myDiv', data, layout);
}); 
 
// Subscribe to image_raw topic
var image_listener = new ROSLIB.Topic( {
  ros: ros,
  name: "/nekton/camera_forward/image_raw",
  messageType: "sensor_msgs/msg/Image",
});
 
//create canvas
const can = document.createElement("canvas");
 
const ctx = can.getContext("2d");
 
image_listener.subscribe(function (image_message) {
  // console.log("Received image message on " + image_listener.name + ": " + image_message.width + " "  + image_message.height);
  // console.log("is_bigendian: " + image_message.is_bigendian);
    
  //creating a canvas of correct size and obtaining a CanvasRenderingContext2D
  can.width = image_message.width;
  can.height = image_message.height; 
 
  //create an image buffer to hold the pixels
  const imgData = ctx.createImageData(image_message.width, image_message.height);
  const data = imgData.data;
  const inData = atob(image_message.data);
 
  var j = 0; i = 4; // j data in , i data out
  while( j < inData.length) {
      const w1 = inData.charCodeAt(j++);  // read 3 16 bit words represent 1 pixel
      const w2 = inData.charCodeAt(j++);
      const w3 = inData.charCodeAt(j++);
      if (!image_message.is_bigendian) {
          data[i++] = w1; // red
          data[i++] = w2; // green
          data[i++] = w3; // blue
      } else {
          data[i++] = (w1 >> 8) + ((w1 & 0xFF) << 8);
          data[i++] = (w2 >> 8) + ((w2 & 0xFF) << 8);
          data[i++] = (w3 >> 8) + ((w3 & 0xFF) << 8);
      }
      data[i++] = 255;  // alpha
  }
 
  //put pixel data into the canvas
    ctx.putImageData(imgData, 0, 0);
 
  //add canvas to the HTML
  document.body.appendChild(can);
 
})
 
// Creating a Service
var pausePhysicsService = new ROSLIB.Service({
  ros: ros,
  name: "/pause_physics",
  serviceType: "std_srvs/srv/Empty",
});
 
var unpausePhysicsService = new ROSLIB.Service({
  ros: ros,
  name: "/unpause_physics",
  serviceType: "std_srvs/srv/Empty",
});
 
var cmdVel = new ROSLIB.Topic({
  ros : ros,
  name : '/nekton/cmd_vel',
  messageType : 'geometry_msgs/Twist'
});
 
//Initializes the Keyboard Map. This way, NaN is not possible with the controls needed.
let keyboard = {w: 0, a: 0, s: 0, d: 0, q: 0, e: 0, ArrowUp: 0, ArrowDown: 0, ArrowRight: 0, ArrowLeft: 0, z: 0, c: 0};
//Feel Free to edit
let velocityMod = 14;
let twistMsg;
 
/*
NOTE: Maybe filter Twist Messages checking for Duplicates. 
Could affect performance but I am unsure atm.
*/
document.addEventListener('keydown', async (e) => {
  /*
  TODO: Add Control to modify velocity by some scalar.
  Was planning on using scroll wheel, but another option is
  checking if shift is down on a key press, then modify.
  */
  keyboard[e.key] = 1.0 + velocityMod;
  twistMsg = await generateTwist();
  //DEBUGGING! Feel Free to Remove
  console.log(twistMsg);
  cmdVel.publish(twistMsg);
})
 
//Resets Value to Zero on key up
document.addEventListener('keyup', async (e) => {
  keyboard[e.key] = 0.0;
  twistMsg = await generateTwist();
  //DEBUGGING! Feel Free to Remove
  console.log(twistMsg);
  cmdVel.publish(twistMsg);
})
 
 
// Function that calls a Service
function pausePhysics() {
  pausePhysicsService.callService(new ROSLIB.ServiceRequest({}));
  console.log('Physics is now paused.')
}
 
function unpausePhysics() {
  unpausePhysicsService.callService(new ROSLIB.ServiceRequest({}));
  console.log('Physics is now unpaused.')
}
 
//Function to get the current control values
const generateTwist = async () => {
  return (new ROSLIB.Message({
    linear : {
      x : keyboard.d - keyboard.a,
      y : keyboard.w - keyboard.s, 
      z : keyboard.q - keyboard.e
    },
    angular : {
      x : keyboard.ArrowUp - keyboard.ArrowDown,
      y : keyboard.c - keyboard.z,
      z : keyboard.ArrowRight - keyboard.ArrowLeft,
    }
  }));
}

V3

Nekton React GUI

🧗‍♂️Random Restart

Explorer

Recent Notes

Joy Package

Power

Computational Geometry

Nekton GUI

Intro

V1

V2

V3

Graph View

Table of Contents

Backlinks