changed so that the drone lands in the middle

CMakeLists.txt

@@ -16,8 +16,6 @@ find_package(catkin REQUIRED COMPONENTS
   rospy
   std_msgs
   geometry_msgs
-  apriltags2_ros
-
 )
 
 ## System dependencies are found with CMake's conventions
@@ -82,7 +80,6 @@ generate_messages(
   actionlib_msgs
   std_msgs
   geometry_msgs
-  apriltags2_ros
  )
 
 ################################################

src/scripts/color_detection.py

@@ -137,10 +137,34 @@ class image_feature:
         # self.image_pub.publish(msg)
         #
         # #self.subscriber.unregister()
-    def find_relative_pose(self, x,y,w,h):
+    def find_relative_pose(self, x, y, w, h):
+        # 1*    2*
+        # 4*    3*
         quad_3d = np.float32([[-0.25, -0.25, 0], [0.25, -0.25, 0], [0.25, 0.25, 0], [-0.25, 0.25, 0]])
-        #nere vanstra hornet
+        # 1*    4*
+        # 2*    3*
         quad = np.float32([[x - w / 2, y - h / 2], [x - w / 2, y + h / 2], [x + w / 2, y + h / 2], [x + w / 2, y - h / 2]])
+        # 1*    4*
+        # 2*    3*
+        #quad_3d = np.float32([[-0.25, -0.25, 0], [-0.25, 0.25, 0], [0.25, 0.25, 0], [0.25, -0.25, 0]])
+        # 1*    4*
+        # 2*    3*
+        #quad = np.float32([[x - w / 2, y - h / 2], [x - w / 2, y + h / 2], [x + w / 2, y + h / 2], [x + w / 2, y - h / 2]])
+        # 3*    2*
+        # 4*    1*
+        #quad_3d = np.float32([[0.25/2, 0.25/2, 0], [0.25/2, -0.25/2, 0], [-0.25/2, -0.25/2, 0], [-0.25/2, 0.25/2, 0]])
+        # 3*    4*
+        # 2*    1*
+        #quad = np.float32([[x + w / 2, y + h / 2], [x - w / 2, y + h / 2], [x - w / 2, y - h / 2], [x + w / 2, y - h / 2]])
+        # 4*    3*
+        # 1*    2*
+        #quad_3d = np.float32([[-0.25, 0.25, 0], [0.25, 0.25, 0], [0.25, -0.25, 0], [-0.25, -0.25, 0]])
+        # 2*    3*
+        # 1*    4*
+        #quad = np.float32([[-w / 2, h / 2], [-w / 2, -h / 2], [-w / 2, h / 2], [w / 2, h / 2]])
+        # 4*    3*
+        # 1*    2*
+        #quad = np.float32([[x-w / 2, y+h / 2], [x+w / 2, y+h / 2], [x+w / 2, y-h / 2], [x-w / 2, y-h / 2]])
         #uppe vanstra hornet
         #quad=np.float32([[x-w/2, y+h/2], [x+w/2, y+h/2], [x+w/2, y-h/2], [x-w/2, y-h/2]])
         # uppe hogra hornet
@@ -153,12 +177,24 @@ class image_feature:
         dist_coef = np.zeros(4)
         _ret, rvec, tvec = cv2.solvePnP(quad_3d, quad, K, dist_coef)
         rmat = cv2.Rodrigues(rvec)[0]
-        cameraRotVector = cv2.Rodrigues(np.matrix(rmat).T)
         cameraTranslatevector = -np.matrix(rmat).T * np.matrix(tvec)
-        self.cam_pose.x = cameraTranslatevector.item((0,0))
-        self.cam_pose.y = cameraTranslatevector.item((1,0))
-        self.cam_pose.z = cameraTranslatevector.item((2,0))
+        T0 = np.zeros((4, 4))
+        T0[:3, :3] = rmat
+        T0[:4, 3] = [0, 0, 0, 1]
+        T0[:3, 3] = np.transpose(cameraTranslatevector)
+
+        p0 = np.array([-0.25/2, -0.25/2, 0, 1])
+        z0 = np.dot(T0, p0)
+        self.cam_pose.x = z0.item(0)
+        self.cam_pose.y = z0.item(1)
+        self.cam_pose.z = z0.item(2)
         self.cam_pose_pub.publish(self.cam_pose)
+        #cameraRotVector = cv2.Rodrigues(np.matrix(rmat).T)
+        #cameraTranslatevector = -np.matrix(rmat).T * np.matrix(tvec)
+        #self.cam_pose.x = cameraTranslatevector.item((0,0))
+        #self.cam_pose.y = cameraTranslatevector.item((1,0))
+        #self.cam_pose.z = cameraTranslatevector.item((2,0))
+        #self.cam_pose_pub.publish(self.cam_pose)
         #camera_position = -np.matrix(rmat).T * np.matrix(tvec)
         #print(camera_position)
 

src/scripts/descend_on_object_server.py

@@ -49,7 +49,7 @@ class descend_on_object_server():
 
                 while not self.target_reached:#abs(self.object_pose.x) > 0.1 and abs(self.object_pose.y) > 0.1:
                     print(self.target_reached)
-                    rospy.sleep(2)
+                    rospy.sleep(1.5)
             elif self.detected and abs(self.object_pose.x) < 0.1 and abs(self.object_pose.y) < 0.1:
                 self.des_pose.pose.position.x = 0
                 self.des_pose.pose.position.y = 0
@@ -58,14 +58,14 @@ class descend_on_object_server():
                 self.vel_control.publish(self.des_pose)
                 while not self.target_reached:#self.local_pose.pose.position.z > self.des_pose.pose.position.z + 0.1:
                     print(self.target_reached)
-                    rospy.sleep(2)
+                    rospy.sleep(1.5)
 
         self.rate.sleep()
 
         rospy.loginfo("Hovering 1 meter above detected object")
         self.des_pose.pose.position.x = 0
         self.des_pose.pose.position.y = 0
-        self.des_pose.pose.position.z = self.local_pose.pose.position.z - 0.5
+        self.des_pose.pose.position.z = self.local_pose.pose.position.z - 0.55
         self.vel_control.publish(self.des_pose)
         rospy.sleep(1)
         self.result.position_reached.data = True

src/scripts/detect_object_server.py

@@ -9,6 +9,7 @@ class detect_object_server():
 
         #variables
         self.local_pose = PoseStamped()
+        self.detected = False
         #publishers
 
         #subscribers

src/scripts/position_control.py

@@ -45,9 +45,9 @@ class position_control():
         self.current_mode.data = 'posctr'
 
         self.vController = VelocityController()
-        self.vController.set_x_pid(2.8, 0.913921, 0.0, 0.5)
-        self.vController.set_y_pid(2.8, 0.913921, 0.0, 0.5)#2.1, 0.713921, 0.350178
-        self.vController.set_z_pid(1.3, 2.4893, 0.102084, 0.5)
+        self.vController.set_x_pid(2.8, 0.913921, 0.0, 0.15)
+        self.vController.set_y_pid(2.8, 0.913921, 0.0, 0.15)#2.1, 0.713921, 0.350178
+        self.vController.set_z_pid(1.3, 2.4893, 0.102084, 0.1)
 
         print 'Init done'
         while not rospy.is_shutdown():