From 6bed9432ceb8067688ab5aa9c36905640abafd42 Mon Sep 17 00:00:00 2001
From: Pateman16 <patrickkarlsson16@gmail.com>
Date: Thu, 22 Feb 2018 13:08:41 +0100
Subject: [PATCH] changed so that the drone lands in the middle
---
CMakeLists.txt | 3 --
src/scripts/color_detection.py | 48 +++++++++++++++++++++----
src/scripts/descend_on_object_server.py | 6 ++--
src/scripts/detect_object_server.py | 1 +
src/scripts/position_control.py | 6 ++--
5 files changed, 49 insertions(+), 15 deletions(-)
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 0eb5293..983f8eb 100644
--- a/CMakeLists.txt
+++ b/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
)
################################################
diff --git a/src/scripts/color_detection.py b/src/scripts/color_detection.py
index 292f959..e15bd8a 100755
--- a/src/scripts/color_detection.py
+++ b/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)
diff --git a/src/scripts/descend_on_object_server.py b/src/scripts/descend_on_object_server.py
index 62c8764..e077869 100755
--- a/src/scripts/descend_on_object_server.py
+++ b/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
diff --git a/src/scripts/detect_object_server.py b/src/scripts/detect_object_server.py
index b6359a9..9cb8e7f 100755
--- a/src/scripts/detect_object_server.py
+++ b/src/scripts/detect_object_server.py
@@ -9,6 +9,7 @@ class detect_object_server():
#variables
self.local_pose = PoseStamped()
+ self.detected = False
#publishers
#subscribers
diff --git a/src/scripts/position_control.py b/src/scripts/position_control.py
index 1c17445..406619c 100755
--- a/src/scripts/position_control.py
+++ b/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():
--
GitLab