From 780c3b88c188812d38ae4f98b4121ce5083cc996 Mon Sep 17 00:00:00 2001 From: Andrew Saba Date: Tue, 10 Jul 2018 01:39:09 -0400 Subject: [PATCH 1/7] 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 2/7] 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 3/7] 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 4/7] 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 5/7] 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 6/7] 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 7/7] 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