From 77832e65fa6bafff8644a868809bf470ac6fbc2a Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Thu, 16 Sep 2021 19:41:11 +0300 Subject: [PATCH] blocks: implement global navigation --- clover_blocks/README.md | 1 + clover_blocks/www/blocks.js | 21 ++++++++++++- clover_blocks/www/index.html | 2 ++ clover_blocks/www/main.js | 1 + clover_blocks/www/python.js | 61 ++++++++++++++++++++++++++++++------ docs/en/blocks.md | 2 ++ docs/ru/blocks.md | 3 +- 7 files changed, 80 insertions(+), 11 deletions(-) diff --git a/clover_blocks/README.md b/clover_blocks/README.md index 4d7b9b8d..3077085d 100644 --- a/clover_blocks/README.md +++ b/clover_blocks/README.md @@ -30,6 +30,7 @@ The frontend files are located in [`www`](./www/) subdirectory. The frontend app Parameters read by frontend: * `~navigate_tolerance` (*float*) – distance tolerance in meters, used for navigate-like blocks (default: 0.2). +* `~navigate_global_tolerance` (*float*) – distance tolerance for global coordinates navigation (default: 1). * `~yaw_tolerance` (*float*) – yaw angle tolerance in degrees, used in set_yaw block (default: 1). * `~sleep_time` (*float*) – duration of sleep in loop cycles, used for navigate-like blocks (default: 0.2). * `~confirm_run` (*bool*) – enable confirmation to run the program (default: true). diff --git a/clover_blocks/www/blocks.js b/clover_blocks/www/blocks.js index 60fb6d7e..b10e315f 100644 --- a/clover_blocks/www/blocks.js +++ b/clover_blocks/www/blocks.js @@ -31,6 +31,14 @@ function considerFrameId(e) { this.getInput('Y').fieldRow[0].setValue('y'); this.getInput('Z').fieldRow[0].setValue('z'); } + if (this.getInput('LAT')) { // block has global coordinates + let global = frameId.startsWith('GLOBAL'); + this.getInput('LAT').setVisible(global); + this.getInput('LON').setVisible(global); + this.getInput('X').setVisible(!global); + this.getInput('Y').setVisible(!global); + this.getInput('Z').fieldRow[0].setValue(frameId == 'GLOBAL' ? 'altitude' : 'z'); + } } if (this.getInput('ID')) { // block has marker id field this.getInput('ID').setVisible(frameId == 'ARUCO'); // toggle id field @@ -65,6 +73,9 @@ function updateSetpointBlock(e) { Blockly.Blocks['navigate'] = { init: function () { + let navFrameId = frameIds.slice(); + navFrameId.push(['global', 'GLOBAL_LOCAL']) + navFrameId.push(['global, WGS 84 alt.', 'GLOBAL']) this.appendDummyInput() .appendField("navigate to point"); this.appendValueInput("X") @@ -73,12 +84,20 @@ Blockly.Blocks['navigate'] = { this.appendValueInput("Y") .setCheck("Number") .appendField("left"); + this.appendValueInput("LAT") + .setCheck("Number") + .appendField("latitude") + .setVisible(false); + this.appendValueInput("LON") + .setCheck("Number") + .appendField("longitude") + .setVisible(false) this.appendValueInput("Z") .setCheck("Number") .appendField("up"); this.appendDummyInput() .appendField("relative to") - .appendField(new Blockly.FieldDropdown(frameIds), "FRAME_ID"); + .appendField(new Blockly.FieldDropdown(navFrameId), "FRAME_ID"); this.appendValueInput("ID") .setCheck("Number") .appendField("with ID") diff --git a/clover_blocks/www/index.html b/clover_blocks/www/index.html index 9022d174..8f394198 100644 --- a/clover_blocks/www/index.html +++ b/clover_blocks/www/index.html @@ -52,6 +52,8 @@ 0 0 0 + 47.397503 + 8.544945 0.5 0 diff --git a/clover_blocks/www/main.js b/clover_blocks/www/main.js index 1bac8f20..485058ab 100644 --- a/clover_blocks/www/main.js +++ b/clover_blocks/www/main.js @@ -39,6 +39,7 @@ var workspace = Blockly.inject('blockly', { function readParams() { return Promise.all([ ros.readParam('navigate_tolerance', true, 0.2), + ros.readParam('navigate_global_tolerance', true, 1), ros.readParam('yaw_tolerance', true, 1), ros.readParam('sleep_time', true, 0.2), ros.readParam('confirm_run', true, true), diff --git a/clover_blocks/www/python.js b/clover_blocks/www/python.js index c97feab9..afc7ae09 100644 --- a/clover_blocks/www/python.js +++ b/clover_blocks/www/python.js @@ -33,6 +33,18 @@ const NAVIGATE_WAIT = () => `\ndef navigate_wait(x=0, y=0, z=0, speed=0.5, frame return rospy.sleep(${params.sleep_time})\n`; +const NAVIGATE_GLOBAL_WAIT = () => `\ndef navigate_global_wait(lat, lon, z, speed=0.5): + res = navigate_global(lat=lat, lon=lon, z=z, yaw=float('inf'), speed=speed) + + if not res.success: + raise Exception(res.message) + + while not rospy.is_shutdown(): + telem = get_telemetry(frame_id='navigate_target') + if math.sqrt(telem.x ** 2 + telem.y ** 2 + telem.z ** 2) < ${params.navigate_global_tolerance}: + return + rospy.sleep(${params.sleep_time})\n`; + const LAND_WAIT = () => `\ndef land_wait(): land() while get_telemetry().armed: @@ -68,6 +80,9 @@ function generateROSDefinitions() { if (rosDefinitions.offboard) { code += `get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry)\n`; code += `navigate = rospy.ServiceProxy('navigate', srv.Navigate)\n`; + if (rosDefinitions.navigateGlobal) { + code += `navigate_global = rospy.ServiceProxy('navigate_global', srv.NavigateGlobal)\n`; + } if (rosDefinitions.setVelocity) { code += `set_velocity = rospy.ServiceProxy('set_velocity', srv.SetVelocity)\n`; } @@ -94,6 +109,10 @@ function generateROSDefinitions() { Blockly.Python.definitions_['import_math'] = 'import math'; code += NAVIGATE_WAIT(); } + if (rosDefinitions.navigateGlobalWait) { + Blockly.Python.definitions_['import_math'] = 'import math'; + code += NAVIGATE_GLOBAL_WAIT(); + } if (rosDefinitions.landWait) { code += LAND_WAIT(); } @@ -161,24 +180,48 @@ Blockly.Python.navigate = function(block) { let x = Blockly.Python.valueToCode(block, 'X', Blockly.Python.ORDER_NONE); let y = Blockly.Python.valueToCode(block, 'Y', Blockly.Python.ORDER_NONE); let z = Blockly.Python.valueToCode(block, 'Z', Blockly.Python.ORDER_NONE); - let frameId = buildFrameId(block); + let lat = Blockly.Python.valueToCode(block, 'LAT', Blockly.Python.ORDER_NONE); + let lon = Blockly.Python.valueToCode(block, 'LON', Blockly.Python.ORDER_NONE); + let wait = block.getFieldValue('WAIT') == 'TRUE'; + let frameId = block.getFieldValue('FRAME_ID'); let speed = Blockly.Python.valueToCode(block, 'SPEED', Blockly.Python.ORDER_NONE); - let params = [`x=${x}`, `y=${y}`, `z=${z}`, `frame_id=${frameId}`, `speed=${speed}`]; - simpleOffboard(); - if (block.getFieldValue('WAIT') == 'TRUE') { - rosDefinitions.navigateWait = true; + // global coordinates + if (frameId.startsWith('GLOBAL')) { + rosDefinitions.navigateGlobal = true; simpleOffboard(); - return `navigate_wait(${params.join(', ')})\n`; + if (frameId == 'GLOBAL') { + z = `${z} + get_telemetry().alt - get_telemetry().z`; + } + + if (wait) { + rosDefinitions.navigateGlobalWait = true; + simpleOffboard(); + return `navigate_global_wait(lat=${lat}, lon=${lon}, z=${z}, speed=${speed})\n`; + + } else { + return `navigate_global(lat=${lat}, lon=${lon}, z=${z}, yaw=float('inf'), speed=${speed})\n`; + } } else { - if (frameId != 'body') { - params.push(`yaw=float('nan')`); + frameId = buildFrameId(block); + let params = [`x=${x}`, `y=${y}`, `z=${z}`, `frame_id=${frameId}`, `speed=${speed}`]; + + if (wait) { + rosDefinitions.navigateWait = true; + simpleOffboard(); + + return `navigate_wait(${params.join(', ')})\n`; + + } else { + if (frameId != 'body') { + params.push(`yaw=float('nan')`); + } + return `navigate(${params.join(', ')})\n`; } - return `navigate(${params.join(', ')})\n`; } } diff --git a/docs/en/blocks.md b/docs/en/blocks.md index 4078f9c7..f7b1665a 100644 --- a/docs/en/blocks.md +++ b/docs/en/blocks.md @@ -72,6 +72,8 @@ This block allows to specify the [coordinate frame](frames.md) of the target poi * *marker* – coordinates, relative to an [ArUco-marker](aruco_marker.md); marker's ID input fields appears. * *last navigate target* – coordinates, relative to the last specified navigate point. * *map* – drone's local coordinate system, linked with the point of its initialization. +* *global* – global coordinates system (latitude and longitude) and relative altitude. +* *global, WGS 84 alt.* – global coordinates system and [WGS 84](https://en.wikipedia.org/wiki/WGS_84) altitude. ### land diff --git a/docs/ru/blocks.md b/docs/ru/blocks.md index 1b899365..8b9fad6a 100644 --- a/docs/ru/blocks.md +++ b/docs/ru/blocks.md @@ -71,7 +71,8 @@ * *markers map* – система координат, связанная с [картой ArUco-маркеров](aruco_map.md). * *marker* – система координат, связанная с [ArUco-маркером](aruco_marker.md); появляется поле для ввода ID маркеа. * *last navigate target* – координаты относительно последней заданной точки для навигации. -* *map* – локальная система координат коптера, связана с местом его инициализации. +* *global* – глобальная система координат (широта и долгота) и относительная высота. +* *global, WGS 84 alt.* – глобальная система координат и высота в [системе WGS 84](https://ru.wikipedia.org/wiki/WGS_84). ### land