def get_desired_command(self):
with self._lock:
if self._canceled:
return (TaskCanceled(), )
if self._state == GoToRoombaState.init:
if (not self.topic_buffer.has_roomba_message()
or not self.topic_buffer.has_odometry_message()):
self._state = GoToRoombaState.waiting
else:
self._state = GoToRoombaState.translate
if self._state == GoToRoombaState.waiting:
if (not self.topic_buffer.has_roomba_message()
or not self.topic_buffer.has_odometry_message()):
return (TaskRunning(), NopCommand())
else:
self._state = GoToRoombaState.translate
if self._state == GoToRoombaState.translate:
try:
transStamped = self.topic_buffer.get_tf_buffer(
).lookup_transform('map', 'quad', rospy.Time(0),
rospy.Duration(self._TRANSFORM_TIMEOUT))
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException) as ex:
rospy.logerr(
'GoToRoombaTask: Exception when looking up transform')
rospy.logerr(ex.message)
return (TaskAborted(
msg=
'Exception when looking up transform during go_to_roomba'
), )
if (transStamped.transform.translation.z >
self._MIN_MANEUVER_HEIGHT):
if not self._check_roomba_in_sight():
return (TaskAborted(
msg='The provided roomba is not in sight of quad'),
)
roomba_x = self._roomba_odometry.pose.pose.position.x
roomba_y = self._roomba_odometry.pose.pose.position.y
self._path_holder.reinit_translation_stop_planner(
roomba_x, roomba_y, self._z_position)
hold_twist = self._path_holder.get_xyz_hold_response()
if not self._path_holder.is_done():
return (TaskRunning(), VelocityCommand(hold_twist))
else:
return (TaskDone(), VelocityCommand(hold_twist))
else:
return (TaskFailed(
msg=
'Fell below minimum manuever height during translation'
), )
return (TaskAborted(
msg='Impossible state in go to roomba task reached'))
def get_desired_command(self):
if self._state == LandTaskState.init:
# Change state to land
self._state = LandTaskState.land
return (TaskRunning(),
GroundInteractionCommand('land', self.land_callback))
# Enter the landing phase
if self._state == LandTaskState.land:
return (TaskRunning(), NopCommand())
# Change state to done
if self._state == LandTaskState.done:
return (TaskDone(), NopCommand())
# Change state to failed
if self._state == LandTaskState.failed:
rospy.logerr('Low level motion failed landing sequence')
return (TaskFailed(msg='Low level motion failed landing sequence'),
NopCommand())
# Cancel the landing
if self._state == LandTaskState.cancelling:
return (TaskRunning(),
GroundInteractionCommand('cancel_land',
self.land_callback))
# Tell linear profile to get to a recovery height
if self._state == LandTaskState.recovering:
rospy.loginfo('LandTask is in recovering state')
odometry = self.topic_buffer.get_odometry_message()
# time to deccelerate to recover height
if odometry.pose.pose.position.z > self._RECOVERY_HEIGHT:
self._state = LandTaskState.cancelled
return (TaskRunning(),
VelocityCommand(
acceleration=self._RECOVERY_ACCELERATION))
# time to accelerate up
else:
velocity = TwistStamped()
velocity.header.stamp = rospy.Time.now()
velocity.twist.linear.z = self._RECOVERY_VELOCITY
return (TaskRunning(),
VelocityCommand(
target_twist=velocity,
start_position_z=odometry.pose.pose.position.z,
start_velocity_x=0.0,
start_velocity_y=0.0,
start_velocity_z=odometry.twist.twist.linear.z,
acceleration=self._RECOVERY_ACCELERATION))
if self._state == LandTaskState.cancelled:
rospy.loginfo('LandTask is cancelled and ready for next task')
return (TaskCanceled(), )
# Impossible state reached
return (TaskAborted(msg='Impossible state in takeoff task reached'))
def get_desired_command(self):
with self._lock:
if (self._state == HoldPositionTaskStates.init
or self._state == HoldPositionTaskStates.waiting):
if self._drone_odometry is None:
self._state = HoldPositionTaskStates.waiting
return (TaskRunning(), NopCommand())
self._set_targets()
self._state = HoldPositionTaskStates.holding
if not self._check_max_error():
return (TaskAborted(msg='Desired position is too far away, task not for translation.'),)
return (TaskRunning(), NopCommand())
elif (self._state == HoldPositionTaskStates.holding):
if not (self._height_checker.above_min_maneuver_height(
self._drone_odometry.pose.pose.position.z)):
return (TaskAborted(msg='Drone is too low'),)
if not (self._check_max_error()):
return (TaskAborted(msg='Error from desired point is too great'),)
if self._canceled:
return (TaskCanceled(),)
# p-controller
x_vel_target = ((self._x_position - self._drone_odometry.pose.pose.position.x)
* self._K_X)
y_vel_target = ((self._y_position - self._drone_odometry.pose.pose.position.y)
* self._K_Y)
z_vel_target = ((self._z_position - self._drone_odometry.pose.pose.position.z)
* self._K_Z)
#caps velocity
vel_target = math.sqrt(x_vel_target**2 + y_vel_target**2)
if vel_target > self._MAX_TRANSLATION_SPEED:
x_vel_target = x_vel_target * (self._MAX_TRANSLATION_SPEED/vel_target)
y_vel_target = y_vel_target * (self._MAX_TRANSLATION_SPEED/vel_target)
if (abs(z_vel_target) > self._MAX_Z_VELOCITY):
z_vel_target = z_vel_target/abs(z_vel_target) * self._MAX_Z_VELOCITY
velocity = TwistStamped()
velocity.header.frame_id = 'level_quad'
velocity.header.stamp = rospy.Time.now()
velocity.twist.linear.x = x_vel_target
velocity.twist.linear.y = y_vel_target
velocity.twist.linear.z = z_vel_target
return (TaskRunning(), VelocityCommand(velocity))
else:
return (TaskAborted(msg='Illegal State Reached'),)
def get_desired_command(self):
if self._canceled:
return (TaskCanceled(), )
if self._state == LandTaskState.init:
# Check if we have a fc status
if self._fc_status is None:
return (TaskRunning(), NopCommand())
# Check that auto pilot is enabled
if not self._fc_status.auto_pilot:
return (TaskFailed(
msg='flight controller not allowing auto pilot'), )
# Check that the FC is not already armed
if not self._fc_status.armed:
return (TaskFailed(
msg=
'flight controller is not armed, how are we in the air??'),
)
# All is good change state to land
self._state = LandTaskState.land
return (TaskRunning(),
GroundInteractionCommand('land', self.land_callback))
# Enter the takeoff phase
if self._state == LandTaskState.land:
return (TaskRunning(), NopCommand())
# Enter the disarming request stage
if self._state == LandTaskState.disarm:
# Check if disarm succeeded
if self._disarm_request_success:
return (TaskDone(), NopCommand())
else:
return (TaskRunning(),
ArmCommand(False, True, False, self.disarm_callback))
# Enter the disarming request stage
if self._state == LandTaskState.failed:
# Check if disarm succeeded
if self._disarm_request_success:
return (TaskFailed(), NopCommand())
else:
rospy.logerr(
'Low level motion action failed, disarming then exiting')
return (TaskRunning(),
ArmCommand(False, True, False, self.disarm_callback))
# Impossible state reached
return (TaskAborted(msg='Impossible state in takeoff task reached'))
def get_desired_command(self):
if self._canceled:
return (TaskCanceled(),)
elif self._state == HeightRecoveryTaskState.init:
self._state = HeightRecoveryTaskState.recover
return (TaskRunning(),)
elif (self._state == HeightRecoveryTaskState.recover or
self._state == HeightRecoveryTaskState.recover_angle):
try:
transStamped = self._tf_buffer.lookup_transform(
'map',
'base_footprint',
rospy.Time.now(),
rospy.Duration(self._TRANSFORM_TIMEOUT))
except (tf2_ros.LookupException,
tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException) as ex:
msg = 'Exception when looking up transform during height recovery'
rospy.logerr('HeightRecoveryTask: {}'.format(msg))
rospy.logerr(ex.message)
return (TaskAborted(msg=msg),)
# Check if we reached the target height
if (transStamped.transform.translation.z > self._RECOVERY_HEIGHT):
self._state = HeightRecoveryTaskState.done
return (TaskDone(), NopCommand())
elif (self._state == HeightRecoveryTaskState.recover and
transStamped.transform.translation.z > self._ANGLE_MODE_HEIGHT):
self._state = HeightRecoveryTaskState.recover_angle
return (TaskRunning(), ArmCommand(True, True, True, lambda _ : None))
else:
velocity = TwistStamped()
velocity.header.frame_id = 'level_quad'
velocity.header.stamp = rospy.Time.now()
velocity.twist.linear.z = self._TAKEOFF_VELOCITY
return (TaskRunning(), VelocityCommand(velocity))
elif self._state == HeightRecoveryTaskState.done:
return (TaskDone(), NopCommand())
elif self._state == HeightRecoveryTaskState.failed:
return (TaskFailed(msg='HeightRecoveryTask experienced failure'),)
else:
return (TaskAborted(msg='Impossible state in HeightRecoveryTask reached'),)
def get_desired_command(self):
if self._canceled:
return (TaskCanceled(), )
if self._state == XYZTranslationTaskState.init:
self._state = XYZTranslationTaskState.translate
if self._state == XYZTranslationTaskState.translate:
try:
transStamped = self.topic_buffer.get_tf_buffer(
).lookup_transform('map', 'quad', rospy.Time(0),
rospy.Duration(self._TRANSFORM_TIMEOUT))
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException) as ex:
rospy.logerr(
'XYZTranslation Task: Exception when looking up transform')
rospy.logerr(ex.message)
return (TaskAborted(
msg=
'Exception when looking up transform during xyztranslation'
), )
if (transStamped.transform.translation.z >
self._MIN_MANEUVER_HEIGHT):
hold_twist = self._path_holder.get_xyz_hold_response()
if not self._path_holder.is_done():
return (TaskRunning(), VelocityCommand(hold_twist))
else:
return (TaskDone(), VelocityCommand(hold_twist))
else:
return (TaskFailed(
msg='Fell below minimum manuever height during translation'
), )
return (TaskAborted(msg='Impossible state in takeoff task reached'))
def get_desired_command(self):
goal = PlanGoal()
goal.goal.motion_point.pose.position.x = 5
goal.goal.motion_point.pose.position.y = 5
self._client.send_goal(goal, None, None, self._feedback_callback)
if self._plan_canceled:
return (TaskDone(), NopCommand())
else:
return (TaskRunning(), )
def get_desired_command(self):
with self._lock:
if self._canceled:
return (TaskCanceled(), )
if (self._state == JoystickVelocityTaskState.init):
if not self.topic_buffer.has_odometry_message():
self._state = JoystickVelocityTaskState.waiting
else:
self._state = JoystickVelocityTaskState.moving
if (self._state == JoystickVelocityTaskState.waiting):
if not self.topic_buffer.has_odometry_message():
return (TaskRunning(), NopCommand())
else:
self._state = JoystickVelocityTaskState.moving
if self._state == JoystickVelocityTaskState.moving:
predicted_motion_point = self._linear_motion_profile_generator.expected_point_at_time(
rospy.Time.now())
odometry = self.topic_buffer.get_odometry_message()
current_height = odometry.pose.pose.position.z
if not (self._height_checker.above_min_maneuver_height(
current_height)):
return (TaskAborted(msg='Drone is too low'), )
velocity = TwistStamped()
velocity.header.frame_id = 'level_quad'
velocity.header.stamp = rospy.Time.now()
velocity.twist.linear.x = self._x_vel
velocity.twist.linear.y = self._y_vel
velocity.twist.linear.z = self._z_vel
velocity_command = VelocityCommand(velocity)
return (TaskRunning(), velocity_command)
return (TaskAborted(
msg='Illegal state reached in JoystickVelocityTask'), )
def get_desired_command(self):
if self.target is None:
self.target = rospy.Time.now() + rospy.Duration(1.5)
self.abort_time = rospy.Time.now() + rospy.Duration(0.75)
result = self.target - rospy.Time.now()
if self.abort > 0.5:
if self.abort_time < rospy.Time.now():
rospy.loginfo("TestTask aborted")
return (TaskAborted(), result)
if self.canceled:
rospy.loginfo("TestTask canceled")
return (TaskCanceled(), )
if self.target < rospy.Time.now():
rospy.loginfo("TestTask done")
return (TaskDone(), result)
else:
rospy.loginfo("TestTask not done")
return (TaskRunning(), NopCommand())
def get_desired_command(self):
if self._canceled:
return (TaskCanceled(),)
# Transition from init to takeoff phase
if self._state == TakeoffTaskState.init:
self._state = TakeoffTaskState.takeoff
return (TaskRunning(),
GroundInteractionCommand(
'takeoff',
self.takeoff_callback),
ResetLinearProfileCommand(
start_position_z = 0.0,
start_velocity_x = 0.0,
start_velocity_y = 0.0,
start_velocity_z = 0.0))
# Enter the takeoff phase
if self._state == TakeoffTaskState.takeoff:
return (TaskRunning(),)
if (self._state == TakeoffTaskState.ascend):
try:
transStamped = self.topic_buffer.get_tf_buffer().lookup_transform(
'map',
'base_footprint',
rospy.Time(0),
rospy.Duration(self._TRANSFORM_TIMEOUT))
except (tf2_ros.LookupException,
tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException) as ex:
msg = 'Exception when looking up transform during takeoff'
rospy.logerr('Takeofftask: {}'.format(msg))
rospy.logerr(ex.message)
return (TaskAborted(msg=msg),)
# Check if we reached the target distance from height
if(transStamped.transform.translation.z
+ self._TAKEOFF_COMPLETE_HEIGHT_TOLERANCE
>= self._TAKEOFF_COMPLETE_HEIGHT):
self._state = TakeoffTaskState.stabilize
self._time_of_stabilize = rospy.Time.now()
else:
velocity = TwistStamped()
velocity.header.frame_id = 'level_quad'
velocity.header.stamp = rospy.Time.now()
velocity.twist.linear.z = min(self._TAKEOFF_VELOCITY,
(rospy.Time.now() - self._time_of_ascension).to_sec()
* self._TAKEOFF_ACCELERATION)
return (TaskRunning(), VelocityCommand(velocity))
if self._state == TakeoffTaskState.stabilize:
if (rospy.Time.now() - self._time_of_stabilize
< rospy.Duration(self._TAKEOFF_STABILIZE_DELAY)):
velocity = TwistStamped()
velocity.header.frame_id = 'level_quad'
velocity.header.stamp = rospy.Time.now()
velocity.twist.linear.z = 0
return (TaskRunning(), VelocityCommand(velocity))
else:
self._state = TakeoffTaskState.done
if self._state == TakeoffTaskState.done:
return (TaskDone(), NopCommand())
if self._state == TakeoffTaskState.failed:
return (TaskFailed(msg='Take off task experienced failure'),)
# Impossible state reached
return (TaskAborted(msg='Impossible state in takeoff task reached'),)
def get_desired_command(self):
with self._lock:
if self._task_start_time is None:
self._task_start_time = rospy.Time.now()
if self._time_to_track != 0 and (
rospy.Time.now() - self._task_start_time >= rospy.Duration(
self._time_to_track)):
rospy.loginfo(
'TrackRoombaTask has tracked the roomba for the specified duration'
)
return (TaskDone(), )
if self._canceled:
return (TaskCanceled(), )
if (self._state == TrackRoombaTaskState.init):
if (not self.topic_buffer.has_roomba_message()
or not self.topic_buffer.has_odometry_message()):
return (TaskRunning(), NopCommand())
else:
self._state = TrackRoombaTaskState.track
if self._state == TrackRoombaTaskState.track:
odometry = self.topic_buffer.get_odometry_message()
# Check conditions that require aborting
if not (self._height_checker.above_min_maneuver_height(
odometry.pose.pose.position.z)):
return (TaskAborted(
msg='TrackRoombaTask: Drone is too low to maneuver'), )
elif not (self._z_holder.check_z_error(
odometry.pose.pose.position.z)):
return (TaskAborted(
msg='TrackRoombaTask: Z height error is too high'), )
(roomba_odom_available, roomba_odometry) = \
self.topic_buffer.get_roomba_odometry(self._roomba_id)
if not roomba_odom_available:
return (TaskAborted(
msg=
'TrackRoombaTask: The tracked roombas odometry is not available'
), )
# Get the roomba's relative location
try:
roomba_transform = self.topic_buffer.get_tf_buffer(
).lookup_transform('level_quad', self._roomba_id,
rospy.Time(0),
rospy.Duration(self._TRANSFORM_TIMEOUT))
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException) as ex:
rospy.logerr(
'TrackRoombaTask: Exception when looking up transform')
rospy.logerr(ex.message)
return (TaskAborted(
msg=
'Exception when looking up transform during TrackRoombaTask'
), )
# Create point centered at drone's center (0,0,0)
stamped_point = PointStamped()
stamped_point.point.x = 0
stamped_point.point.y = 0
stamped_point.point.z = 0
# returns point distances of roomba to center point of level quad
roomba_point = tf2_geometry_msgs.do_transform_point(
stamped_point, roomba_transform)
roomba_h_distance = math.sqrt(roomba_point.point.x**2 +
roomba_point.point.y**2)
if roomba_h_distance > self._MAX_ROOMBA_DIST:
return (TaskAborted(msg='The provided roomba is not found \
or not close enough to the quad'),
)
odometry = self.topic_buffer.get_odometry_message()
roomba_x_velocity = roomba_odometry.twist.twist.linear.x
roomba_y_velocity = roomba_odometry.twist.twist.linear.y
x_v_diff = odometry.twist.twist.linear.x - roomba_x_velocity
y_v_diff = odometry.twist.twist.linear.y - roomba_y_velocity
h_v_diff_mag = math.sqrt(x_v_diff**2 + y_v_diff**2)
roomba_track_msg = Odometry()
roomba_track_msg.header.stamp = rospy.Time.now()
roomba_track_msg.pose.pose.position.x = roomba_point.point.x
roomba_track_msg.pose.pose.position.y = roomba_point.point.y
roomba_track_msg.pose.pose.position.z = roomba_h_distance
roomba_track_msg.twist.twist.linear.x = x_v_diff
roomba_track_msg.twist.twist.linear.y = y_v_diff
roomba_track_msg.twist.twist.linear.z = h_v_diff_mag
self.topic_buffer.publish_roomba_tracking_status(
roomba_track_msg)
# Evaluate whether or not the lock completion sequence was accomplished
if self._time_to_track == 0.0 \
and roomba_h_distance <= self._LOCK_DISTANCE \
and h_v_diff_mag <= self._LOCK_VELOCITY:
if self._lock_start_time is not None:
if rospy.Time.now(
) - self._lock_start_time > self._LOCK_REQUIRED_DURATION:
rospy.loginfo(
'TrackRoombaTask has locked on the roomba for the required amount of time'
)
task_messages = self.topic_buffer.get_task_message_dictionary(
)
task_messages[
'track_x_i_accumulator'] = self._x_pid.get_accumulator(
)
task_messages[
'track_y_I_accumulator'] = self._y_pid.get_accumulator(
)
return (TaskDone(), )
else:
self._lock_start_time = rospy.Time.now()
else:
self._lock_start_time = None
# Calculate the overshoot distance in the drones frame
overshoot = math.sqrt(self._x_overshoot**2 +
self._y_overshoot**2)
x_overshoot = overshoot * math.cos(
math.atan2(roomba_y_velocity, roomba_x_velocity) +
math.atan2(self._y_overshoot, self._x_overshoot))
y_overshoot = overshoot * math.sin(
math.atan2(roomba_y_velocity, roomba_x_velocity) +
math.atan2(self._y_overshoot, self._x_overshoot))
# Find distance between the drones position and the desired position
x_p_diff = roomba_point.point.x + x_overshoot
y_p_diff = roomba_point.point.y + y_overshoot
x_success, x_response = self._x_pid.update(
x_p_diff, rospy.Time.now(), False)
y_success, y_response = self._y_pid.update(
y_p_diff, rospy.Time.now(), False)
# PID controller does setpoint - current;
# the difference from do_transform_point is from the drone to the roomba,
# which is the equivalent of current-setpoint, so take the negative response
# This is done so that the drones position in the map frame doesn't
# have to be calculated
if x_success:
x_vel_target = -x_response + roomba_x_velocity
else:
x_vel_target = roomba_x_velocity
if y_success:
y_vel_target = -y_response + roomba_y_velocity
else:
y_vel_target = roomba_y_velocity
# Get the z response
predicted_motion_point = \
self._linear_motion_profile_generator.expected_point_at_time(
rospy.Time.now())
current_height = odometry.pose.pose.position.z
predicted_height = \
predicted_motion_point.motion_point.pose.position.z
z_vel_target, z_reset = \
self._z_holder.get_height_hold_response(
current_height,
predicted_height)
# Cap the horizontal velocity
h_vel_target = math.sqrt(x_vel_target**2 + y_vel_target**2)
if h_vel_target > self._MAX_HORIZ_SPEED:
x_vel_target = x_vel_target * (self._MAX_HORIZ_SPEED /
h_vel_target)
y_vel_target = y_vel_target * (self._MAX_HORIZ_SPEED /
h_vel_target)
# Cap the z velocity target
if (abs(z_vel_target) > self._MAX_Z_VELOCITY):
z_vel_target = math.copysign(self._MAX_Z_VELOCITY,
z_vel_target)
desired_vel = [x_vel_target, y_vel_target, z_vel_target]
velocity = TwistStamped()
velocity.header.frame_id = 'level_quad'
velocity.header.stamp = rospy.Time.now()
velocity.twist.linear.x = desired_vel[0]
velocity.twist.linear.y = desired_vel[1]
velocity.twist.linear.z = desired_vel[2]
if z_reset:
velocity_command = VelocityCommand(
velocity,
start_position_z=odometry.pose.pose.position.z)
else:
velocity_command = VelocityCommand(velocity)
return (TaskRunning(), velocity_command)
return (TaskAborted(
msg='Illegal state reached in Track Roomba task'), )
def get_desired_command(self):
with self._lock:
if self._canceled:
return (TaskCanceled(), )
if (self._state == BlockRoombaTaskState.init):
if (not self.topic_buffer.has_roomba_message()
or not self.topic_buffer.has_odometry_message()
or not self.topic_buffer.has_landing_message()):
self._state = BlockRoombaTaskState.waiting
else:
self._state = BlockRoombaTaskState.descent
if (self._state == BlockRoombaTaskState.waiting):
if (not self.topic_buffer.has_roomba_message()
or not self.topic_buffer.has_odometry_message()
or not self.topic_buffer.has_landing_message()):
return (TaskRunning(), NopCommand())
else:
self._state = BlockRoombaTaskState.descent
if (self._state == BlockRoombaTaskState.descent):
try:
roomba_transform = self.topic_buffer.get_tf_buffer(
).lookup_transform('level_quad', self._roomba_id,
rospy.Time(0),
rospy.Duration(self._TRANSFORM_TIMEOUT))
drone_transform = self.topic_buffer.get_tf_buffer(
).lookup_transform('level_quad', 'map', rospy.Time(0),
rospy.Duration(self._TRANSFORM_TIMEOUT))
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException) as ex:
rospy.logerr(
'Block Roomba Task: Exception when looking up transform'
)
rospy.logerr(ex.message)
return (TaskAborted(
msg=
'Exception when looking up transform during block roomba'
), )
# Creat point centered at drone's center
stamped_point = PointStamped()
stamped_point.point.x = 0
stamped_point.point.y = 0
stamped_point.point.z = 0
# returns point distances of roomba to center point of level quad
self._roomba_point = tf2_geometry_msgs.do_transform_point(
stamped_point, roomba_transform)
if not self._check_max_roomba_range():
return (TaskAborted(
msg=
'The provided roomba is not close enough to the quad'),
)
if self._on_ground():
return (TaskDone(), )
roomba_x_velocity = self._roomba_odometry.twist.twist.linear.x
roomba_y_velocity = self._roomba_odometry.twist.twist.linear.y
roomba_velocity = math.sqrt(roomba_x_velocity**2 +
roomba_y_velocity**2)
roomba_vector = Vector3Stamped()
roomba_vector.vector.x = roomba_x_velocity / roomba_velocity
roomba_vector.vector.y = roomba_y_velocity / roomba_velocity
roomba_vector.vector.z = 0.0
# p-controller
x_vel_target = (
(self._roomba_point.point.x +
self._overshoot * roomba_vector.vector.x) * self._K_X +
roomba_x_velocity)
y_vel_target = (
(self._roomba_point.point.y +
self._overshoot * roomba_vector.vector.y) * self._K_Y +
roomba_y_velocity)
z_vel_target = self._descent_velocity
#caps velocity
vel_target = math.sqrt(x_vel_target**2 + y_vel_target**2)
if vel_target > self._MAX_TRANSLATION_SPEED:
x_vel_target = x_vel_target * (
self._MAX_TRANSLATION_SPEED / vel_target)
y_vel_target = y_vel_target * (
self._MAX_TRANSLATION_SPEED / vel_target)
if (abs(z_vel_target) > self._MAX_Z_VELOCITY):
z_vel_target = z_vel_target / abs(
z_vel_target) * self._MAX_Z_VELOCITY
rospy.logwarn("Max Z velocity reached in block roomba")
desired_vel = [x_vel_target, y_vel_target, z_vel_target]
odometry = self.topic_buffer.get_odometry_message()
drone_vel_x = odometry.twist.twist.linear.x
drone_vel_y = odometry.twist.twist.linear.y
drone_vel_z = odometry.twist.twist.linear.z
if self._current_velocity is None:
self._current_velocity = [
drone_vel_x, drone_vel_y, drone_vel_z
]
desired_vel = self._limiter.limit_acceleration(
self._current_velocity, desired_vel)
velocity = TwistStamped()
velocity.header.frame_id = 'level_quad'
velocity.header.stamp = rospy.Time.now()
velocity.twist.linear.x = desired_vel[0]
velocity.twist.linear.y = desired_vel[1]
velocity.twist.linear.z = desired_vel[2]
self._current_velocity = desired_vel
return (TaskRunning(), VelocityCommand(velocity))
return (TaskAborted(
msg='Illegal state reached in Block Roomba Task'), )
def get_desired_command(self):
with self._lock:
if self._canceled:
return (TaskCanceled(),)
if (self._state == VelocityTaskState.init):
if self._drone_odometry is None:
self._state = VelocityTaskState.waiting
else:
self._state = VelocityTaskState.moving
return (TaskRunning(), NopCommand())
elif (self._state == VelocityTaskState.waiting):
if self._drone_odometry is None:
self._state = VelocityTaskState.waiting
else:
self._state = VelocityTaskState.moving
return (TaskRunning(), NopCommand())
elif self._state == VelocityTaskState.moving:
try:
transStamped = self._tf_buffer.lookup_transform(
'map',
'base_footprint',
rospy.Time.now(),
rospy.Duration(self._TRANSFORM_TIMEOUT))
except (tf2_ros.LookupException,
tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException) as ex:
rospy.logerr('ObjectTrackTask: Exception when looking up transform')
rospy.logerr(ex.message)
return (TaskAborted(msg='Exception when looking up transform during velocity task'),)
current_height = transStamped.transform.translation.z
if not self._current_height_set:
self._current_height_set = True
self._z_holder.set_height(current_height)
if not (self._height_checker.above_min_maneuver_height(
current_height)):
return (TaskAborted(msg='Drone is too low'),)
x_vel_target = self._HORIZ_X_VEL
y_vel_target = self._HORIZ_Y_VEL
z_vel_target = self._z_holder.get_height_hold_response(current_height)
if (abs(z_vel_target) > self._MAX_Z_VELOCITY):
z_vel_target = math.copysign(self._MAX_Z_VELOCITY, z_vel_target)
desired_vel = [x_vel_target, y_vel_target, z_vel_target]
drone_vel_x = self._drone_odometry.twist.twist.linear.x
drone_vel_y = self._drone_odometry.twist.twist.linear.y
drone_vel_z = self._drone_odometry.twist.twist.linear.z
if self._current_velocity is None:
self._current_velocity = [drone_vel_x, drone_vel_y, drone_vel_z]
desired_vel = self._limiter.limit_acceleration(self._current_velocity, desired_vel)
velocity = TwistStamped()
velocity.header.frame_id = 'level_quad'
velocity.header.stamp = rospy.Time.now()
velocity.twist.linear.x = desired_vel[0]
velocity.twist.linear.y = desired_vel[1]
velocity.twist.linear.z = desired_vel[2]
self._current_velocity = desired_vel
return (TaskRunning(), VelocityCommand(velocity))
else:
return (TaskAborted(msg='Illegal state reached in Velocity test task'),)
def get_desired_command(self):
with self._lock:
if self._canceled:
return (TaskCanceled(), )
elif (self._state == TrackObjectTaskState.init):
if self._roomba_array is None or self._drone_odometry is None:
self._state = TrackObjectTaskState.waiting
else:
self._state = TrackObjectTaskState.track
return (TaskRunning(), NopCommand())
elif (self._state == TrackObjectTaskState.waiting):
if self._roomba_array is None or self._drone_odometry is None:
self._state = TrackObjectTaskState.waiting
else:
self._state = TrackObjectTaskState.track
return (TaskRunning(), NopCommand())
elif self._state == TrackObjectTaskState.track:
if not (self._height_checker.above_min_maneuver_height(
self._drone_odometry.pose.pose.position.z)):
return (TaskAborted(msg='Drone is too low'), )
elif not (self._z_holder.check_z_error(
self._drone_odometry.pose.pose.position.z)):
return (TaskAborted(msg='Z error is too high'), )
elif not self._check_roomba_in_sight():
return (TaskAborted(
msg='The provided roomba is not in sight of quad'), )
try:
roomba_transform = self._tf_buffer.lookup_transform(
'level_quad', self._roomba_id, rospy.Time.now(),
rospy.Duration(self._TRANSFORM_TIMEOUT))
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException) as ex:
rospy.logerr(
'ObjectTrackTask: Exception when looking up transform')
rospy.logerr(ex.message)
return (TaskAborted(
msg=
'Exception when looking up transform during roomba track'
), )
# Creat point centered at drone's center
stamped_point = PointStamped()
stamped_point.point.x = 0
stamped_point.point.y = 0
stamped_point.point.z = 0
# returns point distances of roomba to center point of level quad
self._roomba_point = tf2_geometry_msgs.do_transform_point(
stamped_point, roomba_transform)
if not self._check_max_roomba_range():
return (TaskAborted(
msg=
'The provided roomba is not found or not close enough to the quad'
), )
if self._check_max_ending_roomba_range():
return (TaskDone(), )
roomba_x_velocity = self._roomba_odometry.twist.twist.linear.x
roomba_y_velocity = self._roomba_odometry.twist.twist.linear.y
roomba_velocity = math.sqrt(roomba_x_velocity**2 +
roomba_y_velocity**2)
x_overshoot = roomba_x_velocity / roomba_velocity * self._overshoot
y_overshoot = roomba_y_velocity / roomba_velocity * self._overshoot
# p-controller
x_vel_target = (
(self._roomba_point.point.x + x_overshoot) * self._K_X +
roomba_x_velocity)
y_vel_target = (
(self._roomba_point.point.y + y_overshoot) * self._K_Y +
roomba_y_velocity)
z_vel_target = self._z_holder.get_height_hold_response(
self._drone_odometry.pose.pose.position.z)
#caps velocity
vel_target = math.sqrt(x_vel_target**2 + y_vel_target**2)
if vel_target > self._MAX_HORIZ_SPEED:
x_vel_target = x_vel_target * (self._MAX_HORIZ_SPEED /
vel_target)
y_vel_target = y_vel_target * (self._MAX_HORIZ_SPEED /
vel_target)
if (abs(z_vel_target) > self._MAX_Z_VELOCITY):
z_vel_target = math.copysign(self._MAX_Z_VELOCITY,
z_vel_target)
desired_vel = [x_vel_target, y_vel_target, z_vel_target]
drone_vel_x = self._drone_odometry.twist.twist.linear.x
drone_vel_y = self._drone_odometry.twist.twist.linear.y
drone_vel_z = self._drone_odometry.twist.twist.linear.z
if self._current_velocity is None:
self._current_velocity = [
drone_vel_x, drone_vel_y, drone_vel_z
]
desired_vel = self._limiter.limit_acceleration(
self._current_velocity, desired_vel)
velocity = TwistStamped()
velocity.header.frame_id = 'level_quad'
velocity.header.stamp = rospy.Time.now()
velocity.twist.linear.x = desired_vel[0]
velocity.twist.linear.y = desired_vel[1]
velocity.twist.linear.z = desired_vel[2]
self._current_velocity = desired_vel
return (TaskRunning(), VelocityCommand(velocity))
return (TaskAborted(
msg='Illegal state reached in Track Roomba task'), )
def get_desired_command(self):
if self._canceled:
return (TaskCanceled(), )
if self._state == TakeoffTaskState.init:
# Check if we have an fc status
if self._fc_status is None:
return (TaskRunning(), NopCommand())
# Check that auto pilot is enabled
if not self._fc_status.auto_pilot:
return (TaskFailed(
msg='flight controller not allowing auto pilot'), )
# Check that the FC is not already armed
if self._fc_status.armed:
return (TaskFailed(
msg='flight controller armed prior to takeoff'), )
# All is good change state to request arm
self._state = TakeoffTaskState.request_arm
# Enter the arming request stage
if self._state == TakeoffTaskState.request_arm:
# Check that the FC is not already armed
if self._arm_request_success:
self.pause_start_time = rospy.Time.now()
self._state = TakeoffTaskState.pause_before_takeoff
else:
return (TaskRunning(),
ArmCommand(True, True, False, self.arm_callback))
# Pause before ramping up the motors
if self._state == TakeoffTaskState.pause_before_takeoff:
if rospy.Time.now() - self.pause_start_time > rospy.Duration(
self._DELAY_BEFORE_TAKEOFF):
self._state = TakeoffTaskState.takeoff
return (TaskRunning(),
GroundInteractionCommand('takeoff',
self.takeoff_callback))
else:
return (TaskRunning(), NopCommand())
# Enter the takeoff phase
if self._state == TakeoffTaskState.takeoff:
return (TaskRunning(), )
if (self._state == TakeoffTaskState.ascend
or self._state == TakeoffTaskState.ascend_angle):
try:
transStamped = self._tf_buffer.lookup_transform(
'map', 'base_footprint', rospy.Time.now(),
rospy.Duration(self._TRANSFORM_TIMEOUT))
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException) as ex:
msg = 'Exception when looking up transform during takeoff'
rospy.logerr('Takeofftask: {}'.format(msg))
rospy.logerr(ex.message)
return (TaskAborted(msg=msg), )
# Check if we reached the target height
if (transStamped.transform.translation.z >
self._TAKEOFF_COMPLETE_HEIGHT):
self._state = TakeoffTaskState.done
return (TaskDone(), NopCommand())
elif (self._state == TakeoffTaskState.ascend
and transStamped.transform.translation.z >
self._ANGLE_MODE_HEIGHT):
self._state = TakeoffTaskState.ascend_angle
return (TaskRunning(),
ArmCommand(True, True, True, lambda _: None))
else:
velocity = TwistStamped()
velocity.header.frame_id = 'level_quad'
velocity.header.stamp = rospy.Time.now()
velocity.twist.linear.z = self._TAKEOFF_VELOCITY
return (TaskRunning(), VelocityCommand(velocity))
if self._state == TakeoffTaskState.done:
return (TaskDone(), NopCommand())
if self._state == TakeoffTaskState.failed:
return (TaskFailed(msg='Take off task experienced failure'), )
# Impossible state reached
return (TaskAborted(msg='Impossible state in takeoff task reached'), )
def get_desired_command(self):
with self._lock:
if self._canceled:
return (TaskCanceled(),)
if (self._state == VelocityTaskState.init):
if not self.topic_buffer.has_odometry_message():
self._state = VelocityTaskState.waiting
else:
self._state = VelocityTaskState.moving
self._start_time = rospy.Time.now()
if (self._state == VelocityTaskState.waiting):
if not self.topic_buffer.has_odometry_message():
return (TaskRunning(), NopCommand())
else:
self._state = VelocityTaskState.moving
self._start_time = rospy.Time.now()
if self._state == VelocityTaskState.moving:
if rospy.Time.now() - self._start_time > self._time_duration:
return (TaskDone(), )
predicted_motion_point = self._linear_motion_profile_generator.expected_point_at_time(
rospy.Time.now())
odometry = self.topic_buffer.get_odometry_message()
current_height = odometry.pose.pose.position.z
predicted_height = predicted_motion_point.motion_point.pose.position.z
if not self._current_height_set:
self._current_height_set = True
self._z_holder.set_height(current_height)
if not (self._height_checker.above_min_maneuver_height(
current_height)):
return (TaskAborted(msg='Drone is too low'),)
x_vel_target = self._HORIZ_X_VEL
y_vel_target = self._HORIZ_Y_VEL
(z_vel_target, reset_z) = self._z_holder.get_height_hold_response(current_height, predicted_height)
if (abs(z_vel_target) > self._MAX_Z_VELOCITY):
z_vel_target = math.copysign(self._MAX_Z_VELOCITY, z_vel_target)
desired_vel = [x_vel_target, y_vel_target, z_vel_target]
drone_vel_x = odometry.twist.twist.linear.x
drone_vel_y = odometry.twist.twist.linear.y
drone_vel_z = odometry.twist.twist.linear.z
if self._current_velocity is None:
self._current_velocity = [drone_vel_x, drone_vel_y, drone_vel_z]
obst_avoider = self.topic_buffer.get_obstacle_avoider()
desired_vel = obst_avoider.get_safe_vector(
desired_vel, self._current_velocity[:2])
desired_vel = self._limiter.limit_acceleration(
self._current_velocity, desired_vel)
velocity = TwistStamped()
velocity.header.frame_id = 'level_quad'
velocity.header.stamp = rospy.Time.now()
velocity.twist.linear.x = desired_vel[0]
velocity.twist.linear.y = desired_vel[1]
velocity.twist.linear.z = desired_vel[2]
self._current_velocity = desired_vel
if reset_z:
velocity_command = VelocityCommand(velocity,
start_position_z=odometry.pose.pose.position.z,
start_velocity_z=odometry.twist.twist.linear.z)
else:
velocity_command = VelocityCommand(velocity)
return (TaskRunning(), velocity_command)
return (TaskAborted(msg='Illegal state reached in Velocity test task'),)
