def do_action(self):
print("Descending")
rospy.Subscriber('/mavros/local_position/pose', PoseStamped,
self._local_pose_callback)
rospy.Subscriber('/color_detection/cam_point', Point,
self.get_cam_pos_callback)
if self.descend_on_object_client is None:
self.descend_on_object_client = actionlib.SimpleActionClient(
'descend_on_object', descend_on_objectAction)
self.descend_on_object_client.wait_for_server()
rospy.loginfo("Descending server started")
descend_on_object_goal = descend_on_objectGoal()
descend_on_objectGoall = 2.0
self.descend_on_object_client.send_goal(descend_on_object_goal)
self.descend_on_object_client.wait_for_result()
if self.descend_on_object_client.get_result().position_reached.data:
print("Descending done")
# mv_state.arm(False)
else:
print("Descending didn't succeed")
).detected_position.pose.position.y
goto_position_goal.destination.pose.position.z = detect_object_client.get_result(
).detected_position.pose.position.z
goto_position_client.send_goal(goto_position_goal)
goto_position_client.wait_for_result()
rospy.loginfo("Is at position (x,y,z)=({}, {}, {})".format(
detect_object_client.get_result().detected_position.pose.position.x,
detect_object_client.get_result().detected_position.pose.position.y,
detect_object_client.get_result().detected_position.pose.position.z))
rospy.loginfo("Descending on object")
descend_on_object_client = actionlib.SimpleActionClient(
'descend_on_object', descend_on_objectAction)
descend_on_object_client.wait_for_server()
rospy.loginfo("Descending server started")
descend_on_object_goal = descend_on_objectGoal()
descend_on_objectGoal = 2.0
descend_on_object_client.send_goal(descend_on_object_goal)
descend_on_object_client.wait_for_result()
if descend_on_object_client.get_result().position_reached.data:
print("landing")
mv_state.arm(False)
else:
rospy.loginfo("Couldnt land exiting")
time.sleep(3)
# Grip object
short_grippers_client = actionlib.SimpleActionClient(
'short_grippers', short_grippersAction)
short_grippers_client.wait_for_server()