[tools] Scratch2JdeRobot change in movement blocks and added math blocks

src/tools/scratch2jderobot/extension/scratch2robot.s2e

@@ -3,23 +3,18 @@
     "extensionPort": 12345,
         "blockSpecs": [
                 ["", "stop robot-drone", "stop"],
-                ["", "move robot %m.robotDirections", "robot/move", "forward"],
-                ["", "move drone %m.droneDirections", "drone/move", "forward"],
-                ["", "move drone %m.droneDirections speed %n", "drone/move/speed", "forward", 1],
+                ["", "move robot %n", "robot/move", "vx,vz"],
+                ["", "move drone %n", "drone/move", "vx,vz,vyaw"],
                 ["", "move robot %m.robotDirections speed %n", "robot/move/speed", "forward", 1],
-                ["", "turn drone %m.turnDirections speed %n", "turn/speed", "left", 1],
                 ["", "turn robot %m.turnDirections speed %n", "turn/speed", "left", 1],
                 ["", "take off drone", "drone/takeoff"],
                 ["", "land drone", "drone/land"],
                 ["r", "frontal laser distance", "laser/frontal"],
                 ["r", "color detection %m.color", "camera/all","red"],
-                ["r", "size of object", "camera/size"],
-                ["r", "x position of object", "camera/x_pos"],
-                ["r", "y position of object", "camera/y_pos"],
+                ["r", "get pose3D", "camera/pose3D"],
         ],
         "menus": {
                 "robotDirections": ["forward", "back"],
-                "droneDirections": ["forward", "back", "left", "right", "up", "down"],
                 "turnDirections": ["left", "right"],
                 "color": ["red", "blue"]
         }

src/tools/scratch2jderobot/kurt/commands_src_extras.py

@@ -11,21 +11,17 @@
 # extension:Array
 extras = []
 
-# robotics:ScratchExtension
+# robotics:ScratchExtensio
 extras += [
     # add extensions code if not auto-generated
     [' ', 'stop robot-drone', 'Scratch2JdeRobot/stop'],
-    [' ', 'move robot %m.robotDirections', 'Scratch2JdeRobot/robot/move', 'forward'],
-    [' ', 'move drone %m.droneDirections', 'Scratch2JdeRobot/drone/move', 'forward'],
-    [' ', 'move drone %m.direction speed %n', 'Scratch2JdeRobot/drone/move/speed', 'forward', 1],
+    [' ', 'move robot %n', 'Scratch2JdeRobot/robot/move', 'vx,vz'],
+    [' ', 'move drone %n', 'Scratch2JdeRobot/drone/move', 'vx,vz,vyaw'],
     [' ', 'move robot %m.direction speed %n', 'Scratch2JdeRobot/robot/move/speed', 'forward', 1],
-    [' ', 'turn drone %m.turnDirections speed %n', 'Scratch2JdeRobot/turn/speed', 'left', 1],
     [' ', 'turn robot %m.turnDirections speed %n', 'Scratch2JdeRobot/turn/speed', 'left', 1],
     [' ', 'take off drone', 'Scratch2JdeRobot/drone/takeoff'],
     [' ', 'land drone', 'Scratch2JdeRobot/drone/land'],
     ['r', 'frontal laser distance', 'Scratch2JdeRobot/laser/frontal'],
     ['r', 'color detection %m.color', 'Scratch2JdeRobot/camera/all', 'red'],
-    ['r', 'size of object', 'Scratch2JdeRobot/camera/size'],
-    ['r', 'x position of object', 'Scratch2JdeRobot/camera/x_pos'],
-    ['r', 'y position of object', 'Scratch2JdeRobot/camera/y_pos']
+    ['r', 'get pose3D', 'Scratch2JdeRobot/camera/pose3D'],
 ]

src/tools/scratch2jderobot/scripts/scratch2python.py

@@ -18,6 +18,23 @@
 from parse import parse, compile
 from termcolor import cprint
 
+MATH = [
+    ['sqrt of {}', 'math.sqrt({l[0]})'],
+    ['sin of {}', 'math.sin({l[0]})'],
+    ['cos of {}', 'math.cos({l[0]})'],
+    ['tan of {}', 'math.tan({l[0]})'],
+    ['asin of {}', 'math.asin({l[0]})'],
+    ['acos of {}', 'math.acos({l[0]})'],
+    ['atan of {}', 'math.atan({l[0]})'],
+    ['log of {}', 'math.log10({l[0]})'],
+    ['ln of {}', 'math.log({l[0]})'],
+    ['floor of {}', 'math.floor({l[0]})'],
+    ['ceiling of {}', 'math.ceil({l[0]})'],
+    ['abs of {}', 'abs({l[0]})'],
+    ['{} mod of {}', '{l[0]}%{l[1]}'],
+
+]
+
 GENERAL = [
     ['end', ''],
     ['forever', 'while True:'],
@@ -37,22 +54,21 @@
 ]
 
 ROBOTICS = [
-    ['move robot {}', 'robot.move("{l[0]}")'],
-    ['move drone {}', 'robot.move("{l[0]}")'],
-    ['move drone {} speed {}', 'robot.move("{l[0]}", {l[1]})'],
+    ['move robot {}', 'robot.move_vector({l[0]})'],
+    ['move drone {}', 'robot.move_vector({l[0]})'],
     ['move robot {} speed {}', 'robot.move("{l[0]}", {l[1]})'],
-    ['stop robot-drone', 'robot.stop()'],
-    ['turn drone {} speed {}', 'robot.turn("{l[0]}", {l[1]})'],
     ['turn robot {} speed {}', 'robot.turn("{l[0]}", {l[1]})'],
+    ['stop robot-drone', 'robot.stop()'],
     ['take off drone', 'robot.take_off()'],
     ['land drone', 'robot.land()'],
     ['frontal laser distance', 'robot.get_laser_distance()'],
     ['color detection {}', 'robot.detect_object("{l[0]}")'],
-    ['size of object', 'robot.get_size_object()'],
-    ['x position of object', 'robot.get_x_position()'],
-    ['y position of object', 'robot.get_y_position()'],
+    ['get pose3D', 'robot.get_pose3d()'],
+
 ]
 
+LISTNAMES = []
+
 def is_conditional(sentence):
     """
     Returns if a sentence is conditional or not.
@@ -64,7 +80,7 @@ def is_conditional(sentence):
         return True
 
     return False
-   
+
 
 def is_list(sentence):
     """
@@ -106,11 +122,17 @@ def sentence_mapping(sentence, threshold=0.0):
     translation = None
 
     # first look for general blocks
-    for elem in GENERAL:
+    for elem in MATH:
         if elem[0][:3] == sentence.replace('    ', '').replace('(', '')[:3]:
             options.append(elem)
             found = True
 
+    if not found:
+        for elem in GENERAL:
+            if elem[0][:3] == sentence.replace('    ', '').replace('(', '')[:3]:
+                options.append(elem)
+                found = True
+
     # then look for robotics blocks
     if not found:
         for elem in ROBOTICS:
@@ -123,6 +145,8 @@ def sentence_mapping(sentence, threshold=0.0):
         l = [(m[0], m[1], similar(sentence, m[0])) for m in options]
         original, translation, score = max(l, key=lambda item: item[2])
 
+
+
         if score < threshold:
             return None, None
 
@@ -152,7 +176,6 @@ def sentence_mapping(sentence, threshold=0.0):
     return original, translation
 
 
-
 if __name__ == "__main__":
     # get current working directory
     path = os.getcwd()
@@ -170,7 +193,8 @@ def sentence_mapping(sentence, threshold=0.0):
 import sys\n\
 import comm\n\
 import os\n\
-import yaml\n\n\
+import yaml\n\
+import math\n\n\
 from drone import Drone\n\
 from robot import Robot\n\n\
 def execute(robot):\n\
@@ -222,6 +246,7 @@ def execute(robot):\n\
             sentences += b.stringify().split('\n')
         tab_seq = "\t"
         python_program = ""
+        python_lists = ""
 
         for s in sentences:
             # pre-processing if there is a condition (operators and types)
@@ -232,23 +257,27 @@ def execute(robot):\n\
 
             # 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"
+
+            if is_list(s) and translation.split(".")[0] not in LISTNAMES :
+                newlistname = translation.split(".")[0]
+                LISTNAMES.append(newlistname)
+    	        python_lists += tab_seq
+    	        python_lists += translation.split(".")[0]+" = []\n\t"
 
             python_program += tab_seq * (num_tabs + 1)
 
-            # set the code		
+            # set the code
             if translation != None:
                 python_program += translation
             else:
                 cprint("[WARN] Block <%s> not included yet" % s, 'yellow')
             python_program += "\n" + tab_seq
 
         # join the template with the code and replace the tabs
+        python_lists += python_program
+        python_program = python_lists
         file_text = template % python_program
         file_text = file_text.replace(tab_seq, ' ' * 4)
 

src/tools/scratch2jderobot/src/scratch2jderobot/drone.py

@@ -66,6 +66,18 @@ def close(self):
         self.__navdata_client.stop()
         self.__pose3d_client.stop()
 
