Skip to content
Snippets Groups Projects
Commit 6bed9432 authored by Pateman16's avatar Pateman16
Browse files

changed so that the drone lands in the middle

parent 72184ad5
Branches master
No related tags found
No related merge requests found
......@@ -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
)
################################################
......
......@@ -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)
......
......@@ -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
......
......@@ -9,6 +9,7 @@ class detect_object_server():
#variables
self.local_pose = PoseStamped()
self.detected = False
#publishers
#subscribers
......
......@@ -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():
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment