blocks: add yaw_tolerance parameter

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).
+* `~yaw_tolerance` (*float*) – yaw angle tolerance in degrees, used in set_yaw block (default: 20).
 * `~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).
 

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('yaw_tolerance', true, 20),
 		ros.readParam('sleep_time', true, 0.2),
 		ros.readParam('confirm_run', true, true),
 	]);

clover_blocks/www/python.js

@@ -38,11 +38,10 @@ const LAND_WAIT = () => `\ndef land_wait():
     while get_telemetry().armed:
         rospy.sleep(${params.sleep_time})\n`;
 
-// TODO: tolerance to parameters
 const WAIT_YAW = () => `\ndef wait_yaw():
     while not rospy.is_shutdown():
         telem = get_telemetry(frame_id='navigate_target')
-        if abs(telem.yaw) < math.radians(20):
+        if abs(telem.yaw) < math.radians(${params.yaw_tolerance}):
             return
         rospy.sleep(${params.sleep_time})\n`;