mirror of
https://github.com/CopterExpress/clover.git
synced 2026-06-07 01:59:33 +00:00
Implement block programming (using Blockly) (#272)
* Clover Blockly: add first blocks set * Adjust Blockly settings * Fix get_position output type * Add screenshot * Rename readme.md to README.md * Resize screenshot * Add package.xml * Little change * Fixes * Add python_compressed to blockly * Implement some of the Clover blocks in Python * Make Python indentation 4 spaces * Fixes to Python blocks implementation * Implement set_velocity block in Python * Implement wait_arrival block in Python * Fix indentation in Python implementation of blocks * Fix * Fix land_wait template * Set reserved words in Python * Change default frame_id to aruco_map in get_position block * Fix * Move blocks definitions to blocks.js * Get rid of missing favicon error * Simplify navigate * Rearrange layout, add tabs * Generate Python code * Small style change * -console.log * Code style * Use modules * Move modules to the header * Correct order for ROS definitions + generating "backend" code * Fix rangefinder_distance block * simple_offboard: commands to change only yaw and yaw rate * Implement set_yaw block * Start working on Blockly documentation * Implement print block with a topic * Unneeded code * Little fixes * Fix indentation * Fixes * Fix wait_arival, get_distance * Implement running Blockly programs, implement prompt block, fixes * Add land button * Little change * Fix reserved words + little fixes * +x for main.py * Simplify run button * Auto-save and load workspace * Make land button work * Handle exceptions * Minor change * Add help URL for blocks * Fix * Implement arrived block * Mark blockly and highlight.js as linguist-vendored * Add forgotten CMakeLists.txt * Add wait checkbox to set_yaw block * Disable run button when disconnected * Add message and service files * Add some comments * Add tooltip to some blocks * Implement GPIO blocks * Don’t latch print message to prevent duplication * Prevent duplication prompts * Add ROS init code to backend code anyways * Make GPIO blocks color a constant * Minor fix * More correctly update blocks on input value changes * Minor fixes * Remove unneeded readonly attribute * Add marker ID shadow blocks to toolbox * Add lacking reserved words * Fix frame id generation for complex marker id expressions * Consider frame_id in set_yaw block * Shorten ros module import * Implement stop service * Disable and enable run button correctly * Don’t print KeyboardInterrupt exceptions * Put notifications to notifications element * Add 'running' mark * Disable signal in backend python code * Sleep a little bit to let rospy initialize publishers * Remove accidental code * Make ROS namespace and private namespace constants * editorconfig-lint: don’t check Blockly code * Use private namespace constant in Python generator * Implement ~running topic to display current program status more robustly * Make navigate tolerance and sleep time constants * Make set_leds and and set_effect services proxies persistent * Replace a number with constant * Limit ~block topic publishing rate Otherwise messages get queued making the frontend to freeze * Improve internal documentation * Append 'map' to frames list * Return degrees in get_attitude block * Move getting yaw in a separate block * Improve block tooltips * Add some more files to editorconfig-lint excludes * Add get_yaw block to toolbox * Implement get_time block * Implement ~store and ~load services for storing user programs * Set auto_arm only in take_off block * Minor CSS fixes * Make 'Python' tab textarea-like * Implement saving and loading programs * Adjust styles * Retrieve only .xml files in load service * Forgotten code * Documentation on store and load services * Add some examples * Add blocks programming arg to launch file * Update docs * Add package’s dependencies * Add dependency * Add title to select * Fix syntax * Minor fix in docs * Add forgotten roslib.js * Run user program in the same process * Use print function for print block in Python 2 * Add variables example * Fix url * Add functions example * Fix set_servo block * Fix gpio_read block * Update blocks screenshot * Update docs * Update docs * Fix set_effect block * Minor fix in example * Add setpoint block, remove set_velocity from toolbox * Remove unused modules * Unused variable * Add English article skeleton * Clarify backend node link error * Remove unused variable * Update documentation * Fix link to documentation * Add Blockly logo * Update English article * Add Blocks programming link to the main page * Minor change * Add catkin_install_python to CMakeLists.txt * Make navigate tolerance and sleep time configurable * Add minor todo * Add blockly examples directory to editorconfig-lint excludes * Rename main node to clover_blocks * Add a warning to the old blocks programming article * Fix editorconfig-lint exclude
This commit is contained in:
Oleg Kalachev
176
clover_blocks/src/clover_blocks
Executable file
176
clover_blocks/src/clover_blocks
Executable file
@@ -0,0 +1,176 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
from __future__ import print_function
|
||||
|
||||
import rospy
|
||||
import os
|
||||
import threading
|
||||
import re
|
||||
import uuid
|
||||
from std_msgs.msg import Bool, String
|
||||
from std_srvs.srv import Trigger
|
||||
from clover_blocks.msg import Prompt
|
||||
from clover_blocks.srv import Run, Load, Store
|
||||
|
||||
|
||||
rospy.init_node('clover_blocks')
|
||||
|
||||
stop = None
|
||||
block = ''
|
||||
published_block = None
|
||||
running_lock = threading.Lock()
|
||||
|
||||
running_pub = rospy.Publisher('~running', Bool, queue_size=1, latch=True)
|
||||
block_pub = rospy.Publisher('~block', String, queue_size=1, latch=True)
|
||||
print_pub = rospy.Publisher('~print', String, queue_size=10)
|
||||
prompt_pub = rospy.Publisher('~prompt', Prompt, queue_size=10)
|
||||
error_pub = rospy.Publisher('~error', String, queue_size=10)
|
||||
|
||||
|
||||
running_pub.publish(False)
|
||||
|
||||
|
||||
class Stop(Exception):
|
||||
pass
|
||||
|
||||
|
||||
def publish_block(event):
|
||||
global published_block, block
|
||||
if published_block != block:
|
||||
block_pub.publish(block)
|
||||
published_block = block
|
||||
|
||||
|
||||
rospy.Timer(rospy.Duration(rospy.get_param('block_rate', 0.2)), publish_block)
|
||||
|
||||
|
||||
def change_block(_block):
|
||||
global block
|
||||
block = _block
|
||||
if stop: raise Stop
|
||||
|
||||
|
||||
rospy_sleep = rospy.sleep
|
||||
|
||||
|
||||
def sleep(duration):
|
||||
time_start = rospy.get_time()
|
||||
|
||||
if isinstance(duration, rospy.Duration):
|
||||
duration = duration.to_sec()
|
||||
|
||||
time_until = time_start + duration
|
||||
|
||||
while not rospy.is_shutdown():
|
||||
if stop: raise Stop # check stop condition every half-second
|
||||
if (time_until - rospy.get_time()) > 0.5:
|
||||
print('sleep 0.5')
|
||||
rospy_sleep(0.5)
|
||||
else:
|
||||
rospy_sleep(time_until - rospy.get_time())
|
||||
return
|
||||
|
||||
|
||||
rospy.sleep = sleep
|
||||
rospy.init_node = lambda *args, **kwargs: None
|
||||
|
||||
|
||||
def _print(s):
|
||||
rospy.loginfo(str(s))
|
||||
print_pub.publish(str(s))
|
||||
|
||||
|
||||
def _input(s):
|
||||
rospy.loginfo('Input with message %s', s)
|
||||
prompt_id = str(uuid.uuid4()).replace('-', '')
|
||||
prompt_pub.publish(message=str(s), id=prompt_id)
|
||||
return rospy.wait_for_message('~input/' + prompt_id, String, timeout=30).data;
|
||||
|
||||
|
||||
def run(req):
|
||||
if not running_lock.acquire(False):
|
||||
return {'message': 'Already running'}
|
||||
|
||||
try:
|
||||
rospy.loginfo('Run program')
|
||||
running_pub.publish(True)
|
||||
|
||||
def program_thread():
|
||||
global stop
|
||||
stop = False
|
||||
g = {'rospy': rospy,
|
||||
'_b': change_block,
|
||||
'print': _print,
|
||||
'raw_input': _input}
|
||||
try:
|
||||
exec req.code in g
|
||||
except Stop:
|
||||
rospy.loginfo('Program forced to stop')
|
||||
except Exception as e:
|
||||
rospy.logerr(str(e))
|
||||
error_pub.publish(str(e))
|
||||
|
||||
rospy.loginfo('Program terminated')
|
||||
running_lock.release()
|
||||
running_pub.publish(False)
|
||||
change_block('')
|
||||
|
||||
t = threading.Thread(target=program_thread)
|
||||
t.start()
|
||||
|
||||
return {'success': True}
|
||||
|
||||
except Exception as e:
|
||||
running_lock.release()
|
||||
return {'message': str(e)}
|
||||
|
||||
|
||||
def stop(req):
|
||||
global stop
|
||||
rospy.loginfo('Stop program')
|
||||
stop = True
|
||||
return {'success': True}
|
||||
|
||||
|
||||
programs_path = rospy.get_param('~programs_dir', os.path.dirname(os.path.abspath(__file__)) + '/../programs')
|
||||
|
||||
|
||||
def load(req):
|
||||
res = {'names': [], 'programs': [], 'success': True}
|
||||
try:
|
||||
for currentpath, folders, files in os.walk(programs_path):
|
||||
for f in files:
|
||||
if not f.endswith('.xml'):
|
||||
continue
|
||||
filename = os.path.join(currentpath, f)
|
||||
res['names'].append(os.path.relpath(filename, programs_path))
|
||||
res['programs'].append(open(filename, 'r').read())
|
||||
return res
|
||||
except Exception as e:
|
||||
rospy.logerr(e)
|
||||
return {'names': [], 'programs': [], 'message': str(e)}
|
||||
|
||||
|
||||
name_regexp = re.compile(r'^[a-zA-Z-_.]{0,20}$')
|
||||
|
||||
def store(req):
|
||||
if not name_regexp.match(req.name):
|
||||
return {'message': 'Bad program name'}
|
||||
|
||||
filename = os.path.abspath(os.path.join(programs_path, req.name))
|
||||
try:
|
||||
open(filename, 'w').write(req.program)
|
||||
return {'success': True, 'message': 'Stored to ' + filename}
|
||||
except Exception as e:
|
||||
rospy.logerr(e)
|
||||
return {'names': [], 'programs': [], 'message': str(e)}
|
||||
|
||||
|
||||
rospy.Service('~run', Run, run)
|
||||
rospy.Service('~stop', Trigger, stop)
|
||||
rospy.Service('~load', Load, load)
|
||||
rospy.Service('~store', Store, store)
|
||||
|
||||
|
||||
rospy.loginfo('Ready')
|
||||
rospy.spin()
|
||||
Reference in New Issue
Block a user