Last commit remote anafi

anafi_base/script/anafi_node.py

-#!/home/anafi/code/parrot-olympe/out/olympe-linux/pyenv_root/versions/3.9.5/bin/python3
+#!/usr/bin/env python
 
 #Generic imports
 import os
@@ -24,6 +24,7 @@ from olympe.messages.ardrone3.SpeedSettingsState import MaxRotationSpeedChanged
 
 from olympe.messages.gimbal import set_max_speed
 from olympe.messages.gimbal import max_speed
+from olympe.messages.gimbal import max_speed
 
 from olympe.video.pdraw import Pdraw, PdrawState
 
@@ -51,10 +52,16 @@ class Anafi:
         # Create the olympe.Drone object from its IP address
         try:
             self.drone = olympe.Drone(DRONE_IP)
-            self.drone.connect()
         except Exception as e:
             print("Drone connection failed: %s" %e)
         
+        while not self.drone.connection_state():
+            try:
+                print("Trying to connect to the drone....")
+                self.drone.connect(timeout = 10)
+            except Exception as e:
+                print("Drone connection failed: %s" % e)
+
         #Pdraw init
         self.frame_queue = queue.Queue()
         self.flush_queue_lock = threading.Lock()
@@ -230,47 +237,65 @@ class Anafi:
         self.drone.start_piloting()
 
         #ROS
-        rospy.loginfo("ROS Anafi base: Start")
+       
+
         while not rospy.is_shutdown():
-            #ROS Publishers
-            if self.anafi_img is not None:
-                self.img_pub.publish(self.br.cv2_to_imgmsg(self.anafi_img, "rgb8"))
-
-            #Not None verified in the frame call back    
-            self.bat_pub.publish(self.bat_perc)
-            self.gnd_dist_pub.publish(self.gnd_dist)
-            self.air_speed_pub.publish(self.air_speed)
-            self.quat_pub.publish(self.quat)
-            self.speed_pub.publish(self.speed)
-            self.state_pub.publish(self.state)
-            self.sat_pub.publish(self.sat)
-            self.gps_pub.publish(self.gps)
-
-            #Convert cmd_vel in drone mouvement
-            if (self.vel != Twist()) & ((self.last_vel_time - rospy.get_time()) <= self.time_thrs) & ((self.state == 'FLYING')|(self.state == 'HOVERING') & (self.bat_perc > 10)): 
-                
-                self.drone.piloting(np.int32(self.vel.linear.y).item(),np.int32(self.vel.linear.x).item(),np.int32(self.vel.angular.z).item(),np.int32(self.vel.linear.z).item(),0)
             
-            elif ((self.state == 'HOVERING')|(self.state == 'FLYING')) & (self.vel == Twist()): 
+            rospy.loginfo("ROS Anafi base: Start")
+
+            while self.drone.connection_state(): 
+                #ROS Publishers
+                if self.anafi_img is not None:
+                    self.img_pub.publish(self.br.cv2_to_imgmsg(self.anafi_img, "rgb8"))
+
+                #Not None verified in the frame call back    
+                self.bat_pub.publish(self.bat_perc)
+                self.gnd_dist_pub.publish(self.gnd_dist)
+                self.air_speed_pub.publish(self.air_speed)
+                self.quat_pub.publish(self.quat)
+                self.speed_pub.publish(self.speed)
+                self.state_pub.publish(self.state)
+                self.sat_pub.publish(self.sat)
+                self.gps_pub.publish(self.gps)
+
+                #Convert cmd_vel in drone mouvement
+                if (self.vel != Twist()) & ((self.last_vel_time - rospy.get_time()) <= self.time_thrs) & ((self.state == 'FLYING')|(self.state == 'HOVERING') & (self.bat_perc > 10)): 
+                    
+                    self.drone.piloting(np.int32(self.vel.linear.y).item(),np.int32(self.vel.linear.x).item(),np.int32(self.vel.angular.z).item(),np.int32(self.vel.linear.z).item(),0)
                 
-                self.drone.piloting(0,0,0,0,0)
-            
-            else : 
+                elif ((self.state == 'HOVERING')|(self.state == 'FLYING')) & (self.vel == Twist()): 
+                    
+                    self.drone.piloting(0,0,0,0,0)
+                
+                else : 
+
+                    pass
+
+                if self.bat_perc < 10:
 
-                pass
+                    print("Battery low: Please Land")
 
-            if self.bat_perc < 10:
+                    '''
+                    try:
+                        print("Battery low: Call Land")
+                        print(self.takeOffLandService("land"))
+                    except :
+                        print("Service call failed")
+                    '''
+                print(self.drone.get_state(olympe.messages.gimbal.attitude)[0]['pitch_absolute'])
+                self.loop_rate.sleep()
 
-                print("Battery low: Please Land")
+            rospy.loginfo("ROS Anafi base: End")
 
-                '''
+            while not self.drone.connection_state():
                 try:
-                    print("Battery low: Call Land")
-                    print(self.takeOffLandService("land"))
-                except :
-                    print("Service call failed")
-                '''
-            self.loop_rate.sleep()
+                    print("Trying to connect to the drone....")
+                    self.drone.connect(timeout = 10)
+                except Exception as e:
+                    print("Drone connection failed: %s" % e)
+                    
+        self.pdraw.close()
+        self.pdraw.dispose()
         
         #Stop piloting
         self.drone.stop_piloting()

anafi_control/script/change_node.py

-#!/home/anafi/code/parrot-olympe/out/olympe-linux/pyenv_root/versions/3.9.5/bin/python3
+#!/usr/bin/env python
 
 #ROS imports
 import rospy

anafi_control/script/control_node.py

-#!/home/anafi/code/parrot-olympe/out/olympe-linux/pyenv_root/versions/3.9.5/bin/python3
+#!/usr/bin/env python
 
 from __future__ import print_function
 
@@ -54,11 +54,11 @@ class JoyCommand:
             print("Service call failed: %s" %e)
 
     def ctrl_cb(self, mode):
-        self.ctrl = mode
+        self.ctrl = mode.data
 
     def joy_cb(self, joy):
         #Update Twist values if one of the DMS (Dead Man Switch) is triggered
-        if joy.buttons[4] or joy.buttons[5] and (self.ctrl == 'joy'):
+        if (joy.buttons[4] or joy.buttons[5]) and (self.ctrl == 'joy'):
             self.vel.linear.x =  joy.axes[4] * self.sensivity
             self.vel.linear.y = - joy.axes[3] * self.sensivity 
             self.vel.linear.z = ((joy.axes[2]+1)/2 - (joy.axes[5]+1)/2) * self.sensivity