CopterExpress/clover
1
0
Fork 0
mirror of https://github.com/CopterExpress/clover.git synced 2026-05-26 11:43:25 +00:00
Code Issues Packages Projects Releases Wiki Activity

blocks: implement global navigation

Browse Source
This commit is contained in:
Oleg Kalachev
2021-09-16 19:41:11 +03:00
parent 01edd129ab
commit 77832e65fa
7 changed files with 80 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
clover_blocks/README.md
View File

@@ -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).

21
clover_blocks/www/blocks.js
View File

@@ -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")

2
clover_blocks/www/index.html
View File

@@ -52,6 +52,8 @@
<value name="X"><shadow type="math_number"><field name="NUM">0</field></shadow></value>
<value name="Y"><shadow type="math_number"><field name="NUM">0</field></shadow></value>
<value name="Z"><shadow type="math_number"><field name="NUM">0</field></shadow></value>
<value name="LAT"><shadow type="math_number"><field name="NUM">47.397503</field></shadow></value>
<value name="LON"><shadow type="math_number"><field name="NUM">8.544945</field></shadow></value>
<value name="SPEED"><shadow type="math_number"><field name="NUM">0.5</field></shadow></value>
<value name="ID"><shadow type="math_number"><field name="NUM">0</field></shadow></value>
</block>

1
clover_blocks/www/main.js
View File

@@ -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),

61
clover_blocks/www/python.js
View File

@@ -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`;
}
}