def detect_object(cls):
print("Detect object!...")
detect_object_goal = detect_objectGoal()
cls.detect_object_client.send_goal(detect_object_goal)
the_result = cls.detect_object_client.wait_for_result()
print("obj restult: " + str(the_result))
return the_result
def execute_cb(self, goal):
goto_position_goal = goto_positionGoal()
self.goto_position_client.wait_for_server()
self.detect_object_client.wait_for_server()
goto_position_goal = goto_positionGoal()
self.destination = goal.destination
self.calculate_waypoints()
idx= 0
while(self.detectedPos.pose.position.x == float("inf")):
goto_position_goal.destination.pose.position.x = self.waypoints.item((idx, 0))
goto_position_goal.destination.pose.position.y = self.waypoints.item((idx, 1))
goto_position_goal.destination.pose.position.z = self.waypoints.item((idx, 2))
self.goto_position_client.send_goal(goto_position_goal)
self.goto_position_client.wait_for_result()
idx = idx + 1
if idx == self.waypoints.shape[0]:
idx = 0
detect_object_goal = detect_objectGoal()
self.detect_object_client.send_goal(detect_object_goal)
self.detect_object_client.wait_for_result()
self.detectedPos = self.detect_object_client.get_result().detected_position
time.sleep(0.1)
if self.detectedPos.pose.position.x != float("inf"):
self.result.detected_object_pos = self.detectedPos
self.result.detected_object_pos.pose.position.z = self.searchHeight
self.action_server.set_succeeded(self.result)
class CPSVO2018:
detected = False # If the dead drone is detected by the Camera
mv_state = None
local_pose = PoseStamped() # Local position of the drone
# Things to consider deleting
goto_position_goal = goto_positionGoal()
detect_object_goal = detect_objectGoal()
goto_position_client = actionlib.SimpleActionClient(
'goto_position', goto_positionAction)
def __init__(self):
# Initialize variables and start some servers
rospy.init_node('action_controller') # Create a Node
rospy.loginfo('Setting offboard')
Flying.start_position_server() # Starting position server.
print("Init long grippers") # This will start the Longgrippersserver
LongGrippers.start_grippers()
print("Init done")
# End of Initializing
rospy.loginfo("Taking off")
d = Drone()
d.start() # This will perform the TakeOff action.
while not rospy.is_shutdown():
d.do_next_state()
else:
rospy.loginfo("Error with moving legs to pickup position")
# /Close legs
goto_position_goal.destination.pose.position.x = -105
goto_position_goal.destination.pose.position.y = 0
goto_position_goal.destination.pose.position.z = 3
goto_position_client.send_goal(goto_position_goal)
goto_position_client.wait_for_result()
rospy.loginfo("Arrived at search position.")
rospy.loginfo("Searching...")
detect_object_client = actionlib.SimpleActionClient(
'detect_object', detect_objectAction)
detect_object_client.wait_for_server()
detect_object_goal = detect_objectGoal()
detect_object_client.send_goal(detect_object_goal)
detect_object_client.wait_for_result()
print(detect_object_client.get_result())
rospy.loginfo("Going to detected position")
goto_position_goal.destination.pose.position.x = detect_object_client.get_result(
).detected_position.pose.position.x
goto_position_goal.destination.pose.position.y = detect_object_client.get_result(
).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,