From 7f924fba15b373f704a4a1aa4be30bff0502de73 Mon Sep 17 00:00:00 2001
From: Arthur Golubtsov <goldartt@gmail.com>
Date: Fri, 5 Jun 2020 19:52:13 +0300
Subject: [PATCH] drone: Add clover dir auto detection

---
 drone/client.py                         | 16 +++++++++++++++-
 drone/config/spec/configspec_client.ini |  1 +
 2 files changed, 16 insertions(+), 1 deletion(-)

diff --git a/drone/client.py b/drone/client.py
index e6bdf45..8e12f66 100644
--- a/drone/client.py
+++ b/drone/client.py
@@ -19,6 +19,8 @@ except ImportError:
     print("Can't import rospy! Please check your ROS installation.")
     exit()
 
+import rospkg
+
 # Import clever or clover package
 try:
     from clever import srv
@@ -125,12 +127,25 @@ class CopterClient(client_core.Client):
     def __init__(self, config_path="config/client.ini"):
         super(CopterClient, self).__init__(config_path)
         self.load_config()
+        if self.config.clover_dir == 'auto':
+            self.check_clover_dir()
         self.telemetry = None
         self.animation = animation.Animation(self.config, "animation.csv")
 
     def load_config(self):
         super(CopterClient, self).load_config()
 
+    def check_clover_dir(self):
+        rospack = rospkg.RosPack()
+        try:
+            path = rospack.get_path('clever')
+        except rospkg.common.ResourceNotFound:
+            try:
+                path = rospack.get_path('clover')
+            except rospkg.common.ResourceNotFound:
+                path = 'error'
+        self.config.set('', 'clover_dir', path, write=True)
+
     def on_broadcast_bind(self):
         repair_chrony(self.config.server_host)
 
@@ -150,7 +165,6 @@ class CopterClient(client_core.Client):
         client_thread = threading.Thread(target=super(CopterClient, self).start, name="Client thread")
         client_thread.daemon = True
         client_thread.start()
-        #super(CopterClient, self).start()
 
     def start_floor_frame_broadcast(self):
         if self.config.floor_frame_parent == "gps":
diff --git a/drone/config/spec/configspec_client.ini b/drone/config/spec/configspec_client.ini
index 253358a..a2ae0af 100644
--- a/drone/config/spec/configspec_client.ini
+++ b/drone/config/spec/configspec_client.ini
@@ -2,6 +2,7 @@ config_name = string(default='client')
 config_version = float(default=1.0)
 # Available options: /hostname ; /default ; /ip ; any string 63 characters length
 id = string(default=/hostname, max=63)
+clover_dir = string(default=auto)
 
 [SERVER]
 port = integer(default=25000, min=1)