change_node debug and add of joy

.vscode/c_cpp_properties.json

+{
+  "configurations": [
+    {
+      "browse": {
+        "databaseFilename": "${workspaceFolder}/.vscode/browse.vc.db",
+        "limitSymbolsToIncludedHeaders": false
+      },
+      "includePath": [
+        "/opt/ros/noetic/include/**",
+        "/usr/include/**"
+      ],
+      "name": "ROS",
+      "intelliSenseMode": "gcc-x64",
+      "compilerPath": "/usr/bin/gcc",
+      "cStandard": "gnu11",
+      "cppStandard": "c++14"
+    }
+  ],
+  "version": 4
+}
\ No newline at end of file

.vscode/settings.json

+{
+    "python.autoComplete.extraPaths": [
+        "/opt/ros/noetic/lib/python3/dist-packages"
+    ],
+    "python.analysis.extraPaths": [
+        "/opt/ros/noetic/lib/python3/dist-packages"
+    ]
+}
\ No newline at end of file

README.md

-### ROS Olympe interface for Parrot Anafi drone
+# ROS Olympe interface for Parrot Anafi drone
+
 ## anafi_base
-Package that contains a node that read the drone video and metadata stream and publish relevant infoamtions:
+Inferface package between ROS and Olympe
+### anafi_node 
+Node that takes velocity messages and services as input and allows to fly the drone.
+It also publishes some informations for the user or other nodes:
 * Image stream
 * GPS
 * Battery level
 * State
 * Orientation
 
-## anafi_navigation
-Package that takes waypoints or joystick info to convert them into cmd_vel messages.
-
-## anafi_controll
-Interface that convert ROS services and cmd_vel in Olympe compatible messages. It also allows the user to change the camera orientation.
\ No newline at end of file
+## anafi_control
+### control_node 
+Takes /joy messages as inputs and convert them into velocity messages and ROS services.
+### change_node
+Allows the user to switch between joystick and waypoint control.
\ No newline at end of file

anafi_base/launch/base.launch

 <launch>
-  <node name="anafi_control" pkg="anafi_control" type="control_node.py" output="screen"/>
   <node name="joy" pkg="joy" type="joy_node" output="screen"/>
+  <node name="anafi_change" pkg="anafi_control" type="change_node.py" output="screen"/>
+  <node name="anafi_control" pkg="anafi_control" type="control_node.py" output="screen"/>
+
   <node name="anafi_base" pkg="anafi_base" type="anafi_node.py" output="screen"/>
 </launch>
\ No newline at end of file

anafi_base/script/anafi_node.py

@@ -10,10 +10,10 @@ import olympe
 
 #ROS imports
 import rospy
+from cv_bridge import CvBridge
+
 #Services types
 from anafi_base.srv import TakeOffLand, TakeOffLandResponse
-from cv_bridge import CvBridge
-from geometry_msgs.msg import Quaternion, Twist, Vector3
 
 #Olympe imports
 from olympe.messages.ardrone3.Piloting import Landing, TakeOff
@@ -27,20 +27,17 @@ from olympe.messages.gimbal import max_speed
 
 from olympe.video.pdraw import Pdraw, PdrawState
 
-
 #Pub types
 from sensor_msgs.msg import Image, NavSatFix
 from std_msgs.msg import Bool, Float32, Int8, String
 
+#Sub types
+from geometry_msgs.msg import Quaternion, Twist, Vector3
+
 #Drone OS param
 DRONE_IP = os.environ.get("DRONE_IP", "192.168.42.1")
 DRONE_RTSP_PORT = os.environ.get("DRONE_RTSP_PORT", "554")
 DRONE_STREAM = f"rtsp://{DRONE_IP}:{DRONE_RTSP_PORT}/live"
-
-#Can be deleted
-MEDIA_PATH = '/home/anafi/catkin_ws/src/anafi_base/script/media'
-IMG_PATH = os.path.join(MEDIA_PATH , 'output.jpg')
-
 olympe.log.update_config({"loggers": {"olympe": {"level": "WARNING"}}})
 
 class Anafi:
@@ -64,7 +61,7 @@ class Anafi:
         self.timeout = 5
 
         #Initialise Publishers
-        self.anafi_img = None #cv2.imread(IMG_PATH)
+        self.anafi_img = None
         self.bat_perc = 100
         self.gnd_dist = None
         self.air_speed = None
