├── .gitignore ├── LICENSE ├── README.md ├── javascript ├── README.md ├── constants.js ├── go2webrtc.js ├── index.html ├── index.js ├── joy.min.js ├── md5.js ├── models │ └── axisColor4.png ├── server.py ├── threejs.html ├── threejs.init.js ├── threejs.js └── utils.js └── python ├── README.md ├── examples ├── joystick │ └── go2_joystick.py └── mqtt │ └── go2_mqtt.py ├── go2_webrtc ├── __init__.py ├── constants.py ├── go2_connection.py ├── go2_cv_video.py ├── libvoxel.wasm └── lidar_decoder.py ├── requirements.txt ├── setup.py └── tests └── test_go2connection.py /.gitignore: -------------------------------------------------------------------------------- 1 | python/.env 2 | *.egg-info 3 | __pycache__ 4 | python/go2-webrtc-env 5 | *.bin 6 | *venv* 7 | 8 | server.crt 9 | server.key 10 | example.bin 11 | javascript/assets/tmp/ 12 | .DS_Store 13 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 2-Clause License 2 | 3 | Copyright (c) 2024, RoboVerse community 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 19 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 20 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 21 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 22 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 23 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 24 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # go2-webrtc - WebRTC API or Unitree GO2 Robots 2 | 3 | The `go2-webrtc` project provides a WebRTC API for Unitree GO2 Robots, enabling real-time communication and control over these robots through a web interface. This project simplifies the process of connecting to and controlling Unitree GO2 Robots by leveraging the WebRTC protocol for efficient, low-latency communication. 4 | 5 | Go2's WebRTC API supports all models, including Go2 Air, Pro, and Edu versions. 6 | 7 | There is another, more featureful Python API for GO2 (also via WebRTC) called `go2_webrtc_connect` built by the infamous @legion1581. It has video and audio support among many other cool features: https://github.com/legion1581/go2_webrtc_connect 8 | 9 | ## Features 10 | 11 | - **WebRTC Integration**: Utilizes WebRTC to establish a real-time communication channel between the web client and the robot. 12 | - **User Interface**: Includes a simple web interface for connecting to the robot, sending commands, and viewing the robot's video stream. 13 | - **Command Execution**: Allows users to execute predefined commands on the robot, such as movement and action commands. 14 | - **Persistent Settings**: Saves connection settings (token and robot IP) in the browser's localStorage for easier reconnection. 15 | 16 | ## Getting Started 17 | 18 | To get started with `go2-webrtc`, clone this repository and serve the `index.html` file from the backend server (`server.py`) to a modern web browser. Ensure that your Unitree GO2 Robot is powered on and connected to the same network as your computer. 19 | 20 | ``` 21 | git clone https://github.com/tfoldi/go2-webrtc 22 | cd go2-webrtc 23 | pip install -r python/requirements.txt 24 | cd javascript 25 | python ./server.py 26 | ``` 27 | 28 | ## Sample Frontend Application 29 | The [javascript](https://github.com/tfoldi/go2-webrtc/tree/master/javascript) folder contains a sample frontend application that demonstrates how to use the JS WebRTC API to connect to and control the Unitree GO2 Robots. Explore the [javascript](https://github.com/tfoldi/go2-webrtc/tree/master/javascript) folder for more details and examples. 30 | 31 | ## Python API for Backend/Console Applications 32 | For backend or console applications, the [python](https://github.com/tfoldi/go2-webrtc/tree/master/python) folder provides a Python API that interfaces with the Unitree GO2 Robots. This API is ideal for developers looking to integrate robot control into their Python applications or scripts. Check out the [python](https://github.com/tfoldi/go2-webrtc/tree/master/python) folder for documentation and examples. 33 | 34 | ### Prerequisites 35 | 36 | - A Unitree GO2 Robot accessible over the local network. All models supported including Air, Pro and Edu 37 | - Local network connection (`STA-L`) to the robot 38 | 39 | ### Obtaining security token 40 | 41 | Connecting to your device without a security token is possible and might allow a connection to be established. However, this method limits you to a single active connection at any time. To simultaneously use multiple clients, such as a WebRTC-based application and a phone app, a valid security token is necessary. This ensures secure, multi-client access to your device. 42 | 43 | One way is to sniff the traffic between the dog and your phone. Assuming that you have Linux or Mac: 44 | 45 | 1. Run `tinyproxy` or any other HTTP proxy on your computer 46 | 2. Set your computer's IP and port as HTTP proxy on your phone 47 | 3. Run wireshark or `ngrep` on your box sniffing port 8081 `like ngrep port 8081`. 48 | 4. Look for the token in the TCP stream after you connect your phone to the dog via the app 49 | 50 | The token looks like this in the request payload: 51 | 52 | ``` 53 | { 54 | "token": "eyJ0eXAiOizI1NiJtlbiI[..]CI6MTcwODAxMzUwOX0.hiWOd9tNCIPzOOLNA", 55 | "sdp": "v=0\r\no=- ", 56 | "id": "STA_localNetwork", 57 | "type": "offer" 58 | } 59 | ``` 60 | 61 | Another option is to obtain token via the `/login/email` endpoint. 62 | 63 | ``` 64 | curl -vX POST https://global-robot-api.unitree.com/login/email -d "email=&password=" 65 | ``` 66 | 67 | ## Development 68 | 69 | This project is structured around several key JavaScript files for the frontend and a Python package for backend or console applications. To contribute or modify the project, refer to these resources for implementing additional features or improving the existing codebase. PRs are welcome. 70 | 71 | 72 | ## License 73 | 74 | This project is licensed under the BSD 2-clause License - see the [LICENSE](https://github.com/tfoldi/go2-webrtc/blob/master/LICENSE) file for details. 75 | -------------------------------------------------------------------------------- /javascript/README.md: -------------------------------------------------------------------------------- 1 | # go2-webrtc - WebRTC API or Unitree GO2 Robots 2 | 3 | The `go2-webrtc` project provides a WebRTC API for Unitree GO2 Robots, enabling real-time communication and control over these robots through a web interface. This project simplifies the process of connecting to and controlling Unitree GO2 Robots by leveraging the WebRTC protocol for efficient, low-latency communication. 4 | 5 | Go2's WebRTC API supports all models, including Go2 Air, Pro, and Edu versions. 6 | 7 | This branch does not work with firmwares prior to 1.1.1. If you have an older firmware, use the previous commit from the repository. 8 | 9 | ## Features 10 | 11 | - **WebRTC Integration**: Utilizes WebRTC to establish a real-time communication channel between the web client and the robot. 12 | - **User Interface**: Includes a simple web interface for connecting to the robot, sending commands, and viewing the robot's video stream. 13 | - **Command Execution**: Allows users to execute predefined commands on the robot, such as movement and action commands. 14 | - **Persistent Settings**: Saves connection settings (token and robot IP) in the browser's localStorage for easier reconnection. 15 | 16 | ## Getting Started 17 | 18 | To get started with `go2-webrtc`, clone this repository and serve the `index.html` file from the backend server (`server.py`) to a modern web browser. Ensure that your Unitree GO2 Robot is powered on and connected to the same network as your computer. 19 | 20 | ``` 21 | git clone https://github.com/tfoldi/go2-webrtc 22 | cd go2-webrtc 23 | pip install -r python/requirements.txt 24 | cd javascript 25 | python ./server.py 26 | ``` 27 | 28 | ### Prerequisites 29 | 30 | - A Unitree GO2 Robot accessible over the local network. All models supported. 31 | - A web browser that supports WebRTC (e.g., Chrome, Firefox). 32 | 33 | ### Usage 34 | 35 | 1. Enter your security token and robot IP address in the web interface. 36 | 2. Click the "Connect" button to establish a connection to the robot. 37 | 3. Use the command input to send commands to the robot. 38 | 4. The video stream from the robot (if available) will be displayed in the web interface. 39 | 40 | ### Obtaining security token 41 | 42 | Connecting to your device without a security token is possible and might allow a connection to be established. However, this method limits you to a single active connection at any time. To simultaneously use multiple clients, such as a WebRTC-based application and a phone app, a valid security token is necessary. This ensures secure, multi-client access to your device. 43 | 44 | The easiest way is to sniff the traffic between the dog and your phone. Assuming that you have Linux or Mac: 45 | 46 | 1. Run `tinyproxy` or any other HTTP proxy on your computer 47 | 2. Set your computer's IP and port as HTTP proxy on your phone 48 | 3. Run wireshark or `ngrep` on your box sniffing port 8081 `like ngrep port 8081`. 49 | 4. Look for the token in the TCP stream after you connect your phone to the dog via the app 50 | 51 | The token looks like this in the request payload: 52 | 53 | ``` 54 | { 55 | "token": "eyJ0eXAiOizI1NiJtlbiI[..]CI6MTcwODAxMzUwOX0.hiWOd9tNCIPzOOLNA", 56 | "sdp": "v=0\r\no=- ", 57 | "id": "STA_localNetwork", 58 | "type": "offer" 59 | } 60 | ``` 61 | ## Development 62 | 63 | This project is structured around several key JavaScript files: 64 | 65 | - `index.js`: Main script for handling UI interactions and storing settings. 66 | - `go2webrtc.js`: Core WebRTC functionality for connecting to and communicating with the robot. Can be used standalone as an API wrapper. 67 | - `utils.js`: Utility functions, including encryption helpers. 68 | - `constants.js`: Defines constants and command codes for robot control. 69 | - `server.py`: Python server used for CORS proxying 70 | 71 | To contribute or modify the project, refer to these files for implementing additional features or improving the existing codebase. PRs are welcome. 72 | 73 | ## License 74 | 75 | This project is licensed under the BSD 2-clause License - see the [LICENSE](https://github.com/tfoldi/go2-webrtc/blob/master/LICENSE) file for details. 76 | -------------------------------------------------------------------------------- /javascript/constants.js: -------------------------------------------------------------------------------- 1 | export const SPORT_CMD = { 2 | 1001: "Damp", 3 | 1002: "BalanceStand", 4 | 1003: "StopMove", 5 | 1004: "StandUp", 6 | 1005: "StandDown", 7 | 1006: "RecoveryStand", 8 | 1007: "Euler", 9 | 1008: "Move", 10 | 1009: "Sit", 11 | 1010: "RiseSit", 12 | 1011: "SwitchGait", 13 | 1012: "Trigger", 14 | 1013: "BodyHeight", 15 | 1014: "FootRaiseHeight", 16 | 1015: "SpeedLevel", 17 | 1016: "Hello", 18 | 1017: "Stretch", 19 | 1018: "TrajectoryFollow", 20 | 1019: "ContinuousGait", 21 | 1020: "Content", 22 | 1021: "Wallow", 23 | 1022: "Dance1", 24 | 1023: "Dance2", 25 | 1024: "GetBodyHeight", 26 | 1025: "GetFootRaiseHeight", 27 | 1026: "GetSpeedLevel", 28 | 1027: "SwitchJoystick", 29 | 1028: "Pose", 30 | 1029: "Scrape", 31 | 1030: "FrontFlip", 32 | 1031: "FrontJump", 33 | 1032: "FrontPounce", 34 | 1033: "WiggleHips", 35 | 1034: "GetState", 36 | 1035: "EconomicGait", 37 | 1036: "FingerHeart", 38 | }; 39 | 40 | export const DataChannelType = {}; 41 | 42 | (function initializeDataChannelTypes(types) { 43 | const defineType = (r, name) => (r[name.toUpperCase()] = name.toLowerCase()); 44 | 45 | defineType(types, "VALIDATION"); 46 | defineType(types, "SUBSCRIBE"); 47 | defineType(types, "UNSUBSCRIBE"); 48 | defineType(types, "MSG"); 49 | defineType(types, "REQUEST"); 50 | defineType(types, "RESPONSE"); 51 | defineType(types, "VID"); 52 | defineType(types, "AUD"); 53 | defineType(types, "ERR"); 54 | defineType(types, "HEARTBEAT"); 55 | defineType(types, "RTC_INNER_REQ"); 56 | defineType(types, "RTC_REPORT"); 57 | defineType(types, "ADD_ERROR"); 58 | defineType(types, "RM_ERROR"); 59 | defineType(types, "ERRORS"); 60 | })(DataChannelType); -------------------------------------------------------------------------------- /javascript/go2webrtc.js: -------------------------------------------------------------------------------- 1 | import { encryptKey } from "./utils.js"; 2 | import { SPORT_CMD, DataChannelType } from "./constants.js"; 3 | 4 | // Function to log messages to the console and the log window 5 | // TODO: need better way to handle logging - maybe use a logger 6 | function logMessage(text) { 7 | globalThis.logMessage ? globalThis.logMessage(text) : 0; 8 | } 9 | 10 | export class Go2WebRTC { 11 | constructor(token, robotIP, messageCallback) { 12 | this.token = token; 13 | this.robotIP = robotIP; 14 | this.messageCallback = messageCallback; 15 | 16 | this.msgCallbacks = new Map(); 17 | this.validationResult = "PENDING"; 18 | this.pc = new RTCPeerConnection({ sdpSemantics: "unified-plan" }); 19 | this.channel = this.pc.createDataChannel("data"); 20 | 21 | this.pc.addTransceiver("video", { direction: "recvonly" }); 22 | this.pc.addTransceiver("audio", { direction: "sendrecv" }); 23 | this.pc.addEventListener("track", this.trackEventHandler.bind(this)); 24 | this.channel.onmessage = this.messageEventHandler.bind(this); 25 | 26 | this.heartbeatTimer = null; 27 | } 28 | 29 | trackEventHandler(event) { 30 | if (event.track.kind === "video") { 31 | this.VidTrackEvent = event; 32 | } else { 33 | this.AudTrackEvent = event; 34 | } 35 | } 36 | 37 | messageEventHandler(event) { 38 | if ( 39 | event.data && 40 | event.data.includes && 41 | !event.data.includes("heartbeat") 42 | ) { 43 | console.log("onmessage", event); 44 | this.handleDataChannelMessage(event); 45 | } 46 | } 47 | 48 | handleDataChannelMessage(event) { 49 | const data = 50 | typeof event.data == "string" 51 | ? JSON.parse(event.data) 52 | : this.dealArrayBuffer(event.data); 53 | if (data.type === DataChannelType.VALIDATION) { 54 | this.rtcValidation(data); 55 | } 56 | 57 | if (this.messageCallback) { 58 | this.messageCallback(data); 59 | } 60 | } 61 | 62 | dealArrayBuffer(n) { 63 | const o = new Uint16Array(n.slice(0, 2)), 64 | s = n.slice(4, 4 + o[0]), 65 | c = n.slice(4 + o[0]), 66 | u = new TextDecoder("utf-8"), 67 | l = JSON.parse(u.decode(s)); 68 | return (l.data.data = c), l; 69 | } 70 | 71 | initSDP() { 72 | this.pc 73 | .createOffer() 74 | .then((offer) => this.pc.setLocalDescription(offer)) 75 | .then(() => { 76 | console.log("Offer created"); 77 | logMessage("Offer created"); 78 | console.log(this.pc.localDescription); 79 | logMessage(this.pc.localDescription); 80 | this.initSignaling(); 81 | }) 82 | .catch(console.error); 83 | } 84 | 85 | initSignaling() { 86 | var answer = { 87 | token: this.token, 88 | id: "STA_localNetwork", 89 | type: "offer", 90 | ip: this.robotIP 91 | }; 92 | answer["sdp"] = this.pc.localDescription.sdp; 93 | console.log(answer); 94 | 95 | const options = { 96 | method: "POST", 97 | headers: { 98 | "Content-Type": "application/json", 99 | }, 100 | body: JSON.stringify(answer), 101 | }; 102 | 103 | fetch(`http://${window.location.hostname}:8081/offer`, options) 104 | .then((response) => { 105 | console.log(`statusCode: ${response.status}`); 106 | return response.json(); 107 | }) 108 | .then((data) => { 109 | console.log("Response from signaling server:" + JSON.stringify(data)); 110 | logMessage("Establishing connection..."); 111 | this.pc 112 | .setRemoteDescription(data) 113 | .then(() => { 114 | logMessage("WebRTC connection established"); 115 | this.startHeartbeat(); 116 | }) 117 | .catch((e) => { 118 | console.log(e); 119 | }); 120 | }) 121 | .catch((error) => { 122 | console.error("Error sending message:", error); 123 | }); 124 | } 125 | 126 | startHeartbeat() { 127 | this.heartbeatTimer = window.setInterval(() => { 128 | const date = new Date(); 129 | (this.channel == null ? void 0 : this.channel.readyState) === "open" && 130 | (this.channel == null || 131 | this.channel.send( 132 | JSON.stringify({ 133 | type: DataChannelType.HEARTBEAT, 134 | data: { 135 | timeInStr: this.formatDate(date), 136 | timeInNum: Math.floor(date.valueOf() / 1e3), 137 | }, 138 | }) 139 | )); 140 | }, 2e3); 141 | } 142 | 143 | rtcValidation(msg) { 144 | if (msg.data === "Validation Ok.") { 145 | logMessage("Validation OK"); 146 | this.validationResult = "SUCCESS"; 147 | 148 | // TODO: execute all the registred callbacks in a map defined 149 | // in the initRTC function 150 | 151 | // TODO this should be on the callback for video on message 152 | if (document.getElementById("video-frame")) { 153 | logMessage("Playing video"); 154 | logMessage("Sending video on message"); 155 | this.publish("", "on", DataChannelType.VID); 156 | 157 | document.getElementById("video-frame").srcObject = 158 | this.VidTrackEvent.streams[0]; 159 | } 160 | } else { 161 | logMessage(`Sending validation key ${msg.data}`); 162 | this.publish("", encryptKey(msg.data), DataChannelType.VALIDATION); // ); 163 | } 164 | } 165 | // Function to format date according to unitree's requirements 166 | formatDate(r) { 167 | const n = r, 168 | y = n.getFullYear(), 169 | m = ("0" + (n.getMonth() + 1)).slice(-2), 170 | d = ("0" + n.getDate()).slice(-2), 171 | hh = ("0" + n.getHours()).slice(-2), 172 | mm = ("0" + n.getMinutes()).slice(-2), 173 | ss = ("0" + n.getSeconds()).slice(-2); 174 | return y + "-" + m + "-" + d + " " + hh + ":" + mm + ":" + ss; 175 | } 176 | 177 | dealMsgKey(channelType, channel, id) { 178 | return id || `${channelType} $ ${channel}`; 179 | } 180 | 181 | saveResolve(channelType, channel, res, id) { 182 | const msgKey = this.dealMsgKey(channelType, channel, id), 183 | callback = this.msgCallbacks.get(msgKey); 184 | callback ? callback.push(res) : this.msgCallbacks.set(msgKey, [res]); 185 | } 186 | 187 | publish(topic, data, channelType) { 188 | logMessage( 189 | `<- msg type:${channelType} topic:${topic} data:${JSON.stringify(data)}` 190 | ); 191 | return new Promise((resolve, reject) => { 192 | if (this.channel && this.channel.readyState === "open") { 193 | const msg = { 194 | type: channelType || DataChannelType.MSG, 195 | topic: topic, 196 | data: data, 197 | }; 198 | this.channel.send(JSON.stringify(msg)); 199 | const id = 200 | data && data.uuid 201 | ? data.uuid 202 | : data && data.header && data.header.identity.id; 203 | this.saveResolve( 204 | channelType || DataChannelType.MSG, 205 | topic, 206 | resolve, 207 | id 208 | ); 209 | } else { 210 | console.error("data channel is not open", topic); 211 | reject("data channel is not open"); 212 | } 213 | }); 214 | } 215 | 216 | publishApi(topic, api_id, data) { 217 | const uniqID = 218 | (new Date().valueOf() % 2147483648) + Math.floor(Math.random() * 1e3); 219 | 220 | console.log("Command:", api_id); 221 | 222 | this.publish(topic, { 223 | header: { identity: { id: uniqID, api_id: api_id} }, 224 | parameter: data 225 | }); 226 | } 227 | 228 | // Function to publish a message to the robot with full header 229 | // .publishReqNew(topic, { // api_id: s.api_id, 230 | // data: s.data, 231 | // id: s.id, 232 | // priority: !!s.priority, 233 | // }) 234 | publishReqNew(topic, msg) { 235 | const uniqID = 236 | (new Date().valueOf() % 2147483648) + Math.floor(Math.random() * 1e3); 237 | if (!(msg != null && msg.api_id)) 238 | return console.error("missing api id"), Promise.reject("missing api id"); 239 | const _msg = { 240 | header: { 241 | identity: { 242 | id: msg.id || uniqID, 243 | api_id: (msg == null ? void 0 : msg.api_id) || 0, 244 | }, 245 | }, 246 | parameter: "", 247 | }; 248 | return ( 249 | msg != null && 250 | msg.data && 251 | (_msg.parameter = 252 | typeof msg.data == "string" ? msg.data : JSON.stringify(msg.data)), 253 | msg != null && msg.priority && (_msg.header.policy = { priority: 1 }), 254 | this.publish(topic, _msg, DataChannelType.REQUEST) 255 | // publish(rtc, topic, {api_id: 1016, data: 1016}, DataChannelType.REQUEST) 256 | ); 257 | } 258 | } 259 | 260 | // TODO: to be removed, for debugging 261 | globalThis.SPORT_CMD = SPORT_CMD; 262 | globalThis.DataChannelType = DataChannelType; 263 | -------------------------------------------------------------------------------- /javascript/index.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | Unitree Go2 WebRTC Playground 7 | 11 | 97 | 98 | 99 | 100 | 101 |
102 |
103 |
104 | 105 | 106 |
107 |
108 | 109 | 110 |
111 |
112 | 113 | 114 |
115 |
116 | 117 | 118 |
119 |
120 | 121 | 122 |
123 |
124 | 125 |
126 |
130 | 136 | 137 |
138 |
139 |
140 | 141 |
142 |
143 | 144 |
145 |
146 | 147 | 148 | 149 | 150 | -------------------------------------------------------------------------------- /javascript/index.js: -------------------------------------------------------------------------------- 1 | import { Go2WebRTC } from "./go2webrtc.js"; 2 | 3 | // Function to log messages to the console and the log window 4 | function logMessage(text) { 5 | var log = document.querySelector("#log"); 6 | var msg = document.getElementById("log-code"); 7 | msg.textContent += truncateString(text, 300) + "\n"; 8 | log.scrollTop = log.scrollHeight; 9 | } 10 | globalThis.logMessage = logMessage; 11 | 12 | // Function to load saved values from localStorage 13 | function loadSavedValues() { 14 | const savedToken = localStorage.getItem("token"); 15 | const savedRobotIP = localStorage.getItem("robotIP"); 16 | 17 | if (savedToken) { 18 | document.getElementById("token").value = savedToken; 19 | } 20 | if (savedRobotIP) { 21 | document.getElementById("robot-ip").value = savedRobotIP; 22 | } 23 | 24 | const commandSelect = document.getElementById("command"); 25 | Object.entries(SPORT_CMD).forEach(([value, text]) => { 26 | const option = document.createElement("option"); 27 | option.value = value; 28 | option.textContent = text; 29 | commandSelect.appendChild(option); 30 | }); 31 | } 32 | 33 | // Function to save values to localStorage 34 | function saveValuesToLocalStorage() { 35 | const token = document.getElementById("token").value; 36 | const robotIP = document.getElementById("robot-ip").value; 37 | 38 | localStorage.setItem("token", token); 39 | localStorage.setItem("robotIP", robotIP); 40 | } 41 | 42 | // Function to handle connect button click 43 | function handleConnectClick() { 44 | // You can add connection logic here 45 | // For now, let's just log the values 46 | const token = document.getElementById("token").value; 47 | const robotIP = document.getElementById("robot-ip").value; 48 | 49 | console.log("Token:", token); 50 | console.log("Robot IP:", robotIP); 51 | logMessage(`Connecting to robot on ip ${robotIP}...`); 52 | 53 | // Save the values to localStorage 54 | saveValuesToLocalStorage(); 55 | 56 | // Initialize RTC 57 | globalThis.rtc = new Go2WebRTC(token, robotIP); 58 | globalThis.rtc.initSDP(); 59 | } 60 | 61 | function handleExecuteClick() { 62 | const uniqID = 63 | (new Date().valueOf() % 2147483648) + Math.floor(Math.random() * 1e3); 64 | const command = parseInt(document.getElementById("command").value); 65 | 66 | console.log("Command:", command); 67 | 68 | globalThis.rtc.publish("rt/api/sport/request", { 69 | header: { identity: { id: uniqID, api_id: command } }, 70 | parameter: JSON.stringify(command), 71 | // api_id: command, 72 | }); 73 | } 74 | 75 | 76 | function handleExecuteCustomClick() { 77 | const command = document.getElementById("custom-command").value; 78 | 79 | console.log("Command:", command); 80 | 81 | globalThis.rtc.channel.send(command); 82 | } 83 | 84 | function truncateString(str, maxLength) { 85 | if (typeof str !== "string") { 86 | str = JSON.stringify(str); 87 | } 88 | 89 | if (str.length > maxLength) { 90 | return str.substring(0, maxLength) + "..."; 91 | } else { 92 | return str; 93 | } 94 | } 95 | 96 | function applyGamePadDeadzeone(value, th) { 97 | return Math.abs(value) > th ? value : 0 98 | } 99 | 100 | function joystickTick(joyLeft, joyRight) { 101 | let x,y,z = 0; 102 | let gpToUse = document.getElementById("gamepad").value; 103 | if (gpToUse !== "NO") { 104 | const gamepads = navigator.getGamepads(); 105 | let gp = gamepads[gpToUse]; 106 | 107 | // LB must be pressed 108 | if (gp.buttons[4].pressed == true) { 109 | x = -1 * applyGamePadDeadzeone(gp.axes[1], 0.25); 110 | y = -1 * applyGamePadDeadzeone(gp.axes[2], 0.25); 111 | z = -1 * applyGamePadDeadzeone(gp.axes[0], 0.25); 112 | } 113 | } else { 114 | y = -1 * (joyRight.GetPosX() - 100) / 50; 115 | x = -1 * (joyLeft.GetPosY() - 100) / 50; 116 | z = -1 * (joyLeft.GetPosX() - 100) / 50; 117 | } 118 | 119 | if (x === 0 && y === 0 && z === 0) { 120 | return; 121 | } 122 | 123 | if (x == undefined || y == undefined || z == undefined) { 124 | return; 125 | } 126 | 127 | console.log("Joystick Linear:", x, y, z); 128 | 129 | if(globalThis.rtc == undefined) return; 130 | globalThis.rtc.publishApi("rt/api/sport/request", 1008, JSON.stringify({x: x, y: y, z: z})); 131 | } 132 | 133 | function addJoysticks() { 134 | const joyConfig = { 135 | internalFillColor: "#FFFFFF", 136 | internalLineWidth: 2, 137 | internalStrokeColor: "rgba(240, 240, 240, 0.3)", 138 | externalLineWidth: 1, 139 | externalStrokeColor: "#FFFFFF", 140 | }; 141 | var joyLeft = new JoyStick("joy-left", joyConfig); 142 | var joyRight = new JoyStick("joy-right", joyConfig); 143 | 144 | setInterval( joystickTick, 100, joyLeft, joyRight ); 145 | } 146 | 147 | const buildGamePadsSelect = (e) => { 148 | const gp = navigator.getGamepads().filter(x => x != null && x.id.toLowerCase().indexOf("xbox") != -1); 149 | 150 | const gamepadSelect = document.getElementById("gamepad"); 151 | gamepadSelect.innerHTML = ""; 152 | 153 | const option = document.createElement("option"); 154 | option.value = "NO"; 155 | option.textContent = "Don't use Gamepad" 156 | option.selected = true; 157 | gamepadSelect.appendChild(option); 158 | 159 | Object.entries(gp).forEach(([index, value]) => { 160 | if (!value) return 161 | const option = document.createElement("option"); 162 | option.value = value.index; 163 | option.textContent = value.id; 164 | gamepadSelect.appendChild(option); 165 | }); 166 | }; 167 | 168 | window.addEventListener("gamepadconnected", buildGamePadsSelect); 169 | window.addEventListener("gamepaddisconnected", buildGamePadsSelect); 170 | buildGamePadsSelect(); 171 | 172 | // Load saved values when the page loads 173 | document.addEventListener("DOMContentLoaded", loadSavedValues); 174 | document.addEventListener("DOMContentLoaded", addJoysticks); 175 | 176 | document.getElementById("gamepad").addEventListener("change", () => { 177 | //alert("change"); 178 | }); 179 | 180 | // Attach event listener to connect button 181 | document 182 | .getElementById("connect-btn") 183 | .addEventListener("click", handleConnectClick); 184 | 185 | document 186 | .getElementById("execute-btn") 187 | .addEventListener("click", handleExecuteClick); 188 | 189 | document 190 | .getElementById("execute-custom-btn") 191 | .addEventListener("click", handleExecuteCustomClick); 192 | 193 | 194 | 195 | 196 | document.addEventListener('keydown', function(event) { 197 | const key = event.key.toLowerCase(); 198 | let x = 0, y = 0, z = 0; 199 | 200 | switch (key) { 201 | case 'w': // Forward 202 | x = 0.8; 203 | break; 204 | case 's': // Reverse 205 | x = -0.4; 206 | break; 207 | case 'a': // Sideways left 208 | y = 0.4; 209 | break; 210 | case 'd': // Sideways right 211 | y = -0.4; 212 | break; 213 | case 'q': // Turn left 214 | z = 2; 215 | break; 216 | case 'e': // Turn right 217 | z = -2; 218 | break; 219 | default: 220 | return; // Ignore other keys 221 | } 222 | 223 | if(globalThis.rtc !== undefined) { 224 | globalThis.rtc.publishApi("rt/api/sport/request", 1008, JSON.stringify({x: x, y: y, z: z})); 225 | } 226 | }); 227 | 228 | document.addEventListener('keyup', function(event) { 229 | const key = event.key.toLowerCase(); 230 | if (key === 'w' || key === 's' || key === 'a' || key === 'd' || key === 'q' || key === 'e') { 231 | if(globalThis.rtc !== undefined) { 232 | // Stop movement by sending zero velocity 233 | globalThis.rtc.publishApi("rt/api/sport/request", 1008, JSON.stringify({x: 0, y: 0, z: 0})); 234 | } 235 | } 236 | }); 237 | 238 | -------------------------------------------------------------------------------- /javascript/joy.min.js: -------------------------------------------------------------------------------- 1 | /* 2 | * Name : joy.js 3 | * @author : Roberto D'Amico (Bobboteck) 4 | * Last modified : 09.06.2020 5 | * Revision : 1.1.6 6 | * 7 | * Modification History: 8 | * Date Version Modified By Description 9 | * 2021-12-21 2.0.0 Roberto D'Amico New version of the project that integrates the callback functions, while 10 | * maintaining compatibility with previous versions. Fixed Issue #27 too, 11 | * thanks to @artisticfox8 for the suggestion. 12 | * 2020-06-09 1.1.6 Roberto D'Amico Fixed Issue #10 and #11 13 | * 2020-04-20 1.1.5 Roberto D'Amico Correct: Two sticks in a row, thanks to @liamw9534 for the suggestion 14 | * 2020-04-03 Roberto D'Amico Correct: InternalRadius when change the size of canvas, thanks to 15 | * @vanslipon for the suggestion 16 | * 2020-01-07 1.1.4 Roberto D'Amico Close #6 by implementing a new parameter to set the functionality of 17 | * auto-return to 0 position 18 | * 2019-11-18 1.1.3 Roberto D'Amico Close #5 correct indication of East direction 19 | * 2019-11-12 1.1.2 Roberto D'Amico Removed Fix #4 incorrectly introduced and restored operation with touch 20 | * devices 21 | * 2019-11-12 1.1.1 Roberto D'Amico Fixed Issue #4 - Now JoyStick work in any position in the page, not only 22 | * at 0,0 23 | * 24 | * The MIT License (MIT) 25 | * 26 | * This file is part of the JoyStick Project (https://github.com/bobboteck/JoyStick). 27 | * Copyright (c) 2015 Roberto D'Amico (Bobboteck). 28 | * 29 | * Permission is hereby granted, free of charge, to any person obtaining a copy 30 | * of this software and associated documentation files (the "Software"), to deal 31 | * in the Software without restriction, including without limitation the rights 32 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 33 | * copies of the Software, and to permit persons to whom the Software is 34 | * furnished to do so, subject to the following conditions: 35 | * 36 | * The above copyright notice and this permission notice shall be included in all 37 | * copies or substantial portions of the Software. 38 | * 39 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 40 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 41 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 42 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 43 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 44 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 45 | * SOFTWARE. 46 | */ 47 | let StickStatus={xPosition:0,yPosition:0,x:0,y:0,cardinalDirection:"C"};var JoyStick=function(t,e,i){var o=void 0===(e=e||{}).title?"joystick":e.title,n=void 0===e.width?0:e.width,a=void 0===e.height?0:e.height,r=void 0===e.internalFillColor?"#00AA00":e.internalFillColor,c=void 0===e.internalLineWidth?2:e.internalLineWidth,s=void 0===e.internalStrokeColor?"#003300":e.internalStrokeColor,d=void 0===e.externalLineWidth?2:e.externalLineWidth,u=void 0===e.externalStrokeColor?"#008000":e.externalStrokeColor,h=void 0===e.autoReturnToCenter||e.autoReturnToCenter;i=i||function(t){};var S=document.getElementById(t);S.style.touchAction="none";var f=document.createElement("canvas");f.id=o,0===n&&(n=S.clientWidth),0===a&&(a=S.clientHeight),f.width=n,f.height=a,S.appendChild(f);var l=f.getContext("2d"),k=0,g=2*Math.PI,x=(f.width-(f.width/2+10))/2,v=x+5,P=x+30,m=f.width/2,C=f.height/2,p=f.width/10,y=-1*p,w=f.height/10,L=-1*w,F=m,E=C;function W(){l.beginPath(),l.arc(m,C,P,0,g,!1),l.lineWidth=d,l.strokeStyle=u,l.stroke()}function T(){l.beginPath(),Ff.width&&(F=f.width-v),Ef.height&&(E=f.height-v),l.arc(F,E,x,0,g,!1);var t=l.createRadialGradient(m,C,5,m,C,200);t.addColorStop(0,r),t.addColorStop(1,s),l.fillStyle=t,l.fill(),l.lineWidth=c,l.strokeStyle=s,l.stroke()}function D(){let t="",e=F-m,i=E-C;return i>=L&&i<=w&&(t="C"),iw&&(t="S"),ep&&("C"===t?t="E":t+="E"),t}"ontouchstart"in document.documentElement?(f.addEventListener("touchstart",function(t){k=1},!1),document.addEventListener("touchmove",function(t){1===k&&t.targetTouches[0].target===f&&(F=t.targetTouches[0].pageX,E=t.targetTouches[0].pageY,"BODY"===f.offsetParent.tagName.toUpperCase()?(F-=f.offsetLeft,E-=f.offsetTop):(F-=f.offsetParent.offsetLeft,E-=f.offsetParent.offsetTop),l.clearRect(0,0,f.width,f.height),W(),T(),StickStatus.xPosition=F,StickStatus.yPosition=E,StickStatus.x=((F-m)/v*100).toFixed(),StickStatus.y=((E-C)/v*100*-1).toFixed(),StickStatus.cardinalDirection=D(),i(StickStatus))},!1),document.addEventListener("touchend",function(t){k=0,h&&(F=m,E=C);l.clearRect(0,0,f.width,f.height),W(),T(),StickStatus.xPosition=F,StickStatus.yPosition=E,StickStatus.x=((F-m)/v*100).toFixed(),StickStatus.y=((E-C)/v*100*-1).toFixed(),StickStatus.cardinalDirection=D(),i(StickStatus)},!1)):(f.addEventListener("mousedown",function(t){k=1},!1),document.addEventListener("mousemove",function(t){1===k&&(F=t.pageX,E=t.pageY,"BODY"===f.offsetParent.tagName.toUpperCase()?(F-=f.offsetLeft,E-=f.offsetTop):(F-=f.offsetParent.offsetLeft,E-=f.offsetParent.offsetTop),l.clearRect(0,0,f.width,f.height),W(),T(),StickStatus.xPosition=F,StickStatus.yPosition=E,StickStatus.x=((F-m)/v*100).toFixed(),StickStatus.y=((E-C)/v*100*-1).toFixed(),StickStatus.cardinalDirection=D(),i(StickStatus))},!1),document.addEventListener("mouseup",function(t){k=0,h&&(F=m,E=C);l.clearRect(0,0,f.width,f.height),W(),T(),StickStatus.xPosition=F,StickStatus.yPosition=E,StickStatus.x=((F-m)/v*100).toFixed(),StickStatus.y=((E-C)/v*100*-1).toFixed(),StickStatus.cardinalDirection=D(),i(StickStatus)},!1)),W(),T(),this.GetWidth=function(){return f.width},this.GetHeight=function(){return f.height},this.GetPosX=function(){return F},this.GetPosY=function(){return E},this.GetX=function(){return((F-m)/v*100).toFixed()},this.GetY=function(){return((E-C)/v*100*-1).toFixed()},this.GetDir=function(){return D()}}; -------------------------------------------------------------------------------- /javascript/md5.js: -------------------------------------------------------------------------------- 1 | /* 2 | * JavaScript MD5 3 | * https://github.com/blueimp/JavaScript-MD5 4 | * 5 | * Copyright 2011, Sebastian Tschan 6 | * https://blueimp.net 7 | * 8 | * Licensed under the MIT license: 9 | * https://opensource.org/licenses/MIT 10 | * 11 | * Based on 12 | * A JavaScript implementation of the RSA Data Security, Inc. MD5 Message 13 | * Digest Algorithm, as defined in RFC 1321. 14 | * Version 2.2 Copyright (C) Paul Johnston 1999 - 2009 15 | * Other contributors: Greg Holt, Andrew Kepert, Ydnar, Lostinet 16 | * Distributed under the BSD License 17 | * See http://pajhome.org.uk/crypt/md5 for more info. 18 | */ 19 | 20 | /* global define */ 21 | 22 | /* eslint-disable strict */ 23 | 24 | ;(function ($) { 25 | 'use strict' 26 | 27 | /** 28 | * Add integers, wrapping at 2^32. 29 | * This uses 16-bit operations internally to work around bugs in interpreters. 30 | * 31 | * @param {number} x First integer 32 | * @param {number} y Second integer 33 | * @returns {number} Sum 34 | */ 35 | function safeAdd(x, y) { 36 | var lsw = (x & 0xffff) + (y & 0xffff) 37 | var msw = (x >> 16) + (y >> 16) + (lsw >> 16) 38 | return (msw << 16) | (lsw & 0xffff) 39 | } 40 | 41 | /** 42 | * Bitwise rotate a 32-bit number to the left. 43 | * 44 | * @param {number} num 32-bit number 45 | * @param {number} cnt Rotation count 46 | * @returns {number} Rotated number 47 | */ 48 | function bitRotateLeft(num, cnt) { 49 | return (num << cnt) | (num >>> (32 - cnt)) 50 | } 51 | 52 | /** 53 | * Basic operation the algorithm uses. 54 | * 55 | * @param {number} q q 56 | * @param {number} a a 57 | * @param {number} b b 58 | * @param {number} x x 59 | * @param {number} s s 60 | * @param {number} t t 61 | * @returns {number} Result 62 | */ 63 | function md5cmn(q, a, b, x, s, t) { 64 | return safeAdd(bitRotateLeft(safeAdd(safeAdd(a, q), safeAdd(x, t)), s), b) 65 | } 66 | /** 67 | * Basic operation the algorithm uses. 68 | * 69 | * @param {number} a a 70 | * @param {number} b b 71 | * @param {number} c c 72 | * @param {number} d d 73 | * @param {number} x x 74 | * @param {number} s s 75 | * @param {number} t t 76 | * @returns {number} Result 77 | */ 78 | function md5ff(a, b, c, d, x, s, t) { 79 | return md5cmn((b & c) | (~b & d), a, b, x, s, t) 80 | } 81 | /** 82 | * Basic operation the algorithm uses. 83 | * 84 | * @param {number} a a 85 | * @param {number} b b 86 | * @param {number} c c 87 | * @param {number} d d 88 | * @param {number} x x 89 | * @param {number} s s 90 | * @param {number} t t 91 | * @returns {number} Result 92 | */ 93 | function md5gg(a, b, c, d, x, s, t) { 94 | return md5cmn((b & d) | (c & ~d), a, b, x, s, t) 95 | } 96 | /** 97 | * Basic operation the algorithm uses. 98 | * 99 | * @param {number} a a 100 | * @param {number} b b 101 | * @param {number} c c 102 | * @param {number} d d 103 | * @param {number} x x 104 | * @param {number} s s 105 | * @param {number} t t 106 | * @returns {number} Result 107 | */ 108 | function md5hh(a, b, c, d, x, s, t) { 109 | return md5cmn(b ^ c ^ d, a, b, x, s, t) 110 | } 111 | /** 112 | * Basic operation the algorithm uses. 113 | * 114 | * @param {number} a a 115 | * @param {number} b b 116 | * @param {number} c c 117 | * @param {number} d d 118 | * @param {number} x x 119 | * @param {number} s s 120 | * @param {number} t t 121 | * @returns {number} Result 122 | */ 123 | function md5ii(a, b, c, d, x, s, t) { 124 | return md5cmn(c ^ (b | ~d), a, b, x, s, t) 125 | } 126 | 127 | /** 128 | * Calculate the MD5 of an array of little-endian words, and a bit length. 129 | * 130 | * @param {Array} x Array of little-endian words 131 | * @param {number} len Bit length 132 | * @returns {Array} MD5 Array 133 | */ 134 | function binlMD5(x, len) { 135 | /* append padding */ 136 | x[len >> 5] |= 0x80 << len % 32 137 | x[(((len + 64) >>> 9) << 4) + 14] = len 138 | 139 | var i 140 | var olda 141 | var oldb 142 | var oldc 143 | var oldd 144 | var a = 1732584193 145 | var b = -271733879 146 | var c = -1732584194 147 | var d = 271733878 148 | 149 | for (i = 0; i < x.length; i += 16) { 150 | olda = a 151 | oldb = b 152 | oldc = c 153 | oldd = d 154 | 155 | a = md5ff(a, b, c, d, x[i], 7, -680876936) 156 | d = md5ff(d, a, b, c, x[i + 1], 12, -389564586) 157 | c = md5ff(c, d, a, b, x[i + 2], 17, 606105819) 158 | b = md5ff(b, c, d, a, x[i + 3], 22, -1044525330) 159 | a = md5ff(a, b, c, d, x[i + 4], 7, -176418897) 160 | d = md5ff(d, a, b, c, x[i + 5], 12, 1200080426) 161 | c = md5ff(c, d, a, b, x[i + 6], 17, -1473231341) 162 | b = md5ff(b, c, d, a, x[i + 7], 22, -45705983) 163 | a = md5ff(a, b, c, d, x[i + 8], 7, 1770035416) 164 | d = md5ff(d, a, b, c, x[i + 9], 12, -1958414417) 165 | c = md5ff(c, d, a, b, x[i + 10], 17, -42063) 166 | b = md5ff(b, c, d, a, x[i + 11], 22, -1990404162) 167 | a = md5ff(a, b, c, d, x[i + 12], 7, 1804603682) 168 | d = md5ff(d, a, b, c, x[i + 13], 12, -40341101) 169 | c = md5ff(c, d, a, b, x[i + 14], 17, -1502002290) 170 | b = md5ff(b, c, d, a, x[i + 15], 22, 1236535329) 171 | 172 | a = md5gg(a, b, c, d, x[i + 1], 5, -165796510) 173 | d = md5gg(d, a, b, c, x[i + 6], 9, -1069501632) 174 | c = md5gg(c, d, a, b, x[i + 11], 14, 643717713) 175 | b = md5gg(b, c, d, a, x[i], 20, -373897302) 176 | a = md5gg(a, b, c, d, x[i + 5], 5, -701558691) 177 | d = md5gg(d, a, b, c, x[i + 10], 9, 38016083) 178 | c = md5gg(c, d, a, b, x[i + 15], 14, -660478335) 179 | b = md5gg(b, c, d, a, x[i + 4], 20, -405537848) 180 | a = md5gg(a, b, c, d, x[i + 9], 5, 568446438) 181 | d = md5gg(d, a, b, c, x[i + 14], 9, -1019803690) 182 | c = md5gg(c, d, a, b, x[i + 3], 14, -187363961) 183 | b = md5gg(b, c, d, a, x[i + 8], 20, 1163531501) 184 | a = md5gg(a, b, c, d, x[i + 13], 5, -1444681467) 185 | d = md5gg(d, a, b, c, x[i + 2], 9, -51403784) 186 | c = md5gg(c, d, a, b, x[i + 7], 14, 1735328473) 187 | b = md5gg(b, c, d, a, x[i + 12], 20, -1926607734) 188 | 189 | a = md5hh(a, b, c, d, x[i + 5], 4, -378558) 190 | d = md5hh(d, a, b, c, x[i + 8], 11, -2022574463) 191 | c = md5hh(c, d, a, b, x[i + 11], 16, 1839030562) 192 | b = md5hh(b, c, d, a, x[i + 14], 23, -35309556) 193 | a = md5hh(a, b, c, d, x[i + 1], 4, -1530992060) 194 | d = md5hh(d, a, b, c, x[i + 4], 11, 1272893353) 195 | c = md5hh(c, d, a, b, x[i + 7], 16, -155497632) 196 | b = md5hh(b, c, d, a, x[i + 10], 23, -1094730640) 197 | a = md5hh(a, b, c, d, x[i + 13], 4, 681279174) 198 | d = md5hh(d, a, b, c, x[i], 11, -358537222) 199 | c = md5hh(c, d, a, b, x[i + 3], 16, -722521979) 200 | b = md5hh(b, c, d, a, x[i + 6], 23, 76029189) 201 | a = md5hh(a, b, c, d, x[i + 9], 4, -640364487) 202 | d = md5hh(d, a, b, c, x[i + 12], 11, -421815835) 203 | c = md5hh(c, d, a, b, x[i + 15], 16, 530742520) 204 | b = md5hh(b, c, d, a, x[i + 2], 23, -995338651) 205 | 206 | a = md5ii(a, b, c, d, x[i], 6, -198630844) 207 | d = md5ii(d, a, b, c, x[i + 7], 10, 1126891415) 208 | c = md5ii(c, d, a, b, x[i + 14], 15, -1416354905) 209 | b = md5ii(b, c, d, a, x[i + 5], 21, -57434055) 210 | a = md5ii(a, b, c, d, x[i + 12], 6, 1700485571) 211 | d = md5ii(d, a, b, c, x[i + 3], 10, -1894986606) 212 | c = md5ii(c, d, a, b, x[i + 10], 15, -1051523) 213 | b = md5ii(b, c, d, a, x[i + 1], 21, -2054922799) 214 | a = md5ii(a, b, c, d, x[i + 8], 6, 1873313359) 215 | d = md5ii(d, a, b, c, x[i + 15], 10, -30611744) 216 | c = md5ii(c, d, a, b, x[i + 6], 15, -1560198380) 217 | b = md5ii(b, c, d, a, x[i + 13], 21, 1309151649) 218 | a = md5ii(a, b, c, d, x[i + 4], 6, -145523070) 219 | d = md5ii(d, a, b, c, x[i + 11], 10, -1120210379) 220 | c = md5ii(c, d, a, b, x[i + 2], 15, 718787259) 221 | b = md5ii(b, c, d, a, x[i + 9], 21, -343485551) 222 | 223 | a = safeAdd(a, olda) 224 | b = safeAdd(b, oldb) 225 | c = safeAdd(c, oldc) 226 | d = safeAdd(d, oldd) 227 | } 228 | return [a, b, c, d] 229 | } 230 | 231 | /** 232 | * Convert an array of little-endian words to a string 233 | * 234 | * @param {Array} input MD5 Array 235 | * @returns {string} MD5 string 236 | */ 237 | function binl2rstr(input) { 238 | var i 239 | var output = '' 240 | var length32 = input.length * 32 241 | for (i = 0; i < length32; i += 8) { 242 | output += String.fromCharCode((input[i >> 5] >>> i % 32) & 0xff) 243 | } 244 | return output 245 | } 246 | 247 | /** 248 | * Convert a raw string to an array of little-endian words 249 | * Characters >255 have their high-byte silently ignored. 250 | * 251 | * @param {string} input Raw input string 252 | * @returns {Array} Array of little-endian words 253 | */ 254 | function rstr2binl(input) { 255 | var i 256 | var output = [] 257 | output[(input.length >> 2) - 1] = undefined 258 | for (i = 0; i < output.length; i += 1) { 259 | output[i] = 0 260 | } 261 | var length8 = input.length * 8 262 | for (i = 0; i < length8; i += 8) { 263 | output[i >> 5] |= (input.charCodeAt(i / 8) & 0xff) << i % 32 264 | } 265 | return output 266 | } 267 | 268 | /** 269 | * Calculate the MD5 of a raw string 270 | * 271 | * @param {string} s Input string 272 | * @returns {string} Raw MD5 string 273 | */ 274 | function rstrMD5(s) { 275 | return binl2rstr(binlMD5(rstr2binl(s), s.length * 8)) 276 | } 277 | 278 | /** 279 | * Calculates the HMAC-MD5 of a key and some data (raw strings) 280 | * 281 | * @param {string} key HMAC key 282 | * @param {string} data Raw input string 283 | * @returns {string} Raw MD5 string 284 | */ 285 | function rstrHMACMD5(key, data) { 286 | var i 287 | var bkey = rstr2binl(key) 288 | var ipad = [] 289 | var opad = [] 290 | var hash 291 | ipad[15] = opad[15] = undefined 292 | if (bkey.length > 16) { 293 | bkey = binlMD5(bkey, key.length * 8) 294 | } 295 | for (i = 0; i < 16; i += 1) { 296 | ipad[i] = bkey[i] ^ 0x36363636 297 | opad[i] = bkey[i] ^ 0x5c5c5c5c 298 | } 299 | hash = binlMD5(ipad.concat(rstr2binl(data)), 512 + data.length * 8) 300 | return binl2rstr(binlMD5(opad.concat(hash), 512 + 128)) 301 | } 302 | 303 | /** 304 | * Convert a raw string to a hex string 305 | * 306 | * @param {string} input Raw input string 307 | * @returns {string} Hex encoded string 308 | */ 309 | function rstr2hex(input) { 310 | var hexTab = '0123456789abcdef' 311 | var output = '' 312 | var x 313 | var i 314 | for (i = 0; i < input.length; i += 1) { 315 | x = input.charCodeAt(i) 316 | output += hexTab.charAt((x >>> 4) & 0x0f) + hexTab.charAt(x & 0x0f) 317 | } 318 | return output 319 | } 320 | 321 | /** 322 | * Encode a string as UTF-8 323 | * 324 | * @param {string} input Input string 325 | * @returns {string} UTF8 string 326 | */ 327 | function str2rstrUTF8(input) { 328 | return unescape(encodeURIComponent(input)) 329 | } 330 | 331 | /** 332 | * Encodes input string as raw MD5 string 333 | * 334 | * @param {string} s Input string 335 | * @returns {string} Raw MD5 string 336 | */ 337 | function rawMD5(s) { 338 | return rstrMD5(str2rstrUTF8(s)) 339 | } 340 | /** 341 | * Encodes input string as Hex encoded string 342 | * 343 | * @param {string} s Input string 344 | * @returns {string} Hex encoded string 345 | */ 346 | function hexMD5(s) { 347 | return rstr2hex(rawMD5(s)) 348 | } 349 | /** 350 | * Calculates the raw HMAC-MD5 for the given key and data 351 | * 352 | * @param {string} k HMAC key 353 | * @param {string} d Input string 354 | * @returns {string} Raw MD5 string 355 | */ 356 | function rawHMACMD5(k, d) { 357 | return rstrHMACMD5(str2rstrUTF8(k), str2rstrUTF8(d)) 358 | } 359 | /** 360 | * Calculates the Hex encoded HMAC-MD5 for the given key and data 361 | * 362 | * @param {string} k HMAC key 363 | * @param {string} d Input string 364 | * @returns {string} Raw MD5 string 365 | */ 366 | function hexHMACMD5(k, d) { 367 | return rstr2hex(rawHMACMD5(k, d)) 368 | } 369 | 370 | /** 371 | * Calculates MD5 value for a given string. 372 | * If a key is provided, calculates the HMAC-MD5 value. 373 | * Returns a Hex encoded string unless the raw argument is given. 374 | * 375 | * @param {string} string Input string 376 | * @param {string} [key] HMAC key 377 | * @param {boolean} [raw] Raw output switch 378 | * @returns {string} MD5 output 379 | */ 380 | function md5(string, key, raw) { 381 | if (!key) { 382 | if (!raw) { 383 | return hexMD5(string) 384 | } 385 | return rawMD5(string) 386 | } 387 | if (!raw) { 388 | return hexHMACMD5(key, string) 389 | } 390 | return rawHMACMD5(key, string) 391 | } 392 | 393 | if (typeof define === 'function' && define.amd) { 394 | define(function () { 395 | return md5 396 | }) 397 | } else if (typeof module === 'object' && module.exports) { 398 | module.exports = md5 399 | } else { 400 | $.md5 = md5 401 | } 402 | })(this) -------------------------------------------------------------------------------- /javascript/models/axisColor4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tfoldi/go2-webrtc/e14b3d92d5b63e865eb8aaeb1895a84267382141/javascript/models/axisColor4.png -------------------------------------------------------------------------------- /javascript/server.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2024, RoboVerse community 2 | # 3 | # Redistribution and use in source and binary forms, with or without 4 | # modification, are permitted provided that the following conditions are met: 5 | # 6 | # 1. Redistributions of source code must retain the above copyright notice, this 7 | # list of conditions and the following disclaimer. 8 | # 9 | # 2. Redistributions in binary form must reproduce the above copyright notice, 10 | # this list of conditions and the following disclaimer in the documentation 11 | # and/or other materials provided with the distribution. 12 | # 13 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 14 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 15 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 16 | # DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 17 | # FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 18 | # DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 19 | # SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 20 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 21 | # OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 22 | # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | 24 | import http.server 25 | import socketserver 26 | import json 27 | import os 28 | import sys 29 | 30 | path_to_add = os.path.abspath(os.path.join(os.path.dirname(__file__), "../python")) 31 | if os.path.exists(path_to_add): 32 | sys.path.insert(0, path_to_add) 33 | print(f"Added {path_to_add} to sys.path") 34 | else: 35 | print(f"Path {path_to_add} does not exist") 36 | 37 | import go2_webrtc 38 | 39 | PORT = 8081 40 | 41 | class SDPDict: 42 | def __init__(self, existing_dict): 43 | self.__dict__["_dict"] = existing_dict 44 | 45 | def __getattr__(self, attr): 46 | try: 47 | return self._dict[attr] 48 | except KeyError: 49 | raise AttributeError(f"No such attribute: {attr}") 50 | 51 | 52 | class CORSRequestHandler(http.server.SimpleHTTPRequestHandler): 53 | def do_OPTIONS(self): 54 | # Handle CORS preflight request 55 | self.send_response(200, "ok") 56 | self.send_header("Access-Control-Allow-Origin", "*") 57 | self.send_header("Access-Control-Allow-Methods", "POST, OPTIONS") 58 | self.send_header("Access-Control-Allow-Headers", "Content-Type") 59 | self.end_headers() 60 | 61 | def do_POST(self): 62 | if self.path == "/offer": 63 | # Read the length of the data 64 | content_length = int(self.headers["Content-Length"]) 65 | # Read the incoming data 66 | post_data = self.rfile.read(content_length) 67 | # Parse the JSON data 68 | try: 69 | data = SDPDict(json.loads(post_data)) 70 | except json.JSONDecodeError: 71 | self.send_response(400) 72 | self.end_headers() 73 | return 74 | 75 | response_data = go2_webrtc.Go2Connection.get_peer_answer( 76 | data, data.token, data.ip 77 | ) 78 | self.send_response(200) 79 | self.send_header("Content-Type", "application/json") 80 | self.send_header("Access-Control-Allow-Origin", "*") 81 | self.end_headers() 82 | 83 | self.wfile.write(json.dumps(response_data).encode("utf-8")) 84 | 85 | 86 | # Set up the server 87 | with socketserver.TCPServer(("", PORT), CORSRequestHandler) as httpd: 88 | print(f"Serving on port {PORT}") 89 | httpd.serve_forever() 90 | 91 | 92 | -------------------------------------------------------------------------------- /javascript/threejs.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | ROS2 Vertex viewer 7 | 12 | 13 | 14 | 15 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /javascript/threejs.init.js: -------------------------------------------------------------------------------- 1 | import { Go2WebRTC } from "./go2webrtc.js"; 2 | 3 | globalThis.logMessage = console.log; 4 | 5 | // Function to print incoming data 6 | function setIncomingData(data) { 7 | 8 | console.log("setIncomingData", data); 9 | 10 | if (globalThis.rtc.validationResult === "SUCCESS" && data.type === DataChannelType.VALIDATION) { 11 | console.log("Subscribing to topic rt/utlidar/voxel_map_compressed"); 12 | // globalThis.rtc.channel.send( 13 | // JSON.stringify({ 14 | // type: "subscribe", 15 | // topic: "rt/utlidar/voxel_map_compressed", 16 | // }) 17 | // ); 18 | } 19 | 20 | if (data.type === "msg" && data.topic === "rt/utlidar/voxel_map_compressed") { 21 | globalThis.voxelMap = data.data; 22 | } 23 | } 24 | 25 | // Function to load saved values from localStorage 26 | function loadSavedValues() { 27 | const savedToken = localStorage.getItem("token"); 28 | const savedRobotIP = localStorage.getItem("robotIP"); 29 | 30 | console.log("savedToken", savedToken); 31 | console.log("savedRobotIP", savedRobotIP); 32 | 33 | // Initialize RTC 34 | // globalThis.rtc = new Go2WebRTC(savedToken, savedRobotIP, setIncomingData); 35 | // globalThis.rtc.initSDP(); 36 | } 37 | 38 | // Load saved values when the page loads 39 | document.addEventListener("DOMContentLoaded", loadSavedValues); 40 | -------------------------------------------------------------------------------- /javascript/threejs.js: -------------------------------------------------------------------------------- 1 | import * as THREE from "three"; 2 | 3 | import { OrbitControls } from "three/addons/controls/OrbitControls.js"; 4 | import Stats from "three/addons/libs/stats.module.js"; 5 | import { GUI } from "three/addons/libs/lil-gui.module.min.js"; 6 | 7 | class VoxelWorld { 8 | scene; 9 | mesh = new THREE.Mesh(); 10 | material; 11 | cellSize_X; 12 | cellSize_Y; 13 | cellSize_Z; 14 | tileSize; 15 | tileTextureWidth; 16 | tileTextureHeight; 17 | currCellDataInfo; 18 | faces = [ 19 | { 20 | dir: [-1, 0, 0], 21 | corners: [ 22 | { pos: [0, 1, 0], uv: [0, 1] }, 23 | { pos: [0, 0, 0], uv: [1, 1] }, 24 | { pos: [0, 1, 1], uv: [0, 0] }, 25 | { pos: [0, 0, 1], uv: [1, 0] }, 26 | ], 27 | }, 28 | { 29 | dir: [1, 0, 0], 30 | corners: [ 31 | { pos: [1, 1, 1], uv: [1, 0] }, 32 | { pos: [1, 0, 1], uv: [0, 0] }, 33 | { pos: [1, 1, 0], uv: [1, 1] }, 34 | { pos: [1, 0, 0], uv: [0, 1] }, 35 | ], 36 | }, 37 | { 38 | dir: [0, -1, 0], 39 | corners: [ 40 | { pos: [1, 0, 1], uv: [1, 0] }, 41 | { pos: [0, 0, 1], uv: [0, 0] }, 42 | { pos: [1, 0, 0], uv: [1, 1] }, 43 | { pos: [0, 0, 0], uv: [0, 1] }, 44 | ], 45 | }, 46 | { 47 | dir: [0, 1, 0], 48 | corners: [ 49 | { pos: [0, 1, 1], uv: [0, 0] }, 50 | { pos: [1, 1, 1], uv: [1, 0] }, 51 | { pos: [0, 1, 0], uv: [0, 1] }, 52 | { pos: [1, 1, 0], uv: [1, 1] }, 53 | ], 54 | }, 55 | { 56 | dir: [0, 0, -1], 57 | corners: [ 58 | { pos: [1, 0, 0], uv: [0, 0] }, 59 | { pos: [0, 0, 0], uv: [1, 0] }, 60 | { pos: [1, 1, 0], uv: [0, 1] }, 61 | { pos: [0, 1, 0], uv: [1, 1] }, 62 | ], 63 | }, 64 | { 65 | dir: [0, 0, 1], 66 | corners: [ 67 | { pos: [0, 0, 1], uv: [0, 0] }, 68 | { pos: [1, 0, 1], uv: [1, 0] }, 69 | { pos: [0, 1, 1], uv: [0, 1] }, 70 | { pos: [1, 1, 1], uv: [1, 1] }, 71 | ], 72 | }, 73 | ]; 74 | 75 | constructor(n, o) { 76 | (this.scene = n), 77 | (this.mesh = new THREE.Mesh()), 78 | (this.tileSize = (o == null ? void 0 : o.tileSize) || 1), 79 | (this.tileTextureWidth = (o == null ? void 0 : o.tileTextureWidth) || 1), 80 | (this.tileTextureHeight = 81 | (o == null ? void 0 : o.tileTextureHeight) || 1), 82 | (this.material = 83 | (o == null ? void 0 : o.material) || 84 | new THREE.MeshBasicMaterial({ color: 16777215 })), 85 | (this.currCellDataInfo = void 0), 86 | (this.cellSize_X = 128), 87 | (this.cellSize_Y = 128), 88 | (this.cellSize_Z = 30); 89 | } 90 | clearVoxel() { 91 | this.currCellDataInfo = void 0; 92 | } 93 | adjacent(n, o) { 94 | const { cellSize_X: s, cellSize_Y: c, cellSize_Z: u } = this, 95 | [l, f, _] = o; 96 | return l > s || f > c || _ > u ? 0 : this.getVoxel(n, l, f, _); 97 | } 98 | calBitForIndex(n, o) { 99 | return (n >> (7 - o)) & 1; 100 | } 101 | getVoxel(n, o, s, c) { 102 | const { cellSize_X: u, cellSize_Y: l, calBitForIndex: f } = this, 103 | _ = u * l * c + u * s + o, 104 | g = Math.floor(_ / 8), 105 | v = _ % 8; 106 | return f.call(this, n[g], v); 107 | } 108 | generateGeometryData(n, o, s, c) { 109 | const { 110 | adjacent: u, 111 | cellSize_X: l, 112 | cellSize_Y: f, 113 | tileSize: _, 114 | tileTextureWidth: g, 115 | tileTextureHeight: v, 116 | } = this, 117 | T = [], 118 | E = [], 119 | y = []; 120 | (this.cellSize_X = o[0]), 121 | (this.cellSize_Y = o[1]), 122 | (this.cellSize_Z = o[2]); 123 | let S = 0; 124 | for (let C = 0; C < n.byteLength; C++) 125 | if (n[C] > 0) { 126 | const R = n[C]; 127 | for (let A = 0; A < 8; A++) 128 | if (this.calBitForIndex(R, A)) { 129 | const O = C * 8 + A; 130 | S++; 131 | const L = Math.floor(O / (l * f)), 132 | P = O % (l * f), 133 | $ = Math.floor(P / l), 134 | B = P % l, 135 | F = (L * s + c) * Math.round(1 / s), 136 | J = Math.floor((F < -10 ? -10 : F > 20 ? 20 : F) + 10); 137 | for (const { dir: H, corners: j } of this.faces) 138 | if (!u.call(this, n, [B + H[0], $ + H[1], L + H[2]])) { 139 | const he = T.length / 3; 140 | for (const { pos: Te, uv: re } of j) 141 | T.push(Te[0] + B, Te[1] + $, Te[2] + L), 142 | E.push(((J + re[0]) * _) / g, 1 - ((1 - re[1]) * _) / v); 143 | y.push(he, he + 1, he + 2, he + 2, he + 1, he + 3); 144 | } 145 | } 146 | } 147 | return { 148 | positionsFloat32Array: new Float32Array(T), 149 | uvsFloat32Array: new Float32Array(E), 150 | indices: y, 151 | pointCount: S, 152 | }; 153 | } 154 | updateMeshesForData2() { 155 | // debugger 156 | // // set n to the content of vortex_msg1192132805_pointcloud.json 157 | const { currCellDataInfo, material, scene } = this; 158 | if (!currCellDataInfo || !scene) return; 159 | const { geometryData, resolution: resolution, origin } = currCellDataInfo; 160 | 161 | this.mesh.geometry.dispose(); 162 | this.mesh.material.dispose(); 163 | scene.remove(this.mesh); 164 | 165 | const positions = convert(geometryData.positions); 166 | const uvs = convert(geometryData.uvs); 167 | const indices = convert32(geometryData.indices); 168 | // debugger 169 | const buffGeometry = new THREE.BufferGeometry(); 170 | buffGeometry.setAttribute( 171 | "position", 172 | new THREE.BufferAttribute(positions || [], 3) 173 | ); 174 | buffGeometry.setAttribute( 175 | "uv", 176 | new THREE.BufferAttribute(uvs || [], 2, !0) 177 | ); 178 | buffGeometry.setIndex(new THREE.BufferAttribute(indices || [], 1)); 179 | this.mesh = new THREE.Mesh(buffGeometry, material); 180 | const res = resolution || 0.1; 181 | this.mesh.scale.set(res, res, res); 182 | this.mesh.position.set(origin[0] || 0, origin[1] || 0, origin[2] || 0); 183 | scene.add(this.mesh); 184 | } 185 | } 186 | 187 | function convert(objData) { 188 | return Uint8Array.from(objData); 189 | } 190 | function convert32(objData) { 191 | return Uint32Array.from(objData); 192 | } 193 | 194 | const scene = new THREE.Scene(); 195 | scene.rotation.x -= Math.PI / 2; 196 | 197 | const camera = new THREE.PerspectiveCamera( 198 | 45, 199 | window.innerWidth / window.innerHeight, 200 | 1, 201 | 100 202 | ); 203 | camera.position.z = 10; 204 | 205 | const renderer = new THREE.WebGLRenderer(); 206 | renderer.setSize(window.innerWidth, window.innerHeight); 207 | document.body.appendChild(renderer.domElement); 208 | 209 | const controls = new OrbitControls(camera, renderer.domElement); 210 | 211 | window.addEventListener( 212 | "resize", 213 | function () { 214 | camera.aspect = window.innerWidth / window.innerHeight; 215 | camera.updateProjectionMatrix(); 216 | renderer.setSize(window.innerWidth, window.innerHeight); 217 | render(); 218 | }, 219 | false 220 | ); 221 | 222 | const stats = Stats(); 223 | document.body.appendChild(stats.dom); 224 | 225 | const gui = new GUI(); 226 | const cameraFolder = gui.addFolder("Camera"); 227 | cameraFolder.add(camera.position, "z", 0, 10); 228 | cameraFolder.open(); 229 | 230 | function animate() { 231 | requestAnimationFrame(animate); 232 | controls.update(); 233 | render(); 234 | stats.update(); 235 | } 236 | 237 | function render() { 238 | renderer.render(scene, camera); 239 | } 240 | 241 | const init = ({ 242 | renderParent, 243 | scene = new THREE.Scene(), 244 | renderer = new THREE.WebGLRenderer({ antialias: !0, alpha: !0 }), 245 | camera = new THREE.PerspectiveCamera(50), 246 | controls = new OrbitControls(camera, renderer.domElement), 247 | ambientLight = new THREE.AmbientLight(16777215), 248 | gridHelper = new THREE.GridHelper(40, 40, 8947848), 249 | gridHelperGroup = new THREE.Group(), 250 | viewType = 1, 251 | stats = Stats(), 252 | showStats = !1, 253 | currCameraPosition = new THREE.Vector3(0, 0, 0), 254 | firstViewTargetPoint = new THREE.Mesh(), 255 | firstViewTargetPosition = new THREE.Vector3(4, 0, 0), 256 | firstCameraPosition = new THREE.Vector3(-1.2, 0, 1), 257 | thirdViewInitPosition = new THREE.Vector3(-3, 0, 3), 258 | }) => { 259 | if (!renderParent) return; 260 | renderer.setPixelRatio(Math.min(window.devicePixelRatio, 2)), 261 | (scene.background = new THREE.Color(2631720)); 262 | const A = renderParent.clientWidth || 0, 263 | O = renderParent.clientHeight || 0; 264 | renderer.setSize(A, O), 265 | (renderer.shadowMap.enabled = !0), 266 | renderParent.appendChild(renderer.domElement), 267 | (scene.fog = viewType === 1 ? null : new THREE.Fog(2631720, 0.015, 20)), 268 | (camera.aspect = A / O), 269 | camera.updateProjectionMatrix(), 270 | viewType === 1 271 | ? camera.position.copy(thirdViewInitPosition) 272 | : camera.position.copy(firstCameraPosition), 273 | currCameraPosition.copy(thirdViewInitPosition), 274 | scene.add(camera); 275 | const L = new THREE.BoxGeometry(0.1, 0.1, 0.1), 276 | P = new THREE.MeshBasicMaterial({ 277 | color: 16711680, 278 | transparent: !0, 279 | opacity: 0, 280 | }); 281 | (firstViewTargetPoint.geometry = L), 282 | (firstViewTargetPoint.material = P), 283 | firstViewTargetPoint.position.copy(firstViewTargetPosition), 284 | scene.add(firstViewTargetPoint), 285 | (controls.enableDamping = !0), 286 | (controls.enabled = viewType === 1), 287 | (controls.enablePan = !1), 288 | (controls.minPolarAngle = 0.2), 289 | (controls.maxPolarAngle = (Math.PI / 4) * 3), 290 | showStats && 291 | (document.body.appendChild(stats.dom), 292 | (stats.dom.style.top = "80px"), 293 | (stats.dom.style.left = "115px")), 294 | gridHelperGroup.add(gridHelper), 295 | gridHelper.rotateX(Math.PI / 2), 296 | scene.add(gridHelperGroup), 297 | scene.add(ambientLight); 298 | // this.loadModel(); 299 | const tileSize = 32, 300 | tileTextureWidth = 1024, 301 | tileTextureHeight = 32; 302 | const textureLoader = new THREE.TextureLoader().load( 303 | "./models/axisColor4.png" 304 | ); 305 | textureLoader.magFilter = THREE.NearestFilter; 306 | textureLoader.minFilter = THREE.NearestFilter; 307 | const worldMaterial = new THREE.MeshBasicMaterial({ 308 | map: textureLoader, 309 | side: THREE.DoubleSide, 310 | transparent: !1, 311 | }); 312 | const pointVoxelWorld = new VoxelWorld(scene, { 313 | tileSize, 314 | tileTextureWidth, 315 | tileTextureHeight, 316 | material: worldMaterial, 317 | }); 318 | const pointUpdated = !1; 319 | // window.addEventListener("resize", this.resize.bind(this)); 320 | return { 321 | pointVoxelWorld, 322 | pointUpdated, 323 | }; 324 | }; 325 | 326 | const { pointVoxelWorld, pointUpdated } = init({ 327 | renderParent: document.body, 328 | scene, 329 | camera: camera, 330 | controls, 331 | renderer, 332 | }); 333 | 334 | animate() 335 | 336 | 337 | window.getBinaryData = (filepath) => { 338 | return fetch(filepath) 339 | .then(response => { 340 | if (!response.ok) { 341 | console.error('Fetch failed:', { 342 | status: response.status, 343 | statusText: response.statusText, 344 | url: filepath 345 | }); 346 | throw new Error(`Failed to fetch data from ${filepath} (Status: ${response.status})`); 347 | } 348 | return response.arrayBuffer(); 349 | }) 350 | .then(arrayBuffer => { 351 | return new Uint8Array(arrayBuffer); 352 | }) 353 | .catch(error => { 354 | throw error; 355 | }); 356 | }; 357 | 358 | const threeJSWorker = new Worker( 359 | new URL("/assets/three.worker.js", self.location) 360 | ); 361 | window._threejsworker = threeJSWorker; 362 | threeJSWorker.onmessage = (re) => { 363 | console.log("Binary Data", re); 364 | pointVoxelWorld.currCellDataInfo = re.data 365 | pointVoxelWorld.updateMeshesForData2() 366 | } 367 | 368 | setInterval(() => { 369 | try { 370 | console.warn("TICK"); 371 | window 372 | .getBinaryData(`/assets/example.bin`) 373 | .then((vortexBinaryData) => { 374 | const _jsonLength = vortexBinaryData[0]; 375 | const _jsonOffset = 4; 376 | const _jsonString = String.fromCharCode.apply( 377 | null, 378 | vortexBinaryData.slice( 379 | _jsonOffset, 380 | _jsonOffset + _jsonLength 381 | ) 382 | ); 383 | const jsonOBJ = JSON.parse(_jsonString); 384 | threeJSWorker.postMessage({ 385 | resolution: jsonOBJ.data.resolution, 386 | origin: jsonOBJ.data.origin, 387 | width: jsonOBJ.data.width, 388 | data: vortexBinaryData.slice(_jsonOffset + _jsonLength), 389 | }); 390 | }); 391 | } catch (e) { 392 | console.error("ERROR DURING VERTEX LOAD", e); 393 | } 394 | }, 1000) 395 | -------------------------------------------------------------------------------- /javascript/utils.js: -------------------------------------------------------------------------------- 1 | // import { MD5} from './md5.js'; 2 | 3 | 4 | function hexToBase64(r) { 5 | var o; 6 | const n = 7 | (o = r.match(/.{1,2}/g)) == null ? void 0 : o.map((s) => parseInt(s, 16)); 8 | return window.btoa(String.fromCharCode.apply(null, n)); 9 | } 10 | 11 | export const encryptKey = (r) => { 12 | const n = `UnitreeGo2_${r}`, 13 | o = encryptByMd5(n); 14 | return hexToBase64(o); 15 | }; 16 | 17 | function encryptByMd5(r) { 18 | return md5(r).toString(); 19 | } 20 | -------------------------------------------------------------------------------- /python/README.md: -------------------------------------------------------------------------------- 1 | # Unitree Go2 Python API (WebRTC) 2 | 3 | The Unitree Go2 Python API provides a comprehensive interface for interacting with Unitree Go2 robots, facilitating control over WebRTC connections, video streaming, and MQTT bridgin. 4 | This API is designed to simplify the development of applications that communicate with Unitree Go2 robots, enabling developers to focus on creating innovative solutions. 5 | 6 | The API works with Air, Pro and Edu models. 7 | 8 | ## Features 9 | * WebRTC Connection: Establish and manage WebRTC connections for real-time communication with the robot. 10 | * (WIP) Video Streaming: Capture and stream video data from the robot's cameras. 11 | * MQTT Messaging: Send and receive messages using MQTT for command and control. 12 | 13 | ## Installation 14 | To install the Unitree Go2 Python API, ensure you have Python 3.6 or later. You can install the package using pip: 15 | 16 | ```bash 17 | pip install git+https://github.com/tfoldi/go2-webrtc.git#subdirectory=python 18 | ``` 19 | 20 | ## Usage 21 | 22 | ### Establishing a WebRTC Connection 23 | 24 | ```python 25 | from go2_webrtc import Go2Connection 26 | 27 | # Initialize the connection 28 | conn = Go2Connection(robot_ip="192.168.1.1", token="your_token_here") 29 | 30 | # Connect to the robot 31 | await conn.connect() 32 | ``` 33 | 34 | ### Examples 35 | 36 | Find detailed examples in the examples directory. Current examples 37 | 38 | * MQTT bridge between Go2 and an external MQTT Server 39 | * Joystick support (like xbox controller) via pygame 40 | 41 | 42 | ## Contributing 43 | Contributions to the Unitree Go2 Python API are welcome! Send a PR. 44 | 45 | ## License 46 | This project is licensed under the BSD 3-Clause License - see the LICENSE file for details. 47 | 48 | -------------------------------------------------------------------------------- /python/examples/joystick/go2_joystick.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2024, RoboVerse community 2 | # 3 | # Redistribution and use in source and binary forms, with or without 4 | # modification, are permitted provided that the following conditions are met: 5 | # 6 | # 1. Redistributions of source code must retain the above copyright notice, this 7 | # list of conditions and the following disclaimer. 8 | # 9 | # 2. Redistributions in binary form must reproduce the above copyright notice, 10 | # this list of conditions and the following disclaimer in the documentation 11 | # and/or other materials provided with the distribution. 12 | # 13 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 14 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 15 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 16 | # DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 17 | # FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 18 | # DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 19 | # SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 20 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 21 | # OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 22 | # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | 24 | import asyncio 25 | import json 26 | import os 27 | import pygame 28 | 29 | from go2_webrtc import Go2Connection, ROBOT_CMD 30 | 31 | 32 | JOY_SENSE = 0.2 33 | 34 | 35 | def gen_command(cmd: int): 36 | command = { 37 | "type": "msg", 38 | "topic": "rt/api/sport/request", 39 | "data": { 40 | "header": {"identity": {"id": Go2Connection.generate_id(), "api_id": cmd}}, 41 | "parameter": json.dumps(cmd), 42 | }, 43 | } 44 | command = json.dumps(command) 45 | return command 46 | 47 | 48 | def gen_mov_command(x: float, y: float, z: float): 49 | x = x * JOY_SENSE 50 | y = y * JOY_SENSE 51 | 52 | command = { 53 | "type": "msg", 54 | "topic": "rt/api/sport/request", 55 | "data": { 56 | "header": {"identity": {"id": Go2Connection.generate_id(), "api_id": 1008}}, 57 | "parameter": json.dumps({"x": x, "y": y, "z": z}), 58 | }, 59 | } 60 | command = json.dumps(command) 61 | return command 62 | 63 | 64 | async def get_joystick_values(): 65 | pygame.init() 66 | pygame.joystick.init() 67 | 68 | if pygame.joystick.get_count() > 0: 69 | joystick = pygame.joystick.Joystick(0) 70 | joystick.init() 71 | 72 | axis0 = round(joystick.get_axis(0), 1) * -1 73 | axis1 = round(joystick.get_axis(1), 1) * -1 74 | axis2 = round(joystick.get_axis(2), 1) * -1 75 | axis3 = round(joystick.get_axis(3), 1) * -1 76 | btn_a_is_pressed = joystick.get_button(0) 77 | btn_b_is_pressed = joystick.get_button(1) 78 | 79 | return { 80 | "Axis 0": axis0, 81 | "Axis 1": axis1, 82 | "Axis 2": axis2, 83 | "Axis 3": axis3, 84 | "a": btn_a_is_pressed, 85 | "b": btn_b_is_pressed, 86 | } 87 | 88 | return {"Axis 0": 0, "Axis 1": 0, "Axis 2": 0, "Axis 3": 0, "a": 0, "b": 0} 89 | 90 | 91 | async def start_joy_bridge(robot_conn): 92 | await robot_conn.connect_robot() 93 | 94 | while True: 95 | joystick_values = await get_joystick_values() 96 | joy_move_x = joystick_values["Axis 1"] 97 | joy_move_y = joystick_values["Axis 0"] 98 | joy_move_z = joystick_values["Axis 2"] 99 | joy_btn_a_is_pressed = joystick_values["a"] 100 | joy_btn_b_is_pressed = joystick_values["b"] 101 | 102 | if joy_btn_a_is_pressed == 1: 103 | robot_cmd = gen_command(ROBOT_CMD["StandUp"]) 104 | robot_conn.data_channel.send(robot_cmd) 105 | 106 | if joy_btn_b_is_pressed == 1: 107 | robot_cmd = gen_command(ROBOT_CMD["StandDown"]) 108 | robot_conn.data_channel.send(robot_cmd) 109 | 110 | if abs(joy_move_x) > 0.0 or abs(joy_move_y) > 0.0 or abs(joy_move_z) > 0.0: 111 | robot_cmd = gen_mov_command(joy_move_x, joy_move_y, joy_move_z) 112 | robot_conn.data_channel.send(robot_cmd) 113 | 114 | await asyncio.sleep(0.1) 115 | 116 | 117 | async def main(): 118 | conn = Go2Connection( 119 | os.getenv("GO2_IP"), 120 | os.getenv("GO2_TOKEN"), 121 | ) 122 | 123 | coroutine = await start_joy_bridge(conn) 124 | 125 | loop = asyncio.get_event_loop() 126 | try: 127 | loop.run_until_complete(coroutine) 128 | except KeyboardInterrupt: 129 | pass 130 | finally: 131 | loop.run_until_complete(conn.pc.close()) 132 | 133 | 134 | asyncio.run(main()) 135 | -------------------------------------------------------------------------------- /python/examples/mqtt/go2_mqtt.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2024, RoboVerse community 2 | # 3 | # Redistribution and use in source and binary forms, with or without 4 | # modification, are permitted provided that the following conditions are met: 5 | # 6 | # 1. Redistributions of source code must retain the above copyright notice, this 7 | # list of conditions and the following disclaimer. 8 | # 9 | # 2. Redistributions in binary form must reproduce the above copyright notice, 10 | # this list of conditions and the following disclaimer in the documentation 11 | # and/or other materials provided with the distribution. 12 | # 13 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 14 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 15 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 16 | # DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 17 | # FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 18 | # DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 19 | # SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 20 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 21 | # OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 22 | # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | 24 | import asyncio 25 | import os 26 | import paho.mqtt.client as mqtt 27 | import logging 28 | import json 29 | from queue import Queue, Empty 30 | 31 | 32 | from go2_webrtc import Go2Connection, RTC_TOPIC 33 | 34 | MQTT_TOPIC = "/rt/mqtt/request" 35 | 36 | logging.basicConfig(level=logging.WARN) 37 | logger = logging.getLogger(__name__) 38 | logger.setLevel(logging.DEBUG) 39 | 40 | 41 | class Go2MQTTBridge: 42 | def __init__(self): 43 | self.mqtt_client = None 44 | self.initialize_mqtt_client() 45 | self.msg_queue = Queue() 46 | self.conn = None 47 | 48 | def on_connect(self, client, userdata, flags, rc, properties): 49 | logger.debug("MQTT: Connected with result code %s", rc) 50 | result, mid = self.mqtt_client.subscribe(MQTT_TOPIC) 51 | if result == mqtt.MQTT_ERR_SUCCESS: 52 | logger.debug(f"MQTT: subscription to {MQTT_TOPIC} succeeded with MID {mid}") 53 | else: 54 | logger.debug( 55 | f"MQTT: subscription to {MQTT_TOPIC} failed with result code {result}" 56 | ) 57 | 58 | def on_message(self, client, userdata, msg): 59 | self.msg_queue.put(msg.payload) 60 | 61 | def initialize_mqtt_client(self): 62 | mqtt_broker = os.getenv("MQTT_BROKER") 63 | mqtt_username = os.getenv("MQTT_USERNAME") 64 | mqtt_password = os.getenv("MQTT_PASSWORD") 65 | mqtt_port = int(os.getenv("MQTT_PORT", 1883)) 66 | 67 | self.mqtt_client = mqtt.Client( 68 | mqtt.CallbackAPIVersion.VERSION2, client_id="Go2", protocol=mqtt.MQTTv311 69 | ) 70 | self.mqtt_client.on_connect = self.on_connect 71 | self.mqtt_client.on_message = self.on_message 72 | self.mqtt_client.enable_logger() 73 | 74 | self.mqtt_client.username_pw_set(username=mqtt_username, password=mqtt_password) 75 | self.mqtt_client.connect(host=mqtt_broker, port=mqtt_port, keepalive=120) 76 | self.mqtt_client.loop_start() 77 | 78 | async def mqtt_loop(self, conn): 79 | self.conn = conn 80 | 81 | while True: 82 | if self.conn.data_channel.readyState == "open": 83 | while True: 84 | try: 85 | while not self.msg_queue.empty(): 86 | msg = self.msg_queue.get_nowait() 87 | msg = msg.decode("utf-8") 88 | logger.debug(f"MQTT->RTC Sending message {msg} to Go2") 89 | conn.data_channel.send(msg) 90 | except Empty: 91 | pass 92 | # wait for a short time before checking the queue again 93 | await asyncio.sleep(0.1) 94 | else: 95 | # wait until the data channel is open 96 | await asyncio.sleep(1) 97 | 98 | def on_validated(self): 99 | for topic in RTC_TOPIC.values(): 100 | conn.data_channel.send(json.dumps({"type": "subscribe", "topic": topic})) 101 | 102 | def on_data_channel_message(self, message, msgobj): 103 | logger.debug( 104 | "RTC->MQTT Received message type=%s topic=%s", 105 | msgobj.get("type"), 106 | msgobj.get("topic"), 107 | ) 108 | 109 | if self.mqtt_client: 110 | self.mqtt_client.publish(msgobj.get("topic", "rt/system"), message, qos=1) 111 | else: 112 | logger.warn("MQTT client not initialized") 113 | 114 | 115 | # TODO: parse command line arguments 116 | if __name__ == "__main__": 117 | # connect to MQTT broker 118 | mqtt_bridge = Go2MQTTBridge() 119 | 120 | # set up connection to Go2 121 | conn = Go2Connection( 122 | os.getenv("GO2_IP"), 123 | os.getenv("GO2_TOKEN", ""), 124 | on_validated=mqtt_bridge.on_validated, 125 | on_message=mqtt_bridge.on_data_channel_message, 126 | ) 127 | 128 | loop = asyncio.get_event_loop() 129 | 130 | try: 131 | loop.run_until_complete(conn.connect_robot()) 132 | loop.run_until_complete(mqtt_bridge.mqtt_loop(conn)) 133 | except KeyboardInterrupt: 134 | pass 135 | finally: 136 | loop.run_until_complete(conn.pc.close()) 137 | -------------------------------------------------------------------------------- /python/go2_webrtc/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2024, RoboVerse community 2 | # 3 | # Redistribution and use in source and binary forms, with or without 4 | # modification, are permitted provided that the following conditions are met: 5 | # 6 | # 1. Redistributions of source code must retain the above copyright notice, this 7 | # list of conditions and the following disclaimer. 8 | # 9 | # 2. Redistributions in binary form must reproduce the above copyright notice, 10 | # this list of conditions and the following disclaimer in the documentation 11 | # and/or other materials provided with the distribution. 12 | # 13 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 14 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 15 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 16 | # DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 17 | # FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 18 | # DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 19 | # SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 20 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 21 | # OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 22 | # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | 24 | from .go2_connection import Go2Connection 25 | from .constants import SPORT_CMD, DATA_CHANNEL_TYPE, RTC_TOPIC, ROBOT_CMD 26 | -------------------------------------------------------------------------------- /python/go2_webrtc/constants.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2024, RoboVerse community 2 | # 3 | # Redistribution and use in source and binary forms, with or without 4 | # modification, are permitted provided that the following conditions are met: 5 | # 6 | # 1. Redistributions of source code must retain the above copyright notice, this 7 | # list of conditions and the following disclaimer. 8 | # 9 | # 2. Redistributions in binary form must reproduce the above copyright notice, 10 | # this list of conditions and the following disclaimer in the documentation 11 | # and/or other materials provided with the distribution. 12 | # 13 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 14 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 15 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 16 | # DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 17 | # FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 18 | # DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 19 | # SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 20 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 21 | # OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 22 | # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | 24 | SPORT_CMD = { 25 | 1001: "Damp", 26 | 1002: "BalanceStand", 27 | 1003: "StopMove", 28 | 1004: "StandUp", 29 | 1005: "StandDown", 30 | 1006: "RecoveryStand", 31 | 1007: "Euler", 32 | 1008: "Move", 33 | 1009: "Sit", 34 | 1010: "RiseSit", 35 | 1011: "SwitchGait", 36 | 1012: "Trigger", 37 | 1013: "BodyHeight", 38 | 1014: "FootRaiseHeight", 39 | 1015: "SpeedLevel", 40 | 1016: "Hello", 41 | 1017: "Stretch", 42 | 1018: "TrajectoryFollow", 43 | 1019: "ContinuousGait", 44 | 1020: "Content", 45 | 1021: "Wallow", 46 | 1022: "Dance1", 47 | 1023: "Dance2", 48 | 1024: "GetBodyHeight", 49 | 1025: "GetFootRaiseHeight", 50 | 1026: "GetSpeedLevel", 51 | 1027: "SwitchJoystick", 52 | 1028: "Pose", 53 | 1029: "Scrape", 54 | 1030: "FrontFlip", 55 | 1031: "FrontJump", 56 | 1032: "FrontPounce", 57 | 1033: "WiggleHips", 58 | 1034: "GetState", 59 | 1035: "EconomicGait", 60 | 1036: "FingerHeart", 61 | } 62 | 63 | ROBOT_CMD = { 64 | "Damp": 1001, 65 | "BalanceStand": 1002, 66 | "StopMove": 1003, 67 | "StandUp": 1004, 68 | "StandDown": 1005, 69 | "RecoveryStand": 1006, 70 | "Euler": 1007, 71 | "Move": 1008, 72 | "Sit": 1009, 73 | "RiseSit": 1010, 74 | "SwitchGait": 1011, 75 | "Trigger": 1012, 76 | "BodyHeight": 1013, 77 | "FootRaiseHeight": 1014, 78 | "SpeedLevel": 1015, 79 | "Hello": 1016, 80 | "Stretch": 1017, 81 | "TrajectoryFollow": 1018, 82 | "ContinuousGait": 1019, 83 | "Content": 1020, 84 | "Wallow": 1021, 85 | "Dance1": 1022, 86 | "Dance2": 1023, 87 | "GetBodyHeight": 1024, 88 | "GetFootRaiseHeight": 1025, 89 | "GetSpeedLevel": 1026, 90 | "SwitchJoystick": 1027, 91 | "Pose": 1028, 92 | "Scrape": 1029, 93 | "FrontFlip": 1030, 94 | "FrontJump": 1031, 95 | "FrontPounce": 1032, 96 | "WiggleHips": 1033, 97 | "GetState": 1034, 98 | "EconomicGait": 1035, 99 | "FingerHeart": 1036, 100 | } 101 | 102 | DATA_CHANNEL_TYPE = { 103 | "VALIDATION": "validation", 104 | "SUBSCRIBE": "subscribe", 105 | "UNSUBSCRIBE": "unsubscribe", 106 | "MSG": "msg", 107 | "REQUEST": "request", 108 | "RESPONSE": "response", 109 | "VID": "vid", 110 | "AUD": "aud", 111 | "ERR": "err", 112 | "HEARTBEAT": "heartbeat", 113 | "RTC_INNER_REQ": "rtc_inner_req", 114 | "RTC_REPORT": "rtc_report", 115 | "ADD_ERROR": "add_error", 116 | "RM_ERROR": "rm_error", 117 | "ERRORS": "errors", 118 | } 119 | 120 | RTC_TOPIC = { 121 | "LOW_STATE": "rt/lf/lowstate", 122 | "MULTIPLE_STATE": "rt/multiplestate", 123 | "FRONT_PHOTO_REQ": "rt/api/videohub/request", 124 | "ULIDAR_SWITCH": "rt/utlidar/switch", 125 | "ULIDAR": "rt/utlidar/voxel_map", 126 | "ULIDAR_ARRAY": "rt/utlidar/voxel_map_compressed", 127 | "ULIDAR_STATE": "rt/utlidar/lidar_state", 128 | "ROBOTODOM": "rt/utlidar/robot_pose", 129 | "UWB_REQ": "rt/api/uwbswitch/request", 130 | "UWB_STATE": "rt/uwbstate", 131 | "LOW_CMD": "rt/lowcmd", 132 | "WIRELESS_CONTROLLER": "rt/wirelesscontroller", 133 | "SPORT_MOD": "rt/api/sport/request", 134 | "SPORT_MOD_STATE": "rt/sportmodestate", 135 | "LF_SPORT_MOD_STATE": "rt/lf/sportmodestate", 136 | "BASH_REQ": "rt/api/bashrunner/request", 137 | "SELF_TEST": "rt/selftest", 138 | "GRID_MAP": "rt/mapping/grid_map", 139 | "SERVICE_STATE": "rt/servicestate", 140 | "GPT_FEEDBACK": "rt/gptflowfeedback", 141 | "VUI": "rt/api/vui/request", 142 | "OBSTACLES_AVOID": "rt/api/obstacles_avoid/request", 143 | "SLAM_QT_COMMAND": "rt/qt_command", 144 | "SLAM_ADD_NODE": "rt/qt_add_node", 145 | "SLAM_ADD_EDGE": "rt/qt_add_edge", 146 | "SLAM_QT_NOTICE": "rt/qt_notice", 147 | "SLAM_PC_TO_IMAGE_LOCAL": "rt/pctoimage_local", 148 | "SLAM_ODOMETRY": "rt/lio_sam_ros2/mapping/odometry", 149 | "ARM_COMMAND": "rt/arm_Command", 150 | "ARM_FEEDBACK": "rt/arm_Feedback", 151 | "AUDIO_HUB_REQ": "rt/api/audiohub/request", 152 | "AUDIO_HUB_PLAY_STATE": "rt/audiohub/player/state", 153 | } 154 | -------------------------------------------------------------------------------- /python/go2_webrtc/go2_connection.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2024, RoboVerse community 2 | # 3 | # Redistribution and use in source and binary forms, with or without 4 | # modification, are permitted provided that the following conditions are met: 5 | # 6 | # 1. Redistributions of source code must retain the above copyright notice, this 7 | # list of conditions and the following disclaimer. 8 | # 9 | # 2. Redistributions in binary form must reproduce the above copyright notice, 10 | # this list of conditions and the following disclaimer in the documentation 11 | # and/or other materials provided with the distribution. 12 | # 13 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 14 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 15 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 16 | # DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 17 | # FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 18 | # DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 19 | # SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 20 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 21 | # OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 22 | # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | 24 | import asyncio 25 | from aiortc import ( 26 | RTCPeerConnection, 27 | RTCSessionDescription, 28 | AudioStreamTrack, 29 | VideoStreamTrack, 30 | ) 31 | from aiortc.contrib.media import MediaBlackhole, MediaRecorder 32 | import aiohttp 33 | import datetime 34 | import random 35 | import binascii 36 | import uuid 37 | from Crypto.PublicKey import RSA 38 | from Crypto.Cipher import AES 39 | from Crypto.Cipher import PKCS1_v1_5 40 | import requests 41 | 42 | 43 | # from go2_webrtc.go2_cv_video import Go2CvVideo 44 | from go2_webrtc.constants import SPORT_CMD, DATA_CHANNEL_TYPE 45 | import logging 46 | from dotenv import load_dotenv 47 | import os 48 | import json 49 | import hashlib 50 | import struct 51 | import base64 52 | 53 | from go2_webrtc.lidar_decoder import LidarDecoder 54 | 55 | 56 | load_dotenv() 57 | 58 | 59 | logging.basicConfig(level=logging.WARN) 60 | logger = logging.getLogger(__name__) 61 | logger.setLevel(logging.DEBUG) 62 | 63 | 64 | decoder = LidarDecoder() 65 | 66 | 67 | class Go2AudioTrack(AudioStreamTrack): 68 | kind = "audio" 69 | 70 | 71 | class Go2VideoTrack(VideoStreamTrack): 72 | kind = "video" 73 | 74 | 75 | class Go2Connection: 76 | def __init__( 77 | self, ip=None, token="", on_validated=None, on_message=None, on_open=None 78 | ): 79 | self.pc = RTCPeerConnection() 80 | self.ip = ip 81 | self.token = token 82 | self.validation_result = "PENDING" 83 | self.on_validated = on_validated 84 | self.on_message = on_message 85 | self.on_open = on_open 86 | 87 | # self.audio_track = Go2AudioTrack() 88 | # self.video_track = Go2VideoTrack() 89 | # self.video_track = Go2CvVideo() 90 | self.audio_track = MediaBlackhole() 91 | self.video_track = MediaBlackhole() 92 | 93 | # Create and add a data channel 94 | self.data_channel = self.pc.createDataChannel("data", id=2, negotiated=False) 95 | self.data_channel.on("open", self.on_data_channel_open) 96 | self.data_channel.on("message", self.on_data_channel_message) 97 | 98 | # self.pc.addTransceiver("video", direction="recvonly") 99 | # self.pc.addTransceiver("audio", direction="sendrecv") 100 | # self.pc.addTrack(AudioStreamTrack()) 101 | 102 | self.pc.on("track", self.on_track) 103 | self.pc.on("connectionstatechange", self.on_connection_state_change) 104 | 105 | def on_connection_state_change(self): 106 | logger.info("Connection state is %s", self.pc.connectionState) 107 | 108 | def on_track(self, track): 109 | logger.debug("Receiving %s", track.kind) 110 | if track.kind == "audio": 111 | pass 112 | # self.audio_track.addTrack(track) 113 | elif track.kind == "video": 114 | # self.video_track.addTrack(track) 115 | pass 116 | 117 | async def generate_offer(self): 118 | logger.debug("Generating offer") 119 | await self.audio_track.start() 120 | await self.video_track.start() 121 | 122 | offer = await self.pc.createOffer() 123 | logger.debug(offer.sdp) 124 | 125 | await self.pc.setLocalDescription(offer) 126 | return offer.sdp 127 | 128 | async def connect(self): 129 | logger.info("Connected to the robot") 130 | 131 | async def set_answer(self, sdp): 132 | """Set the remote description with the provided answer.""" 133 | answer = RTCSessionDescription(sdp, type="answer") 134 | await self.pc.setRemoteDescription(answer) 135 | 136 | def on_data_channel_open(self): 137 | logger.debug("Data channel is open") 138 | if self.on_open: 139 | self.on_open() 140 | 141 | def on_data_channel_message(self, message): 142 | logger.debug("Received message: %s", message) 143 | 144 | # If the data channel is not open, open it 145 | # it should not be closed if got a message 146 | if self.data_channel.readyState != "open": 147 | self.data_channel._setReadyState("open") 148 | 149 | try: 150 | if isinstance(message, str): 151 | msgobj = json.loads(message) 152 | if msgobj.get("type") == "validation": 153 | self.validate(msgobj) 154 | elif isinstance(message, bytes): 155 | msgobj = Go2Connection.deal_array_buffer(message) 156 | 157 | if self.on_message: 158 | self.on_message(message, msgobj) 159 | 160 | except json.JSONDecodeError: 161 | pass 162 | 163 | def validate(self, message): 164 | if message.get("data") == "Validation Ok.": 165 | self.validation_result = "SUCCESS" 166 | if self.on_validated: 167 | self.on_validated() 168 | else: 169 | self.publish( 170 | "", 171 | self.encrypt_key(message.get("data")), 172 | DATA_CHANNEL_TYPE["VALIDATION"], 173 | ) 174 | 175 | def publish(self, topic, data, msg_type): 176 | if not self.data_channel or not self.data_channel.readyState == "open": 177 | logger.error( 178 | "Data channel is not open. State is %s", self.data_channel.readyState 179 | ) 180 | return 181 | payload = { 182 | "type": msg_type or DATA_CHANNEL_TYPE["MESSAGE"], 183 | "topic": topic, 184 | "data": data, 185 | } 186 | logger.debug("-> Sending message %s", json.dumps(payload)) 187 | self.data_channel.send(json.dumps(payload)) 188 | 189 | async def connect_robot_v10(self): 190 | """Post the offer to an HTTP server and set the received answer.""" 191 | offer_sdp = await self.generate_offer() 192 | async with aiohttp.ClientSession() as session: 193 | url = f"http://{self.ip}:8081/offer" 194 | headers = {"content-type": "application/json"} 195 | data = { 196 | "sdp": offer_sdp, 197 | "id": "STA_localNetwork", 198 | "type": "offer", 199 | "token": self.token, 200 | } 201 | logger.debug("Sending offer to the signaling server at %s", url) 202 | 203 | async with session.post(url, json=data, headers=headers) as resp: 204 | if resp.status == 200: 205 | answer_data = await resp.json() 206 | logger.info("Received answer from server") 207 | logger.debug(answer_data["sdp"]) 208 | answer_sdp = answer_data.get("sdp") 209 | await self.set_answer(answer_sdp) 210 | # await self.connect() 211 | else: 212 | logger.info("Failed to get answer from server") 213 | 214 | async def connect_robot(self): 215 | try: 216 | return await self.connect_robot_v10() 217 | except Exception as e: 218 | logger.info( 219 | "Failed to connect to the robot with firmware 1.0.x method, trying new method... %s", 220 | e, 221 | ) 222 | 223 | logging.info("Trying to send SDP using a NEW method...") 224 | 225 | offer = await self.pc.createOffer() 226 | await self.pc.setLocalDescription(offer) 227 | 228 | sdp_offer = self.pc.localDescription 229 | 230 | peer_answer = Go2Connection.get_peer_answer(sdp_offer, self.token, self.ip) 231 | answer = RTCSessionDescription(sdp=peer_answer["sdp"], type=peer_answer["type"]) 232 | await self.pc.setRemoteDescription(answer) 233 | 234 | @staticmethod 235 | def get_peer_answer(sdp_offer, token, robot_ip): 236 | sdp_offer_json = { 237 | "id": "STA_localNetwork", 238 | "sdp": sdp_offer.sdp, 239 | "type": sdp_offer.type, 240 | "token": token, 241 | } 242 | 243 | new_sdp = json.dumps(sdp_offer_json) 244 | url = f"http://{robot_ip}:9991/con_notify" 245 | response = Go2Connection.make_local_request(url, body=None, headers=None) 246 | 247 | if response: 248 | # Decode the response text from base64 249 | decoded_response = base64.b64decode(response.text).decode("utf-8") 250 | 251 | # Parse the decoded response as JSON 252 | decoded_json = json.loads(decoded_response) 253 | 254 | # Extract the 'data1' field from the JSON 255 | data1 = decoded_json.get("data1") 256 | 257 | # Extract the public key from 'data1' 258 | public_key_pem = data1[10 : len(data1) - 10] 259 | path_ending = Go2Connection.calc_local_path_ending(data1) 260 | 261 | # Generate AES key 262 | aes_key = Go2Connection.generate_aes_key() 263 | 264 | # Load Public Key 265 | public_key = Go2Connection.rsa_load_public_key(public_key_pem) 266 | 267 | # Encrypt the SDP and AES key 268 | body = { 269 | "data1": Go2Connection.aes_encrypt(new_sdp, aes_key), 270 | "data2": Go2Connection.rsa_encrypt(aes_key, public_key), 271 | } 272 | 273 | # URL for the second request 274 | url = f"http://{robot_ip}:9991/con_ing_{path_ending}" 275 | 276 | # Set the appropriate headers for URL-encoded form data 277 | headers = {"Content-Type": "application/x-www-form-urlencoded"} 278 | 279 | # Send the encrypted data via POST 280 | response = Go2Connection.make_local_request( 281 | url, body=json.dumps(body), headers=headers 282 | ) 283 | 284 | # If response is successful, decrypt it 285 | if response: 286 | decrypted_response = Go2Connection.aes_decrypt(response.text, aes_key) 287 | peer_answer = json.loads(decrypted_response) 288 | 289 | return peer_answer 290 | else: 291 | raise ValueError(f"Failed to get answer from server") 292 | 293 | else: 294 | raise ValueError( 295 | "Failed to receive initial public key response with new method." 296 | ) 297 | 298 | @staticmethod 299 | def hex_to_base64(hex_str): 300 | # Convert hex string to bytes 301 | bytes_array = bytes.fromhex(hex_str) 302 | # Encode the bytes to Base64 and return as a string 303 | return base64.b64encode(bytes_array).decode("utf-8") 304 | 305 | @staticmethod 306 | def encrypt_key(key): 307 | # Append the prefix to the key 308 | prefixed_key = f"UnitreeGo2_{key}" 309 | # Encrypt the key using MD5 and convert to hex string 310 | encrypted = Go2Connection.encrypt_by_md5(prefixed_key) 311 | # Convert the hex string to Base64 312 | return Go2Connection.hex_to_base64(encrypted) 313 | 314 | @staticmethod 315 | def encrypt_by_md5(input_str): 316 | # Create an MD5 hash object 317 | hash_obj = hashlib.md5() 318 | # Update the hash object with the bytes of the input string 319 | hash_obj.update(input_str.encode("utf-8")) 320 | # Return the hex digest of the hash 321 | return hash_obj.hexdigest() 322 | 323 | @staticmethod 324 | def generate_id(): 325 | return int( 326 | datetime.datetime.now().timestamp() * 1000 % 2147483648 327 | ) + random.randint(0, 999) 328 | 329 | @staticmethod 330 | def deal_array_buffer(n): 331 | # Unpack the first 2 bytes as an unsigned short (16-bit) to get the length 332 | length = struct.unpack("H", n[:2])[0] 333 | 334 | # Extract the JSON segment and the remaining data 335 | json_segment = n[4 : 4 + length] 336 | remaining_data = n[4 + length :] 337 | 338 | # Decode the JSON segment from UTF-8 and parse it 339 | json_str = json_segment.decode("utf-8") 340 | obj = json.loads(json_str) 341 | 342 | decoded_data = decoder.decode(remaining_data, obj["data"]) 343 | 344 | # Attach the remaining data to the object 345 | obj["data"]["data"] = decoded_data 346 | 347 | return obj 348 | 349 | @staticmethod 350 | def calc_local_path_ending(data1): 351 | # Initialize an array of strings 352 | strArr = ["A", "B", "C", "D", "E", "F", "G", "H", "I", "J"] 353 | 354 | # Extract the last 10 characters of data1 355 | last_10_chars = data1[-10:] 356 | 357 | # Split the last 10 characters into chunks of size 2 358 | chunked = [last_10_chars[i : i + 2] for i in range(0, len(last_10_chars), 2)] 359 | 360 | # Initialize an empty list to store indices 361 | arrayList = [] 362 | 363 | # Iterate over the chunks and find the index of the second character in strArr 364 | for chunk in chunked: 365 | if len(chunk) > 1: 366 | second_char = chunk[1] 367 | try: 368 | index = strArr.index(second_char) 369 | arrayList.append(index) 370 | except ValueError: 371 | # Handle case where the character is not found in strArr 372 | print(f"Character {second_char} not found in strArr.") 373 | 374 | # Convert arrayList to a string without separators 375 | joinToString = "".join(map(str, arrayList)) 376 | 377 | return joinToString 378 | 379 | @staticmethod 380 | def generate_aes_key() -> str: 381 | uuid_32 = uuid.uuid4().bytes 382 | uuid_32_hex_string = binascii.hexlify(uuid_32).decode("utf-8") 383 | return uuid_32_hex_string 384 | 385 | @staticmethod 386 | def rsa_load_public_key(pem_data: str) -> RSA.RsaKey: 387 | """Load an RSA public key from a PEM-formatted string.""" 388 | key_bytes = base64.b64decode(pem_data) 389 | return RSA.import_key(key_bytes) 390 | 391 | @staticmethod 392 | def pad(data: str) -> bytes: 393 | """Pad data to be a multiple of 16 bytes (AES block size).""" 394 | block_size = AES.block_size 395 | padding = block_size - len(data) % block_size 396 | padded_data = data + chr(padding) * padding 397 | return padded_data.encode("utf-8") 398 | 399 | @staticmethod 400 | def aes_encrypt(data: str, key: str) -> str: 401 | """Encrypt the given data using AES (ECB mode with PKCS5 padding).""" 402 | # Ensure key is 32 bytes for AES-256 403 | key_bytes = key.encode("utf-8") 404 | # Pad the data to ensure it is a multiple of block size 405 | padded_data = Go2Connection.pad(data) 406 | # Create AES cipher in ECB mode 407 | cipher = AES.new(key_bytes, AES.MODE_ECB) 408 | encrypted_data = cipher.encrypt(padded_data) 409 | encoded_encrypted_data = base64.b64encode(encrypted_data).decode("utf-8") 410 | return encoded_encrypted_data 411 | 412 | @staticmethod 413 | def rsa_encrypt(data: str, public_key: RSA.RsaKey) -> str: 414 | """Encrypt data using RSA and a given public key.""" 415 | cipher = PKCS1_v1_5.new(public_key) 416 | # Maximum chunk size for encryption with RSA/ECB/PKCS1Padding is key size - 11 bytes 417 | max_chunk_size = public_key.size_in_bytes() - 11 418 | data_bytes = data.encode("utf-8") 419 | encrypted_bytes = bytearray() 420 | for i in range(0, len(data_bytes), max_chunk_size): 421 | chunk = data_bytes[i : i + max_chunk_size] 422 | encrypted_chunk = cipher.encrypt(chunk) 423 | encrypted_bytes.extend(encrypted_chunk) 424 | # Base64 encode the final encrypted data 425 | encoded_encrypted_data = base64.b64encode(encrypted_bytes).decode("utf-8") 426 | return encoded_encrypted_data 427 | 428 | @staticmethod 429 | def unpad(data: bytes) -> str: 430 | """Remove padding from data.""" 431 | padding = data[-1] 432 | return data[:-padding].decode("utf-8") 433 | 434 | @staticmethod 435 | def aes_decrypt(encrypted_data: str, key: str) -> str: 436 | """Decrypt the given data using AES (ECB mode with PKCS5 padding).""" 437 | # Ensure key is 32 bytes for AES-256 438 | key_bytes = key.encode("utf-8") 439 | # Decode Base64 encrypted data 440 | encrypted_data_bytes = base64.b64decode(encrypted_data) 441 | # Create AES cipher in ECB mode 442 | cipher = AES.new(key_bytes, AES.MODE_ECB) 443 | # Decrypt data 444 | decrypted_padded_data = cipher.decrypt(encrypted_data_bytes) 445 | # Unpad the decrypted data 446 | decrypted_data = Go2Connection.unpad(decrypted_padded_data) 447 | return decrypted_data 448 | 449 | @staticmethod 450 | def make_local_request(path, body=None, headers=None): 451 | try: 452 | # Send POST request with provided path, body, and headers 453 | response = requests.post(url=path, data=body, headers=headers) 454 | # Check if the request was successful (status code 200) 455 | response.raise_for_status() # Raises an HTTPError for bad responses (4xx, 5xx) 456 | if response.status_code == 200: 457 | return response # Returning the whole response object if needed 458 | else: 459 | # Handle non-200 responses 460 | return None 461 | except requests.exceptions.RequestException as e: 462 | # Handle any exception related to the request (e.g., connection errors, timeouts) 463 | logging.error(f"An error occurred: {e}") 464 | return None 465 | 466 | 467 | # Example usage 468 | if __name__ == "__main__": 469 | conn = Go2Connection(os.getenv("GO2_IP"), os.getenv("GO2_TOKEN")) 470 | 471 | # Connect to the robot and disconnect after 3 seconds 472 | async def connect_then_disconnect(conn): 473 | await conn.connect_robot() 474 | for _ in range(3): 475 | await asyncio.sleep(1) 476 | 477 | task = connect_then_disconnect(conn) 478 | 479 | loop = asyncio.get_event_loop() 480 | try: 481 | loop.run_until_complete(task) 482 | except KeyboardInterrupt: 483 | pass 484 | finally: 485 | loop.run_until_complete(conn.pc.close()) 486 | -------------------------------------------------------------------------------- /python/go2_webrtc/go2_cv_video.py: -------------------------------------------------------------------------------- 1 | import logging 2 | import math 3 | 4 | import cv2 5 | import numpy 6 | from aiortc import ( 7 | VideoStreamTrack, 8 | ) 9 | from av import VideoFrame 10 | 11 | 12 | class Go2CvVideo(VideoStreamTrack): 13 | """ 14 | A video track that returns an animated flag. 15 | """ 16 | 17 | def __init__(self): 18 | super().__init__() # don't forget this! 19 | self.counter = 0 20 | height, width = 480, 640 21 | 22 | # generate flag 23 | data_bgr = numpy.hstack( 24 | [ 25 | self._create_rectangle( 26 | width=213, height=480, color=(255, 0, 0) 27 | ), # blue 28 | self._create_rectangle( 29 | width=214, height=480, color=(255, 255, 255) 30 | ), # white 31 | self._create_rectangle(width=213, height=480, color=(0, 0, 255)), # red 32 | ] 33 | ) 34 | 35 | # shrink and center it 36 | M = numpy.float32([[0.5, 0, width / 4], [0, 0.5, height / 4]]) 37 | data_bgr = cv2.warpAffine(data_bgr, M, (width, height)) 38 | 39 | # compute animation 40 | omega = 2 * math.pi / height 41 | id_x = numpy.tile(numpy.array(range(width), dtype=numpy.float32), (height, 1)) 42 | id_y = numpy.tile( 43 | numpy.array(range(height), dtype=numpy.float32), (width, 1) 44 | ).transpose() 45 | 46 | self.frames = [] 47 | for k in range(30): 48 | phase = 2 * k * math.pi / 30 49 | map_x = id_x + 10 * numpy.cos(omega * id_x + phase) 50 | map_y = id_y + 10 * numpy.sin(omega * id_x + phase) 51 | self.frames.append( 52 | VideoFrame.from_ndarray( 53 | cv2.remap(data_bgr, map_x, map_y, cv2.INTER_LINEAR), format="bgr24" 54 | ) 55 | ) 56 | 57 | async def recv(self): 58 | pts, time_base = await self.next_timestamp() 59 | 60 | frame = self.frames[self.counter % 30] 61 | frame.pts = pts 62 | frame.time_base = time_base 63 | self.counter += 1 64 | return frame 65 | 66 | def _create_rectangle(self, width, height, color): 67 | data_bgr = numpy.zeros((height, width, 3), numpy.uint8) 68 | data_bgr[:, :] = color 69 | return data_bgr 70 | 71 | -------------------------------------------------------------------------------- /python/go2_webrtc/libvoxel.wasm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tfoldi/go2-webrtc/e14b3d92d5b63e865eb8aaeb1895a84267382141/python/go2_webrtc/libvoxel.wasm -------------------------------------------------------------------------------- /python/go2_webrtc/lidar_decoder.py: -------------------------------------------------------------------------------- 1 | # Redistribution and use in source and binary forms, with or without 2 | # modification, are permitted provided that the following conditions are met: 3 | # 4 | # 1. Redistributions of source code must retain the above copyright notice, this 5 | # list of conditions and the following disclaimer. 6 | # 7 | # 2. Redistributions in binary form must reproduce the above copyright notice, 8 | # this list of conditions and the following disclaimer in the documentation 9 | # and/or other materials provided with the distribution. 10 | # 11 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 12 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 13 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 14 | # DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 15 | # FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 16 | # DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 17 | # SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 18 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 19 | # OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 20 | # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 21 | 22 | import math 23 | import ctypes 24 | import numpy as np 25 | import os 26 | 27 | from wasmtime import Config, Engine, Store, Module, Instance, Func, FuncType 28 | from wasmtime import ValType 29 | 30 | 31 | class LidarDecoder: 32 | def __init__(self) -> None: 33 | 34 | config = Config() 35 | config.wasm_multi_value = True 36 | config.debug_info = True 37 | self.store = Store(Engine(config)) 38 | 39 | module_dir = os.path.dirname(os.path.abspath(__file__)) 40 | 41 | self.module = Module.from_file(self.store.engine, os.path.join(module_dir,"libvoxel.wasm")) 42 | 43 | self.a_callback_type = FuncType([ValType.i32()], [ValType.i32()]) 44 | self.b_callback_type = FuncType([ValType.i32(), ValType.i32(), ValType.i32()], []) 45 | 46 | a = Func(self.store, self.a_callback_type, self.adjust_memory_size) 47 | b = Func(self.store, self.b_callback_type, self.copy_memory_region) 48 | 49 | self.instance = Instance(self.store, self.module, [a, b]) 50 | 51 | self.generate = self.instance.exports(self.store)["e"] 52 | self.malloc = self.instance.exports(self.store)["f"] 53 | self.free = self.instance.exports(self.store)["g"] 54 | self.wasm_memory = self.instance.exports(self.store)["c"] 55 | 56 | self.buffer = self.wasm_memory.data_ptr(self.store) 57 | self.memory_size = self.wasm_memory.data_len(self.store) 58 | 59 | self.buffer_ptr = int.from_bytes(self.buffer, "little") 60 | 61 | self.HEAP8 = (ctypes.c_int8 * self.memory_size).from_address(self.buffer_ptr) 62 | self.HEAP16 = (ctypes.c_int16 * (self.memory_size // 2)).from_address(self.buffer_ptr) 63 | self.HEAP32 = (ctypes.c_int32 * (self.memory_size // 4)).from_address(self.buffer_ptr) 64 | self.HEAPU8 = (ctypes.c_uint8 * self.memory_size).from_address(self.buffer_ptr) 65 | self.HEAPU16 = (ctypes.c_uint16 * (self.memory_size // 2)).from_address(self.buffer_ptr) 66 | self.HEAPU32 = (ctypes.c_uint32 * (self.memory_size // 4)).from_address(self.buffer_ptr) 67 | self.HEAPF32 = (ctypes.c_float * (self.memory_size // 4)).from_address(self.buffer_ptr) 68 | self.HEAPF64 = (ctypes.c_double * (self.memory_size // 8)).from_address(self.buffer_ptr) 69 | 70 | self.input = self.malloc(self.store, 61440) 71 | self.decompressBuffer = self.malloc(self.store, 80000) 72 | self.positions = self.malloc(self.store, 2880000) 73 | self.uvs = self.malloc(self.store, 1920000) 74 | self.indices = self.malloc(self.store, 5760000) 75 | self.decompressedSize = self.malloc(self.store, 4) 76 | self.faceCount = self.malloc(self.store, 4) 77 | self.pointCount = self.malloc(self.store, 4) 78 | self.decompressBufferSize = 80000 79 | 80 | def adjust_memory_size(self, t): 81 | return len(self.HEAPU8) 82 | 83 | def copy_within(self, target, start, end): 84 | # Copy the sublist for the specified range [start:end] 85 | sublist = self.HEAPU8[start:end] 86 | 87 | # Replace elements in the list starting from index 'target' 88 | for i in range(len(sublist)): 89 | if target + i < len(self.HEAPU8): 90 | self.HEAPU8[target + i] = sublist[i] 91 | 92 | def copy_memory_region(self, t, n, a): 93 | self.copy_within(t, n, n + a) 94 | 95 | def get_value(self, t, n="i8"): 96 | if n.endswith("*"): 97 | n = "*" 98 | if n == "i1" or n == "i8": 99 | return self.HEAP8[t] 100 | elif n == "i16": 101 | return self.HEAP16[t >> 1] 102 | elif n == "i32" or n == "i64": 103 | return self.HEAP32[t >> 2] 104 | elif n == "float": 105 | return self.HEAPF32[t >> 2] 106 | elif n == "double": 107 | return self.HEAPF64[t >> 3] 108 | elif n == "*": 109 | return self.HEAPU32[t >> 2] 110 | else: 111 | raise ValueError(f"invalid type for getValue: {n}") 112 | 113 | def add_value_arr(self, start, value): 114 | if start + len(value) <= len(self.HEAPU8): 115 | for i, byte in enumerate(value): 116 | self.HEAPU8[start + i] = byte 117 | else: 118 | raise ValueError("Not enough space to insert bytes at the specified index.") 119 | 120 | def decode(self, compressed_data, data): 121 | self.add_value_arr(self.input, compressed_data) 122 | 123 | some_v = math.floor(data["origin"][2] / data["resolution"]) 124 | 125 | self.generate( 126 | self.store, 127 | self.input, 128 | len(compressed_data), 129 | self.decompressBufferSize, 130 | self.decompressBuffer, 131 | self.decompressedSize, 132 | self.positions, 133 | self.uvs, 134 | self.indices, 135 | self.faceCount, 136 | self.pointCount, 137 | some_v 138 | ) 139 | 140 | self.get_value(self.decompressedSize, "i32") 141 | c = self.get_value(self.pointCount, "i32") 142 | u = self.get_value(self.faceCount, "i32") 143 | 144 | positions_slice = self.HEAPU8[self.positions:self.positions + u * 12] 145 | positions_copy = bytearray(positions_slice) 146 | p = np.frombuffer(positions_copy, dtype=np.uint8) 147 | 148 | uvs_slice = self.HEAPU8[self.uvs:self.uvs + u * 8] 149 | uvs_copy = bytearray(uvs_slice) 150 | r = np.frombuffer(uvs_copy, dtype=np.uint8) 151 | 152 | indices_slice = self.HEAPU8[self.indices:self.indices + u * 24] 153 | indices_copy = bytearray(indices_slice) 154 | o = np.frombuffer(indices_copy, dtype=np.uint32) 155 | 156 | return { 157 | "point_count": c, 158 | "face_count": u, 159 | "positions": p, 160 | "uvs": r, 161 | "indices": o 162 | } 163 | -------------------------------------------------------------------------------- /python/requirements.txt: -------------------------------------------------------------------------------- 1 | aiortc 2 | aiohttp 3 | paho-mqtt 4 | python-dotenv 5 | pygame 6 | wasmtime 7 | pycryptodome 8 | requests 9 | 10 | -------------------------------------------------------------------------------- /python/setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup, find_packages 2 | 3 | setup( 4 | name='go2-webrtc', 5 | version='0.1.0', 6 | packages=find_packages(), 7 | install_requires=[ 8 | 'aiortc', 9 | 'aiohttp' 10 | ], 11 | ) 12 | 13 | -------------------------------------------------------------------------------- /python/tests/test_go2connection.py: -------------------------------------------------------------------------------- 1 | import pytest 2 | import asyncio 3 | from go2_webrtc import ( 4 | Go2Connection, 5 | ) # Adjust the import according to your project structure 6 | 7 | 8 | @pytest.mark.asyncio 9 | async def test_generate_offer(): 10 | conn = Go2Connection() 11 | offer = await conn.generate_offer() 12 | assert ( 13 | isinstance(offer, str) and len(offer) > 0 14 | ), "Offer should be a non-empty string" 15 | 16 | 17 | @pytest.mark.asyncio 18 | async def test_set_answer(): 19 | conn = Go2Connection() 20 | offer = await conn.generate_offer() 21 | # Assuming we have a valid SDP answer string (this is just a placeholder) 22 | answer = "v=0\no=- 4612925294212871715 2 IN IP4 127.0.0.1\ns=-\nt=0 0\na=group:BUNDLE 0\na=msid-semantic: WMS\nm=application 9 DTLS/SCTP 5000\nc=IN IP4 0.0.0.0\na=mid:0\na=sctpmap:5000 webrtc-datachannel 1024" 23 | await conn.set_answer(answer) 24 | # Since set_answer doesn't return a value, we're just checking if it runs without raising an exception 25 | assert True, "set_answer should complete without error" 26 | --------------------------------------------------------------------------------