-
pateman16 authored0e474b24
descend_on_object_server.py 6.38 KiB
#!/usr/bin/env python
import rospy
import actionlib
import autonomous_offboard.msg
import tf
import math
from geometry_msgs.msg import PoseStamped, Point
from std_msgs.msg import Bool, String, Float32
import time
class descend_on_object_server():
def __init__(self):
# variables
self.local_pose = PoseStamped()
self.des_pose = PoseStamped()
quaternion = tf.transformations.quaternion_from_euler(0, 0, math.radians(90))
des_pose.pose.orientation.x = quaternion[0]
des_pose.pose.orientation.y = quaternion[1]
des_pose.pose.orientation.z = quaternion[2]
des_pose.pose.orientation.w = quaternion[3]
self.object_pose = Point()
self.object_pose_center = Point()
# publishers
self.mode_control = rospy.Publisher('/position_control/set_mode', String, queue_size=10)
self.vel_control = rospy.Publisher('/position_control/set_velocity', PoseStamped, queue_size=10)
self.set_xy_vel = rospy.Publisher('/position_control/set_xy_vel', Float32, queue_size=10)
self.set_z_vel = rospy.Publisher('/position_control/set_z_vel', Float32, queue_size=10)
# subscribers
rospy.Subscriber('/tensorflow_detection/cam_point', Point, self.get_cam_pos_callback)
rospy.Subscriber('/tensorflow_detection/cam_point_center', Point, self.get_cam_pos_center_callback)
#rospy.Subscriber('/mavros/local_position/pose', PoseStamped, self._local_pose_callback)
rospy.Subscriber('/position_control/distance', Bool, self.distance_reached_cb)
rospy.Subscriber('/position_control/Real_pose', PoseStamped, self._real_pose_callback)
self.rate = rospy.Rate(20)
self.result = autonomous_offboard.msg.descend_on_objectResult()
self.action_server = actionlib.SimpleActionServer('descend_on_object',
autonomous_offboard.msg.descend_on_objectAction,
execute_cb=self.execute_cb,
auto_start=False)
self.last_object_pose = Point()
self.last_object_pose_center = Point()
self.action_server.start()
def execute_cb(self, goal):
xyVel = Float32()
xyVel.data = 0.2
self.set_xy_vel.publish(xyVel)
rospy.loginfo("Starting to descend")
self.mode_control.publish('velctr')
rospy.sleep(0.1)
while self.local_pose.pose.position.z > 2.0:
self.rate.sleep()
print("x = ", self.local_pose.pose.position.x)
print("y = ", self.local_pose.pose.position.y)
print("z = ", self.local_pose.pose.position.z)
rospy.sleep(0.2)
if self.detected and (abs(self.object_pose.x) > 0.2 or abs(self.object_pose.y) > 0.2):
self.last_object_pose = self.object_pose
self.des_pose.pose.position.x = self.object_pose.x
self.des_pose.pose.position.y = self.object_pose.y
self.des_pose.pose.position.z = self.local_pose.pose.position.z
self.vel_control.publish(self.des_pose)
rospy.loginfo("Centering...")
while not self.target_reached:
rospy.sleep(2)
elif self.detected and abs(self.object_pose.x) < 0.2 and abs(self.object_pose.y) < 0.2:
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.9
rospy.loginfo("Descending...")
self.vel_control.publish(self.des_pose)
while not self.target_reached:
rospy.sleep(2)
while self.local_pose.pose.position.z < 2.0 and self.local_pose.pose.position.z > 0.5:
self.rate.sleep()
print("x = ", self.local_pose.pose.position.x)
print("y = ", self.local_pose.pose.position.y)
print("z = ", self.local_pose.pose.position.z)
rospy.sleep(0.2)
if self.detected and (abs(self.object_pose_center.x) > 0.05 or abs(self.object_pose_center.y) > 0.05):
self.last_object_pose_center = self.object_pose_center
self.des_pose.pose.position.x = self.object_pose_center.x
self.des_pose.pose.position.y = self.object_pose_center.y
self.vel_control.publish(self.des_pose)
rospy.loginfo("Centering...")
while not self.target_reached:
rospy.sleep(2)
elif self.detected and abs(self.object_pose_center.x) < 0.05 and abs(self.object_pose_center.y) < 0.05:
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.1
rospy.loginfo("Descending...")
self.vel_control.publish(self.des_pose)
while not self.target_reached:
rospy.sleep(2)
print("Landing")
self.des_pose.pose.position.x = 0
self.des_pose.pose.position.y = 0
self.des_pose.pose.position.z = -0.15
self.vel_control.publish(self.des_pose)
time.sleep(0.1)
while not self.target_reached:
time.sleep(2)
print("x = ", self.local_pose.pose.position.x)
print("y = ", self.local_pose.pose.position.y)
print("z = ", self.local_pose.pose.position.z)
self.rate.sleep()
self.result.position_reached.data = True
self.action_server.set_succeeded(self.result)
def get_cam_pos_callback(self, data):
if data.x != float("inf"):
self.detected = True
self.object_pose = data
else:
self.detected = False
def get_cam_pos_center_callback(self, data):
if data.x != float("inf"):
self.detected = True
self.object_pose_center = data
else:
self.detected = False
# def _local_pose_callback(self, data):
# self.local_pose = data
def distance_reached_cb(self, data):
self.target_reached = data.data
def _real_pose_callback(self, data):
self.local_pose = data
if __name__ == '__main__':
try:
rospy.init_node('descend_on_object_server')
descend_on_object_server()
except rospy.ROSInterruptException:
pass