+
+
+    def get_pose3d(self):
+        """
+        Get the value of odometry sensor.
+
+        @return: return the asked value.
+        """
+
+        return self.__pose3d_client.getPose3d()
+
+
     def toggleCam(self):
         self.frontalCamera = True
         self.__extra_client.toggleCam()
@@ -130,27 +142,6 @@ def detect_object(self, color):
 
         return size, x_position, y_position
 
-        # if position == "x position":
-        #     return x_position
-        # if position == "y position":
-        #     return y_position
-        # else:
-        #     return size
-
-    def get_size_object(self):
-
-        size, _, _ = self.detect_object("red")
-        return size
-
-    def get_x_position(self):
-
-        _, x_position, _ = self.detect_object("red")
-        return x_position
-
-    def get_y_position(self):
-
-        _, _, y_position = self.detect_object("red")
-        return y_position
 
     def go_up_down(self, direction):
         """
@@ -171,55 +162,71 @@ def go_up_down(self, direction):
         # publish movement
         self.__cmdvel_client.sendVelocities()
 
-
-    def move(self, direction, vel=None):
+    def move_vector(self, velocities):
         """
-        Set the horizontal movement of the drone.
-
-        @param direction: direction of the move. Options: forward (default), back.
-        @param vel: a number with the velocity in m/s. Default: 1 m/s.
-        """
-
-        if vel == None:
-            vel = 1
-        # set different direction
-        if direction == "back":
-            self.__cmdvel_client.setVX(-vel)
-        elif direction == "forward":
-            self.__cmdvel_client.setVX(vel)
-        elif direction == "left":
-            self.__cmdvel_client.setVY(vel)
-        elif direction == "right":
-            self.__cmdvel_client.setVY(-vel)
-        elif direction == "down":
-            self.__cmdvel_client.setVZ(-vel)
-        elif direction == "up":
-            self.__cmdvel_client.setVZ(vel)
-        print direction
+        Set the movements of the drone.
 
-        # publish movement
-        self.__cmdvel_client.sendVelocities()
-
-    def turn(self, direction, vel=None):
+        @param velocities: a scratch list [x,z,yaw] with the velocities in m/s.
         """
-        Set the angular velocity.
-
-        @param direction: direction of the move. Options: left (default), right.
-        @param vel: a number with the velocity in m/s. Default: 1 m/s.
-        """
-        if vel == None:
-            vel = 0.5
-        # set default velocity (m/s)
-        yaw = vel * math.pi
-
-        if direction == "right":
-            yaw = -yaw
-
-        # assign velocity
-        self.__cmdvel_client.setYaw(yaw)
+        vx = float(velocities[0])
+        vz = float(velocities[1])
+        vyaw = float(velocities[2])
+        print "velocities vector:","x:",vx,"z:",vz,"yaw:",vyaw
+        self.__cmdvel_client.setVX(vx)
+        self.__cmdvel_client.setVZ(vz)
+        self.__cmdvel_client.setYaw(vyaw)
 
         # publish movement
         self.__cmdvel_client.sendVelocities()
+    #
+    # def move(self, direction, vel=None):
+    #     """
+    #     Set the horizontal movement of the drone.
+    #
+    #     @param direction: direction of the move. Options: forward (default), back.
+    #     @param vel: a number with the velocity in m/s. Default: 1 m/s.
+    #     """
+    #
+    #     if vel == None:
+    #         vel = 1
+    #     # set different direction
+    #     if direction == "back":
+    #         self.__cmdvel_client.setVX(-vel)
+    #     elif direction == "forward":
+    #         self.__cmdvel_client.setVX(vel)
+    #     elif direction == "left":
+    #         self.__cmdvel_client.setVY(vel)
+    #     elif direction == "right":
+    #         self.__cmdvel_client.setVY(-vel)
+    #     elif direction == "down":
+    #         self.__cmdvel_client.setVZ(-vel)
+    #     elif direction == "up":
+    #         self.__cmdvel_client.setVZ(vel)
+    #     print direction
+    #
+    #     # publish movement
+    #     self.__cmdvel_client.sendVelocities()
+    #
+    # def turn(self, direction, vel=None):
+    #     """
+    #     Set the angular velocity.
+    #
+    #     @param direction: direction of the move. Options: left (default), right.
+    #     @param vel: a number with the velocity in m/s. Default: 1 m/s.
+    #     """
+    #     if vel == None:
+    #         vel = 0.5
+    #     # set default velocity (m/s)
+    #     yaw = vel * math.pi
+    #
+    #     if direction == "right":
+    #         yaw = -yaw
+    #
+    #     # assign velocity
+    #     self.__cmdvel_client.setYaw(yaw)
+    #
+    #     # publish movement
+    #     self.__cmdvel_client.sendVelocities()
 
     def stop(self):
         """