@@ -233,7 +230,7 @@ class Anafi:
         self.drone.start_piloting()
 
         #ROS
-        rospy.loginfo("ROS Anafi Start")
+        rospy.loginfo("ROS Anafi base: Start")
         while not rospy.is_shutdown():
             #ROS Publishers
             if self.anafi_img is not None:
@@ -284,6 +281,7 @@ class Anafi:
         self.pdraw.close()
         self.pdraw.dispose()
         self.drone.disconnect()
+        rospy.loginfo("ROS Anafi base: End")
 
 def anafi_com():
     anafi = Anafi()

anafi_control/script/change_node.py

@@ -13,17 +13,19 @@ class Change:
         #ROS Node init
         rospy.init_node('anafi_change_control', anonymous=True)
         self.br = CvBridge()
-        self.loop_rate = rospy.Rate(10)
+        self.loop_rate = rospy.Rate(1)
 
         #ROS Publishers init
-        self.vel_pub = rospy.Publisher('ctrl_mode', String, queue_size=2)
+        self.change_pub = rospy.Publisher('ctrl_mode', String, queue_size=2, latch=True)
 
     def start(self):
 
         #ROS
         rospy.loginfo("ROS Change control: Start")
+
+        self.change_pub.publish('joy')
+
         while not rospy.is_shutdown():
-            pass
             self.loop_rate.sleep()
 
     def stop(self):

anafi_control/script/control_node.py

@@ -2,23 +2,22 @@
 
 from __future__ import print_function
 
-#Olympe and basics imports
-import olympe
-
 #ROS imports
 import rospy
+from cv_bridge import CvBridge
 
 #Services types
 from anafi_control.srv import *
-from cv_bridge import CvBridge
-from geometry_msgs.msg import Twist
 
 #Pub types
+from std_msgs.msg import Float32, String
+from geometry_msgs.msg import Twist
+
+#Sub types
 from sensor_msgs.msg import Joy
-from std_msgs.msg import Float32
 
-#Drone OS param
-olympe.log.update_config({"loggers": {"olympe": {"level": "WARNING"}}})
+#Control mode
+
 
 class JoyCommand:
 
@@ -31,6 +30,7 @@ class JoyCommand:
         #Initialise Publishers
         self.vel = Twist()
         self.cam = 0
+        self.ctrl = 'joy'
 
         #Other variables
         self.TO = False
@@ -41,7 +41,8 @@ class JoyCommand:
         self.cam_pub = rospy.Publisher('cmd_cam', Float32, queue_size=2)
 
         #ROS Subscriber init
-        rospy.Subscriber("/joy", Joy, self.joyCallback)
+        rospy.Subscriber("/joy", Joy, self.joy_cb)
+        rospy.Subscriber("/ctrl_mode", String, self.ctrl_cb)
     
     def takeOffLandService(self, str):
         rospy.wait_for_service("take_off_land")
@@ -52,9 +53,12 @@ class JoyCommand:
         except rospy.ServiceException as e:
             print("Service call failed: %s" %e)
 
-    def joyCallback(self, joy):
+    def ctrl_cb(self, mode):
+        self.ctrl = mode
+
+    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]:
+        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
@@ -91,26 +95,30 @@ class JoyCommand:
                     print(self.takeOffLandService("land"))
                 except :
                     print("Service call failed")
-        else:
+
+        elif (self.ctrl == 'joy'):
             self.vel = Twist()
 
+        else:
+            pass
+
     def start(self):
 
         #ROS
-        rospy.loginfo("ROS Control Start")
+        rospy.loginfo("ROS Control node: Start")
         while not rospy.is_shutdown():
+            if self.ctrl == 'joy':
+                #Publishers
+                if self.vel is not None:
+                    self.vel_pub.publish(self.vel)
 
-            #Publishers
-            if self.vel is not None:
-                self.vel_pub.publish(self.vel)
-
-            if self.cam is not None:
-                self.cam_pub.publish(self.cam)
+                if self.cam is not None:
+                    self.cam_pub.publish(self.cam)
                 
             self.loop_rate.sleep()
 
     def stop(self):
-        pass
+        rospy.loginfo("ROS Control node: End")
 
 if __name__ == "__main__":
     joy_cmd = JoyCommand()