Skip to content

Instantly share code, notes, and snippets.

@JEnoch
Last active December 13, 2022 21:19
Show Gist options
  • Save JEnoch/560f9b2b9ab4243e20e9fcf5bfa764ed to your computer and use it in GitHub Desktop.
Save JEnoch/560f9b2b9ab4243e20e9fcf5bfa764ed to your computer and use it in GitHub Desktop.
Webinar 06/12/2022: ROS2 Robot-to-Anything with Zenoh

Turtlebot3 simulation demo

Setup

On a Ubuntu VM in your prefered cloud:

  • zenohd:
    • wget https://download.eclipse.org/zenoh/zenoh/0.6.0-beta.1/x86_64-unknown-linux-gnu/zenoh-0.6.0-beta.1-x86_64-unknown-linux-gnu.zip
    • unzip zenoh-0.6.0-beta.1-x86_64-unknown-linux-gnu.zip
  • Optional (to serve the HTML page):
    • sudo apt install apache2
    • copy index.html (from this Gist) into /var/www/html/ and edit it to replace demo.zenoh.io with your VM's IP address

On localhost:

  • Ubuntu with ROS2 (pick your distro!) :
  • Turtlebot 3 simulation (https://emanual.robotis.com/docs/en/platform/turtlebot3/simulation/):
    • sudo apt install "ros-${ROS_DISTRO}-turtbot3-*"
      mkdir -p ~/turtlebot3_ws/src/
      cd ~/turtlebot3_ws/src/
      git clone -b ${ROS_DISTRO}-devel https://github.com/ROBOTIS-GIT/turtlebot3_simulations.git
      cd ~/turtlebot3_ws && colcon build --symlink-install 
  • zenoh-bridge-dds:
    • cd && wget https://download.eclipse.org/zenoh/zenoh-plugin-dds/0.6.0-beta.1/x86_64-unknown-linux-gnu/zenoh-bridge-dds-0.6.0-beta.1-x86_64-unknown-linux-gnu.zip
    • unzip zenoh-bridge-dds-0.6.0-beta.1-x86_64-unknown-linux-gnu.zip
    • copy bridge-config.json5 (from this Gist) to ~/ and edit it to replace demo.zenoh.io with your VM's IP address

Run

On VM in cloud:

  • Start the Zenoh router (default configuration):
    ./zenohd

On localhost:

  • Start Turtlebot3 simulation:
    cd ~/turtlebot3_ws && ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py
  • Start zenoh-bridge-dds:
    ~/zenoh-bridge-dds -c ~/bridge-config.json5

In a web broswer:

  • Go to http://<your-VM-IP>

Adding a REAL Turtlebot3

Setup

On VM in cloud:

  • Install the WebServer Zenoh plugin (to server the camera stream):
    wget https://download.eclipse.org/zenoh/zenoh-plugin-webserver/0.6.0-beta.1/x86_64-unknown-linux-gnu/zenoh-plugin-webserver-0.6.0-beta.1-x86_64-unknown-linux-gnu.zip
    unzip zenoh-plugin-webserver-0.6.0-beta.1-x86_64-unknown-linux-gnu.zip
  • copy zenohd-config.json5 (from this Gist) to ~/ and edit it to replace demo.zenoh.io with your VM's IP address

On REAL Turtlebot3:

Run

On VM in cloud:

  • Start the Zenoh router (with REST + WebServer plugins):
    ./zenohd -c ./zenohd-config.json5

On REAL Turtlebot3:

  • Start Turtlebot3:
    ros2 launch turtlebot3_bringup robot.launch.py
  • Start zenoh-bridge-dds:
    ~/zenoh-bridge-dds -c ~/bridge-config.json5
  • Start zcapture:
    zcapture -e tcp/<your-VM-IP>:7447 -k bot1/cams/0 -d 100 -r 400

In a web broswer:

  • Go to http://<your-VM-IP>
  • change "Subscriptions scope" to bot1 and click "Reset subscriptions"
  • change "Pub Twists to" to bot1/rt/cmd_vel

Adding a Zigbee/MQTT to Zenoh bridge

On a RaspberryPi with a Zigbee USB key (e.g. Conbee 2)

  • Install zigbee2mqtt:
    https://www.zigbee2mqtt.io/
  • Clone and build zenoh-bridge-mqtt:
    https://github.com/eclipse-zenoh/zenoh-plugin-mqtt
  • Copy mqtt-bridge-config.json5 (from this Gist) to ~/ and edit it to replace demo.zenoh.io with your VM's IP address
  • Run zenoh-bridge-mqtt that acts as a MQTT broker:
    zenoh-bridge-mqtt -c ~/mqtt-bridge-config.json5
  • Run zigbee2mqtt that will by default connect to zenoh-bridge-mqtt (it uses mqtt://localhost as server)
  • Associate in zigbee2mqtt frontend a Zigbee button that you name devide/button

On localhost (where Turtlebot3 simulation and zenoh-bridge-dds are still runnung):

  • Install the Zenoh python API if you don't already have:
    pip install eclipse-zenoh==0.6.0b1
  • Copy zigbee-ros2-teleop.py (from this Gist) to ~/ and edit it to replace demo.zenoh.io with your VM's IP address
  • Run:
    python3 ./zigbee-ros2-teleop.py
  • Push the Zigbee button and see logs from zigbee-ros2-teleop.py and the Turtlebot3 simulation to spin
//
// Configuration for zenoh-bridge-dds running on localhost
//
{
plugins: {
dds: {
// allow only necessary topics
allow: "/cmd_vel|/scan|/battery_state",
// configure maximum frequency of retransmission over Zenoh
max_frequencies: ["/scan=5", "/battery_state=0.2"],
// set a scope and configure Zenoh discovery to be minimal (only 2 messages)
scope: "simu",
generalise_subs: ["simu/**"],
generalise_pubs: ["simu/**"],
// forward_discovery: true,
},
// Activate the REST API
rest: {
http_port: 8000,
},
},
// Connect to a remote Zenoh router
// (or to another bridge if it's configured to listen)
connect: {
endpoints: [ "tcp/demo.zenoh.io:7447" ]
},
}
<!DOCTYPE html>
<html>
<head>
<link rel="stylesheet" href="https://cdnjs.cloudflare.com/ajax/libs/font-awesome/6.1.1/css/all.min.css" />
<link rel="stylesheet" href="https://www.w3schools.com/w3css/4/w3.css">
<link rel="stylesheet" href="https://uicdn.toast.com/chart/latest/toastui-chart.min.css" />
<script src="https://uicdn.toast.com/chart/latest/toastui-chart.min.js"></script>
<meta name="viewport" content="width=device-width, initial-scale=1.0">
</head>
<body class="w3-container">
<!-- Title bar -->
<div class="w3-container w3-bar w3-card-4 w3-green w3-margin-bottom">
<h3 class="w3-bar-item">Zenoh teleop</h3>
<!-- Battery percentage -->
<h3 id="battery" class="w3-bar-item w3-right">Battery: - %</h3>
</div>
<div class="w3-row-padding">
<!-- Zenoh REST API URL input -->
<div class="w3-third">
<label for="zenoh_rest_url">Zenoh REST URL</label>
<input id="zenoh_rest_url" class="w3-input w3-border w3-padding-small" type="text" required
value="http://demo.zenoh.iò:8000/">
</div>
<!-- Subscriptions scope input -->
<div class="w3-third">
<label>Subscriptions scope</label>
<input id="sub_scope" class="w3-input w3-border w3-padding-small" type="text" value="simu">
</div>
<div class="w3-third">
<label>.</label>
<input type="submit" class="w3-btn w3-input w3-green w3-border w3-padding-small" value="Reset subscriptions"
onclick="resetAllSubscriptions(); return false"><br />
</div>
</div>
<!-- Camera panel -->
<div class="w3-card-4 w3-margin-bottom">
<header class="w3-bar w3-green" onclick="document.getElementById('Camera').classList.toggle('w3-hide');">
<h5 id="camera_title" class="w3-bar-item" style="margin: 0;">Camera</h5>
<h5 class="w3-bar-item w3-right" style="margin: 0;"><i class='fa fa-video-camera'></i></h5>
</header>
<div id="Camera" class="w3-container w3-padding">
<img id="camera_img" class="w3-container w3-padding" style="max-width: 95%; max-height: 300px;" src="" />
</div>
<br />
</div>
<!-- Teleop panel -->
<div class="w3-card-4 w3-margin-bottom">
<header class="w3-bar w3-green" onclick="document.getElementById('Drive').classList.toggle('w3-hide');">
<h5 class="w3-bar-item" style="margin: 0;">Drive</h5>
<h5 class="w3-bar-item w3-right" style="margin: 0;"><i class='fa fa-gamepad'></i></h5>
</header>
<div id="Drive" class="w3-container w3-padding">
<div class="w3-container w3-cell">
<label for="twist_keyexpr">Pub Twists to:</label>
</div>
<div class="w3-container w3-cell">
<input id="twist_keyexpr" class="w3-input w3-border w3-padding-small" type="text"
value="simu/rt/cmd_vel">
</div>
<div class="w3-auto" style="display: grid; width:12em; height:9em; ">
<button onmousedown="pubTwist(1.0, 0.0);" onmouseup="pubTwist(0.0, 0.0);"
ontouchstart="pubTwist(1.0, 0.0);" ontouchend="pubTwist(0.0, 0.0);"
style='font-size:2em; grid-column: 2; grid-row: 1;'>
<i class='fas fa-caret-up'></i>
</button>
<button onmousedown="pubTwist(0.0, 1.0);" onmouseup="pubTwist(0.0, 0.0);"
ontouchstart="pubTwist(0.0, 1.0);" ontouchend="pubTwist(0.0, 0.0);"
style='font-size:2em; grid-column: 1; grid-row: 2;'>
<i class='fas fa-caret-left'></i>
</button>
<button onmousedown="pubTwist(-1.0, 0.0);" onmouseup="pubTwist(0.0, 0.0);"
ontouchstart="pubTwist(-1.0, 0.0);" ontouchend="pubTwist(0.0, 0.0);"
style='font-size:2em; grid-column: 2; grid-row: 2;'>
<i class='fas fa-caret-down'></i>
</button>
<button onmousedown="pubTwist(0.0, -1.0);" onmouseup="pubTwist(0.0, 0.0);"
ontouchstart="pubTwist(0.0, -1.0);" ontouchend="pubTwist(0.0, 0.0);"
style='font-size:2em; grid-column: 3; grid-row: 2;'>
<i class='fas fa-caret-right'></i>
</button>
<button onclick="pubTwist(0.0, 0.0);" style='font-size:2em; grid-column: 1 / 4; grid-row: 3;'>
STOP
</button>
</div>
<br />
</div>
</div>
<!-- Lidar panel -->
<div class="w3-card-4 w3-margin-bottom">
<header class="w3-bar w3-green" onclick="document.getElementById('Lidar').classList.toggle('w3-hide');">
<h5 id="lidar_title" class="w3-bar-item" style="margin: 0;">Lidar</h5>
<h5 class="w3-bar-item w3-right" style="margin: 0;"><i class='fa fa-line-chart'></i></h5>
</header>
<div id="Lidar" class="w3-container w3-padding" style="width: 95%; height: 300px;">
</div>
<br />
</div>
<!-- rosout logs panel -->
<div class="w3-card-4 w3-margin-bottom">
<header class="w3-bar w3-green" onclick="document.getElementById('Logs').classList.toggle('w3-hide');">
<h5 id="rosout_title" class="w3-bar-item" style="margin: 0;">Logs</h5>
<h5 class="w3-bar-item w3-right" style="margin: 0;"><i class='fa fa-envelope'></i></h5>
</header>
<div id="Logs" class="w3-container w3-padding">
<div style="overflow:auto; height:200px; border:1px solid black;" id="rosout_logs"></div>
</div>
</div>
<!-- Config panel -->
<div class="w3-card-4 w3-margin-bottom">
<header class="w3-bar w3-green" onclick="document.getElementById('Config').classList.toggle('w3-hide');">
<h5 class="w3-bar-item" style="margin: 0;">Config</h5>
<h5 class="w3-bar-item w3-right" style="margin: 0;"><i class='fa fa-gear'></i></h5>
</header>
<form id="Config" class="w3-container w3-hide">
<label for="linear_scale">Twists linear scale:</label>
<input id="linear_scale" class="w3-input w3-border w3-padding-small" type="number" required
value="0.12"><br />
<label for="angular_scale">Twists angular scale:</label>
<input id="angular_scale" class="w3-input w3-border w3-padding-small" type="number" required
value="1.0"><br />
<label for="scan_res">Subscribe to LaserScan from:</label>
<input id="scan_res" class="w3-input w3-border w3-padding-small" type="text" required value="rt/scan"><br />
<label for="rosout_res">Subscribe to Log messages from:</label>
<input id="rosout_res" class="w3-input w3-border w3-padding-small" type="text" required
value="rt/rosout"><br />
<input type="submit" class="w3-btn w3-green" value="Reset subscriptions"
onclick="resetAllSubscriptions(); return false"><br />
<br />
</form>
</div>
<script src="https://cdn.jsdelivr.net/npm/bytebuffer@5.0.1"></script>
<script src="https://cdn.jsdelivr.net/npm/jscdr@0.0.1"></script>
<script>
// ROS2 Time type
class Time {
constructor(sec, nsec) {
this.sec = sec;
this.nsec = nsec;
}
static decode(cdrReader) {
let sec = cdrReader.readInt32();
let nsec = cdrReader.readUint32();
return new Time(sec, nsec);
}
}
// ROS2 Log type (received in 'rosout' topic)
class Log {
constructor(time, level, name, msg, file, fn, line) {
this.time = time;
this.level = level;
this.name = name;
this.msg = msg;
this.file = file;
this.fn = fn;
this.line = line;
}
static decode(cdrReader) {
let time = Time.decode(cdrReader);
let level = cdrReader.readByte();
let name = cdrReader.readString();
let msg = cdrReader.readString();
let file = cdrReader.readString();
let fn = cdrReader.readString();
let line = cdrReader.readUint32();
return new Log(time, level, name, msg, file, fn, line);
}
}
// ROS2 Vector3 type
class Vector3 {
constructor(x, y, z) {
this.x = x;
this.y = y;
this.z = z;
}
encode(cdrWriter) {
cdrWriter.writeFloat64(this.x);
cdrWriter.writeFloat64(this.y);
cdrWriter.writeFloat64(this.z);
}
static decode(cdrReader) {
let x = cdrReader.readFloat64();
let y = cdrReader.readFloat64();
let z = cdrReader.readFloat64();
return new Vector3(x, y, z);
}
}
// ROS2 Quaternion type
class Quaternion {
constructor(x, y, z, w) {
this.x = x;
this.y = y;
this.z = z;
this.w = w;
}
static decode(cdrReader) {
let x = cdrReader.readFloat64();
let y = cdrReader.readFloat64();
let z = cdrReader.readFloat64();
let w = cdrReader.readFloat64();
return new Quaternion(x, y, z, w);
}
}
// ROS2 Twist type (published in 'cmd_vel' topic)
class Twist {
constructor(linear, angular) {
this.linear = linear;
this.angular = angular;
}
encode(cdrWriter) {
this.linear.encode(cdrWriter);
this.angular.encode(cdrWriter);
}
}
// ROS2 Header type
class Header {
constructor(time, frame_id) {
this.time = time;
this.frame_id = frame_id;
}
static decode(cdrReader) {
let time = Time.decode(cdrReader);
let frame_id = cdrReader.readString();
}
}
// ROS2 BatteryState type (received in 'battery_state' topic)
// Warning: not complete, since we only need to decode up-to 'percentage'
class BatteryState {
constructor(header, voltage, temperature, current, charge, capacity, design_capacity, percentage) {
this.header = header;
this.voltage = voltage;
this.temperature = temperature;
this.current = current;
this.charge = charge;
this.capacity = capacity;
this.design_capacity = design_capacity;
this.percentage = percentage;
}
static decode(cdrReader) {
let header = Header.decode(cdrReader);
let voltage = cdrReader.readFloat32();
let temperature = cdrReader.readFloat32();
let current = cdrReader.readFloat32();
let charge = cdrReader.readFloat32();
let capacity = cdrReader.readFloat32();
let design_capacity = cdrReader.readFloat32();
let percentage = cdrReader.readFloat32();
return new BatteryState(header, voltage, temperature, current, charge, capacity, design_capacity, percentage);
}
}
// LaserScan (Lidar publications)
class LaserScan {
constructor(header, angle_min, angle_max, angle_increment, time_increment, scan_time, range_min, range_max, ranges, intensities) {
this.header = header;
this.angle_min = angle_min;
this.angle_max = angle_max;
this.angle_increment = angle_increment;
this.time_increment = time_increment;
this.scan_time = scan_time;
this.range_min = range_min;
this.range_max = range_max;
this.ranges = ranges;
this.intensities = intensities;
}
static decode(cdrReader) {
let header = Header.decode(cdrReader);
let angle_min = cdrReader.readFloat32();
let angle_max = cdrReader.readFloat32();
let angle_increment = cdrReader.readFloat32();
let time_increment = cdrReader.readFloat32();
let scan_time = cdrReader.readFloat32();
let range_min = cdrReader.readFloat32();
let range_max = cdrReader.readFloat32();
let ranges_length = cdrReader.readInt32()
let ranges = [];
for (const x of Array(ranges_length).keys()) {
ranges.push(cdrReader.readFloat32())
}
let intensities_length = cdrReader.readInt32()
let intensities = [];
for (const x of Array(intensities_length).keys()) {
intensities.push(cdrReader.readFloat32())
}
return new LaserScan(header, angle_min, angle_max, angle_increment, time_increment, scan_time, range_min, range_max, ranges, intensities);
}
}
const Http = new XMLHttpRequest();
function pubTwist(linear, angular) {
// Get scales from HTML
var linear_scale = document.getElementById("linear_scale").value
var angular_scale = document.getElementById("angular_scale").value
// Create a Twist message
var twist = new Twist(
new Vector3(linear * linear_scale, 0.0, 0.0),
new Vector3(0.0, 0.0, angular * angular_scale));
console.log(twist);
// Since it's going to DDS, encode it using a jscdr.CDRWriter
var writer = new jscdr.CDRWriter();
twist.encode(writer);
// Get bridge URL and twist_keyexpr resource to publish from HTML
var bridge_url = document.getElementById("zenoh_rest_url").value
var twist_keyexpr = document.getElementById("twist_keyexpr").value
console.log("Send Twist to " + bridge_url + twist_keyexpr);
// PUT it to zenoh via its REST API
Http.open('PUT', bridge_url + twist_keyexpr, true);
Http.setRequestHeader('Content-Type', 'application/octet-stream');
Http.send(writer.buf.buffer);
}
function onkeydown(e) {
e = e || window.event;
console.log("KeyPressed: " + e);
if (e.keyCode == '38') {
// up arrow
pubTwist(1.0, 0.0);
}
else if (e.keyCode == '40') {
// down arrow
pubTwist(-1.0, 0.0);
}
else if (e.keyCode == '37') {
// left arrow
pubTwist(0.0, 1.0);
}
else if (e.keyCode == '39') {
// right arrow
pubTwist(0.0, -1.0);
}
else if (e.keyCode == '32') {
// spacebar
pubTwist(0.0, 0.0);
}
}
function onkeyup(e) {
// if key pressed was an arrow, send a Twist(0,0) to stop the robot
if (e.keyCode == '37' || e.keyCode == '38' || e.keyCode == '39' || e.keyCode == '40')
pubTwist(0.0, 0.0);
}
// register callback on key down
document.onkeydown = onkeydown;
// register callback on key up
document.onkeyup = onkeyup;
// EventSource receiving Logs
var rosout_source = null;
// subscribe to rosout topic
function subscribeToRosout() {
// close previous if exists
if (rosout_source != null) {
console.log("CLOSE ROSOUT EventSource");
rosout_source.close();
}
if (typeof (EventSource) !== "undefined") {
var zenoh_rest_url = document.getElementById("zenoh_rest_url").value
var rosout_res = document.getElementById("rosout_res").value
var scope = document.getElementById("sub_scope").value
if (scope.length > 0 && !scope.endsWith("/")) { scope += "/" }
console.log("Subscribe to EventSource: " + zenoh_rest_url + scope + rosout_res);
rosout_source = new EventSource(zenoh_rest_url + scope + rosout_res);
rosout_source.addEventListener("PUT", function (e) {
console.log("Received sample: " + e.data);
// The zenoh REST API sends JSON objects
// that includes "key", "value", "encoding" and "time" (same than a result to GET)
let sample = JSON.parse(e.data)
// The payload buffer is in "value" field, encoded as base64.
// Since it's comming from DDS, we decode it using a jscdr.CDRReader.
let reader = new jscdr.CDRReader(dcodeIO.ByteBuffer.fromBase64(sample.value));
// Decode the buffer as a Log message
let log = Log.decode(reader);
// Add it to "rosout_logs" HTML element
let elem = document.getElementById("rosout_logs");
elem.innerHTML += "[" + log.time.sec + "." + log.time.nsec + "] [" + log.name + "]: " + log.msg + "<br>";
// Auto-scroll to the bottom
elem.scrollTop = elem.scrollHeight;
}, false);
// update panel title
var title = document.getElementById("rosout_title");
title.innerHTML = "Logs: " + scope + rosout_res;
} else {
document.getElementById("rosout_logs").innerHTML = "Sorry, your browser does not support server-sent events...";
}
}
// EventSource receiving battery state
var battery_source = null;
// subscribe to battery_state topic
function subscribeToBattery() {
// close previous if exists
if (battery_source != null) {
console.log("CLOSE Battery EventSource");
battery_source.close();
}
if (typeof (EventSource) !== "undefined") {
var zenoh_rest_url = document.getElementById("zenoh_rest_url").value
// battery_state key is the same than rousout key but replacing "rosout" with "battery_state"
var battery_key = document.getElementById("rosout_res").value.replace("rosout", "battery_state")
var scope = document.getElementById("sub_scope").value
if (scope.length > 0 && !scope.endsWith("/")) { scope += "/" }
console.log("Subscribe to EventSource: " + zenoh_rest_url + scope + battery_key);
battery_source = new EventSource(zenoh_rest_url + scope + battery_key);
battery_source.addEventListener("PUT", function (e) {
console.log("Received sample: " + e.data);
// The zenoh REST API sends JSON objects
// that includes "key", "value", "encoding" and "time" (same than a result to GET)
let sample = JSON.parse(e.data)
// The payload buffer is in "value" field, encoded as base64.
// Since it's comming from DDS, we decode it using a jscdr.CDRReader.
let reader = new jscdr.CDRReader(dcodeIO.ByteBuffer.fromBase64(sample.value));
// Decode the buffer as a BatteryState message
let battery = BatteryState.decode(reader);
// Set it to "battery" HTML element
let elem = document.getElementById("battery");
elem.innerHTML = "Battery: " + Math.round(battery.percentage) + " %";
}, false);
} else {
document.getElementById("rosout_logs").innerHTML = "Sorry, your browser does not support server-sent events...";
}
}
// Setup Chart for Lidar display
data = {
categories: [],
series: [
{
data: []
},
],
};
chart = toastui.Chart.radarChart({
el: document.getElementById("Lidar"),
data,
options: {
chart: { height: 'auto', width: 'auto', animation: false },
plot: { type: 'circle' },
verticalAxis: { scale: { max: 3 } },
legend: { visible: false },
exportMenu: { visible: false },
tooltip: { template: () => `` },
series: {
selectable: false,
showDot: true,
showArea: true,
},
theme: {
series: { lineWidth: 0.00001 },
verticalAxis: {
label: { fontSize: 0, },
},
},
}
});
// EventSource receiving LaserScan
var scan_source = null;
function subscribeToScan() {
// close previous if exists
if (scan_source != null) {
console.log("CLOSE Scan EventSource");
scan_source.close();
}
if (typeof (EventSource) !== "undefined") {
var zenoh_rest_url = document.getElementById("zenoh_rest_url").value
var scan_res = document.getElementById("scan_res").value
var scope = document.getElementById("sub_scope").value
if (scope.length > 0 && !scope.endsWith("/")) { scope += "/" }
console.log("Subscribe to EventSource: " + zenoh_rest_url + scope + scan_res);
scan_source = new EventSource(zenoh_rest_url + scope + scan_res);
scan_source.addEventListener("PUT", function (e) {
let sample = JSON.parse(e.data);
// The payload buffer is in "value" field, encoded as base64.
// Since it's comming from DDS, we decode it using a jscdr.CDRReader.
let reader = new jscdr.CDRReader(dcodeIO.ByteBuffer.fromBase64(sample.value));
// Decode the buffer as an LaserScan message
let scan = LaserScan.decode(reader);
data.categories = [];
data.series[0].data = [];
chart.setData(data);
data.categories = Array(scan.ranges.length).fill('');
data.series[0].data = scan.ranges.reverse();
chart.setData(data);
}, false);
// update panel title
var title = document.getElementById("lidar_title");
title.innerHTML = "Lidar: " + scope + scan_res;
} else {
document.document.getElementById("Lidar").innerHTML = "Sorry, your browser does not support server-sent events...";
}
}
function setCameraSrc() {
// If your robot has a camera and zcapture installed (from zenoh-demos/computer-vision/zcam/),
// uncomment the camera <div> block on top of this file.
// The zcapture must be started with "-k <scope>/cams/0", and the zenoh router must have the WebServer plugin running
if (document.getElementById("camera_img") != null) {
// Set "camera_img" element's src to the same URL host, but with port 8080 (WebServer plugin)
// and with path: "demo/zcam?_method=SUB"
var zenoh_rest_url = document.getElementById("zenoh_rest_url").value
var scope = document.getElementById("sub_scope").value
if (scope.length > 0 && !scope.endsWith("/")) { scope += "/" }
img_url = zenoh_rest_url.replace(":8000", ":8080") + scope + "cams/0?_method=SUB";
document.getElementById("camera_img").src = img_url;
// update panel title
var title = document.getElementById("camera_title");
title.innerHTML = "Camera: " + scope + "cams/0";
}
}
function resetAllSubscriptions() {
subscribeToBattery();
subscribeToRosout();
subscribeToScan();
setCameraSrc();
}
// Get "host" and "scope" from URL params, and set "zenoh_rest_url" and "scope" accordingly
const urlParams = new URLSearchParams(window.location.search);
const host = urlParams.get('host')
if (host != null && host.length > 0) {
console.log("host: " + host);
document.getElementById("zenoh_rest_url").value = "http://" + host + "/";
}
const scope = urlParams.get('scope')
if (host != null && host.length > 0) {
console.log("scope: " + scope);
document.getElementById("scope").value = scope;
}
// subscribe at page loading
resetAllSubscriptions();
</script>
<script type="text/javascript">
</script>
</body>
</html>
//
// Configuration for zenoh-bridge-dds running on a Respberry Pi
// with a Zigbee key and zigbee2mqtt
//
{
plugins: {
mqtt: {
// allow all topics containing "zigbee2mqtt/device" in name
allow: "zigbee2mqtt/device",
},
},
// Connect to a remote Zenoh router
// (or to another bridge if it's configured to listen)
connect: {
endpoints: [ "tcp/demo.zenoh.io:7447" ]
},
}
//
// Configuration for zenohd running in Cloud VM
//
{
plugins: {
// REST API plugin, for pub/sub from web browser
rest: {
http_port: 8000,
},
// WebServer plugin, for web browser to get the camera stream as motion-JPEG
webserver: {
http_port: 8080,
},
}
}
import sys, time, json, zenoh
from datetime import datetime
from pycdr import cdr
from pycdr.types import int8, int32, uint32, float64
@cdr
class Vector3:
x: float64
y: float64
z: float64
@cdr
class Twist:
linear: Vector3
angular: Vector3
cmd_vel = 'simu/rt/cmd_vel'
button = 'zigbee2mqtt/device/button'
angular_scale = 1.0
linear_scale = 1.0
# initiate logging
zenoh.init_logger()
conf = zenoh.Config()
conf.insert_json5(zenoh.config.CONNECT_KEY, '["tcp/demo.zenoh.io:7447"]')
print("Openning session...")
session = zenoh.open(conf)
# Function to publish a Twist message
def pub_twist(linear, angular):
print("Pub twist: {} - {}".format(linear, angular))
t = Twist(linear=Vector3(x=linear, y=0.0, z=0.0),
angular=Vector3(x=0.0, y=0.0, z=angular))
session.put(cmd_vel, t.serialize())
# Subscriber callback receiving MQTT messages (JSON) and publishing Twists
def button_callback(sample):
m = json.loads(sample.payload)
print('Received sample {}'.format(sample))
if m['action'] == 'single':
print(' => single')
pub_twist(0.0, 1.0 * angular_scale)
time.sleep(2.0)
pub_twist(0.0, 0.0)
elif m['action'] == 'double':
print(' => double')
pub_twist(0.0, -1.0 * angular_scale)
time.sleep(2.0)
pub_twist(0.0, 0.0)
print("Subscriber on '{}'...".format(button))
sub = session.declare_subscriber(button, button_callback)
print("Enter 'q' to quit...")
c = '\0'
while c != 'q':
c = sys.stdin.read(1)
if c == '':
time.sleep(1)
sub.undeclare()
session.close()
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment