From 780c3b88c188812d38ae4f98b4121ce5083cc996 Mon Sep 17 00:00:00 2001 From: Andrew Saba Date: Tue, 10 Jul 2018 01:39:09 -0400 Subject: [PATCH 01/18] Made abstract remain in arena constraints; added sleep for debugging --- scripts/takeoff_translate_land.py | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/scripts/takeoff_translate_land.py b/scripts/takeoff_translate_land.py index bc40880..80d5b97 100755 --- a/scripts/takeoff_translate_land.py +++ b/scripts/takeoff_translate_land.py @@ -37,30 +37,38 @@ def takeoff_land(): rospy.sleep(2.0) # Fly around in square all diagonals when possible. - goal = QuadMoveGoal(movement_type="xyztranslate", x_position=4.0, y_position=4.0, z_position=3.0) + goal = QuadMoveGoal(movement_type="xyztranslate", x_position=4.0, y_position=4.0, z_position=2.0) client.send_goal(goal) client.wait_for_result() if rospy.is_shutdown(): return rospy.logwarn("Waypoint 1 success: {}".format(client.get_result())) - goal = QuadMoveGoal(movement_type="xyztranslate", x_position=-4.0, y_position=-4.0, z_position=8.0) + rospy.sleep(1.0) + + goal = QuadMoveGoal(movement_type="xyztranslate", x_position=-4.0, y_position=-4.0, z_position=1.0) client.send_goal(goal) client.wait_for_result() if rospy.is_shutdown(): return rospy.logwarn("Waypoint 2 success: {}".format(client.get_result())) + rospy.sleep(1.0) + goal = QuadMoveGoal(movement_type="xyztranslate", x_position=4.0, y_position=-4.0, z_position=1.0) client.send_goal(goal) client.wait_for_result() if rospy.is_shutdown(): return rospy.logwarn("Waypoint 3 success: {}".format(client.get_result())) - goal = QuadMoveGoal(movement_type="xyztranslate", x_position=-4.0, y_position=4.0, z_position=3.0) + rospy.sleep(1.0) + + goal = QuadMoveGoal(movement_type="xyztranslate", x_position=-4.0, y_position=4.0, z_position=2.0) client.send_goal(goal) client.wait_for_result() if rospy.is_shutdown(): return rospy.logwarn("Waypoint 4 success: {}".format(client.get_result())) + rospy.sleep(1.0) + # Test land goal = QuadMoveGoal(movement_type="land") # Sends the goal to the action server. From c3df0de3fbb2299c7bbb72bf9394b67eb09d728f Mon Sep 17 00:00:00 2001 From: Andrew Saba Date: Wed, 11 Jul 2018 23:53:34 -0400 Subject: [PATCH 02/18] reduce what test abstract does --- scripts/takeoff_translate_land.py | 26 ++++---------------------- 1 file changed, 4 insertions(+), 22 deletions(-) diff --git a/scripts/takeoff_translate_land.py b/scripts/takeoff_translate_land.py index 80d5b97..41fbe75 100755 --- a/scripts/takeoff_translate_land.py +++ b/scripts/takeoff_translate_land.py @@ -37,37 +37,19 @@ def takeoff_land(): rospy.sleep(2.0) # Fly around in square all diagonals when possible. - goal = QuadMoveGoal(movement_type="xyztranslate", x_position=4.0, y_position=4.0, z_position=2.0) + goal = QuadMoveGoal(movement_type="xyztranslate", x_position=1.0, y_position=1.0, z_position=0.8) client.send_goal(goal) client.wait_for_result() if rospy.is_shutdown(): return rospy.logwarn("Waypoint 1 success: {}".format(client.get_result())) - rospy.sleep(1.0) - goal = QuadMoveGoal(movement_type="xyztranslate", x_position=-4.0, y_position=-4.0, z_position=1.0) - client.send_goal(goal) - client.wait_for_result() - if rospy.is_shutdown(): return - rospy.logwarn("Waypoint 2 success: {}".format(client.get_result())) - - rospy.sleep(1.0) - - goal = QuadMoveGoal(movement_type="xyztranslate", x_position=4.0, y_position=-4.0, z_position=1.0) - client.send_goal(goal) - client.wait_for_result() - if rospy.is_shutdown(): return - rospy.logwarn("Waypoint 3 success: {}".format(client.get_result())) - - rospy.sleep(1.0) - - goal = QuadMoveGoal(movement_type="xyztranslate", x_position=-4.0, y_position=4.0, z_position=2.0) + # Fly around in square all diagonals when possible. + goal = QuadMoveGoal(movement_type="xyztranslate", x_position=0.0, y_position=0.0, z_position=0.8) client.send_goal(goal) client.wait_for_result() if rospy.is_shutdown(): return - rospy.logwarn("Waypoint 4 success: {}".format(client.get_result())) - - rospy.sleep(1.0) + rospy.logwarn("Waypoint 1 success: {}".format(client.get_result())) # Test land goal = QuadMoveGoal(movement_type="land") From 08f67681a344636f6f97adb89dbcd7d12e6b0b8e Mon Sep 17 00:00:00 2001 From: Andrew Saba Date: Thu, 12 Jul 2018 01:00:58 -0400 Subject: [PATCH 03/18] fixes --- scripts/takeoff_translate_land.py | 1 - 1 file changed, 1 deletion(-) diff --git a/scripts/takeoff_translate_land.py b/scripts/takeoff_translate_land.py index 41fbe75..c5261bc 100755 --- a/scripts/takeoff_translate_land.py +++ b/scripts/takeoff_translate_land.py @@ -43,7 +43,6 @@ def takeoff_land(): if rospy.is_shutdown(): return rospy.logwarn("Waypoint 1 success: {}".format(client.get_result())) - # Fly around in square all diagonals when possible. goal = QuadMoveGoal(movement_type="xyztranslate", x_position=0.0, y_position=0.0, z_position=0.8) client.send_goal(goal) From 7771d88bc2c7914ed0b2fd2c655c3ccf0249acbf Mon Sep 17 00:00:00 2001 From: Andrew Saba Date: Thu, 12 Jul 2018 19:37:58 -0400 Subject: [PATCH 04/18] testing --- scripts/takeoff_translate_land.py | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/scripts/takeoff_translate_land.py b/scripts/takeoff_translate_land.py index c5261bc..cb50633 100755 --- a/scripts/takeoff_translate_land.py +++ b/scripts/takeoff_translate_land.py @@ -37,7 +37,19 @@ def takeoff_land(): rospy.sleep(2.0) # Fly around in square all diagonals when possible. - goal = QuadMoveGoal(movement_type="xyztranslate", x_position=1.0, y_position=1.0, z_position=0.8) + goal = QuadMoveGoal(movement_type="xyztranslate", x_position=2.0, y_position=0, z_position=0.75) + client.send_goal(goal) + client.wait_for_result() + if rospy.is_shutdown(): return + rospy.logwarn("Waypoint 1 success: {}".format(client.get_result())) + + goal = QuadMoveGoal(movement_type="xyztranslate", x_position=2.0, y_position=3.0, z_position=0.75) + client.send_goal(goal) + client.wait_for_result() + if rospy.is_shutdown(): return + rospy.logwarn("Waypoint 1 success: {}".format(client.get_result())) + + goal = QuadMoveGoal(movement_type="xyztranslate", x_position=2.0, y_position=0.0, z_position=0.75) client.send_goal(goal) client.wait_for_result() if rospy.is_shutdown(): return From d625bb5fb02114e071969a35799f84f53cde18fe Mon Sep 17 00:00:00 2001 From: Andrew Saba Date: Sun, 22 Jul 2018 18:23:28 -0400 Subject: [PATCH 05/18] test abstract --- scripts/takeoff_translate_land.py | 16 ++++---- scripts/test_obstacle_avoidance.py | 66 ++++++++++++++++++++++++++++++ 2 files changed, 75 insertions(+), 7 deletions(-) create mode 100755 scripts/test_obstacle_avoidance.py diff --git a/scripts/takeoff_translate_land.py b/scripts/takeoff_translate_land.py index cb50633..8a8fa0a 100755 --- a/scripts/takeoff_translate_land.py +++ b/scripts/takeoff_translate_land.py @@ -36,31 +36,33 @@ def takeoff_land(): rospy.sleep(2.0) + Z_HEIGHT = 0.75 + # Fly around in square all diagonals when possible. - goal = QuadMoveGoal(movement_type="xyztranslate", x_position=2.0, y_position=0, z_position=0.75) + goal = QuadMoveGoal(movement_type="xyztranslate", x_position=1.5, y_position=0.0, z_position=Z_HEIGHT) client.send_goal(goal) client.wait_for_result() if rospy.is_shutdown(): return rospy.logwarn("Waypoint 1 success: {}".format(client.get_result())) - goal = QuadMoveGoal(movement_type="xyztranslate", x_position=2.0, y_position=3.0, z_position=0.75) + goal = QuadMoveGoal(movement_type="xyztranslate", x_position=0.0, y_position=1.5, z_position=Z_HEIGHT) client.send_goal(goal) client.wait_for_result() if rospy.is_shutdown(): return - rospy.logwarn("Waypoint 1 success: {}".format(client.get_result())) + rospy.logwarn("Waypoint 2 success: {}".format(client.get_result())) - goal = QuadMoveGoal(movement_type="xyztranslate", x_position=2.0, y_position=0.0, z_position=0.75) + goal = QuadMoveGoal(movement_type="xyztranslate", x_position=-1.5, y_position=0.0, z_position=Z_HEIGHT) client.send_goal(goal) client.wait_for_result() if rospy.is_shutdown(): return - rospy.logwarn("Waypoint 1 success: {}".format(client.get_result())) + rospy.logwarn("Waypoint 3 success: {}".format(client.get_result())) # Fly around in square all diagonals when possible. - goal = QuadMoveGoal(movement_type="xyztranslate", x_position=0.0, y_position=0.0, z_position=0.8) + goal = QuadMoveGoal(movement_type="xyztranslate", x_position=0.0, y_position=-1.5, z_position=Z_HEIGHT) client.send_goal(goal) client.wait_for_result() if rospy.is_shutdown(): return - rospy.logwarn("Waypoint 1 success: {}".format(client.get_result())) + rospy.logwarn("Waypoint 4 success: {}".format(client.get_result())) # Test land goal = QuadMoveGoal(movement_type="land") diff --git a/scripts/test_obstacle_avoidance.py b/scripts/test_obstacle_avoidance.py new file mode 100755 index 0000000..99d640f --- /dev/null +++ b/scripts/test_obstacle_avoidance.py @@ -0,0 +1,66 @@ +#! /usr/bin/env python +import sys +import rospy +from iarc7_msgs.msg import OdometryArray +import actionlib +import tf2_ros +from iarc7_motion.msg import QuadMoveGoal, QuadMoveAction +from iarc7_safety.SafetyClient import SafetyClient + +def velocity_test(): + safety_client = SafetyClient('test_obstacle_avoidance_abstract') + # Since this abstract is top level in the control chain there is no need to check + # for a safety state. We can also get away with not checking for a fatal state since + # all nodes below will shut down. + assert(safety_client.form_bond()) + if rospy.is_shutdown(): return + + # Creates the SimpleActionClient, passing the type of the action + # (QuadMoveAction) to the constructor. (Look in the action folder) + client = actionlib.SimpleActionClient("motion_planner_server", QuadMoveAction) + + # Waits until the action server has started up and started + # listening for goals. + client.wait_for_server() + if rospy.is_shutdown(): return + + X_VELOCITY = 0.5 + Y_VELOCITY = 0.5 + TRANSLATE_DELAY = 7.0 + + # Test takeoff + goal = QuadMoveGoal(movement_type="takeoff") + # Sends the goal to the action server. + client.send_goal(goal) + # Waits for the server to finish performing the action. + client.wait_for_result() + if rospy.is_shutdown(): return + rospy.logwarn("Takeoff success: {}".format(client.get_result())) + + goal = QuadMoveGoal(movement_type="velocity_test", x_velocity=X_VELOCITY, y_velocity=Y_VELOCITY, z_position=0.8) + # Sends the goal to the action server. + client.send_goal(goal) + rospy.sleep(TRANSLATE_DELAY ) + client.cancel_goal() + rospy.logwarn("Translation 1 canceled") + + goal = QuadMoveGoal(movement_type="velocity_test", x_velocity=-X_VELOCITY, y_velocity=-Y_VELOCITY, z_position=0.8) + # Sends the goal to the action server. + client.send_goal(goal) + rospy.sleep(TRANSLATE_DELAY ) + client.cancel_goal() + rospy.logwarn("Returned Home") + + # Test land + goal = QuadMoveGoal(movement_type="land") + # Sends the goal to the action server. + client.send_goal(goal) + # Waits for the server to finish performing the action. + client.wait_for_result() + if rospy.is_shutdown(): return + rospy.logwarn("Land success: {}".format(client.get_result())) + +if __name__ == '__main__': + rospy.init_node('test_obstacle_avoidance_abstract') + velocity_test() + rospy.spin() From a034dea0e5748e5d5131729e063d463f63792bfe Mon Sep 17 00:00:00 2001 From: Andrew Saba Date: Sun, 22 Jul 2018 18:49:03 -0400 Subject: [PATCH 06/18] go 4ever --- scripts/takeoff_translate_land.py | 47 ++++++++++++++++--------------- 1 file changed, 24 insertions(+), 23 deletions(-) diff --git a/scripts/takeoff_translate_land.py b/scripts/takeoff_translate_land.py index 8a8fa0a..9501930 100755 --- a/scripts/takeoff_translate_land.py +++ b/scripts/takeoff_translate_land.py @@ -36,33 +36,34 @@ def takeoff_land(): rospy.sleep(2.0) - Z_HEIGHT = 0.75 + Z_HEIGHT = 1.5 - # Fly around in square all diagonals when possible. - goal = QuadMoveGoal(movement_type="xyztranslate", x_position=1.5, y_position=0.0, z_position=Z_HEIGHT) - client.send_goal(goal) - client.wait_for_result() - if rospy.is_shutdown(): return - rospy.logwarn("Waypoint 1 success: {}".format(client.get_result())) + for i in range(100): + # Fly around in square all diagonals when possible. + goal = QuadMoveGoal(movement_type="xyztranslate", x_position=1.5, y_position=0.0, z_position=Z_HEIGHT) + client.send_goal(goal) + client.wait_for_result() + if rospy.is_shutdown(): return + rospy.logwarn("Waypoint 1 success: {}".format(client.get_result())) - goal = QuadMoveGoal(movement_type="xyztranslate", x_position=0.0, y_position=1.5, z_position=Z_HEIGHT) - client.send_goal(goal) - client.wait_for_result() - if rospy.is_shutdown(): return - rospy.logwarn("Waypoint 2 success: {}".format(client.get_result())) + goal = QuadMoveGoal(movement_type="xyztranslate", x_position=0.0, y_position=1.5, z_position=Z_HEIGHT) + client.send_goal(goal) + client.wait_for_result() + if rospy.is_shutdown(): return + rospy.logwarn("Waypoint 2 success: {}".format(client.get_result())) - goal = QuadMoveGoal(movement_type="xyztranslate", x_position=-1.5, y_position=0.0, z_position=Z_HEIGHT) - client.send_goal(goal) - client.wait_for_result() - if rospy.is_shutdown(): return - rospy.logwarn("Waypoint 3 success: {}".format(client.get_result())) + goal = QuadMoveGoal(movement_type="xyztranslate", x_position=-1.5, y_position=0.0, z_position=Z_HEIGHT) + client.send_goal(goal) + client.wait_for_result() + if rospy.is_shutdown(): return + rospy.logwarn("Waypoint 3 success: {}".format(client.get_result())) - # Fly around in square all diagonals when possible. - goal = QuadMoveGoal(movement_type="xyztranslate", x_position=0.0, y_position=-1.5, z_position=Z_HEIGHT) - client.send_goal(goal) - client.wait_for_result() - if rospy.is_shutdown(): return - rospy.logwarn("Waypoint 4 success: {}".format(client.get_result())) + # Fly around in square all diagonals when possible. + goal = QuadMoveGoal(movement_type="xyztranslate", x_position=0.0, y_position=-1.5, z_position=Z_HEIGHT) + client.send_goal(goal) + client.wait_for_result() + if rospy.is_shutdown(): return + rospy.logwarn("Waypoint 4 success: {}".format(client.get_result())) # Test land goal = QuadMoveGoal(movement_type="land") From def7d6a535ab3455c8eb5a1db9d843c0ec38f267 Mon Sep 17 00:00:00 2001 From: Andrew Saba Date: Mon, 23 Jul 2018 20:00:20 -0400 Subject: [PATCH 07/18] fixes --- scripts/takeoff_translate_land.py | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/scripts/takeoff_translate_land.py b/scripts/takeoff_translate_land.py index 9501930..285dabc 100755 --- a/scripts/takeoff_translate_land.py +++ b/scripts/takeoff_translate_land.py @@ -37,29 +37,30 @@ def takeoff_land(): rospy.sleep(2.0) Z_HEIGHT = 1.5 + TRANSLATION_DISTANCE = 1.5 for i in range(100): # Fly around in square all diagonals when possible. - goal = QuadMoveGoal(movement_type="xyztranslate", x_position=1.5, y_position=0.0, z_position=Z_HEIGHT) + goal = QuadMoveGoal(movement_type="xyztranslate", x_position=TRANSLATION_DISTANCE, y_position=0.0, z_position=Z_HEIGHT) client.send_goal(goal) client.wait_for_result() if rospy.is_shutdown(): return rospy.logwarn("Waypoint 1 success: {}".format(client.get_result())) - goal = QuadMoveGoal(movement_type="xyztranslate", x_position=0.0, y_position=1.5, z_position=Z_HEIGHT) + goal = QuadMoveGoal(movement_type="xyztranslate", x_position=0.0, y_position=TRANSLATION_DISTANCE, z_position=Z_HEIGHT) client.send_goal(goal) client.wait_for_result() if rospy.is_shutdown(): return rospy.logwarn("Waypoint 2 success: {}".format(client.get_result())) - goal = QuadMoveGoal(movement_type="xyztranslate", x_position=-1.5, y_position=0.0, z_position=Z_HEIGHT) + goal = QuadMoveGoal(movement_type="xyztranslate", x_position=-TRANSLATION_DISTANCE, y_position=0.0, z_position=Z_HEIGHT) client.send_goal(goal) client.wait_for_result() if rospy.is_shutdown(): return rospy.logwarn("Waypoint 3 success: {}".format(client.get_result())) # Fly around in square all diagonals when possible. - goal = QuadMoveGoal(movement_type="xyztranslate", x_position=0.0, y_position=-1.5, z_position=Z_HEIGHT) + goal = QuadMoveGoal(movement_type="xyztranslate", x_position=0.0, y_position=-TRANSLATION_DISTANCE, z_position=Z_HEIGHT) client.send_goal(goal) client.wait_for_result() if rospy.is_shutdown(): return From 9dbd440182936b8d2397b2b8c57bced70e0fcee4 Mon Sep 17 00:00:00 2001 From: Levi Burner Date: Wed, 25 Jul 2018 02:42:25 -0400 Subject: [PATCH 08/18] Begin mission7.py, executes basic search pattern until roomba is found --- param/arena_settings_20m.yaml | 5 ++ scripts/mission7.py | 136 ++++++++++++++++++++++++++++++++++ 2 files changed, 141 insertions(+) create mode 100644 param/arena_settings_20m.yaml create mode 100755 scripts/mission7.py diff --git a/param/arena_settings_20m.yaml b/param/arena_settings_20m.yaml new file mode 100644 index 0000000..28c77ea --- /dev/null +++ b/param/arena_settings_20m.yaml @@ -0,0 +1,5 @@ +arena_starting_position_x: -9.5 +arena_starting_position_y: 9.5 + +arena_size_x: 20.0 +arena_size_y: 20.0 diff --git a/scripts/mission7.py b/scripts/mission7.py new file mode 100755 index 0000000..85df898 --- /dev/null +++ b/scripts/mission7.py @@ -0,0 +1,136 @@ +#! /usr/bin/env python +import sys +import rospy +import math +import numpy as np + +import actionlib +from iarc7_motion.msg import QuadMoveGoal, QuadMoveAction +from iarc7_safety.SafetyClient import SafetyClient +from iarc7_msgs.msg import OdometryArray +from nav_msgs.msg import Odometry +from actionlib_msgs.msg import GoalStatus + +from iarc7_abstract.arena_position_estimator import ArenaPositionEstimator + +TRANSLATION_HEIGHT = 1.5 + + +SEARCH_POINTS = np.asarray( +( + ( 0.0, 0.0), # Center + ( 0.0, 7.5), # Mid left + (-7.5, 7.5), # Bottom left + (-7.5, -7.5), # Bottom right + ( 0.0, -7.5), # Mid right + ( 7.5, 0.0), # Top middle +)) + +def target_roomba_law(roombas): + return roombas[0] + +def construct_xyz_goal(arena_pos, height=TRANSLATION_HEIGHT): + map_pos = arena_position_estimator.arena_to_map(arena_pos) + return QuadMoveGoal(movement_type="xyztranslate", x_position=map_pos[0], y_position=map_pos[1], z_position=height) + +def roomba_distance(roomba_odom, drone_odom): + x_diff = drone_odom.pose.pose.position.x - roomba_odom.pose.pose.position.x + y_diff = drone_odom.pose.pose.position.y - roomba_odom.pose.pose.position.y + return math.sqrt(x_diff**2 + y_diff**2) + +class Mission7(object): + def __init__(self): + self.safety_client = SafetyClient('mission7') + # Since this abstract is top level in the control chain there is no need to check + # for a safety state. We can also get away with not checking for a fatal state since + # all nodes below will shut down. + assert(self.safety_client.form_bond()) + if rospy.is_shutdown(): return + + # Creates the SimpleActionClient, passing the type of the action + # (QuadMoveAction) to the constructor. (Look in the action folder) + self._client = actionlib.SimpleActionClient("motion_planner_server", QuadMoveAction) + + # Waits until the action server has started up and started + # listening for goals. + self._client.wait_for_server() + if rospy.is_shutdown(): return + + self._avail_roomba = None + self._search_state = 0 + self._roomba_sub = rospy.Subscriber('/roombas', OdometryArray, self.roomba_callback) + self._odom_sub = rospy.Subscriber('/odometry/filtered/', Odometry, self.odom_callback) + + def roomba_callback(self, data): + # Sort roombas by their distance to the drone + self._avail_roombas = sorted([(roomba_distance(r, self._odom), r) for r in data.data]) + + def odom_callback(self, data): + self._odom = data + + def arena_translate(self, arena_pos, height=TRANSLATION_HEIGHT): + map_pos = construct_xyz_goal(arena_pos, height=height) + print 'MAP POS', map_pos + self._client.send_goal(map_pos) + self._client.wait_for_result() + rospy.loginfo('XYZ Translate success: {}'.format(self._client.get_result())) + + def begin_translate(self, arena_pos, height=TRANSLATION_HEIGHT): + map_pos = construct_xyz_goal(arena_pos, height=height) + self._client.send_goal(map_pos) + + def basic_goal(self, goal): + self._client.send_goal(QuadMoveGoal(movement_type=goal)) + self._client.wait_for_result() + rospy.loginfo('{} success: {}'.format(goal, self._client.get_result())) + + def wait_for_roomba(self): + rate = rospy.Rate(30) + while True: + state = self._client.get_state() + if (state == GoalStatus.ABORTED or state == GoalStatus.SUCCEEDED): + return None + if self._avail_roombas is not None and len(self._avail_roombas) > 0: + target_roomba = target_roomba_law(self._avail_roombas) + if target_roomba is not None: + return target_roomba + rate.sleep() + + def search_for_roomba(self): + # Decide which waypoint to go to + if self._search_state != 0: + self._search_state = 1 + + # Execute search waypoints + while True: + self.begin_translate(SEARCH_POINTS[self._search_state]) + roomba = self.wait_for_roomba() + if roomba is None: + self._search_state = self._search_state + 1 if self._search_state + 1 < SEARCH_POINTS.shape[0] else 1 + else: + break + + self._search_state = 1 + return roomba + + def attempt_mission7(self): + # Takeoff + self.basic_goal('takeoff') + + roomba = self.search_for_roomba() + + rospy.logerr('GOT A ROOMBA') + rospy.logerr(roomba) + + self.basic_goal('land') + +if __name__ == '__main__': + # Initializes a rospy node so that the SimpleActionClient can + # publish and subscribe over ROS. + rospy.init_node('mission7') + arena_position_estimator = ArenaPositionEstimator() + + mission7 = Mission7() + mission7.attempt_mission7() + + rospy.spin() From 6270521d363505715cbbe5a8e23693fbe75a51b5 Mon Sep 17 00:00:00 2001 From: Levi Burner Date: Fri, 27 Jul 2018 14:22:16 -0400 Subject: [PATCH 09/18] Make track roomba to completion and make things more robust --- param/arena_settings_10m.yaml | 4 +- param/arena_settings_20m.yaml | 4 +- scripts/mission7.py | 188 ++++++++++++++++-- scripts/takeoff_translate_land.py | 2 - .../arena_position_estimator.py | 14 +- 5 files changed, 178 insertions(+), 34 deletions(-) diff --git a/param/arena_settings_10m.yaml b/param/arena_settings_10m.yaml index 05ecf9a..209da2f 100644 --- a/param/arena_settings_10m.yaml +++ b/param/arena_settings_10m.yaml @@ -1,5 +1,5 @@ arena_starting_position_x: 0.0 arena_starting_position_y: 0.0 -arena_size_x: 15.0 -arena_size_y: 15.0 +arena_size_x: 10.0 +arena_size_y: 10.0 diff --git a/param/arena_settings_20m.yaml b/param/arena_settings_20m.yaml index 28c77ea..e73b4b3 100644 --- a/param/arena_settings_20m.yaml +++ b/param/arena_settings_20m.yaml @@ -1,5 +1,5 @@ -arena_starting_position_x: -9.5 -arena_starting_position_y: 9.5 +arena_starting_position_x: -5 +arena_starting_position_y: 5 arena_size_x: 20.0 arena_size_y: 20.0 diff --git a/scripts/mission7.py b/scripts/mission7.py index 85df898..96a1864 100755 --- a/scripts/mission7.py +++ b/scripts/mission7.py @@ -5,6 +5,7 @@ import numpy as np import actionlib +from iarc7_msgs.msg import BoolStamped from iarc7_motion.msg import QuadMoveGoal, QuadMoveAction from iarc7_safety.SafetyClient import SafetyClient from iarc7_msgs.msg import OdometryArray @@ -13,8 +14,17 @@ from iarc7_abstract.arena_position_estimator import ArenaPositionEstimator +from tf.transformations import euler_from_quaternion + TRANSLATION_HEIGHT = 1.5 +MIN_GOTO_DISTANCE = 0.5 +USE_PLANNER = True + +TARGET_NUM_ROOMBAS = 2 +MAX_FLIGHT_DURATION = 1 * 60 +# Used if the planner is disabled +TRANSLATION_VELOCITY = 4.0 SEARCH_POINTS = np.asarray( ( @@ -26,8 +36,27 @@ ( 7.5, 0.0), # Top middle )) -def target_roomba_law(roombas): - return roombas[0] +def target_roomba_law(roombas, odom): + # Sort roombas by their distance to the drone + sorted_roombas = sorted([(roomba_distance(r, odom), r) for r in roombas]) + return sorted_roombas[0][1] + +def construct_velocity_goal(arena_pos, quad_odom, height=TRANSLATION_HEIGHT): + map_pos = arena_position_estimator.arena_to_map(arena_pos) + diff_x = arena_pos[0] - quad_odom.pose.pose.position.x + diff_y = arena_pos[1] - quad_odom.pose.pose.position.y + hypot = math.sqrt(diff_x**2 + diff_y**2) + u_x = diff_x / hypot + u_y = diff_y / hypot + v_x = u_x * TRANSLATION_VELOCITY + v_y = u_y * TRANSLATION_VELOCITY + time = rospy.Duration(hypot / TRANSLATION_VELOCITY) + return QuadMoveGoal(movement_type="velocity_test", + x_velocity=v_x, + y_velocity=v_y, + z_position=height, + time_velocity=time) + def construct_xyz_goal(arena_pos, height=TRANSLATION_HEIGHT): map_pos = arena_position_estimator.arena_to_map(arena_pos) @@ -38,6 +67,43 @@ def roomba_distance(roomba_odom, drone_odom): y_diff = drone_odom.pose.pose.position.y - roomba_odom.pose.pose.position.y return math.sqrt(x_diff**2 + y_diff**2) +def find_roomba_by_id(roombas, desired_id): + roomba_list = [x for x in roombas if x.child_frame_id == desired_id] + if len(roomba_list) == 0: + return None + return roomba_list[0] + +def roomba_yaw(roomba): + orientation_list = [roomba.pose.pose.orientation.x, + roomba.pose.pose.orientation.y, + roomba.pose.pose.orientation.z, + roomba.pose.pose.orientation.w] + + angles = euler_from_quaternion(orientation_list) + return angles[2] + +def construct_goto_roomba_goal(roomba): + return QuadMoveGoal(movement_type="xyztranslate", + x_position = roomba.pose.pose.position.x, + y_position = roomba.pose.pose.position.y, + z_position = TRANSLATION_HEIGHT) + +def did_task_finish(client): + return did_task_fail(client) or did_task_succeed(client) + +def did_task_fail(client, state=None): + if state is None: + state = client.get_state() + return (state == GoalStatus.ABORTED + or state == GoalStatus.REJECTED + or state == GoalStatus.PREEMPTED + or state == GoalStatus.RECALLED) + +def did_task_succeed(client, state=None): + if state is None: + state = client.get_state() + return (state == GoalStatus.SUCCEEDED) + class Mission7(object): def __init__(self): self.safety_client = SafetyClient('mission7') @@ -62,22 +128,17 @@ def __init__(self): self._odom_sub = rospy.Subscriber('/odometry/filtered/', Odometry, self.odom_callback) def roomba_callback(self, data): - # Sort roombas by their distance to the drone - self._avail_roombas = sorted([(roomba_distance(r, self._odom), r) for r in data.data]) + self._avail_roombas = data.data def odom_callback(self, data): self._odom = data - def arena_translate(self, arena_pos, height=TRANSLATION_HEIGHT): - map_pos = construct_xyz_goal(arena_pos, height=height) - print 'MAP POS', map_pos - self._client.send_goal(map_pos) - self._client.wait_for_result() - rospy.loginfo('XYZ Translate success: {}'.format(self._client.get_result())) - def begin_translate(self, arena_pos, height=TRANSLATION_HEIGHT): - map_pos = construct_xyz_goal(arena_pos, height=height) - self._client.send_goal(map_pos) + if USE_PLANNER: + map_pos = construct_xyz_goal(arena_pos, height=height) + self._client.send_goal(map_pos) + else: + self._client.send_goal(construct_velocity_goal(arena_pos, self._odom, height=height)) def basic_goal(self, goal): self._client.send_goal(QuadMoveGoal(movement_type=goal)) @@ -87,16 +148,18 @@ def basic_goal(self, goal): def wait_for_roomba(self): rate = rospy.Rate(30) while True: - state = self._client.get_state() - if (state == GoalStatus.ABORTED or state == GoalStatus.SUCCEEDED): - return None + if did_task_fail(self._client): + return (False, None) + elif did_task_succeed(self._client): + return (True, None) if self._avail_roombas is not None and len(self._avail_roombas) > 0: - target_roomba = target_roomba_law(self._avail_roombas) + target_roomba = target_roomba_law(self._avail_roombas, self._odom) if target_roomba is not None: - return target_roomba + return (True, target_roomba) rate.sleep() def search_for_roomba(self): + rospy.loginfo('Entering search') # Decide which waypoint to go to if self._search_state != 0: self._search_state = 1 @@ -104,24 +167,100 @@ def search_for_roomba(self): # Execute search waypoints while True: self.begin_translate(SEARCH_POINTS[self._search_state]) - roomba = self.wait_for_roomba() + (xyz_translate_ok, roomba) = self.wait_for_roomba() + if not xyz_translate_ok: + continue + if roomba is None: self._search_state = self._search_state + 1 if self._search_state + 1 < SEARCH_POINTS.shape[0] else 1 else: break + # Todo better initial search position self._search_state = 1 return roomba + def track_roomba_to_completion(self, roomba): + track_state = 0 + roomba_id = roomba.child_frame_id + rospy.loginfo('Entering track roomba: {}'.format(roomba_id)) + + rate = rospy.Rate(30) + while True: + roomba = find_roomba_by_id(self._avail_roombas, roomba_id) + if roomba is None: + rospy.loginfo('Roomba no longer is tracked {}'.format(roomba_id)) + return False + + d = roomba_distance(roomba, self._odom) + + # Need to decide what kind of positional action to take + if track_state == 0: + + # Regardless of the positional action necessary, make sure the last task is canceled + + if d >= MIN_GOTO_DISTANCE: + self._client.send_goal(construct_goto_roomba_goal(roomba)) + rospy.loginfo('GOTO ROOMBA') + track_state = 1 + else: + self._client.send_goal(QuadMoveGoal(movement_type="track_roomba", + frame_id=roomba_id, + x_overshoot=0.0, y_overshoot=0.0)) + rospy.loginfo('TRACKING ROOMBA') + track_state = 2 + + # Going to roomba + if track_state == 1: + state = self._client.get_state() + if did_task_succeed(self._client): + rospy.loginfo('GOTO ROOMBA SUCCESS') + track_state = 0 + if did_task_fail(self._client): + rospy.loginfo('GOTO ROOMBA FAILED') + track_state = 0 + + if d < MIN_GOTO_DISTANCE: + self._client.cancel_goal() + self._client.send_goal(QuadMoveGoal(movement_type="track_roomba", + frame_id=roomba_id, + x_overshoot=0.0, y_overshoot=0.0, time_to_track=100000.0)) + rospy.loginfo('TRACKING ROOMBA') + track_state = 2 + + if track_state == 2: + state = self._client.get_state() + if did_task_finish(self._client): + rospy.loginfo('Track roomba kicked back') + track_state = 0 + else: + roomba_heading = roomba_yaw(roomba) + + rate.sleep() + def attempt_mission7(self): # Takeoff + + flight_start_time = rospy.Time.now() + self.basic_goal('takeoff') - roomba = self.search_for_roomba() + mission7_completed = False + gotten_roombas = 0 + while not mission7_completed: + roomba = self.search_for_roomba() - rospy.logerr('GOT A ROOMBA') - rospy.logerr(roomba) + got_roomba = self.track_roomba_to_completion(roomba) + if got_roomba: + gotten_roombas += 1 + if gotten_roombas > TARGET_NUM_ROOMBAS: + break + + if rospy.Time.now() > flight_start_time + rospy.Duration(MAX_FLIGHT_DURATION): + break + + # self.goto_safe_landing_spot() self.basic_goal('land') if __name__ == '__main__': @@ -131,6 +270,11 @@ def attempt_mission7(self): arena_position_estimator = ArenaPositionEstimator() mission7 = Mission7() + + pub = rospy.Publisher('/start_roombas', BoolStamped, queue_size=100, latch=True) + msg = BoolStamped() + msg.header.stamp = rospy.Time.now() + pub.publish(msg) mission7.attempt_mission7() rospy.spin() diff --git a/scripts/takeoff_translate_land.py b/scripts/takeoff_translate_land.py index 285dabc..0f9fc76 100755 --- a/scripts/takeoff_translate_land.py +++ b/scripts/takeoff_translate_land.py @@ -34,8 +34,6 @@ def takeoff_land(): if rospy.is_shutdown(): return rospy.logwarn("Takeoff success: {}".format(client.get_result())) - rospy.sleep(2.0) - Z_HEIGHT = 1.5 TRANSLATION_DISTANCE = 1.5 diff --git a/src/iarc7_abstract/arena_position_estimator.py b/src/iarc7_abstract/arena_position_estimator.py index db1bd89..909ac1c 100644 --- a/src/iarc7_abstract/arena_position_estimator.py +++ b/src/iarc7_abstract/arena_position_estimator.py @@ -13,11 +13,12 @@ def __init__(self): size_y = rospy.get_param('~arena_size_y') # left, right, top, bottom - self.arena_line_positions = [-arena_starting_position_x + size_y/2, - arena_starting_position_x - size_y/2, - -arena_starting_position_y + size_x/2, - arena_starting_position_y - size_x/2] + self.arena_line_positions = [-arena_starting_position_y + size_y/2, + -(arena_starting_position_y + size_y/2), + -arena_starting_position_x + size_x/2, + -(arena_starting_position_x + size_x/2)] + print 'STARTING LINE POSITIONS', self.arena_line_positions self.odom = None rospy.Subscriber('/floor_detector/boundaries', Boundary, self.boundary_callback) @@ -42,8 +43,9 @@ def boundary_callback(self, data): def arena_to_map(self, arena_pos): # Arena pos (0,0) is the center of the arena - from_left_corner = (arena_pos[0] + 10.0, arena_pos[1] + 10.0) - return (self.arena_line_positions[3] + from_left_corner[0], self.arena_line_positions[0] + from_left_corner[1]) + from_left_corner = (arena_pos[0] + 10.0, -arena_pos[1] + 10.0) + # print arena_pos[0], 10.0, self.arena_line_positions[3] + return (self.arena_line_positions[3] + from_left_corner[0], self.arena_line_positions[0] - from_left_corner[1]) def distance_to_left(self): if self.odom is not None: From 1ae4ccbe81a169b354ec9386b786f8a6635879e2 Mon Sep 17 00:00:00 2001 From: Levi Burner Date: Sun, 29 Jul 2018 18:54:32 -0400 Subject: [PATCH 10/18] Jank roomba guiding and new movement/motion pattern --- param/arena_settings_20m.yaml | 4 +- scripts/mission7.py | 102 ++++++++++++++---- .../arena_position_estimator.py | 16 +-- 3 files changed, 89 insertions(+), 33 deletions(-) diff --git a/param/arena_settings_20m.yaml b/param/arena_settings_20m.yaml index e73b4b3..28c77ea 100644 --- a/param/arena_settings_20m.yaml +++ b/param/arena_settings_20m.yaml @@ -1,5 +1,5 @@ -arena_starting_position_x: -5 -arena_starting_position_y: 5 +arena_starting_position_x: -9.5 +arena_starting_position_y: 9.5 arena_size_x: 20.0 arena_size_y: 20.0 diff --git a/scripts/mission7.py b/scripts/mission7.py index 96a1864..d12965f 100755 --- a/scripts/mission7.py +++ b/scripts/mission7.py @@ -16,24 +16,28 @@ from tf.transformations import euler_from_quaternion -TRANSLATION_HEIGHT = 1.5 -MIN_GOTO_DISTANCE = 0.5 -USE_PLANNER = True +TRANSLATION_HEIGHT = 2.0 +MIN_GOTO_DISTANCE = 0.25 +USE_PLANNER = False TARGET_NUM_ROOMBAS = 2 MAX_FLIGHT_DURATION = 1 * 60 # Used if the planner is disabled -TRANSLATION_VELOCITY = 4.0 +TRANSLATION_VELOCITY = 1.0 + +# SEARCH_POINTS = np.asarray( +# ( +# (-6.0, 6.0), # In a little +# (-6.0, -6.0), # Right side +# (-6.0, 6.0) # Left side +# )) SEARCH_POINTS = np.asarray( ( - ( 0.0, 0.0), # Center - ( 0.0, 7.5), # Mid left - (-7.5, 7.5), # Bottom left - (-7.5, -7.5), # Bottom right - ( 0.0, -7.5), # Mid right - ( 7.5, 0.0), # Top middle + (-7.0, 7.0), # In a little + (-7.0, -2.0), # Right side + (-7.0, 7.0) # Left side )) def target_roomba_law(roombas, odom): @@ -41,21 +45,25 @@ def target_roomba_law(roombas, odom): sorted_roombas = sorted([(roomba_distance(r, odom), r) for r in roombas]) return sorted_roombas[0][1] -def construct_velocity_goal(arena_pos, quad_odom, height=TRANSLATION_HEIGHT): +def construct_velocity_goal(arena_pos, supposed_to_be_at, height=TRANSLATION_HEIGHT): map_pos = arena_position_estimator.arena_to_map(arena_pos) - diff_x = arena_pos[0] - quad_odom.pose.pose.position.x - diff_y = arena_pos[1] - quad_odom.pose.pose.position.y + supposed_to_be_at = arena_position_estimator.arena_to_map(supposed_to_be_at) + diff_x = map_pos[0] - supposed_to_be_at[0] + diff_y = map_pos[1] - supposed_to_be_at[1] + print arena_pos + print map_pos + print diff_x, diff_y hypot = math.sqrt(diff_x**2 + diff_y**2) u_x = diff_x / hypot u_y = diff_y / hypot v_x = u_x * TRANSLATION_VELOCITY v_y = u_y * TRANSLATION_VELOCITY - time = rospy.Duration(hypot / TRANSLATION_VELOCITY) + rospy.loginfo('Time: {}'.format(hypot / TRANSLATION_VELOCITY)) return QuadMoveGoal(movement_type="velocity_test", x_velocity=v_x, y_velocity=v_y, z_position=height, - time_velocity=time) + velocity_duration=hypot / TRANSLATION_VELOCITY) def construct_xyz_goal(arena_pos, height=TRANSLATION_HEIGHT): @@ -104,6 +112,19 @@ def did_task_succeed(client, state=None): state = client.get_state() return (state == GoalStatus.SUCCEEDED) +def guide_roomba_law(roomba): + roomba_heading = roomba_yaw(roomba) + rospy.loginfo('Roomba heading: {} degrees'.format(roomba_heading * 180.0/math.pi)) + if -5 * math.pi / 4 > roomba_heading and roomba_heading <= 1 * math.pi /4: + # Block + return 1 + elif 1 * math.pi / 4 > roomba_heading and roomba_heading <= 3 * math.pi / 4: + # Hit + return 2 + else: + # Do nothing + return 0 + class Mission7(object): def __init__(self): self.safety_client = SafetyClient('mission7') @@ -126,6 +147,8 @@ def __init__(self): self._search_state = 0 self._roomba_sub = rospy.Subscriber('/roombas', OdometryArray, self.roomba_callback) self._odom_sub = rospy.Subscriber('/odometry/filtered/', Odometry, self.odom_callback) + self._supposed_to_be_at = (rospy.get_param('~arena_starting_position_x'), + rospy.get_param('~arena_starting_position_y')) def roomba_callback(self, data): self._avail_roombas = data.data @@ -138,7 +161,8 @@ def begin_translate(self, arena_pos, height=TRANSLATION_HEIGHT): map_pos = construct_xyz_goal(arena_pos, height=height) self._client.send_goal(map_pos) else: - self._client.send_goal(construct_velocity_goal(arena_pos, self._odom, height=height)) + self._client.send_goal(construct_velocity_goal(arena_pos, self._supposed_to_be_at, height=height)) + self._supposed_to_be_at = arena_pos def basic_goal(self, goal): self._client.send_goal(QuadMoveGoal(movement_type=goal)) @@ -152,10 +176,10 @@ def wait_for_roomba(self): return (False, None) elif did_task_succeed(self._client): return (True, None) - if self._avail_roombas is not None and len(self._avail_roombas) > 0: - target_roomba = target_roomba_law(self._avail_roombas, self._odom) - if target_roomba is not None: - return (True, target_roomba) + #if self._avail_roombas is not None and len(self._avail_roombas) > 0: + # target_roomba = target_roomba_law(self._avail_roombas, self._odom) + # if target_roomba is not None: + # return (True, target_roomba) rate.sleep() def search_for_roomba(self): @@ -198,6 +222,9 @@ def track_roomba_to_completion(self, roomba): if track_state == 0: # Regardless of the positional action necessary, make sure the last task is canceled + if not did_task_finish(self._client): + rospy.loginfo('Canceling goal') + self._client.cancel_goal() if d >= MIN_GOTO_DISTANCE: self._client.send_goal(construct_goto_roomba_goal(roomba)) @@ -234,7 +261,36 @@ def track_roomba_to_completion(self, roomba): rospy.loginfo('Track roomba kicked back') track_state = 0 else: - roomba_heading = roomba_yaw(roomba) + action = guide_roomba_law(roomba) + # If block + if action == 1: + self._client.cancel_goal() + rospy.loginfo('SENDING BLOCK ROOMBA') + self._client.send_goal(QuadMoveGoal(movement_type="block_roomba", frame_id = roomba_id)) + track_state = 3 + # If hit + elif action == 2: + self._client.cancel_goal() + rospy.loginfo('SENDING HIT ROOMBA') + self._client.send_goal(QuadMoveGoal(movement_type="block_roomba", frame_id = roomba_id)) + track_state = 4 + + if track_state == 3: + if did_task_fail(self._client): + rospy.loginfo('BLOCK FAILED') + track_state = 2 + if did_task_succeed(self._client): + rospy.loginfo('BLOCK SUCCEEDED') + track_state = 2 + + if track_state == 4: + if did_task_fail(self._client): + rospy.loginfo('HIT FAILED') + track_state = 2 + if did_task_succeed(self._client): + rospy.loginfo('HIT SUCCEEDED') + track_state = 2 + rate.sleep() @@ -249,8 +305,8 @@ def attempt_mission7(self): gotten_roombas = 0 while not mission7_completed: roomba = self.search_for_roomba() - - got_roomba = self.track_roomba_to_completion(roomba) + break + #got_roomba = self.track_roomba_to_completion(roomba) if got_roomba: gotten_roombas += 1 diff --git a/src/iarc7_abstract/arena_position_estimator.py b/src/iarc7_abstract/arena_position_estimator.py index 909ac1c..f057121 100644 --- a/src/iarc7_abstract/arena_position_estimator.py +++ b/src/iarc7_abstract/arena_position_estimator.py @@ -9,14 +9,14 @@ def __init__(self): arena_starting_position_x = rospy.get_param('~arena_starting_position_x') arena_starting_position_y = rospy.get_param('~arena_starting_position_y') - size_x = rospy.get_param('~arena_size_x') - size_y = rospy.get_param('~arena_size_y') + self._size_x = rospy.get_param('~arena_size_x') + self._size_y = rospy.get_param('~arena_size_y') # left, right, top, bottom - self.arena_line_positions = [-arena_starting_position_y + size_y/2, - -(arena_starting_position_y + size_y/2), - -arena_starting_position_x + size_x/2, - -(arena_starting_position_x + size_x/2)] + self.arena_line_positions = [-arena_starting_position_y + self._size_y/2, + -(arena_starting_position_y + self._size_y/2), + -arena_starting_position_x + self._size_x/2, + -(arena_starting_position_x + self._size_x/2)] print 'STARTING LINE POSITIONS', self.arena_line_positions self.odom = None @@ -28,6 +28,7 @@ def odom_callback(self, data): self.odom = data def boundary_callback(self, data): + return if data.boundary_type == 0: self.arena_line_positions[0] = data.position self.arena_line_positions[1] = data.position - 20.0 @@ -43,8 +44,7 @@ def boundary_callback(self, data): def arena_to_map(self, arena_pos): # Arena pos (0,0) is the center of the arena - from_left_corner = (arena_pos[0] + 10.0, -arena_pos[1] + 10.0) - # print arena_pos[0], 10.0, self.arena_line_positions[3] + from_left_corner = (arena_pos[0] + self._size_x/2, - arena_pos[1] + self._size_y/2) return (self.arena_line_positions[3] + from_left_corner[0], self.arena_line_positions[0] - from_left_corner[1]) def distance_to_left(self): From cc042330f3597f6d15ef2f4af0ca21ce79db1b1b Mon Sep 17 00:00:00 2001 From: Pitt RAS IARC Team Date: Sun, 29 Jul 2018 19:00:52 -0400 Subject: [PATCH 11/18] Make velocity test use height --- scripts/velocity_test.py | 21 +++++++++++---------- 1 file changed, 11 insertions(+), 10 deletions(-) diff --git a/scripts/velocity_test.py b/scripts/velocity_test.py index 4681fb2..62b31fc 100755 --- a/scripts/velocity_test.py +++ b/scripts/velocity_test.py @@ -26,9 +26,10 @@ def velocity_test(): X_VELOCITY = 0.5 Y_VELOCITY = 0.5 - X_DELAY = 2.0 - Y_DELAY = 2.0 + X_DELAY = 5.0 + Y_DELAY = 5.0 STOP_DELAY = 2.0 + HEIGHT = 1.5 # Test takeoff goal = QuadMoveGoal(movement_type="takeoff") @@ -40,56 +41,56 @@ def velocity_test(): rospy.logwarn("Takeoff success: {}".format(client.get_result())) for i in range(0, 1): - goal = QuadMoveGoal(movement_type="velocity_test", x_velocity=X_VELOCITY, y_velocity=0.0, z_position=0.8) + goal = QuadMoveGoal(movement_type="velocity_test", x_velocity=X_VELOCITY, y_velocity=0.0, z_position=HEIGHT) # Sends the goal to the action server. client.send_goal(goal) rospy.sleep(X_DELAY) client.cancel_goal() rospy.logwarn("Translation 1 canceled") - goal = QuadMoveGoal(movement_type="velocity_test", x_velocity=0.0, y_velocity=0.0, z_position=0.8) + goal = QuadMoveGoal(movement_type="velocity_test", x_velocity=0.0, y_velocity=0.0, z_position=HEIGHT) # Sends the goal to the action server. client.send_goal(goal) rospy.sleep(STOP_DELAY) client.cancel_goal() rospy.logwarn("Stop 1 canceled") - goal = QuadMoveGoal(movement_type="velocity_test", x_velocity=0.0, y_velocity=-Y_VELOCITY, z_position=0.8) + goal = QuadMoveGoal(movement_type="velocity_test", x_velocity=0.0, y_velocity=-Y_VELOCITY, z_position=HEIGHT) # Sends the goal to the action server. client.send_goal(goal) rospy.sleep(Y_DELAY) client.cancel_goal() rospy.logwarn("Translation 2 canceled") - goal = QuadMoveGoal(movement_type="velocity_test", x_velocity=0.0, y_velocity=0.0, z_position=0.8) + goal = QuadMoveGoal(movement_type="velocity_test", x_velocity=0.0, y_velocity=0.0, z_position=HEIGHT) # Sends the goal to the action server. client.send_goal(goal) rospy.sleep(STOP_DELAY) client.cancel_goal() rospy.logwarn("Stop 2 canceled") - goal = QuadMoveGoal(movement_type="velocity_test", x_velocity=-X_VELOCITY, y_velocity=0.0, z_position=0.8) + goal = QuadMoveGoal(movement_type="velocity_test", x_velocity=-X_VELOCITY, y_velocity=0.0, z_position=HEIGHT) # Sends the goal to the action server. client.send_goal(goal) rospy.sleep(X_DELAY) client.cancel_goal() rospy.logwarn("Translation 3 canceled") - goal = QuadMoveGoal(movement_type="velocity_test", x_velocity=0.0, y_velocity=0.0, z_position=0.8) + goal = QuadMoveGoal(movement_type="velocity_test", x_velocity=0.0, y_velocity=0.0, z_position=HEIGHT) # Sends the goal to the action server. client.send_goal(goal) rospy.sleep(STOP_DELAY) client.cancel_goal() rospy.logwarn("Stop 3 canceled") - goal = QuadMoveGoal(movement_type="velocity_test", x_velocity=0.0, y_velocity=Y_VELOCITY, z_position=0.8) + goal = QuadMoveGoal(movement_type="velocity_test", x_velocity=0.0, y_velocity=Y_VELOCITY, z_position=HEIGHT) # Sends the goal to the action server. client.send_goal(goal) rospy.sleep(Y_DELAY) client.cancel_goal() rospy.logwarn("Translation 4 canceled") - goal = QuadMoveGoal(movement_type="velocity_test", x_velocity=0.0, y_velocity=0.0, z_position=0.8) + goal = QuadMoveGoal(movement_type="velocity_test", x_velocity=0.0, y_velocity=0.0, z_position=HEIGHT) # Sends the goal to the action server. client.send_goal(goal) rospy.sleep(STOP_DELAY) From 3e40b525c916f92970b4ea7bc42220f11e1e8034 Mon Sep 17 00:00:00 2001 From: Levi Burner Date: Mon, 30 Jul 2018 15:44:53 -0400 Subject: [PATCH 12/18] Fly back and forth with fake position hold, make sure it lands --- scripts/mission7.py | 77 ++++++++++++++++++++++++++------------------- 1 file changed, 45 insertions(+), 32 deletions(-) diff --git a/scripts/mission7.py b/scripts/mission7.py index d12965f..005b12f 100755 --- a/scripts/mission7.py +++ b/scripts/mission7.py @@ -25,45 +25,55 @@ # Used if the planner is disabled TRANSLATION_VELOCITY = 1.0 - -# SEARCH_POINTS = np.asarray( -# ( -# (-6.0, 6.0), # In a little -# (-6.0, -6.0), # Right side -# (-6.0, 6.0) # Left side -# )) +PAUSE_TIME = 1.5 SEARCH_POINTS = np.asarray( ( - (-7.0, 7.0), # In a little - (-7.0, -2.0), # Right side - (-7.0, 7.0) # Left side + (-4.0, 6.0), # In a little + (-100.0, -100.0), # Pause + (-4.0, -4.0), # Right side + (-100.0, -100.0), # Pause + (-4.0, 4.0) # Left side )) +# SEARCH_POINTS = np.asarray( +# ( +# (-7.0, 7.0), # In a little +# (-7.0, -2.0), # Right side +# (-7.0, 7.0) # Left side +# )) + def target_roomba_law(roombas, odom): # Sort roombas by their distance to the drone sorted_roombas = sorted([(roomba_distance(r, odom), r) for r in roombas]) return sorted_roombas[0][1] -def construct_velocity_goal(arena_pos, supposed_to_be_at, height=TRANSLATION_HEIGHT): +def construct_velocity_goal(arena_pos, odom, height=TRANSLATION_HEIGHT): map_pos = arena_position_estimator.arena_to_map(arena_pos) - supposed_to_be_at = arena_position_estimator.arena_to_map(supposed_to_be_at) - diff_x = map_pos[0] - supposed_to_be_at[0] - diff_y = map_pos[1] - supposed_to_be_at[1] - print arena_pos - print map_pos - print diff_x, diff_y - hypot = math.sqrt(diff_x**2 + diff_y**2) - u_x = diff_x / hypot - u_y = diff_y / hypot - v_x = u_x * TRANSLATION_VELOCITY - v_y = u_y * TRANSLATION_VELOCITY - rospy.loginfo('Time: {}'.format(hypot / TRANSLATION_VELOCITY)) + #supposed_to_be_at = arena_position_estimator.arena_to_map(supposed_to_be_at) + if arena_pos[0] == -100.0 and arena_pos[1] == -100.0: + v_x = 0.0 + v_y = 0.0 + length_of_time = PAUSE_TIME + else: + supposed_to_be_at = (odom.pose.pose.position.x, odom.pose.pose.position.y) + diff_x = map_pos[0] - supposed_to_be_at[0] + diff_y = map_pos[1] - supposed_to_be_at[1] + print arena_pos + print map_pos + print diff_x, diff_y + hypot = math.sqrt(diff_x**2 + diff_y**2) + u_x = diff_x / hypot + u_y = diff_y / hypot + v_x = u_x * TRANSLATION_VELOCITY + v_y = u_y * TRANSLATION_VELOCITY + length_of_time = hypot / TRANSLATION_VELOCITY + return QuadMoveGoal(movement_type="velocity_test", x_velocity=v_x, y_velocity=v_y, z_position=height, - velocity_duration=hypot / TRANSLATION_VELOCITY) + velocity_duration=length_of_time) def construct_xyz_goal(arena_pos, height=TRANSLATION_HEIGHT): @@ -161,7 +171,7 @@ def begin_translate(self, arena_pos, height=TRANSLATION_HEIGHT): map_pos = construct_xyz_goal(arena_pos, height=height) self._client.send_goal(map_pos) else: - self._client.send_goal(construct_velocity_goal(arena_pos, self._supposed_to_be_at, height=height)) + self._client.send_goal(construct_velocity_goal(arena_pos, self._odom, height=height)) self._supposed_to_be_at = arena_pos def basic_goal(self, goal): @@ -199,6 +209,9 @@ def search_for_roomba(self): self._search_state = self._search_state + 1 if self._search_state + 1 < SEARCH_POINTS.shape[0] else 1 else: break + rospy.loginfo('Flight time {}'.format((rospy.Time.now() - self.flight_start_time).to_sec())) + if rospy.Time.now() > self.flight_start_time + rospy.Duration(MAX_FLIGHT_DURATION): + return None # Todo better initial search position self._search_state = 1 @@ -297,7 +310,7 @@ def track_roomba_to_completion(self, roomba): def attempt_mission7(self): # Takeoff - flight_start_time = rospy.Time.now() + self.flight_start_time = rospy.Time.now() self.basic_goal('takeoff') @@ -305,15 +318,15 @@ def attempt_mission7(self): gotten_roombas = 0 while not mission7_completed: roomba = self.search_for_roomba() - break + #got_roomba = self.track_roomba_to_completion(roomba) - if got_roomba: - gotten_roombas += 1 + #if got_roomba: + # gotten_roombas += 1 - if gotten_roombas > TARGET_NUM_ROOMBAS: - break + #if gotten_roombas > TARGET_NUM_ROOMBAS: + # break - if rospy.Time.now() > flight_start_time + rospy.Duration(MAX_FLIGHT_DURATION): + if rospy.Time.now() > self.flight_start_time + rospy.Duration(MAX_FLIGHT_DURATION): break # self.goto_safe_landing_spot() From 14f7838def524e79ec169444b8dedca578ee101c Mon Sep 17 00:00:00 2001 From: Levi Burner Date: Tue, 31 Jul 2018 21:52:18 -0400 Subject: [PATCH 13/18] Start button and make sure we land --- scripts/mission7.py | 29 +++++++++++++++++++++++------ 1 file changed, 23 insertions(+), 6 deletions(-) diff --git a/scripts/mission7.py b/scripts/mission7.py index 005b12f..56bde12 100755 --- a/scripts/mission7.py +++ b/scripts/mission7.py @@ -15,13 +15,14 @@ from iarc7_abstract.arena_position_estimator import ArenaPositionEstimator from tf.transformations import euler_from_quaternion +from control_button_interpreter import ControlButtonInterpreter TRANSLATION_HEIGHT = 2.0 MIN_GOTO_DISTANCE = 0.25 USE_PLANNER = False TARGET_NUM_ROOMBAS = 2 -MAX_FLIGHT_DURATION = 1 * 60 +MAX_FLIGHT_DURATION = 1.0 * 60 # Used if the planner is disabled TRANSLATION_VELOCITY = 1.0 @@ -29,11 +30,11 @@ SEARCH_POINTS = np.asarray( ( - (-4.0, 6.0), # In a little + (-5.0, 6.0), # In a little (-100.0, -100.0), # Pause - (-4.0, -4.0), # Right side + (-5.0, -4.0), # Right side (-100.0, -100.0), # Pause - (-4.0, 4.0) # Left side + (-5.0, 4.0) # Left side )) # SEARCH_POINTS = np.asarray( @@ -308,6 +309,10 @@ def track_roomba_to_completion(self, roomba): rate.sleep() def attempt_mission7(self): + + + control_buttons.wait_for_start() + # Takeoff self.flight_start_time = rospy.Time.now() @@ -329,8 +334,17 @@ def attempt_mission7(self): if rospy.Time.now() > self.flight_start_time + rospy.Duration(MAX_FLIGHT_DURATION): break - # self.goto_safe_landing_spot() - self.basic_goal('land') + landing_completed = False + while not landing_completed: + # self.goto_safe_landing_spot() + self.basic_goal('land') + while not did_task_finish(self._client): + rate.sleep() + + if did_task_succeed(self._client): + landing_completed = True + + rospy.loginfo('LANDING COMPLETED') if __name__ == '__main__': # Initializes a rospy node so that the SimpleActionClient can @@ -344,6 +358,9 @@ def attempt_mission7(self): msg = BoolStamped() msg.header.stamp = rospy.Time.now() pub.publish(msg) + + control_buttons = ControlButtonInterpreter() + mission7.attempt_mission7() rospy.spin() From d902ed771cc2e683999703b82c27c32dd17396d9 Mon Sep 17 00:00:00 2001 From: Levi Burner Date: Wed, 1 Aug 2018 09:05:41 -0400 Subject: [PATCH 14/18] All variables and heights --- scripts/mission7.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/scripts/mission7.py b/scripts/mission7.py index 56bde12..88ea747 100755 --- a/scripts/mission7.py +++ b/scripts/mission7.py @@ -26,7 +26,7 @@ # Used if the planner is disabled TRANSLATION_VELOCITY = 1.0 -PAUSE_TIME = 1.5 +PAUSE_TIME = 1.2 SEARCH_POINTS = np.asarray( ( @@ -334,6 +334,7 @@ def attempt_mission7(self): if rospy.Time.now() > self.flight_start_time + rospy.Duration(MAX_FLIGHT_DURATION): break + rate = rospy.Rate(30) landing_completed = False while not landing_completed: # self.goto_safe_landing_spot() From a7cb05ae554d1d0ee37ab546e471e8c8a098e66f Mon Sep 17 00:00:00 2001 From: Levi Burner Date: Wed, 1 Aug 2018 09:51:56 -0400 Subject: [PATCH 15/18] Attempt to hit twice --- scripts/mission7.py | 25 ++++++++++++++----------- 1 file changed, 14 insertions(+), 11 deletions(-) diff --git a/scripts/mission7.py b/scripts/mission7.py index 88ea747..0f47723 100755 --- a/scripts/mission7.py +++ b/scripts/mission7.py @@ -18,7 +18,7 @@ from control_button_interpreter import ControlButtonInterpreter TRANSLATION_HEIGHT = 2.0 -MIN_GOTO_DISTANCE = 0.25 +MIN_GOTO_DISTANCE = 0.5 USE_PLANNER = False TARGET_NUM_ROOMBAS = 2 @@ -241,9 +241,10 @@ def track_roomba_to_completion(self, roomba): self._client.cancel_goal() if d >= MIN_GOTO_DISTANCE: - self._client.send_goal(construct_goto_roomba_goal(roomba)) - rospy.loginfo('GOTO ROOMBA') - track_state = 1 + #self._client.send_goal(construct_goto_roomba_goal(roomba)) + #rospy.loginfo('GOTO ROOMBA') + #track_state = 1 + pass else: self._client.send_goal(QuadMoveGoal(movement_type="track_roomba", frame_id=roomba_id, @@ -276,6 +277,7 @@ def track_roomba_to_completion(self, roomba): track_state = 0 else: action = guide_roomba_law(roomba) + action = 2 # If block if action == 1: self._client.cancel_goal() @@ -286,7 +288,7 @@ def track_roomba_to_completion(self, roomba): elif action == 2: self._client.cancel_goal() rospy.loginfo('SENDING HIT ROOMBA') - self._client.send_goal(QuadMoveGoal(movement_type="block_roomba", frame_id = roomba_id)) + self._client.send_goal(QuadMoveGoal(movement_type="hit_roomba", frame_id = roomba_id)) track_state = 4 if track_state == 3: @@ -304,9 +306,10 @@ def track_roomba_to_completion(self, roomba): if did_task_succeed(self._client): rospy.loginfo('HIT SUCCEEDED') track_state = 2 - + break rate.sleep() + return True def attempt_mission7(self): @@ -324,12 +327,12 @@ def attempt_mission7(self): while not mission7_completed: roomba = self.search_for_roomba() - #got_roomba = self.track_roomba_to_completion(roomba) - #if got_roomba: - # gotten_roombas += 1 + got_roomba = self.track_roomba_to_completion(roomba) + if got_roomba: + gotten_roombas += 1 - #if gotten_roombas > TARGET_NUM_ROOMBAS: - # break + if gotten_roombas > TARGET_NUM_ROOMBAS: + break if rospy.Time.now() > self.flight_start_time + rospy.Duration(MAX_FLIGHT_DURATION): break From 2f8d578640136475da8c6628647dc85afee6271e Mon Sep 17 00:00:00 2001 From: Levi Burner Date: Wed, 1 Aug 2018 11:13:38 -0400 Subject: [PATCH 16/18] Actually track the roomba --- scripts/mission7.py | 27 ++++++++++++++------------- 1 file changed, 14 insertions(+), 13 deletions(-) diff --git a/scripts/mission7.py b/scripts/mission7.py index 0f47723..1f410ee 100755 --- a/scripts/mission7.py +++ b/scripts/mission7.py @@ -32,9 +32,9 @@ ( (-5.0, 6.0), # In a little (-100.0, -100.0), # Pause - (-5.0, -4.0), # Right side + (-5.0, -1.0), # Right side (-100.0, -100.0), # Pause - (-5.0, 4.0) # Left side + (-5.0, 5.0) # Left side )) # SEARCH_POINTS = np.asarray( @@ -187,10 +187,10 @@ def wait_for_roomba(self): return (False, None) elif did_task_succeed(self._client): return (True, None) - #if self._avail_roombas is not None and len(self._avail_roombas) > 0: - # target_roomba = target_roomba_law(self._avail_roombas, self._odom) - # if target_roomba is not None: - # return (True, target_roomba) + if self._avail_roombas is not None and len(self._avail_roombas) > 0: + target_roomba = target_roomba_law(self._avail_roombas, self._odom) + if target_roomba is not None: + return (True, target_roomba) rate.sleep() def search_for_roomba(self): @@ -327,15 +327,16 @@ def attempt_mission7(self): while not mission7_completed: roomba = self.search_for_roomba() - got_roomba = self.track_roomba_to_completion(roomba) - if got_roomba: - gotten_roombas += 1 + if roomba is not None: + got_roomba = self.track_roomba_to_completion(roomba) + if got_roomba: + gotten_roombas += 1 - if gotten_roombas > TARGET_NUM_ROOMBAS: - break + if gotten_roombas > TARGET_NUM_ROOMBAS: + break - if rospy.Time.now() > self.flight_start_time + rospy.Duration(MAX_FLIGHT_DURATION): - break + if rospy.Time.now() > self.flight_start_time + rospy.Duration(MAX_FLIGHT_DURATION): + break rate = rospy.Rate(30) landing_completed = False From 1dfee85d839092f790e0f289a12d91590c9fe756 Mon Sep 17 00:00:00 2001 From: Pitt RAS IARC Team Date: Wed, 1 Aug 2018 10:47:46 -0400 Subject: [PATCH 17/18] Stuff --- scripts/mission7.py | 6 +++--- scripts/velocity_test.py | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/scripts/mission7.py b/scripts/mission7.py index 1f410ee..129364b 100755 --- a/scripts/mission7.py +++ b/scripts/mission7.py @@ -17,15 +17,15 @@ from tf.transformations import euler_from_quaternion from control_button_interpreter import ControlButtonInterpreter -TRANSLATION_HEIGHT = 2.0 +TRANSLATION_HEIGHT = 1.1 MIN_GOTO_DISTANCE = 0.5 USE_PLANNER = False TARGET_NUM_ROOMBAS = 2 -MAX_FLIGHT_DURATION = 1.0 * 60 +MAX_FLIGHT_DURATION = 3.0 * 60 # Used if the planner is disabled -TRANSLATION_VELOCITY = 1.0 +TRANSLATION_VELOCITY = 0.3 PAUSE_TIME = 1.2 SEARCH_POINTS = np.asarray( diff --git a/scripts/velocity_test.py b/scripts/velocity_test.py index 62b31fc..c7fa487 100755 --- a/scripts/velocity_test.py +++ b/scripts/velocity_test.py @@ -29,7 +29,7 @@ def velocity_test(): X_DELAY = 5.0 Y_DELAY = 5.0 STOP_DELAY = 2.0 - HEIGHT = 1.5 + HEIGHT = 1.1 # Test takeoff goal = QuadMoveGoal(movement_type="takeoff") From da76273b26829a56011c403af6de9ed81754ab21 Mon Sep 17 00:00:00 2001 From: Pitt RAS IARC Team Date: Sun, 5 Aug 2018 15:47:15 -0400 Subject: [PATCH 18/18] Competition changes --- scripts/mission7.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/scripts/mission7.py b/scripts/mission7.py index 129364b..55c3de8 100755 --- a/scripts/mission7.py +++ b/scripts/mission7.py @@ -21,11 +21,11 @@ MIN_GOTO_DISTANCE = 0.5 USE_PLANNER = False -TARGET_NUM_ROOMBAS = 2 -MAX_FLIGHT_DURATION = 3.0 * 60 +TARGET_NUM_ROOMBAS = 4 +MAX_FLIGHT_DURATION = 6.0 * 60 # Used if the planner is disabled -TRANSLATION_VELOCITY = 0.3 +TRANSLATION_VELOCITY = 0.6 PAUSE_TIME = 1.2 SEARCH_POINTS = np.asarray( @@ -208,7 +208,7 @@ def search_for_roomba(self): if roomba is None: self._search_state = self._search_state + 1 if self._search_state + 1 < SEARCH_POINTS.shape[0] else 1 - else: + elif self._search_state > 0: break rospy.loginfo('Flight time {}'.format((rospy.Time.now() - self.flight_start_time).to_sec())) if rospy.Time.now() > self.flight_start_time + rospy.Duration(MAX_FLIGHT_DURATION): @@ -314,7 +314,7 @@ def track_roomba_to_completion(self, roomba): def attempt_mission7(self): - control_buttons.wait_for_start() + #control_buttons.wait_for_start() # Takeoff