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 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 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")
from iarc7_motion.msg import GroundInteractionGoal, GroundInteractionAction
from iarc7_msgs.msg import TwistStampedArray
from iarc7_msgs.srv import Arm
from geometry_msgs.msg import TwistStamped
from iarc7_safety.SafetyClient import SafetyClient
def constrain(x, l, h):
return min(h, max(x, l))
if __name__ == '__main__':
rospy.init_node('waypoints_test')
safety_client = SafetyClient('motion_planner')
assert safety_client.form_bond()
velocity_pub = rospy.Publisher('movement_velocity_targets',
TwistStampedArray,
queue_size=0)
tf_listener = tf.TransformListener()
while not rospy.is_shutdown() and rospy.Time.now() == 0:
pass
start_time = rospy.Time.now()
arm_service = rospy.ServiceProxy('uav_arm', Arm)
arm_service.wait_for_service()
armed = False
while not rospy.is_shutdown() and not armed:
try:
class VelocityFilter(object):
def __init__(self):
rospy.init_node('velocity_filter')
self._lock = threading.Lock()
self._optical_flow_sub = rospy.Subscriber(
'/optical_flow_estimator/twist', TwistWithCovarianceStamped,
lambda msg: self._callback('optical', msg))
# This topic is assumed to have rotation around +z
# (angle from +x with +y being +pi/2)
self._kalman_filter_sub = rospy.Subscriber(
'/odometry/filtered', Odometry,
lambda msg: self._callback('kalman', msg))
self._kalman_weight = rospy.get_param('~kalman_weight')
self._message_queue_length = rospy.get_param('~message_queue_length')
self._position_passthrough = rospy.get_param('~position_passthrough')
initial_msg = TwistWithCovarianceStamped()
initial_msg.header.stamp = rospy.Time()
# This queue contains 3-tuples, sorted by timestamp, oldest at index 0
#
# First item of each 3-tuple is a list with 3 items containing the x,
# y, and z velocities. The x and y values are the current values of the
# filter at that point, the z is too except that it's simply propogated
# forward from the last 'kalman' message.
#
# Second item of each 3-tuple is the message itself
#
# Third item of each 3-tuple is a string indicating the type of the
# message, either 'optical' or 'kalman'
self._queue = [([0.0, 0.0, 0.0], initial_msg, 'optical')]
self._vel_pub = rospy.Publisher('double_filtered_vel',
Odometry,
queue_size=10)
self._last_published_stamp = rospy.Time(0)
def _publish_odom(self, x, y, z, vx, vy, vz, time):
msg = Odometry()
msg.header.stamp = time
msg.header.frame_id = 'map'
msg.child_frame_id = 'level_quad'
assert not math.isnan(x)
assert not math.isnan(y)
assert not math.isnan(z)
assert not math.isinf(x)
assert not math.isinf(y)
assert not math.isinf(z)
assert not math.isnan(vx)
assert not math.isnan(vy)
assert not math.isnan(vz)
assert not math.isinf(vx)
assert not math.isinf(vy)
assert not math.isinf(vz)
msg.twist.twist.linear.x = vx
msg.twist.twist.linear.y = vy
msg.twist.twist.linear.z = vz
msg.pose.pose.position.x = x
msg.pose.pose.position.y = y
msg.pose.pose.position.z = z
self._vel_pub.publish(msg)
def _get_last_index(self, klass, time=None):
'''
Return the index of the last message of type `klass` (or the last
message of type `klass` before `time` if `time` is specified)
Returns `-1` if there are no messages of type `klass` in the queue
'''
for i in xrange(len(self._queue) - 1, -1, -1):
if (self._queue[i][2] == klass and
(time is None or self._queue[i][1].header.stamp < time)):
return i
return -1
def _callback(self, klass, msg):
with self._lock:
new_vel_vector = msg.twist.twist.linear
last_i = self._get_last_index(klass)
if (last_i != -1 and
msg.header.stamp <= self._queue[last_i][1].header.stamp):
# This message isn't newer than the last one of type klass
rospy.logerr((
'Ignoring message of type {} with timestamp {} older than '
+ 'previous message with timestamp {}').format(
klass, msg.header.stamp,
self._queue[last_i][1].header.stamp))
return
if (math.isnan(new_vel_vector.x) or math.isnan(new_vel_vector.y)
or math.isinf(new_vel_vector.x)
or math.isinf(new_vel_vector.y)):
rospy.logerr(
'Velocity filter rejecting bad message: {}'.format(msg))
return
for i in xrange(len(self._queue) - 1, -1, -1):
if self._queue[i][1].header.stamp < msg.header.stamp:
index = i + 1
break
else:
rospy.logerr(
'Skipping message older than everything in the queue')
return
z_vel = float('NaN') if klass == 'optical' else new_vel_vector.z
self._queue.insert(
index, ([float('NaN'), float('NaN'), z_vel], msg, klass))
self._reprocess(index)
# Make sure new timestamp is at least 0.1ms newer than the last one
new_stamp = max(
self._last_published_stamp + rospy.Duration(0, 100),
self._queue[-1][1].header.stamp)
last_kalman_i = self._get_last_index('kalman')
if last_kalman_i == -1 or not self._position_passthrough:
self._publish_odom(0.0,
0.0,
0.0,
*self._queue[-1][0],
time=new_stamp)
else:
last_kalman_msg = self._queue[last_kalman_i][1]
self._publish_odom(last_kalman_msg.pose.pose.position.x,
last_kalman_msg.pose.pose.position.y,
last_kalman_msg.pose.pose.position.z,
*self._queue[-1][0],
time=new_stamp)
self._last_published_stamp = new_stamp
while len(self._queue) > self._message_queue_length:
self._queue.pop(0)
def _reprocess(self, index):
for i in xrange(index, len(self._queue)):
msg = self._queue[i][1]
klass = self._queue[i][2]
# Get index of last differential measurement in the queue
last_differential_i = self._get_last_index('kalman',
msg.header.stamp)
assert last_differential_i < i and last_differential_i >= -1
if klass == 'kalman':
if last_differential_i == -1:
# No other differential measurements in the queue, so
# assume our velocity hasn't changed since the previous
dx = 0.0
dy = 0.0
else:
last_diff_vector = \
self._queue[last_differential_i][1].twist.twist.linear
dx = msg.twist.twist.linear.x - last_diff_vector.x
dy = msg.twist.twist.linear.y - last_diff_vector.y
self._queue[i][0][0] = self._queue[i - 1][0][0] + dx
self._queue[i][0][1] = self._queue[i - 1][0][1] + dy
elif klass == 'optical':
self._queue[i][0][0] = (
(1 - self._kalman_weight) * msg.twist.twist.linear.x +
self._kalman_weight * self._queue[i - 1][0][0])
self._queue[i][0][1] = (
(1 - self._kalman_weight) * msg.twist.twist.linear.y +
self._kalman_weight * self._queue[i - 1][0][1])
self._queue[i][0][2] = self._queue[i - 1][0][2]
else:
assert False
def run(self):
self._safety_client = SafetyClient('velocity_filter')
assert self._safety_client.form_bond()
rate = rospy.Rate(15)
while not rospy.is_shutdown():
if (self._safety_client.is_fatal_active()
or self._safety_client.is_safety_active()):
break
rate.sleep()
class MotionPlanner:
def __init__(self, _action_server, update_rate):
self._action_server = _action_server
self._update_rate = update_rate
self._task = None
self._velocity_pub = rospy.Publisher('movement_velocity_targets',
TwistStampedArray,
queue_size=0)
self._in_passthrough = False
self._passthrough_pub = rospy.Publisher('passthrough_command',
OrientationThrottleStamped,
queue_size=10)
self._arm_service = rospy.ServiceProxy('uav_arm', Arm)
self._safety_client = SafetyClient('motion_planner')
self._safety_land_complete = False
self._safety_land_requested = False
self._timeout_vel_sent = False
# Create action client to request a landing
self._action_client = actionlib.SimpleActionClient(
"motion_planner_server",
QuadMoveAction)
# Creates the SimpleActionClient for requesting ground interaction
self._ground_interaction_client = actionlib.SimpleActionClient(
'ground_interaction_action',
GroundInteractionAction)
self._ground_interaction_task_callback = None
self._command_implementations = {
task_commands.VelocityCommand: self._handle_velocity_command,
task_commands.ArmCommand: self._handle_arm_command,
task_commands.NopCommand: self._handle_nop_command,
task_commands.GroundInteractionCommand: self._handle_ground_interaction_command,
task_commands.ConfigurePassthroughMode: self._handle_passthrough_mode_command,
task_commands.AngleThrottleCommand: self._handle_passthrough_command
}
self._time_of_last_task = None
self._last_twist = None
try:
self._task_timeout = rospy.Duration(rospy.get_param('~task_timeout'))
self._task_timeout_deceleration_time = rospy.Duration(rospy.get_param('~task_timeout_deceleration_time'))
except KeyError as e:
rospy.logerr('Could not lookup a parameter for motion_planner')
raise
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 handle_ground_interaction_done(self, status, result):
if self._ground_interaction_task_callback is not None:
try:
self._ground_interaction_task_callback(status, result)
except Exception as e:
rospy.logerr('Exception sending result using ground interaction done cb')
rospy.logerr(str(e))
rospy.logerr(traceback.format_exc())
rospy.logwarn('Motion planner aborted task')
self._action_server.set_aborted()
self._task = None
self._ground_interaction_task_callback = None
else:
rospy.logerr('Ground interaction done callback received with no task callback available')
def _handle_ground_interaction_command(self, ground_interaction_command):
if self._ground_interaction_task_callback is not None:
rospy.logerr('Task requested another ground interaction action before the last completed')
rospy.logwarn('Motion planner aborted task')
self._action_server.set_aborted()
self._task = None
self._ground_interaction_task_callback = None
self._ground_interaction_client.cancel_goal()
self._ground_interaction_client.stop_tracking_goal()
return
self._ground_interaction_task_callback = ground_interaction_command.completion_callback
# Request ground interaction of llm
goal = GroundInteractionGoal(interaction_type=ground_interaction_command.interaction_type)
# Sends the goal to the action server.
self._ground_interaction_client.send_goal(goal, done_cb=self.handle_ground_interaction_done)
def _handle_nop_command(self, nop_command):
pass
def _handle_velocity_command(self, velocity_command):
self._publish_twist(velocity_command.target_twist)
def _handle_arm_command(self, arm_command):
arm_result = self._use_arm_service(arm_command.arm_state, arm_command.set_mode, arm_command.angle)
self._call_tasks_arm_service_callback(arm_command.completion_callback, arm_result)
def _call_tasks_arm_service_callback(self, callback, arm_result):
try:
callback(arm_result)
except Exception as e:
rospy.logerr('Exception setting result using an arm callback')
rospy.logerr(str(e))
rospy.logerr(traceback.format_exc())
rospy.logwarn('Motion planner aborted task')
self._action_server.set_aborted()
self._task = None
def _use_arm_service(self, arm, set_mode, angle):
try:
armed = self._arm_service(arm, set_mode, angle)
except rospy.ServiceException as exc:
rospy.logerr("Could not arm: " + str(exc))
armed = False
return armed
def _handle_passthrough_mode_command(self, passthrough_mode_command):
if passthrough_mode_command.enable:
if self._ground_interaction_task_callback is not None:
rospy.logerr('Task requested a passthrough action before the last was completed')
rospy.logwarn('Motion planner aborted task')
self._action_server.set_aborted()
self._task = None
self._ground_interaction_task_callback = None
self._ground_interaction_client.cancel_goal()
self._ground_interaction_client.stop_tracking_goal()
return
self._ground_interaction_task_callback = passthrough_mode_command.completion_callback
goal = GroundInteractionGoal(interaction_type='passthrough')
self._ground_interaction_client.send_goal(goal, done_cb=self.handle_ground_interaction_done)
self._in_passthrough = True
else:
self._ground_interaction_client.cancel_goal()
self._in_passthrough = False
def _handle_passthrough_command(self, passthrough_command):
if self._in_passthrough:
msg = OrientationThrottleStamped()
msg.header.stamp = rospy.Time.now()
msg.data.pitch = passthrough_command.pitch
msg.data.roll = passthrough_command.roll
msg.data.yaw = passthrough_command.vyaw
msg.throttle = passthrough_command.vz
self._passthrough_pub.publish(msg)
else:
rospy.logerr('Task requested a passthrough command when not in passthrough mode')
def _publish_twist(self, twist):
if type(twist) is TwistStampedArray:
self._last_twist = twist.twists[-1]
self._velocity_pub.publish(twist)
elif type(twist) is TwistStamped:
self._last_twist = twist
velocity_msg = TwistStampedArray()
velocity_msg.twists = [twist]
self._velocity_pub.publish(velocity_msg)
else:
raise TypeError('Illegal type provided in Motion Planner')
def _safety_task_complete_callback(self, status, response):
if response.success:
rospy.logwarn('Motion planner supposedly safely landed the aircraft')
else:
rospy.logerr('Motion planner did not safely land aircraft')
self._safety_land_complete = True
def _get_task_command(self):
if (self._task is None) and self._action_server.has_new_task():
self._task = self._action_server.get_new_task()
if self._task:
self._time_of_last_task = rospy.Time.now()
self._timeout_vel_sent = False
if self._action_server.is_canceled():
try:
self._task.cancel()
except Exception as e:
rospy.logerr('Exception canceling task')
rospy.logerr(str(e))
rospy.logerr(traceback.format_exc())
rospy.logwarn('Motion planner aborted task')
self._action_server.set_aborted()
self._task = None
return (task_commands.NopCommand(),)
try:
task_request = self._task.get_desired_command()
except Exception as e:
rospy.logerr('Exception getting tasks preferred velocity')
rospy.logerr(str(e))
rospy.logerr(traceback.format_exc())
rospy.logwarn('Motion planner aborted task')
self._action_server.set_aborted()
self._task = None
return (task_commands.NopCommand(),)
task_state = task_request[0]
if isinstance(task_state, task_states.TaskCanceled):
self._action_server.set_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
return task_request[1:]
else:
assert isinstance(task_state, task_states.TaskRunning)
return task_request[1:]
else:
# There is no current task running
if self._time_of_last_task is None:
self._time_of_last_task = rospy.Time.now()
if ((rospy.Time.now() - self._time_of_last_task) > self._task_timeout):
if not self._timeout_vel_sent:
commands = TwistStampedArray()
if self._last_twist is None:
self._last_twist = TwistStamped()
self._last_twist.header.stamp = rospy.Time.now()
future_twist = TwistStamped()
future_twist.header.stamp = rospy.Time.now() + self._task_timeout_deceleration_time
commands.twists = [self._last_twist, future_twist]
# If past the timeout send a zero velocity command
self._publish_twist(commands)
self._timeout_vel_sent = True
rospy.logwarn('Task running timeout. Setting zero velocity')
else:
self._timeout_vel_sent = False
# No action to take return a nop
return (task_commands.NopCommand(),)
class TaskManager(object):
def __init__(self, action_server):
self._action_server = action_server
self._lock = threading.RLock()
self._task = None
# construct provided command handler
self._handler = CommandHandler()
# construct safety responder
self._safety_responder = SafetyResponder()
# safety client, used if Safety Enabled
self._safety_client = SafetyClient('task_manager')
try:
# load params
self._safety_enabled = rospy.get_param('~safety_enabled')
self._update_rate = rospy.Rate(rospy.get_param('~update_rate'))
self._timeout = rospy.Duration(rospy.get_param('~startup_timeout'))
self._force_cancel = rospy.get_param('~force_cancel')
except KeyError:
rospy.logfatal('TaskManager: Error getting params')
raise
def run(self):
# this check is needed, as sometimes there are issues with simulated time
while rospy.Time.now() == rospy.Time(0) and not rospy.is_shutdown():
rospy.logwarn('TaskManager: waiting on time')
rospy.sleep(0.005)
if rospy.is_shutdown():
raise rospy.ROSInterruptException()
# check to make sure all dependencies are ready
if not self._wait_until_ready(self._timeout):
raise rospy.ROSInitException()
# forming bond with safety client, if enabled
if self._safety_enabled and not self._safety_client.form_bond():
raise IARCFatalSafetyException(
'TaskManager: could not form bond with safety client')
while not rospy.is_shutdown():
# main loop
with self._lock:
if self._safety_enabled and self._safety_client.is_fatal_active(
):
raise IARCFatalSafetyException('TaskManager: Safety Fatal')
if self._safety_enabled and self._safety_client.is_safety_active(
):
rospy.logerr('TaskManager: Activating safety response')
self._safety_responder.activate_safety_response()
return
if self._task is None and self._action_server.has_new_task():
# new task is available
try:
request = self._action_server.get_new_task()
# use provided handler to check that request is valid
new_task = self._handler.check_request(request)
if new_task is not None:
self._task = new_task
self._action_server.set_accepted()
else:
rospy.logerr('TaskManager: new task is invalid')
self._action_server.set_rejected()
except Exception as e:
rospy.logfatal(
'TaskManager: Exception getting new task')
raise
if self._task is not None and self._action_server.task_canceled(
):
# there is a task running, but it is canceled
try:
success = self._task.cancel()
if not success:
rospy.logwarn(
'TaskManager: task refusing to cancel')
if self._force_cancel:
rospy.logwarn(
'TaskManager: forcing task cancel')
self._action_server.set_canceled()
self._task = None
elif success:
self._action_server.set_canceled()
self._task = None
except Exception as e:
rospy.logfatal('TaskManager: Error canceling task')
raise
if self._task is not None:
# can get next command and task state
try:
state, command = self._task.get_desired_command()
except Exception as e:
rospy.logerr(
'TaskManager: Error getting command from task. Aborting task'
)
rospy.logerr(str(e))
self._action_server.set_aborted()
self._task = None
try:
task_state = self._handler.handle(command, state)
except Exception as e:
# this is a fatal error, as the handler should be able to
# handle commands of valid tasks without raising exceptions
rospy.logfatal(
'TaskManager: Error handling task command')
rospy.logfatal(str(e))
raise
if task_state == TaskHandledState.ABORTED:
rospy.logerr('TaskManager: aborting task')
self._action_server.set_aborted()
self._task = None
elif task_state == TaskHandledState.DONE:
rospy.loginfo(
'TaskManager: task has completed cleanly')
self._action_server.set_succeeded()
self._task = None
elif task_state == TaskHandledState.FAILED:
rospy.logerr('TaskManager: Task failed')
self._action_server.set_failed()
self._task = None
elif task_state == TaskHandledState.OKAY:
rospy.loginfo_throttle(1, 'TaskManager: task running')
else:
raise IARCFatalSafetyException(
'TaskManager: invalid task handled state returned from handler: '
+ str(task_state))
self._update_rate.sleep()
def _wait_until_ready(self, timeout):
# wait until dependencies are ready
try:
self._handler.wait_until_ready(timeout)
self._safety_responder.wait_until_ready(timeout)
except Exception:
rospy.logfatal('TaskManager: Error waiting for dependencies')
raise
return True
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()))
class MotionCommandCoordinator(object):
def __init__(self, action_server):
# action server for getting requests from AI
self._action_server = action_server
# current state of motion coordinator
self._task = None
self._first_task_seen = False
# used for timeouts & extrapolating
self._time_of_last_task = None
self._timeout_vel_sent = False
# to keep things thread safe
self._lock = threading.RLock()
# handles monitoring of state of drone
self._state_monitor = StateMonitor()
# handles communicating between tasks and LLM
self._task_command_handler = TaskCommandHandler()
self._idle_obstacle_avoider = IdleObstacleAvoider()
self._avoid_magnitude = rospy.get_param("~obst_avoid_magnitude")
self._kickout_distance = rospy.get_param('~kickout_distance')
self._new_task_distance = rospy.get_param('~new_task_distance')
self._safe_distance = rospy.get_param('~safe_distance')
# safety
self._safety_client = SafetyClient('motion_command_coordinator')
self._safety_land_complete = False
self._safety_land_requested = False
# Create action client to request a safety landing
self._action_client = actionlib.SimpleActionClient(
"motion_planner_server", QuadMoveAction)
try:
# update rate for motion coordinator
self._update_rate = rospy.get_param('~update_rate')
# task timeout values
self._task_timeout = rospy.Duration(
rospy.get_param('~task_timeout'))
# startup timeout
self._startup_timeout = rospy.Duration(
rospy.get_param('~startup_timeout'))
except KeyError as e:
rospy.logerr('Could not lookup a parameter for motion coordinator')
raise
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()
# fills out the Intermediary State for the task
def _get_current_transition(self):
state = TransitionData()
state.last_twist = self._task_command_handler.get_last_twist()
state.last_task_ending_state = self._task_command_handler.get_state()
state.timeout_sent = self._timeout_vel_sent
return self._state_monitor.fill_out_transition(state)
# callback for safety task completition
def _safety_task_complete_callback(self, status, response):
with self._lock:
if response.success:
rospy.logwarn(
'Motion Coordinator supposedly safely landed the aircraft')
else:
rospy.logerr('Motion Coordinator did not safely land aircraft')
self._safety_land_complete = True
class OrientationFilter(object):
def __init__(self):
rospy.init_node('orientation_filter')
self._lock = threading.Lock()
with self._lock:
self._orientation_sub = rospy.Subscriber(
'fc_orientation', OrientationAnglesStamped,
lambda msg: self._callback(OrientationAnglesStamped, msg))
# This topic is assumed to have rotation around +z
# (angle from +x with +y being +pi/2)
self._line_yaw_sub = rospy.Subscriber(
'line_yaw', Float64Stamped,
lambda msg: self._callback(Float64Stamped, msg))
self._debug_orientation_pub = rospy.Publisher(
'orientation_filter/debug_orientation',
OrientationAnglesStamped,
queue_size=10)
self._line_weight = rospy.get_param('~line_weight')
self._message_queue_length = rospy.get_param(
'~message_queue_length')
initial_msg = Float64Stamped()
initial_msg.header.stamp = rospy.Time()
initial_msg.data = rospy.get_param('~initial_yaw', float('NaN'))
# This queue contains pairs, sorted by timestamp, oldest at index 0
#
# First item of each pair is a 3-tuple containing:
# yaw (rotation around +z)
# pitch (rotation around +y')
# roll (rotation around +x'')
# Second item of each pair is the message itself
#
# The yaw in the 3-tuple is the current value of the filter at that
# point, the pitch and roll are too except that they're simply
# propogated forward from the last OrientationAnglesStamped message
self._queue = [([initial_msg.data,
float('NaN'),
float('NaN')], initial_msg)]
self._transform_broadcaster = tf2_ros.TransformBroadcaster()
self._last_published_stamp = rospy.Time(0)
def _publish_transform(self, r, p, y, time):
transform_msg = TransformStamped()
transform_msg.header.stamp = time
transform_msg.header.frame_id = 'level_quad'
transform_msg.child_frame_id = 'quad'
quaternion = tf.transformations.quaternion_from_euler(r, p, y)
transform_msg.transform.rotation.x = quaternion[0]
transform_msg.transform.rotation.y = quaternion[1]
transform_msg.transform.rotation.z = quaternion[2]
transform_msg.transform.rotation.w = quaternion[3]
self._transform_broadcaster.sendTransform(transform_msg)
transform_msg.header.stamp = time
transform_msg.header.frame_id = 'level_quad'
transform_msg.child_frame_id = 'heading_quad'
quaternion = tf.transformations.quaternion_from_euler(0, 0, y)
transform_msg.transform.rotation.x = quaternion[0]
transform_msg.transform.rotation.y = quaternion[1]
transform_msg.transform.rotation.z = quaternion[2]
transform_msg.transform.rotation.w = quaternion[3]
self._transform_broadcaster.sendTransform(transform_msg)
orientation_message = OrientationAnglesStamped()
orientation_message.header.stamp = time
orientation_message.data.pitch = p
orientation_message.data.roll = r
orientation_message.data.yaw = y
self._debug_orientation_pub.publish(orientation_message)
def _get_last_index(self, klass, time=None):
'''
Return the index of the last message of type `klass` (or the
last message of type `klass` before `time` if `time` is
specified)
Returns `-1` if there are no messages of type `klass` in the
queue
'''
for i in xrange(len(self._queue) - 1, -1, -1):
if (type(self._queue[i][1]) == klass and
(time is None or self._queue[i][1].header.stamp < time)):
return i
return -1
def _callback(self, klass, msg):
with self._lock:
last_i = self._get_last_index(klass)
if (last_i != -1 and
msg.header.stamp <= self._queue[last_i][1].header.stamp):
# This message isn't newer than the last one of type klass
rospy.logwarn((
'Ignoring message of type {} with timestamp {} older than '
+ 'previous message with timestamp {}').format(
klass, msg.header.stamp,
self._queue[last_i][1].header.stamp))
return
for i in xrange(len(self._queue) - 1, -1, -1):
if self._queue[i][1].header.stamp < msg.header.stamp:
index = i + 1
break
else:
index = 0
data = [float('NaN')] * 3
try:
data[1] = -msg.data.pitch
data[2] = msg.data.roll
except AttributeError:
# If this message type doesn't have pitch and roll, skip it
pass
self._queue.insert(index, (data, msg))
self._reprocess(index)
if (not math.isnan(self._queue[-1][0][0])
and not math.isnan(self._queue[-1][0][1])
and not math.isnan(self._queue[-1][0][2])):
# Make sure new timestamp is at least 1us newer than the
# last one. Using 1ns was dangerous and caused the tf library
# to create transforms with NaN values
new_stamp = max(
self._last_published_stamp + rospy.Duration(0, 1000),
self._queue[-1][1].header.stamp)
self._publish_transform(*reversed(self._queue[-1][0]),
time=new_stamp)
self._last_published_stamp = new_stamp
while len(self._queue) > self._message_queue_length:
self._queue.pop(0)
def _reprocess(self, index):
for i in xrange(index, len(self._queue)):
last_yaw = self._queue[i - 1][0][0]
msg = self._queue[i][1]
last_orientation_i = self._get_last_index(OrientationAnglesStamped,
msg.header.stamp)
if type(msg) == OrientationAnglesStamped:
if last_orientation_i == -1:
dyaw = 0
else:
dyaw = -(msg.data.yaw -
self._queue[last_orientation_i][1].data.yaw)
if dyaw > math.pi:
dyaw -= 2 * math.pi
elif dyaw < -math.pi:
dyaw += 2 * math.pi
self._queue[i][0][0] = last_yaw + dyaw
elif type(msg) == Float64Stamped:
new_yaw = msg.data
if new_yaw - last_yaw > math.pi:
new_yaw -= 2 * math.pi
elif new_yaw - last_yaw < -math.pi:
new_yaw += 2 * math.pi
# Skip the filtering step if we don't already have a valid
# measurement
if math.isnan(last_yaw):
self._queue[i][0][0] = new_yaw
else:
self._queue[i][0][0] = (self._line_weight * new_yaw +
(1 - self._line_weight) * last_yaw)
if last_orientation_i != -1:
self._queue[i][0][1] = self._queue[last_orientation_i][0][
1]
self._queue[i][0][2] = self._queue[last_orientation_i][0][
2]
else:
assert False
self._queue[i][0][0] += 2 * math.pi
self._queue[i][0][0] %= 2 * math.pi
def run(self):
self._safety_client = SafetyClient('orientation_filter')
assert self._safety_client.form_bond()
rate = rospy.Rate(15)
while not rospy.is_shutdown():
if (self._safety_client.is_fatal_active()
or self._safety_client.is_safety_active()):
break
rate.sleep()
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()))
from iarc7_safety.SafetyClient import SafetyClient
from iarc7_msgs.msg import OrientationThrottleStamped
from iarc7_msgs.msg import BoolStamped
from iarc7_msgs.srv import Arm
from geometry_msgs.msg import TwistStamped
if __name__ == '__main__':
rospy.init_node('waypoints_test', anonymous=True)
rospy.wait_for_service('uav_arm')
arm_service = rospy.ServiceProxy('uav_arm', Arm)
rospy.logerr('done waiting')
safety_client = SafetyClient('motors_test')
safety_client.form_bond()
command_pub = rospy.Publisher('uav_direction_command',
OrientationThrottleStamped,
queue_size=0)
armed = False
while armed == False:
try:
rospy.logerr('Trying to arm')
armed = arm_service(True, True, False)
except rospy.ServiceException as exc:
rospy.logerr('Could not arm: ' + str(exc))
rospy.logerr('Armed')
rate = rospy.Rate(30)
throttle = 0
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 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")
class CrazyflieFcComms:
def __init__(self):
self._uav_command = None
self._connected = False
self._timestamp_offset = None
self._armed = False
self._cf = None
self._commands_allowed = True
self._uav_command = None
self._vbat = None
self._crash_detected = False
self._crash_landed = False
# safety
self._safety_client = SafetyClient('fc_comms_crazyflie')
# Publishers for FC sensors
self._battery_publisher = rospy.Publisher('fc_battery',
Float64Stamped,
queue_size=5)
self._status_publisher = rospy.Publisher('fc_status',
FlightControllerStatus,
queue_size=5)
self._imu_publisher = rospy.Publisher('fc_imu', Imu, queue_size=5)
self._orientation_pub = rospy.Publisher('fc_orientation',
OrientationAnglesStamped,
queue_size=5)
self._velocity_pub = rospy.Publisher('optical_flow_estimator/twist',
TwistWithCovarianceStamped,
queue_size=5)
self._range_pub = rospy.Publisher('short_distance_lidar',
Range,
queue_size=5)
self._switch_pub = rospy.Publisher('landing_gear_contact_switches',
LandingGearContactsStamped,
queue_size=5)
self._mag_pub = rospy.Publisher('fc_mag', MagneticField, queue_size=5)
# Subscriber for uav_angle values
self._uav_angle_subscriber = rospy.Subscriber(
'uav_direction_command', OrientationThrottleStamped,
self._uav_command_handler)
# Service to arm copter
self._uav_arm_service = rospy.Service('uav_arm', Arm,
self._arm_service_handler)
self._current_height = 0.0
self._over_height_counts = 0
def run(self):
# Initialize the low-level drivers (don't list the debug drivers)
cflib.crtp.init_drivers(enable_debug_driver=False)
scan_rate = rospy.Rate(2)
while not rospy.is_shutdown():
# Use first Crazyflie found
rospy.loginfo('Crazyflie FC Comms scanning for Crazyflies')
crazyflies = cflib.crtp.scan_interfaces()
if len(crazyflies) != 0:
break
rospy.logerr(
'Crazyflie FC Comms could not find Crazyflie. Trying again.')
scan_rate.sleep()
uri = crazyflies[0][0]
self._cf = Crazyflie()
self._cf.connected.add_callback(self._connection_made)
self._cf.disconnected.add_callback(self._disconnected)
self._cf.connection_failed.add_callback(self._connection_failed)
self._cf.connection_lost.add_callback(self._connection_lost)
rospy.loginfo('Crazyflie FC Comms connecting to: {}'.format(uri))
self._cf.open_link(uri)
connected_check_rate = rospy.Rate(2)
while not self._connected and not rospy.is_shutdown():
connected_check_rate.sleep()
pass
rospy.loginfo('Crazyflie FC Comms reseting kalmann filter')
self._cf.param.set_value('kalman.resetEstimation', '1')
time.sleep(0.1)
self._cf.param.set_value('kalman.resetEstimation', '0')
time.sleep(2)
fast_log_stab = LogConfig(name='high_update_rate', period_in_ms=20)
medium_log_stab = LogConfig(name='medium_update_rate', period_in_ms=30)
slow_log_stab = LogConfig(name='slow_update_rate', period_in_ms=100)
fast_log_stab.add_variable('acc.x', 'float')
fast_log_stab.add_variable('acc.y', 'float')
fast_log_stab.add_variable('acc.z', 'float')
fast_log_stab.add_variable('stabilizer.roll', 'float')
fast_log_stab.add_variable('stabilizer.pitch', 'float')
fast_log_stab.add_variable('stabilizer.yaw', 'float')
# kalman_states.ox and kalman_states.oy are also available
# and provide position estimates
medium_log_stab.add_variable('kalman_states.vx', 'float')
medium_log_stab.add_variable('kalman_states.vy', 'float')
medium_log_stab.add_variable('range.zrange', 'uint16_t')
slow_log_stab.add_variable('pm.vbat', 'float')
slow_log_stab.add_variable('mag.x', 'float')
slow_log_stab.add_variable('mag.y', 'float')
slow_log_stab.add_variable('mag.z', 'float')
try:
self._cf.log.add_config(fast_log_stab)
self._cf.log.add_config(medium_log_stab)
self._cf.log.add_config(slow_log_stab)
fast_log_stab.data_received_cb.add_callback(
self._receive_crazyflie_data)
medium_log_stab.data_received_cb.add_callback(
self._receive_crazyflie_data)
slow_log_stab.data_received_cb.add_callback(
self._receive_crazyflie_data)
fast_log_stab.error_cb.add_callback(self._receive_crazyflie_error)
medium_log_stab.error_cb.add_callback(
self._receive_crazyflie_error)
slow_log_stab.error_cb.add_callback(self._receive_crazyflie_error)
fast_log_stab.start()
medium_log_stab.start()
slow_log_stab.start()
except KeyError as e:
rospy.logerr('Crazyflie FC Comms could not start logging,'
'{} not found'.format(str(e)))
except AttributeError as e:
rospy.logerr(
'Crazyflie FC Comms could not finishing configuring logging: {}'
.format(str(e)))
rospy.loginfo("Crazyflie FC Comms finished configured logs")
# Publish a landing gear contact message so that the landing detector can start up
switch_msg = LandingGearContactsStamped()
switch_msg.front = True
switch_msg.back = True
switch_msg.left = True
switch_msg.right = True
self._switch_pub.publish(switch_msg)
rospy.loginfo('Crazyflie FC Comms trying to form bond')
# forming bond with safety client
if not self._safety_client.form_bond():
raise IARCFatalSafetyException(
'Crazyflie FC Comms could not form bond with safety client')
rospy.loginfo('Crazyflie FC Comms done forming bond')
# Update at the same rate as low level motion
rate = rospy.Rate(60)
while not rospy.is_shutdown() and self._connected:
status = FlightControllerStatus()
status.header.stamp = rospy.Time.now()
status.armed = self._armed
status.auto_pilot = True
status.failsafe = False
self._status_publisher.publish(status)
if self._safety_client.is_fatal_active():
raise IARCFatalSafetyException('Safety Client is fatal active')
if self._safety_client.is_safety_active():
rospy.logwarn_throttle(
1.0, 'Crazyflie FC Comms activated safety response')
self._commands_allowed = False
self._cf.commander.send_setpoint(0, 0, 0, 0)
if self._crash_detected:
rospy.logwarn_throttle(1.0,
'Crazyflie FC Comms detected crash')
self._commands_allowed = False
if self._current_height > 0.15 and not self._crash_landed:
self._cf.commander.send_setpoint(
0, 0, 0, 0.47 * 65535) # Max throttle is 65535
else:
self._crash_landed = True
rospy.logwarn_throttle(
1.0, 'Crazyflie FC Comms shut down motors')
self._cf.commander.send_setpoint(0, 0, 0, 0)
if self._armed and self._commands_allowed:
if self._uav_command is None:
# Keep the connection alive
self._cf.commander.send_setpoint(0, 0, 0, 0)
else:
throttle = max(10000.0, self._uav_command.throttle *
65535.0) # Max throttle is 65535
self._cf.commander.send_setpoint(
self._uav_command.data.roll * 180.0 / math.pi,
self._uav_command.data.pitch * 180.0 / math.pi,
-1.0 * self._uav_command.data.yaw * 180.0 / math.pi,
throttle)
rate.sleep()
self._cf.commander.send_setpoint(0, 0, 0, 0)
# Give time for buffer to flush
time.sleep(0.1)
self._cf.close_link()
# Always return success because making sure the crazyflie is
# armed correctly isn't critical
def _arm_service_handler(self, arm_request):
if self._armed and not arm_request.data:
self._cf.commander.send_setpoint(0, 0, 0, 0)
self._armed = False
return ArmResponse(success=True)
elif not self._armed and arm_request.data:
if self._vbat < 3.55 or self._vbat is None:
rospy.logerr(
'Crazyflie FC Comms refusing to arm, battery to low or not received'
)
return ArmResponse(success=False)
else:
self._cf.commander.send_setpoint(0, 0, 0, 0)
self._armed = True
return ArmResponse(success=True)
# State is the same as it was before. So simply return true.
return ArmResponse(success=True)
def _uav_command_handler(self, data):
self._uav_command = data
def _receive_crazyflie_data(self, timestamp, data, logconf):
# Catch all exceptions because otherwise the crazyflie
# library catches them all silently
try:
stamp = self._calculate_timestamp(timestamp)
if logconf.name == 'high_update_rate':
imu = Imu()
imu.header.stamp = stamp
imu.header.frame_id = 'quad'
imu.linear_acceleration.x = data['acc.x'] * 9.81
imu.linear_acceleration.y = data['acc.y'] * 9.81
imu.linear_acceleration.z = data['acc.z'] * 9.81
imu.orientation_covariance[0] = -1
imu.angular_velocity_covariance[0] = -1
variance = 6.0
imu.linear_acceleration_covariance[0] = variance
imu.linear_acceleration_covariance[4] = variance
imu.linear_acceleration_covariance[8] = variance
self._imu_publisher.publish(imu)
orientation = OrientationAnglesStamped()
orientation.header.stamp = stamp
orientation.data.roll = data[
'stabilizer.roll'] * math.pi / 180.0
orientation.data.pitch = data[
'stabilizer.pitch'] * math.pi / 180.0
# Zero out the yaw for now. Crazyflie has a problem with yaw calculation
# and having correct yaw is not critical for this target yet.
orientation.data.yaw = -0.0 * data[
'stabilizer.yaw'] * math.pi / 180.0
self._orientation_pub.publish(orientation)
if abs(orientation.data.roll) > 1.5 or abs(
orientation.data.pitch) > 1.5:
rospy.logwarn(
'Crazyflie FC Comms maximimum attitude angle exceeded')
self._crash_detected = True
elif logconf.name == 'medium_update_rate':
twist = TwistWithCovarianceStamped()
twist.header.stamp = stamp - rospy.Duration.from_sec(0.020)
twist.header.frame_id = 'heading_quad'
twist.twist.twist.linear.x = data['kalman_states.vx']
twist.twist.twist.linear.y = data['kalman_states.vy']
twist.twist.twist.linear.z = 0.0
variance = 0.0001
twist.twist.covariance[0] = variance
twist.twist.covariance[7] = variance
self._velocity_pub.publish(twist)
range_msg = Range()
range_msg.header.stamp = stamp - rospy.Duration.from_sec(0.050)
range_msg.radiation_type = Range.INFRARED
range_msg.header.frame_id = '/short_distance_lidar'
range_msg.field_of_view = 0.3
range_msg.min_range = 0.01
range_msg.max_range = 1.6
self._current_height = data['range.zrange'] / 1000.0
range_msg.range = self._current_height
if range_msg.range > 2.0:
self._over_height_counts = self._over_height_counts + 1
else:
self._over_height_counts = 0
if self._over_height_counts > 10:
rospy.logwarn(
'Crazyflie FC Comms maximimum height exceeded')
self._crash_detected = True
self._range_pub.publish(range_msg)
switch_msg = LandingGearContactsStamped()
switch_msg.header.stamp = stamp
pressed = data['range.zrange'] < 50
switch_msg.front = pressed
switch_msg.back = pressed
switch_msg.left = pressed
switch_msg.right = pressed
self._switch_pub.publish(switch_msg)
elif logconf.name == 'slow_update_rate':
battery = Float64Stamped()
battery.header.stamp = stamp
battery.data = data['pm.vbat']
self._vbat = data['pm.vbat']
self._battery_publisher.publish(battery)
mag = MagneticField()
mag.header.stamp = stamp
mag.magnetic_field.x = data['mag.x']
mag.magnetic_field.y = data['mag.y']
mag.magnetic_field.z = data['mag.z']
self._mag_pub.publish(mag)
else:
rospy.logerr(
'Crazyflie FC Comms received message block with unkown name'
)
except Exception as e:
rospy.logerr("Crazyflie FC Comms error receiving data: {}".format(
str(e)))
rospy.logerr(traceback.format_exc())
def _calculate_timestamp(self, timestamp):
if self._timestamp_offset is None:
self._timestamp_offset = (rospy.Time.now(), timestamp)
stamp = self._timestamp_offset[0] + rospy.Duration.from_sec(
(float(timestamp - self._timestamp_offset[1]) / 1000.0) - 0.025)
return stamp
def _receive_crazyflie_error(self, logconf, msg):
rospy.logerr('Crazyflie FC Comms error: {}, {}'.format(
logconf.name, msg))
def _connection_made(self, uri):
rospy.loginfo('Crazyflie FC Comms connected to {}'.format(uri))
self._connected = True
def _connection_failed(self, uri, msg):
rospy.logerr('Crazyflie FC Comms connection {} failed: {}'.format(
uri, msg))
self._connected = False
def _connection_lost(self, uri, msg):
rospy.logerr('Crazyflie FC Comms connection to {} lost: {}'.format(
uri, msg))
self._connected = False
def _disconnected(self, uri):
rospy.loginfo('Crazyflie FC Comms disconnected from {}'.format(uri))
self._connected = False
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()))
