Skip to content
This repository was archived by the owner on Feb 21, 2021. It is now read-only.
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Binary file not shown.
36 changes: 28 additions & 8 deletions src/tools/scratch2jderobot/scripts/scratch2python.py
Original file line number Diff line number Diff line change
Expand Up @@ -60,11 +60,25 @@ def is_conditional(sentence):
@param sentence: The sentence to check.
@return: True if it has a conditional, False otherwise.
"""

if "if" in sentence:
return True

return False


def is_list(sentence):
"""
Returns if a sentence use list or not.

@param sentence: The sentence to check.
@return: True if it has a list, False otherwise.
"""
if "insert" in sentence:
return True
if "add" in sentence:
return True

return False


def similar(a, b):
Expand Down Expand Up @@ -210,18 +224,24 @@ def execute(robot):\n\
python_program = ""

for s in sentences:
# count number of tabs
num_tabs = s.replace(' ', tab_seq).count(tab_seq)
python_program += tab_seq * (num_tabs + 1)

# pre-processing if there is a condition (operators and types)
if is_conditional(s):
s = s.replace("'", "").replace("=", "==")

# mapping
original, translation = sentence_mapping(s)
# print original, translation
# set the code

# count number of tabs
num_tabs = s.replace(' ', tab_seq).count(tab_seq)

# check if there is list

if is_list(s):
python_program += tab_seq * (num_tabs + 1)
python_program += translation.split(".")[0]+" = []\n\t"

python_program += tab_seq * (num_tabs + 1)

# set the code
if translation != None:
python_program += translation
else:
Expand Down
9 changes: 9 additions & 0 deletions src/tools/scratch2jderobot/src/scratch2jderobot/drone.py
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@
import imutils
import cv2


from parallelIce.cameraClient import CameraClient
from parallelIce.cmdvel import CMDVel
from parallelIce.extra import Extra
Expand Down Expand Up @@ -47,6 +48,7 @@ def __init__(self, jdrc):
"""

#variables
self.frontalCamera = False

#get clients
self.__pose3d_client = jdrc.getPose3dClient("drone.Pose3D")
Expand All @@ -64,6 +66,10 @@ def close(self):
self.__navdata_client.stop()
self.__pose3d_client.stop()

def toggleCam(self):
self.frontalCamera = True
self.__extra_client.toggleCam()


def detect_object(self, color):
"""
Expand All @@ -73,6 +79,9 @@ def detect_object(self, color):

@return: size and center of the object detected in the frame
"""

if not self.frontalCamera:
self.toggleCam()
# define the lower and upper boundaries of the basic colors
GREEN_RANGE = ((29, 86, 6), (64, 255, 255))
RED_RANGE = ((139, 0, 0), (255, 160, 122))
Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,80 @@
#!/usr/bin/env python
# -*- coding: utf-8 -*-

import time
import config
import sys
import comm
import os
import yaml

from drone import Drone
from robot import Robot

def execute(robot):
try:
robot.take_off()
while True:
mylist = []
mylist.insert(0, robot.detect_object("red"))
size = mylist[0][0]
x = mylist[0][1]
y = mylist[0][2]
if size > 0:
if size > 700:
robot.move("back", 2)
else:
robot.move("forward", 2)

if x > 165:
robot.turn("right", 2)
else:
robot.turn("left", 2)

if y > 110:
robot.move("down", 1)
else:
robot.move("up", 1)

else:
robot.stop()
robot.turn("left", 2)


except KeyboardInterrupt:
raise

if __name__ == '__main__':
if len(sys.argv) == 2:
path = os.getcwd()
open_path = path[:path.rfind('src')] + 'cfg/'
filename = sys.argv[1]

else:
sys.exit("ERROR: Example:python my_generated_script.py cfgfile.yml")

# loading the ICE and ROS parameters
cfg = config.load(open_path + filename)
stream = open(open_path + filename, "r")
yml_file = yaml.load(stream)

for section in yml_file:
if section == 'drone':
#starting comm
jdrc = comm.init(cfg,'drone')

# creating the object
robot = Drone(jdrc)

break
elif section == 'robot':
#starting comm
jdrc = comm.init(cfg,'robot')

# creating the object
robot = Robot(jdrc)

break
# executing the scratch program
execute(robot)

Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,6 @@

from drone import Drone
from robot import Robot
from mylist import MyList

def execute(robot):
try:
Expand Down Expand Up @@ -38,7 +37,6 @@ def execute(robot):
raise

if __name__ == '__main__':
mylist=MyList()
if len(sys.argv) == 2:
path = os.getcwd()
open_path = path[:path.rfind('src')] + 'cfg/'
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,7 @@

from drone import Drone
from robot import Robot

def execute(robot):
try:
robot.move("forward")
Expand Down