Merge pull request #4 from simonpavlov/master

Thank you, Simon!

Author: akru

.gitignore

@@ -1 +1,4 @@
 *.pyc
+.ycm_extra_conf.py
+.*.swp
+.*.swo

drone_employee/doc/delivery.md

@@ -0,0 +1,3 @@
+## Drop API
+In order to relieve the load you simply publish the `latitude` and `longitude` of the point of delivery to the example as [here](https://github.com/simonpavlov/contracts/blob/master/drone_carrier.sol).
+To perform additional actions may be modified `drop_cargo()` in [quad_controller.py](https://github.com/DroneEmployee/drone_employee_ros/blob/master/drone_employee/scripts/quad_controller.py).

drone_employee/launch/controller.launch

@@ -1,5 +1,5 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <launch>
     <node name="state" pkg="drone_employee" type="state_publisher.py" />
-    <node name="controller" pkg="drone_employee" type="quad_controller.py" />
+    <node name="controller" pkg="drone_employee" type="quad_controller.py" output="screen"/>
 </launch>

drone_employee/scripts/quad_controller.py

@@ -2,14 +2,16 @@
 
 from mission_wrapper import waypointWrap
 from quad_commander import *
+from geo_math import len_diff_coord
 
-from rospy import init_node, is_shutdown, spin, sleep, logerr, loginfo
+from rospy import init_node, is_shutdown, spin, sleep, logerr, loginfo, logwarn
 from rospy import Subscriber, Publisher
 from small_atc_msgs.msg import *
 from sensor_msgs.msg import NavSatFix
-from std_msgs.msg import Bool, UInt32
+from std_msgs.msg import Bool, UInt32, Int64
+from mavros_msgs.msg import WaypointList
 from threading import Thread
-from Queue import Queue
+from Queue import Queue, Empty
 
 def drop_all(queue):
     while not queue.empty():
@@ -19,36 +21,77 @@ def take_current(queue):
     drop_all(queue)
     return queue.get(True, None)
 
-def quad_controller(route_queue, position_queue, arming_queue):
-    flight_done_pub = Publisher('remove', UInt32, queue_size=1) 
+def find_waypoint_id(route, target_pos):
+    for i in range(0, len(route.route)):
+        rt = route.route[i]
+        if rt.latitude == target_pos[0] and rt.longitude == target_pos[1]:
+            return i
+
+def find_cur_waypoint_id(route):
+    for i in range(0, len(route)):
+        if route[i].is_current:
+            return i
+
+def drop_cargo():
+    logwarn("DROP!!!")
+
+def quad_controller(route_queue, position_queue, arming_queue,
+                    trgt_lat_queue, trgt_lon_queue, waypoints_queue):
+    flight_done_pub = Publisher('remove', UInt32, queue_size=1)
     while not is_shutdown():
         # Take a route
         route = route_queue.get(True, None)
         loginfo('Received route with id #'+str(route.id)+'.')
         if not route.valid:
             logerr('Route invalid!')
             return
+        target_pos = None
+        need_waypoint_id = None
+        if trgt_lat_queue.empty() or trgt_lon_queue.empty():
+            logwarn('No target!')
+        else:
+            target_pos = (trgt_lat_queue.get().data / 1e6,
+                          trgt_lon_queue.get().data / 1e6)
+            # Determine the point where you want to dump the load
+            need_waypoint_id = find_waypoint_id(route, target_pos)
+            loginfo('need_waypoint_id: ' + str(need_waypoint_id))
+
         # Push a mission into flight controller
-        push_mission(waypointWrap(target.route))
+        push_mission(waypointWrap(route.route))
         loginfo('Mission loaded to flight controller.')
         # Set manual mode
         set_mode('ACRO')
-        # Enable motors 
+        # Enable motors
         arming()
         # Set autopilot mode
         set_mode('AUTO')
         loginfo('Flight!')
 
         # Wainting for arming
-        # TODO: More elegant case for mission finish check
         sleep(5)
         drop_all(arming_queue)
+
+        # If you do not have a goal to clear cargo,
+        # we believe that already dropped
+        droped = (need_waypoint_id == None)
+        while arming_queue.get().data and not droped:
+            waypoints = None
+            # To not accumulate elements in arming_queue
+            try:
+                waypoints = waypoints_queue.get_nowait().waypoints
+            except Empty:
+                continue
+            current_waypoint_id = find_cur_waypoint_id(waypoints)
+            if current_waypoint_id > need_waypoint_id:
+                droped = True
+                drop_cargo()
+
+        # TODO: More elegant case for mission finish check
         while arming_queue.get().data:
             pass
 
         loginfo('Mission #'+str(route.id)+' complete!')
         flight_done_pub.publish(route.id)
-        
 
 def main():
     ''' The main routine '''
@@ -57,20 +100,28 @@ def main():
     route_queue = Queue()
     position_queue = Queue()
     arming_queue = Queue()
+    trgt_lat_queue = Queue()
+    trgt_lon_queue = Queue()
+    waypoints_queue = Queue()
     # Create controller thread
     controller = Thread(target=quad_controller,
-                        args=(target_queue, position_queue, arming_queue))
-    
+                        args=(route_queue, position_queue, arming_queue,
+                              trgt_lat_queue, trgt_lon_queue, waypoints_queue))
+
     # Route message handler
     def quad_route(msg):
         if not controller.isAlive():
             controller.start()
         route_queue.put(msg)
-    
-    Subscriber('mavros/global_position/global', NavSatFix, position_queue.put) 
+
+    Subscriber('mavros/global_position/global', NavSatFix, position_queue.put)
     Subscriber('armed', Bool, arming_queue.put)
+    # TODO: combine next two topic in one
+    Subscriber('drop_target_lat', Int64, trgt_lat_queue.put)
+    Subscriber('drop_target_lon', Int64, trgt_lon_queue.put)
+    Subscriber('mavros/mission/waypoints', WaypointList, waypoints_queue.put)
     Subscriber('route', RouteResponse, quad_route)
     spin()
-            
+
 if __name__ == '__main__':
     main()

small_atc/include/small_atc/ObstacleProvider.h

@@ -56,9 +56,9 @@ class ObstacleProviderImpl : public ObstacleProvider
     }
 
     void publishAll() {
-        for (auto k = obstacles.begin(); k != obstacles.end(); ++k) {
-            int id = k->first;
-            const Obstacle *map = k->second;
+        for (auto k : obstacles) {
+            int id = k.first;
+            const Obstacle *map = k.second;
             publishers[id].publish(map->getOctomapMsg());
         }
     }

small_atc/launch/demo.launch

@@ -1,13 +1,13 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <launch>
     <include ns="small_atc" file="$(find small_atc)/launch/atc.launch"/>
-    <include file="$(find small_atc_dron)/launch/apm_sitl.launch">
+    <include file="$(find drone_employee)/launch/apm_sitl.launch">
         <arg name="ident" value="1" />
     </include>
-    <include file="$(find small_atc_dron)/launch/apm_sitl.launch">
+    <include file="$(find drone_employee)/launch/apm_sitl.launch">
         <arg name="ident" value="2" />
     </include>
-    <include file="$(find small_atc_dron)/launch/apm_sitl.launch">
+    <include file="$(find drone_employee)/launch/apm_sitl.launch">
         <arg name="ident" value="3" />
     </include>
 </launch>

small_atc/launch/demo_delivery.launch

@@ -0,0 +1,7 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+    <include ns="small_atc" file="$(find small_atc)/launch/atc.launch"/>
+    <include file="$(find drone_employee)/launch/apm_sitl.launch">
+        <arg name="ident" value="1" />
+    </include>
+</launch>

small_atc/scripts/geo_convert.py

@@ -0,0 +1,51 @@
+#!/usr/bin/python2
+
+from geo_helper import satFix2Point, point2SatFix
+from geometry_msgs.msg import Point
+from small_atc_msgs.msg import SatFix
+from sys import argv
+
+DESCRIPTION = '''DESCRIPTION:\n\tThe script for simple convert from SatFix to Point and from Point to SatFix'''
+USAGE = '''USAGE:\n\t{0} -p x y z\n\t{0} -s lat lon alt'''.format(argv[0])
+
+def main():
+
+    def wrong_usage():
+        print(USAGE)
+        exit(1)
+
+    def convert_init(cls, members_name, init_mass):
+        res = cls()
+        for i in zip(members_name, map(float, init_mass)):
+            res.__setattr__(*i)
+        return res
+
+    if argv[1] == '-h':
+        print DESCRIPTION
+        print USAGE
+        exit(0)
+
+    if len(argv) != 5:
+        wrong_usage()
+
+    if argv[1] == '-p':
+        print 'convert to SatFix'
+        try:
+            point = convert_init(Point, ['x', 'y', 'z'], argv[2:])
+        except ValueError:
+            wrong_usage()
+        print point2SatFix(point)
+
+    elif argv[1] == '-s':
+        print 'convert to Point'
+        try:
+            satfix = convert_init(SatFix, ['latitude', 'longitude', 'altitude'], argv[2:])
+        except ValueError:
+            wrong_usage()
+        print satFix2Point(satfix)
+
+    else:
+        wrong_usage()
+
+if __name__ == '__main__':
+    main()

small_atc/scripts/geo_helper.py

@@ -15,7 +15,7 @@ class Origin(object):
 origin = Origin()
 origin.latitude = 37.874815
 origin.longitude = -122.616569
-origin.altitude = -1000
+origin.altitude = -1139
 origin.offset = (1060, 1930)
 #
 ###

small_atc/scripts/geo_math.py

@@ -0,0 +1,12 @@
+import math
+
+def len_diff_coord(lat1, lon1, lat2, lon2):
+     R = 6378.137 # Radius of earth in KM
+     dLat = (lat2 - lat1) * math.pi / 180
+     dLon = (lon2 - lon1) * math.pi / 180;
+     a = (math.sin(dLat/2) * math.sin(dLat/2) + 
+          math.cos(lat1 * math.pi / 180) * math.cos(lat2 * math.pi / 180) *
+          math.sin(dLon/2) * math.sin(dLon/2))
+     c = 2 * math.atan2(math.sqrt(a), math.sqrt(1-a))
+     d = R * c
+     return d * 1000 # meters