def motion_planner_client():
# 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()
# Test takeoff
goal = QuadMoveGoal(movement_type="takeoff", takeoff_height=3.0)
# Sends the goal to the action server.
client.send_goal(goal)
# Waits for the server to finish performing the action.
client.wait_for_result()
rospy.logwarn("Takeoff success: {}".format(client.get_result()))
# Test takeoff
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()
rospy.logwarn("Land success: {}".format(client.get_result()))
def takeoff_land():
safety_client = SafetyClient('takeoff_land_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())
# 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()
rospy.sleep(5.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()
rospy.logwarn("Takeoff success: {}".format(client.get_result()))
rospy.sleep(5.0)
# 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()
rospy.logwarn("Land success: {}".format(client.get_result()))
def hit_roomba_land():
safety_client = SafetyClient('hit_roomba_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())
# 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()
rospy.sleep(2.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()
rospy.logwarn("Takeoff success: {}".format(client.get_result()))
rospy.sleep(2.0)
# change element in array to test diff roombas
roomba_id = roomba_array.data[5].child_frame_id
roomba_id = roomba_id[0:len(roomba_id) - 10]
# Test tracking
goal = QuadMoveGoal(movement_type="track_roomba",
frame_id=roomba_id,
tracking_mode=True)
# Sends the goal to the action server.
client.send_goal(goal)
# Waits for the server to finish performing the action.
client.wait_for_result()
rospy.logwarn("Track Roomba success: {}".format(client.get_result()))
# Test tracking
goal = QuadMoveGoal(movement_type="hit_roomba", frame_id=roomba_id)
# Sends the goal to the action server.
client.send_goal(goal)
# Waits for the server to finish performing the action.
client.wait_for_result()
rospy.logwarn("Hit Roomba success: {}".format(client.get_result()))
goal = QuadMoveGoal(movement_type="height_recovery")
# Sends the goal to the action server.
client.send_goal(goal)
# Waits for the server to finish performing the action.
client.wait_for_result()
rospy.logwarn("Height Recovery success: {}".format(client.get_result()))
def hold_current_position_land():
safety_client = SafetyClient('hold_position_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())
# 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()
rospy.sleep(2.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()
rospy.logwarn("Takeoff success: {}".format(client.get_result()))
rospy.sleep(2.0)
_x_position = _drone_odometry.pose.pose.position.x - .25
_y_position = _drone_odometry.pose.pose.position.y - .25
_z_position = _drone_odometry.pose.pose.position.z + .1
# Test tracking
goal = QuadMoveGoal(movement_type="hold_position",
hold_current_position=False,
x_position=_x_position,
y_position=_y_position,
z_position=_z_position)
# Sends the goal to the action server.
client.send_goal(goal)
rospy.sleep(5)
client.cancel_goal()
rospy.logwarn("Hold Position Task canceled")
def test_rre():
safety_client = SafetyClient('roomba_request_executer_test_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
rospy.sleep(2.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()))
rospy.sleep(2.0)
while roomba_array is None and not rospy.is_shutdown():
rospy.sleep(2)
# change element in array to test diff roombas
roomba_id = roomba_array.data[-1].child_frame_id
RoombaRequestExecuter.init('motion_planner_server')
roomba_request = RoombaRequest(roomba_id, RoombaRequest.BUMP, 5)
RoombaRequestExecuter.run(roomba_request, _receive_roomba_executer_status)
while RoombaRequestExecuter.has_running_task():
rospy.logwarn('Current Roomba Request Executer state: ' +
executer_state.name)
rospy.sleep(2)
if executer_state == RoombaRequestExecuterState.SUCCESS:
rospy.logwarn("Roomba request executer completed successfully")
else:
rospy.logerr("Roomba request executer failed")
def run(self):
rate = rospy.Rate(self._update_rate)
rospy.logwarn('trying to form bond')
if not self._safety_client.form_bond():
rospy.logerr('Motion planner could not form bond with safety client')
return
rospy.logwarn('done forming bond')
rospy.wait_for_service('uav_arm')
self._action_client.wait_for_server()
self._ground_interaction_client.wait_for_server()
while not rospy.is_shutdown():
# Exit immediately if fatal
if self._safety_client.is_fatal_active() or self._safety_land_complete:
return
# Land if put into safety mode
if self._safety_client.is_safety_active() and not self._safety_land_requested:
# Request landing
goal = QuadMoveGoal(movement_type="land", preempt=True)
self._action_client.send_goal(goal,
done_cb=self._safety_task_complete_callback)
rospy.logwarn(
'motion planner attempting to execute safety land')
self._safety_land_requested = True
task_commands = self._get_task_command()
for task_command in task_commands:
try:
self._command_implementations[type(task_command)](task_command)
except (KeyError, TypeError) as e:
rospy.logerr("Task requested unimplemented command, noping: %s", type(task_command))
self._handle_nop_command(None)
rate.sleep()
def motion_planner_client():
# 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()
# Test standard send receive
# Creates a goal to send to the action server.
goal = QuadMoveGoal(movement_type="test_task")
# Sends the goal to the action server.
client.send_goal(goal)
# Waits for the server to finish performing the action.
client.wait_for_result()
rospy.loginfo("Request/wait for request success: {}".format(
client.get_result()))
# Test canceling goal
client.send_goal(goal)
rospy.sleep(0.5)
client.cancel_goal()
rospy.loginfo("Goal canceled")
# Test aborting goal
# Creates a goal to send to the action server, uses takeoff_height as a flag
goal = QuadMoveGoal(movement_type="test_task", takeoff_height=1.0)
client.send_goal(goal)
client.wait_for_result()
rospy.loginfo("Task aborted result (false is expected): {}".format(
client.get_result()))
rospy.loginfo("Attempting a cancel on a queued operation")
rospy.sleep(3.0)
# Test queueing then canceling a queued goal
# Creates a goal to send to the action server.
goal = QuadMoveGoal(movement_type="test_task")
# This is improper usage because the simple action client only supports one active goal at a time
# But iarc action server will queue multiple requests. By sending multiple goals, the simple action
# client loses the ability to stop or abort the first goal
client.send_goal(goal)
client.send_goal(goal)
rospy.sleep(0.5)
client.cancel_goal()
# We should call wait_for_result but because the simple action client thinks the task is done
# it immediately returns, sleep instead
#client.wait_for_result()
rospy.loginfo("Attempting a preempt operation")
rospy.sleep(3.0)
# Test preempting
# This is also improper usage....
goal = QuadMoveGoal(movement_type="test_task")
client.send_goal(goal)
client.send_goal(goal)
rospy.sleep(0.5)
goal = QuadMoveGoal(movement_type="test_task", preempt=True)
client.send_goal(goal)
client.wait_for_result()
def velocity_test():
safety_client = SafetyClient('velocity_test_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
X_DELAY = 2.0
Y_DELAY = 2.0
STOP_DELAY = 2.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()))
for i in range(0, 1):
goal = QuadMoveGoal(movement_type="velocity_test",
x_velocity=X_VELOCITY,
y_velocity=0.0,
z_position=0.8)
# 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)
# 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)
# 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)
# 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)
# 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)
# 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)
# 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)
# Sends the goal to the action server.
client.send_goal(goal)
rospy.sleep(STOP_DELAY)
client.cancel_goal()
rospy.logwarn("Stop 4 canceled")
# 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()))
def wall_bounce():
safety_client = SafetyClient('wall_bounce_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
VELOCITY = 0.5
STOP_DELAY = 2.0
# Start going forward
angle = 0
distance_to_wall_threshold = 2.0
last_wall_hit = -1
# 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()))
rospy.sleep(0.5)
for i in range(0, 100):
# Go in a direction until the distance to any wall (that was not the last wall hit) is too small
x_vel = math.cos(angle) * VELOCITY
y_vel = math.sin(angle) * VELOCITY
rospy.logwarn('SET A VELOCITY')
rospy.logwarn('Changing direction, new angle: {} degrees'.format(angle * 180.0 / math.pi))
goal = QuadMoveGoal(movement_type="velocity_test", x_velocity=x_vel, y_velocity=y_vel, z_position=1.5)
# Sends the goal to the action server.
client.send_goal(goal)
rospy.sleep(0.1)
CONE_WIDTH_RAD = 30 * math.pi / 180.0
CONE_OFFSET_RAD = ((math.pi/2) - CONE_WIDTH_RAD) / 2.0
rate = rospy.Rate(30)
while True:
if (last_wall_hit != 0 and last_wall_hit != 3) and arena_position_estimator.distance_to_left() < distance_to_wall_threshold:
last_wall_hit = 0
rospy.logerr('LEFT WALL HIT')
# random angle pointed SE
angle = -((random.random() * CONE_WIDTH_RAD) + CONE_OFFSET_RAD) - (math.pi/2)
break
if (last_wall_hit != 1 and last_wall_hit != 2) and arena_position_estimator.distance_to_right() < distance_to_wall_threshold:
last_wall_hit = 1
rospy.logerr('RIGHT WALL HIT')
# random angle pointed NW
angle = ((random.random() * CONE_WIDTH_RAD) + CONE_OFFSET_RAD)
break
if (last_wall_hit != 2 and last_wall_hit != 0) and arena_position_estimator.distance_to_top() < distance_to_wall_threshold:
last_wall_hit = 2
rospy.logerr('TOP WALL HIT')
# random angle pointed SW
angle = ((random.random() * CONE_WIDTH_RAD) + CONE_OFFSET_RAD) + (math.pi/2)
break
if (last_wall_hit != 3 and last_wall_hit != 1) and arena_position_estimator.distance_to_bottom() < distance_to_wall_threshold:
last_wall_hit = 3
rospy.logerr('BOTTOM WALL HIT')
# random angle pointed NE
angle = -((random.random() * CONE_WIDTH_RAD) + CONE_OFFSET_RAD)
break
rate.sleep()
client.cancel_goal()
goal = QuadMoveGoal(movement_type="velocity_test", x_velocity=0.0, y_velocity=0.0, z_position=1.5)
# Sends the goal to the action server.
client.send_goal(goal)
rospy.sleep(STOP_DELAY)
client.cancel_goal()
rospy.logwarn("Stopped")
# 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()))
def test():
safety_client = SafetyClient('invalid_task_transition_test')
# 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
rospy.sleep(2.0)
# arbitrarily picked fifth roomba to test
roomba_id = roomba_array.data[5].child_frame_id
rospy.logwarn("Testing illegal state #1")
goal = QuadMoveGoal(movement_type="track_roomba",
frame_id=roomba_id,
time_to_track=5.0,
x_overshoot=0,
y_overshoot=0)
# 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
assert client.get_result().success == False
rospy.sleep(2.0)
# Testing trying to cancel takeoff
rospy.logwarn("Testing illegal state #2, canceling takeoff")
goal = QuadMoveGoal(movement_type="takeoff")
# Sends the goal to the action server.
client.send_goal(goal)
rospy.sleep(1.0)
client.cancel_goal()
rospy.sleep(2.0)
rospy.logwarn("RESET STATE")
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()))
rospy.sleep(2.0)
rospy.logwarn("Testing illegal state #3")
goal = QuadMoveGoal(movement_type="hit_roomba", frame_id=roomba_id)
# 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
assert client.get_result().success == False
rospy.sleep(2.0)
rospy.logwarn("RESET STATE")
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()))
rospy.sleep(2.0)
rospy.logwarn("Testing invalid RoombaID")
goal = QuadMoveGoal(movement_type="track_roomba",
time_to_track=5.0,
x_overshoot=.5,
y_overshoot=.5)
# 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
assert client.get_result().success == False
rospy.logwarn("Preparing to test illegal state #4")
goal = QuadMoveGoal(movement_type="hit_roomba", frame_id=roomba_id)
# 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("Hit Roomba success: {}".format(client.get_result()))
rospy.sleep(2.0)
rospy.logwarn("Testing illegal state #4")
goal = QuadMoveGoal(movement_type="track_roomba",
frame_id=roomba_id,
time_to_track=5.0,
x_overshoot=.5,
y_overshoot=.5)
# 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
assert client.get_result().success == False
rospy.sleep(2.0)
rospy.logwarn("RESET STATE")
goal = QuadMoveGoal(movement_type="height_recovery")
# 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("Height Recovery success: {}".format(client.get_result()))
rospy.sleep(2.0)
rospy.logwarn("Sending Test Task")
goal = QuadMoveGoal(movement_type="test_task", takeoff_height=0.75)
# 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("Test Task success: {}".format(client.get_result()))
rospy.sleep(2.0)
rospy.logwarn("RESET STATE")
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()))
rospy.logwarn("Testing is complete")
def competition_demo():
safety_client = SafetyClient('competition_demo_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
rospy.sleep(2.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()))
rospy.sleep(2.0)
goal = QuadMoveGoal(movement_type="velocity_test",
x_velocity=0.2,
y_velocity=-0.2)
# Sends the goal to the action server.
client.send_goal(goal)
rospy.sleep(3.0)
client.cancel_goal()
rospy.logwarn("Translation 1 canceled")
rospy.sleep(2.0)
goal = QuadMoveGoal(movement_type="velocity_test",
x_velocity=-0.2,
y_velocity=0.2)
# Sends the goal to the action server.
client.send_goal(goal)
rospy.sleep(0.5)
client.cancel_goal()
rospy.logwarn("Translation 2 canceled")
rospy.sleep(2.0)
goal = QuadMoveGoal(movement_type="velocity_test",
x_velocity=0.3,
y_velocity=0.0)
# Sends the goal to the action server.
client.send_goal(goal)
rospy.sleep(1.0)
client.cancel_goal()
rospy.logwarn("Translation 3 canceled")
rospy.sleep(2.0)
goal = QuadMoveGoal(movement_type="velocity_test",
x_velocity=-0.2,
y_velocity=-0.2)
# Sends the goal to the action server.
client.send_goal(goal)
rospy.sleep(0.5)
client.cancel_goal()
rospy.logwarn("Translation 4 canceled")
rospy.sleep(2.0)
goal = QuadMoveGoal(movement_type="velocity_test",
x_velocity=0.0,
y_velocity=0.3)
# Sends the goal to the action server.
client.send_goal(goal)
rospy.sleep(1.0)
client.cancel_goal()
rospy.logwarn("Translation 5 canceled")
rospy.sleep(6.0)
# 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()))
def hit_roomba_land():
safety_client = SafetyClient('hit_roomba_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
control_buttons.wait_for_start()
# 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
# 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=0.0, y_velocity=0.0, z_position=1.5)
# Sends the goal to the action server.
client.send_goal(goal)
rospy.sleep(2.0)
client.cancel_goal()
rospy.logwarn("Done ascending")
goal = QuadMoveGoal(movement_type="velocity_test", x_velocity=0.7, y_velocity=0.0, z_position=1.5)
# Sends the goal to the action server.
client.send_goal(goal)
search_start_time = rospy.Time.now()
rate = rospy.Rate(30)
roomba_detected = False
while not rospy.is_shutdown():
if rospy.Time.now() - search_start_time > rospy.Duration(2.5):
rospy.loginfo("Searching for Roomba timed out")
break
elif len(roomba_array.data) > 0:
roomba_detected = True
rospy.loginfo("FOUND ROOMBA")
break
rate.sleep()
client.cancel_goal()
rospy.logwarn("Translation to roomba canceled")
if roomba_detected:
# change element in array to test diff roombas
roomba_id = roomba_array.data[0].child_frame_id
for i in range(0, 1):
# Test tracking
goal = QuadMoveGoal(movement_type="track_roomba", frame_id=roomba_id,
time_to_track=0.0, x_overshoot=0, y_overshoot=0)
# Sends the goal to the action server.
client.send_goal(goal)
# Waits for the server to finish performing the action.
client.wait_for_result()
rospy.logwarn("Track Roomba success: {}".format(client.get_result()))
# Test hitting
goal = QuadMoveGoal(movement_type="hit_roomba", frame_id = roomba_id)
# Sends the goal to the action server.
client.send_goal(goal)
# Waits for the server to finish performing the action.
client.wait_for_result()
rospy.logwarn("Hit Roomba success: {}".format(client.get_result()))
else:
rospy.logerr("Roomba not found while searching, returning")
goal = QuadMoveGoal(movement_type="velocity_test", x_velocity=-0.5, y_velocity=0.0, z_position=1.5)
# Sends the goal to the action server.
client.send_goal(goal)
rospy.sleep(3.0)
client.cancel_goal()
rospy.logwarn("Translation to roomba canceled")
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()))
def takeoff_land():
safety_client = SafetyClient('takeoff_land_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
rospy.sleep(5.0)
for i in range(0, 2):
# 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()))
#rospy.logwarn("Begin hovering")
#goal = QuadMoveGoal(movement_type="velocity_test", x_velocity=0.0, y_velocity=0.0, z_position=0.4)
# Sends the goal to the action server.
#client.send_goal(goal)
#rospy.sleep(10.0)
#client.cancel_goal()
#if rospy.is_shutdown(): return
#rospy.logwarn("Done hovering")
for i in range(0, 1):
rospy.logwarn("Ascending")
goal = QuadMoveGoal(movement_type="velocity_test",
x_velocity=0.0,
y_velocity=0.0,
z_position=1.0)
# Sends the goal to the action server.
client.send_goal(goal)
rospy.sleep(3.0)
client.cancel_goal()
if rospy.is_shutdown(): return
rospy.logwarn("Done asending")
rospy.logwarn("Descending")
goal = QuadMoveGoal(movement_type="velocity_test",
x_velocity=0.0,
y_velocity=0.0,
z_position=0.8)
# Sends the goal to the action server.
client.send_goal(goal)
rospy.sleep(3.0)
client.cancel_goal()
if rospy.is_shutdown(): return
rospy.logwarn("Done descending")
# 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()))
def run(self):
# rate limiting of updates of motion coordinator
rate = rospy.Rate(self._update_rate)
# waiting for dependencies to be ready
while rospy.Time.now() == rospy.Time(0) and not rospy.is_shutdown():
rospy.sleep(0.005)
if rospy.is_shutdown():
raise rospy.ROSInterruptException()
start_time = rospy.Time.now()
if not self._action_client.wait_for_server(self._startup_timeout):
raise IARCFatalSafetyException(
'Motion Coordinator could not initialize action client')
self._state_monitor.wait_until_ready(self._startup_timeout -
(rospy.Time.now() - start_time))
self._task_command_handler.wait_until_ready(self._startup_timeout -
(rospy.Time.now() -
start_time))
self._idle_obstacle_avoider.wait_until_ready(self._startup_timeout -
(rospy.Time.now() -
start_time))
AbstractTask().topic_buffer.wait_until_ready(self._startup_timeout -
(rospy.Time.now() -
start_time))
# forming bond with safety client
if not self._safety_client.form_bond():
raise IARCFatalSafetyException(
'Motion Coordinator could not form bond with safety client')
while not rospy.is_shutdown():
with self._lock:
# Exit immediately if fatal
if self._safety_client.is_fatal_active():
raise IARCFatalSafetyException(
'Safety Client is fatal active')
elif self._safety_land_complete:
return
# Land if put into safety mode
if self._safety_client.is_safety_active(
) and not self._safety_land_requested:
# Request landing
goal = QuadMoveGoal(movement_type="land", preempt=True)
self._action_client.send_goal(
goal, done_cb=self._safety_task_complete_callback)
rospy.logwarn(
'motion coordinator attempting to execute safety land')
self._safety_land_requested = True
self._state_monitor.signal_safety_active()
# set the time of last task to now if we have not seen a task yet
if not self._first_task_seen:
self._time_of_last_task = rospy.Time.now()
self._first_task_seen = True
closest_obstacle_dist = self._idle_obstacle_avoider.get_distance_to_obstacle(
)
if self._task is None and self._action_server.has_new_task():
new_task = self._action_server.get_new_task()
if not self._state_monitor.check_transition(new_task):
rospy.logerr(
'Illegal task transition request requested in motion coordinator. Aborting requested task.'
)
self._action_server.set_aborted()
elif not closest_obstacle_dist >= self._new_task_distance:
rospy.logerr(
'Attempt to start task too close to obstacle.' +
' Aborting requested task.')
self._action_server.set_aborted()
else:
self._time_of_last_task = None
self._task = new_task
self._task_command_handler.new_task(
new_task, self._get_current_transition())
if self._task is not None:
task_canceled = False
if self._action_server.is_canceled():
task_canceled = self._task_command_handler.cancel_task(
)
if not task_canceled and closest_obstacle_dist < self._kickout_distance:
# Abort current task
task_canceled = self._task_command_handler.abort_task(
'Too close to obstacle')
# if canceling task did not result in an error
if not task_canceled:
self._task_command_handler.run()
task_state = self._task_command_handler.get_state()
# handles state of task, motion coordinator, and action server
if isinstance(task_state, task_states.TaskCanceled):
self._action_server.set_canceled()
rospy.logwarn('Task was canceled')
self._task = None
elif isinstance(task_state, task_states.TaskAborted):
rospy.logwarn('Task aborted with: %s', task_state.msg)
self._action_server.set_aborted()
self._task = None
elif isinstance(task_state, task_states.TaskFailed):
rospy.logwarn('Task failed with: %s', task_state.msg)
self._action_server.set_succeeded(False)
self._task = None
elif isinstance(task_state, task_states.TaskDone):
self._action_server.set_succeeded(True)
self._task = None
elif not isinstance(task_state, task_states.TaskRunning):
rospy.logerr(
"Invalid task state returned, aborting task")
self._action_server.set_aborted()
self._task = None
task_state = task_states.TaskAborted(
msg='Invalid task state returned')
# as soon as we set a task to None, start time
# and send ending state to State Monitor
if self._task is None:
self._time_of_last_task = rospy.Time.now()
self._timeout_vel_sent = False
self._state_monitor.set_last_task_end_state(task_state)
# No task is running, run obstacle avoider
else:
vel = AbstractTask.topic_buffer.get_linear_motion_profile_generator(
).expected_point_at_time(
rospy.Time.now()).motion_point.twist.linear
vel_vec_2d = np.array([vel.x, vel.y], dtype=np.float)
avoid_vector, acceleration = self._idle_obstacle_avoider.get_safest(
vel_vec_2d)
avoid_twist = TwistStamped()
avoid_twist.header.stamp = rospy.Time.now()
avoid_twist.twist.linear.x = avoid_vector[0]
avoid_twist.twist.linear.y = avoid_vector[1]
self._task_command_handler.send_timeout(
avoid_twist, acceleration=acceleration)
rospy.logwarn_throttle(
1.0, 'Task running timeout. Running obstacle avoider')
rate.sleep()
def takeoff_land():
safety_client = SafetyClient('takeoff_translate_land_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
rospy.sleep(2.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()))
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)
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)
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=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)
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")
# 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()))
def _run(cls, roomba_request, status_callback):
"""
Roomba Request Executer run function
Description:
Fires off tasks until the roomba request is finished, canceled, or
fails.
"""
cls._roomba_id = roomba_request.frame_id
cls._roomba_turning = False
time_to_hold = roomba_request.time_to_hold
tracking_mode = roomba_request.tracking_mode
holding = (time_to_hold != 0)
state = RoombaRequestExecuterState.GOING_TO
track_sent = False
status_callback(state)
while not rospy.is_shutdown():
# rate limit at 10Hz
rate = rospy.Rate(10)
""" determining goals """
if (state == RoombaRequestExecuterState.RECOVER_FROM_FAILURE or
state == RoombaRequestExecuterState.RECOVER_FROM_SUCCESS):
goal = QuadMoveGoal(movement_type="height_recovery")
# Sends the goal to the action server.
cls._client.send_goal(goal)
# Waits for the server to finish performing the action.
cls._client.wait_for_result()
if rospy.is_shutdown():
raise rospy.ROSInterruptException()
rospy.logwarn("Recover Height success: {}".format(
cls._client.get_result()))
elif state == RoombaRequestExecuterState.GOING_TO:
goal = QuadMoveGoal(movement_type="go_to_roomba",
frame_id=cls._roomba_id)
# Sends the goal to the action server.
cls._client.send_goal(goal)
# Waits for the server to finish performing the action.
cls._client.wait_for_result()
rospy.logwarn("Go to Roomba success: {}".format(
cls._client.get_result()))
elif state == RoombaRequestExecuterState.FOLLOWING:
if not track_sent:
goal = QuadMoveGoal(movement_type="track_roomba",
frame_id=cls._roomba_id,
x_overshoot=-.25,
y_overshoot=-.25)
# Sends the goal to the action server.
cls._client.send_goal(goal)
rospy.logwarn("Following Roomba")
track_sent = True
if cls._roomba_turning:
cls._client.cancel_goal()
elif state == RoombaRequestExecuterState.HIT:
goal = QuadMoveGoal(movement_type="hit_roomba",
frame_id=cls._roomba_id)
# Sends the goal to the action server.
cls._client.send_goal(goal)
# Waits for the server to finish performing the action.
cls._client.wait_for_result()
if rospy.is_shutdown():
raise rospy.ROSInterruptException()
rospy.logwarn("Hit Roomba success: {}".format(
cls._client.get_result()))
elif state == RoombaRequestExecuterState.BLOCK:
goal = QuadMoveGoal(movement_type="block_roomba",
frame_id=cls._roomba_id)
# Sends the goal to the action server.
cls._client.send_goal(goal)
# Waits for the server to finish performing the action.
cls._client.wait_for_result()
if rospy.is_shutdown():
raise rospy.ROSInterruptException()
rospy.logwarn("BlockRoomba success: {}".format(
cls._client.get_result()))
elif state == RoombaRequestExecuterState.HOLD_POSITION:
goal = QuadMoveGoal(movement_type="hold_position",
hold_current_position=True)
# Sends the goal to the action server.
cls._client.send_goal(goal)
# waits to cancel hold task
rospy.sleep(time_to_hold)
cls._client.cancel_goal()
rospy.logwarn("Hold Position Task canceled")
"""state transitioning"""
# see http://docs.ros.org/api/actionlib_msgs/html/msg/GoalStatus.html
# for all status IDs
# 4: ABORTED
# 5: REJECTED
# 9: LOST
if cls._client.get_state() not in [4, 5, 9]:
# the task was successful
# Hit and Block roomba require recovery afterwards
if state == RoombaRequestExecuterState.HIT:
state = RoombaRequestExecuterState.RECOVER_FROM_SUCCESS
elif state == RoombaRequestExecuterState.BLOCK:
state = RoombaRequestExecuterState.RECOVER_FROM_SUCCESS
# transition after going to a roomba
elif state == RoombaRequestExecuterState.GOING_TO:
if tracking_mode == RoombaRequest.BLOCK:
state = RoombaRequestExecuterState.BLOCK
elif tracking_mode == RoombaRequest.HIT:
state = RoombaRequestExecuterState.HIT
else:
state = RoombaRequestExecuterState.FOLLOWING
# bump roomba logic
elif state == RoombaRequestExecuterState.FOLLOWING:
if cls._roomba_turning:
state = RoombaRequestExecuterState.BLOCK
# successfully recovered, so hold now, if requested
elif state == RoombaRequestExecuterState.RECOVER_FROM_SUCCESS:
if holding:
state = RoombaRequestExecuterState.HOLD_POSITION
else:
state = RoombaRequestExecuterState.SUCCESS
# roomba task failed, but recover succeeded
elif state == RoombaRequestExecuterState.RECOVER_FROM_FAILURE:
state = RoombaRequestExecuterState.FAILED_TASK
# success after holding
elif state == RoombaRequestExecuterState.HOLD_POSITION:
state = RoombaRequestExecuterState.SUCCESS
else:
rospy.logerr(
"Roomba Request Executer is in an invalid state")
state = RoombaRequestExecuterState.INVALID_STATE
else:
if state == RoombaRequestExecuterState.RECOVER_FROM_FAILURE:
state = RoombaRequestExecuterState.FAILED_TASK_AND_RECOVERY
elif state == RoombaRequestExecuterState.RECOVER_FROM_SUCCESS:
state = RoombaRequestExecuterState.FAILED_RECOVERY
else:
state = RoombaRequestExecuterState.RECOVER_FROM_FAILURE
status_callback(state)
if state in (RoombaRequestExecuterState.SUCCESS,
RoombaRequestExecuterState.INVALID_STATE,
RoombaRequestExecuterState.FAILED_RECOVERY,
RoombaRequestExecuterState.FAILED_TASK,
RoombaRequestExecuterState.FAILED_TASK_AND_RECOVERY):
break
rate.sleep()
cls._running = False