Update tutorial 6

demo/tutorial_1/1_px4_mavros_offboard_controller/commander.py

@@ -63,7 +63,8 @@ def set_pose(self, x=0, y=0, z=2, BODY_OFFSET_ENU = True):
 if __name__ == "__main__":
 
     con = Commander()
-    con.move(1, 0, 0)
+    time.sleep(2)
+    con.move(5, 0, 0)
     time.sleep(2)
     con.turn(90)
     time.sleep(2)

demo/tutorial_2/2_Struction_from_Motion/px4_mavros_run.py

@@ -19,7 +19,7 @@ def __init__(self):
         self.local_pose = None
         self.current_state = None
         self.current_heading = None
-        self.takeoff_height = 3.2
+        self.takeoff_height = 1.2
         self.local_enu_position = None
 
         self.cur_target_pose = None

demo/tutorial_6/6_object_tracking/commander.py

@@ -0,0 +1,73 @@
+import rospy
+from mavros_msgs.msg import GlobalPositionTarget, State
+from mavros_msgs.srv import CommandBool, CommandTOL, SetMode
+from geometry_msgs.msg import PoseStamped, Twist
+from sensor_msgs.msg import Imu, NavSatFix
+from std_msgs.msg import Float32, String
+from pyquaternion import Quaternion
+import time
+import math
+
+
+class Commander:
+    def __init__(self):
+        rospy.init_node("commander_node")
+        rate = rospy.Rate(20)
+        self.position_target_pub = rospy.Publisher('gi/set_pose/position', PoseStamped, queue_size=10)
+        self.yaw_target_pub = rospy.Publisher('gi/set_pose/orientation', Float32, queue_size=10)
+        self.custom_activity_pub = rospy.Publisher('gi/set_activity/type', String, queue_size=10)
+
+
+    def move(self, x, y, z, BODY_OFFSET_ENU=True):
+        self.position_target_pub.publish(self.set_pose(x, y, z, BODY_OFFSET_ENU))
+
+
+    def turn(self, yaw_degree):
+        self.yaw_target_pub.publish(yaw_degree)
+
+
+    # land at current position
+    def land(self):
+        self.custom_activity_pub.publish(String("LAND"))
+
+
+    # hover at current position
+    def hover(self):
+        self.custom_activity_pub.publish(String("HOVER"))
+
+
+    # return to home position with defined height
+    def return_home(self, height):
+        self.position_target_pub.publish(self.set_pose(0, 0, height, False))
+
+
+    def set_pose(self, x=0, y=0, z=2, BODY_OFFSET_ENU = True):
+        pose = PoseStamped()
+        pose.header.stamp = rospy.Time.now()
+
+        # ROS uses ENU internally, so we will stick to this convention
+
+        if BODY_OFFSET_ENU:
+            pose.header.frame_id = 'base_link'
+
+        else:
+            pose.header.frame_id = 'map'
+
+        pose.pose.position.x = x
+        pose.pose.position.y = y
+        pose.pose.position.z = z
+
+        return pose
+
+
+if __name__ == "__main__":
+
+    con = Commander()
+    time.sleep(2)
+    con.move(5, 0, 0)
+    time.sleep(2)
+    con.turn(90)
+    time.sleep(2)
+    con.land()
+
+

demo/tutorial_6/6_object_tracking/config.py

@@ -0,0 +1,14 @@
+K = [
+        [376., 0.,   376.],
+        [0.,   376., 240.],
+        [0.,   0.,   1.]
+    ]
+
+resolution = (752, 480)
+
+left_topic='/gi/simulation/left/image_raw'
+right_topic='/gi/simulation/right/image_raw'
+
+object_position_topic='/track_rect_pub'
+
+init_rect_img_topic = '/gi/simulation/left/image_raw'

demo/tutorial_6/6_object_tracking/init_drone.py

@@ -0,0 +1,13 @@
+from commander import Commander
+import time
+import rospy
+
+rospy.init_node('init_drone')
+
+con = Commander()
+
+time.sleep(0.5)
+
+con.move(-5,0,0)
+
+time.sleep(0.5)