bugifx + test with drone

comm_pkg/src/drone.py

@@ -5,6 +5,7 @@
 import imutils
 import cv2
 import numpy as np
+import comm
 
 from jderobotTypes import CMDVel
 from jderobotTypes import Pose3d
@@ -38,6 +39,7 @@ def __init__(self, cfg):
         self.__cmdvel_client = jdrc.getCMDVelClient("drone.CMDVel")
         self.__extra_client = jdrc.getArDroneExtraClient("drone.Extra")
         self.__navdata_client = jdrc.getNavdataClient("drone.Navdata")
+        # self.__motors_client = jdrc.getMotorsClient("drone.Motors")
 
     def close(self):
         """

comm_pkg/src/drone_example_2.py

@@ -0,0 +1,84 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+
+import time
+import config
+import sys
+import os
+import yaml
+import math
+
+from drone import Drone
+from robot import Robot
+
+def execute(robot):
+    try:
+        mylist = []
+        myvelocity = []
+        robot.take_off()
+        while True:
+            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:
+                    velx = '-2'
+                else:
+                    velx = '2'
+
+                if x > 165:
+                    velyaw = '-2'
+                else:
+                    velyaw = '2'
+
+                if y > 110:
+                    velz = '-1'
+                else:
+                    velz = '1'
+
+            else:
+                robot.stop()
+                velx = '0'
+                velz = '0'
+                velyaw = '2'
+
+            myvelocity.insert(0, velx)
+            myvelocity.insert(1, velz)
+            myvelocity.insert(2, velyaw)
+            robot.move_vector(myvelocity)
+
+    except KeyboardInterrupt:
+        raise
+
+if __name__ == '__main__':
+    if len(sys.argv) == 2:
+        path = os.getcwd()
+        open_path = path+'/'
+        filename = sys.argv[1]
+
+    else:
+        sys.exit("ERROR: Example:python my_generated_script.py cfgfile.yml")
+
+    # loading parameters
+    if os.path.isabs(filename):
+        stream = open(filename, "r")
+        cfg = config.load(filename)
+    else:
+        stream = open(open_path + filename, "r")
+        cfg = config.load(open_path + filename)
+    yml_file = yaml.load(stream)
+
+    for section in yml_file:
+        if section == 'drone':
+            #TODO
+            robot = Drone(cfg)
+
+            break
+        elif section == 'robot':
+            robot = Robot(cfg)
+
+            break
+    # executing the scratch program
+    execute(robot)
+

comm_pkg/src/scratch2python_comm.py

@@ -215,16 +215,12 @@ def execute(robot):\n\
 \tyml_file = yaml.load(stream)\n\n\
 \tfor section in yml_file:\n\
 \t\tif section == 'drone':\n\
-\t\t\t#starting comm\n\
-\t\t\tjdrc = comm.init(cfg,'drone')\n\n\
 \t\t\t# creating the object\n\
-\t\t\trobot = Drone(jdrc)\n\n\
+\t\t\trobot = Drone(cfg)\n\n\
 \t\t\tbreak\n\
 \t\telif section == 'robot':\n\
-\t\t\t#starting comm\n\
-\t\t\tjdrc = comm.init(cfg,'robot')\n\n\
-\t\t\t# creating the object\n\
-\t\t\trobot = Robot(jdrc)\n\n\
+\t\t\t# creating the object\n\n\
+\t\t\trobot = Robot(cfg)\n\n\
 \t\t\tbreak\n\
 \t# executing the scratch program\n\
 \texecute(robot)\n\n\

comm_pkg/src/test_drone_blocks_2.py

@@ -0,0 +1,72 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+
+import time
+import config
+import sys
+import comm
+import os
+import yaml
+import math
+
+from drone import Drone
+from robot import Robot
+
+def execute(robot):
+    try:
+        mylist2 = []
+        mylist = []
+        mylist2.append('0')
+        mylist2.append('0')
+        mylist2.append('-2')
+        mylist.append('0')
+        mylist.append('0')
+        mylist.append('2')
+        robot.take_off()
+        time.sleep(1)
+        robot.move_vector(mylist)
+        time.sleep(3)
+        robot.stop()
+        time.sleep(1)
+        robot.move_vector(mylist2)
+        time.sleep(3)
+        robot.stop()
+        time.sleep(1)
+        robot.land()
+        time.sleep(1)
+    except KeyboardInterrupt:
+        raise
+
+if __name__ == '__main__':
+    if len(sys.argv) == 2:
+        path = os.getcwd()
+        open_path = path+'/'
+        filename = sys.argv[1]
+
+    else:
+        sys.exit("ERROR: Example:python my_generated_script.py cfgfile.yml")
+
+    # loading parameters
+    if os.path.isabs(filename):
+        stream = open(filename, "r")
+        cfg = config.load(filename)
+    else:
+        stream = open(open_path + filename, "r")
+        cfg = config.load(open_path + filename)
+    yml_file = yaml.load(stream)
+
+    for section in yml_file:
+        if section == 'drone':
+            # creating the object
+            robot = Drone(cfg)
+
+            break
+        elif section == 'robot':
+            # creating the object
+
+            robot = Robot(cfg)
+
+            break
+    # executing the scratch program
+    execute(robot)
+

examples/drone_example/src/drone_example_2.py

@@ -0,0 +1,84 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+
+import time
+import config
+import sys
+import os
+import yaml
+import math
+
+from drone import Drone
+from robot import Robot
+
+def execute(robot):
+    try:
+        mylist = []
+        myvelocity = []
+        robot.take_off()
+        while True:
+            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:
+                    velx = '-2'
+                else:
+                    velx = '2'
+
+                if x > 165:
+                    velyaw = '-2'
+                else:
+                    velyaw = '2'
+
+                if y > 110:
+                    velz = '-1'
+                else:
+                    velz = '1'
+
+            else:
+                robot.stop()
+                velx = '0'
+                velz = '0'
+                velyaw = '2'
+
+            myvelocity.insert(0, velx)
+            myvelocity.insert(1, velz)
+            myvelocity.insert(2, velyaw)
+            robot.move_vector(myvelocity)
+
+    except KeyboardInterrupt:
+        raise
+
+if __name__ == '__main__':
+    if len(sys.argv) == 2:
+        path = os.getcwd()
+        open_path = path+'/'
+        filename = sys.argv[1]
+
+    else:
+        sys.exit("ERROR: Example:python my_generated_script.py cfgfile.yml")
+
+    # loading parameters
+    if os.path.isabs(filename):
+        stream = open(filename, "r")
+        cfg = config.load(filename)
+    else:
+        stream = open(open_path + filename, "r")
+        cfg = config.load(open_path + filename)
+    yml_file = yaml.load(stream)
+
+    for section in yml_file:
+        if section == 'drone':
+            #TODO
+            robot = Drone(cfg)
+
+            break
+        elif section == 'robot':
+            robot = Robot(cfg)
+
+            break
+    # executing the scratch program
+    execute(robot)
+

examples/robot_example/src/robot_example_2.py

@@ -0,0 +1,67 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+
+import time
+import config
+import sys
+import os
+import yaml
+import math
+
+from drone import Drone
+from robot import Robot
+
+def execute(robot):
+    try:
+        while True:
+            robot.move("forward", 2)
+            time.sleep(5)
+            robot.stop()
+            time.sleep(1)
+            robot.move("back", 2)
+            time.sleep(5)
+            robot.stop()
+            time.sleep(1)
+            robot.turn("left", 0.5)
+            time.sleep(2)
+            robot.stop()
+            time.sleep(1)
+            robot.turn("right", 0.5)
+            time.sleep(2)
+            robot.stop()
+            time.sleep(1)
+
+    except KeyboardInterrupt:
+        raise
+
+if __name__ == '__main__':
+    if len(sys.argv) == 2:
+        path = os.getcwd()
+        open_path = path+'/'
+        filename = sys.argv[1]
+
+    else:
+        sys.exit("ERROR: Example:python my_generated_script.py cfgfile.yml")
+
+    # loading parameters
+    if os.path.isabs(filename):
+        stream = open(filename, "r")
+        cfg = config.load(filename)
+    else:
+        stream = open(open_path + filename, "r")
+        cfg = config.load(open_path + filename)
+    yml_file = yaml.load(stream)
+
+    for section in yml_file:
+        if section == 'drone':
+            #TODO
+            robot = Drone(cfg)
+
+            break
+        elif section == 'robot':
+            robot = Robot(cfg)
+
+            break
+    # executing the scratch program
+    execute(robot)
+

gazebo/urdf/quadrotor.urdf.xacro

@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+
+<robot name="quadrotor"
+    xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
+    xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
+    xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
+    xmlns:xacro="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface">
+
+    <!-- Included URDF Files -->
+    <include filename="$(find cvg_sim_gazebo)/urdf/quadrotor_base.urdf.xacro" />
+
+    <!-- Instantiate quadrotor_base_macro once (has no parameters atm) -->
+    <quadrotor_base_macro />
+
+</robot>