class TestGoToWaypointAction(unittest.TestCase):
@classmethod
def setUpClass(cls):
rospy.init_node('test_go_to_waypoint')
def setUp(self):
self.waypoint = None
self._waypoint_sub = rospy.Subscriber('/mngr/waypoint', Waypoint,
self._waypoint_callback)
self._state_pub = rospy.Publisher('/state', StateStamped, queue_size=1)
self.mission = MissionInterface()
def _waypoint_callback(self, msg):
self.waypoint = msg
def test_mission_that_goes_to_waypoint(self):
self.assertTrue(self.mission.load_mission('waypoint_mission_test.xml'))
self.mission.execute_mission()
def waypoint_is_set():
return self.waypoint is not None
self.assertTrue(wait_for(waypoint_is_set), msg='No waypoint was set')
ena_mask = Waypoint.LAT_ENA | Waypoint.LONG_ENA
ena_mask |= Waypoint.ALTITUDE_ENA
ena_mask |= Waypoint.SPEED_KNOTS_ENA
self.assertEqual(self.waypoint.ena_mask, ena_mask)
self.mission.read_behavior_parameters('GoToWaypoint')
latitude = float(self.mission.get_behavior_parameter('latitude'))
longitude = float(self.mission.get_behavior_parameter('longitude'))
altitude = float(self.mission.get_behavior_parameter('altitude'))
speed_knots = float(self.mission.get_behavior_parameter('speed_knots'))
self.assertEqual(self.waypoint.latitude, latitude)
self.assertEqual(self.waypoint.longitude, longitude)
self.assertEqual(self.waypoint.altitude, altitude)
self.assertEqual(self.waypoint.speed_knots, speed_knots)
msg = StateStamped()
msg.header.stamp = rospy.Time.now()
msg.state.geolocation.position.latitude = latitude
msg.state.geolocation.position.longitude = longitude
msg.state.geolocation.position.altitude = altitude
self._state_pub.publish(msg)
def mission_complete():
return (ReportExecuteMissionState.ABORTING
not in self.mission.execute_mission_state
and ReportExecuteMissionState.COMPLETE
== self.mission.execute_mission_state[-1])
self.assertTrue(wait_for(mission_complete),
msg='Mission did not complete')
class TestAttitudeServoAction(unittest.TestCase):
@classmethod
def setUpClass(cls):
rospy.init_node('test_attitude_servo')
def setUp(self):
self.attitude_servo_goal = AttitudeServo()
self.mission = MissionInterface()
# Subscribers
self.attitude_servo_msg = rospy.Subscriber(
'/mngr/attitude_servo', AttitudeServo,
self.attitude_servo_goal_callback)
self.simulated_auv_interface_data_pub = rospy.Publisher('/state',
StateStamped,
queue_size=1)
def attitude_servo_goal_callback(self, msg):
self.attitude_servo_goal = msg
def test_mission_with_attitude_servo_action(self):
"""
This test:
- Loads a mission with a single AttitudeServo action.
- Executes the mission just loaded.
- Waits until the action publishes an AttitudeServo setpoint.
- Simulates state updates for the action to complete.
- Waits until the mission is COMPLETE.
"""
self.mission.load_mission('attitude_servo_mission_test.xml')
self.mission.execute_mission()
self.mission.read_behavior_parameters('AttitudeServo')
roll = float(self.mission.get_behavior_parameter('roll'))
pitch = float(self.mission.get_behavior_parameter('pitch'))
pitch_units = self.mission.get_behavior_parameter('pitch-units')
self.assertEqual('degrees', pitch_units)
pitch = math.radians(float(pitch))
yaw = float(self.mission.get_behavior_parameter('yaw'))
speed_knots = float(self.mission.get_behavior_parameter('speed_knots'))
# Calculate the mask
enable_mask = 0
if roll is not None:
enable_mask |= AttitudeServo.ROLL_ENA
if pitch is not None:
enable_mask |= AttitudeServo.PITCH_ENA
if yaw is not None:
enable_mask |= AttitudeServo.YAW_ENA
if speed_knots is not None:
enable_mask |= AttitudeServo.SPEED_KNOTS_ENA
def attitude_servo_goals_are_set():
return (isclose(self.attitude_servo_goal.roll, roll) and isclose(
self.attitude_servo_goal.pitch, pitch, rel_tol=1e-6)
and isclose(self.attitude_servo_goal.yaw, yaw) and isclose(
self.attitude_servo_goal.speed_knots, speed_knots)
and self.attitude_servo_goal.ena_mask == enable_mask)
self.assertTrue(wait_for(attitude_servo_goals_are_set),
msg='Mission control must publish goals')
# send data to finish the mission
msg = StateStamped()
msg.state.manoeuvring.pose.mean.orientation.x = roll
msg.state.manoeuvring.pose.mean.orientation.y = pitch
msg.state.manoeuvring.pose.mean.orientation.z = yaw
self.simulated_auv_interface_data_pub.publish(msg)
def success_mission_status_is_reported():
return (ReportExecuteMissionState.ABORTING
not in self.mission.execute_mission_state
and ReportExecuteMissionState.COMPLETE
in self.mission.execute_mission_state)
self.assertTrue(wait_for(success_mission_status_is_reported),
msg='Mission control must report only COMPLETE')
class TestSetDepthHeadingAction(unittest.TestCase):
@classmethod
def setUpClass(cls):
rospy.init_node('test_set_depth_heading')
def setUp(self):
self.depth_heading_goal = DepthHeading()
self.mission = MissionInterface()
self.depth_heading_msg = rospy.Subscriber(
'/mngr/depth_heading', DepthHeading,
self.depth_heading_goal_callback)
self.simulated_auv_interface_data_pub = rospy.Publisher('/state',
StateStamped,
queue_size=1)
def depth_heading_goal_callback(self, msg):
self.depth_heading_goal = msg
def test_mission_with_set_depth_heading_action(self):
"""
The test:
- Loads a mission with a single SetDepthHeading action.
- Executes the mission.
- Waits until the action publishes a DepthHeading setpoint.
- Simulates state updates for the action to complete.
- waits until the mission is COMPLETE.
"""
self.mission.load_mission('depth_heading_mission_test.xml')
self.mission.execute_mission()
self.mission.read_behavior_parameters('SetDepthHeading')
depth = float(self.mission.get_behavior_parameter('depth'))
heading = float(self.mission.get_behavior_parameter('heading'))
heading_units = self.mission.get_behavior_parameter('heading-units')
self.assertEqual('degrees', heading_units)
heading = math.radians(float(heading))
speed_knots = float(self.mission.get_behavior_parameter('speed_knots'))
# Calculate the mask
enable_mask = DepthHeading.DEPTH_ENA
enable_mask |= DepthHeading.HEADING_ENA
enable_mask |= DepthHeading.SPEED_KNOTS_ENA
def depth_heading_goals_are_set():
return (isclose(self.depth_heading_goal.depth, depth) and isclose(
self.depth_heading_goal.heading, heading, rel_tol=1e-6) and
isclose(self.depth_heading_goal.speed_knots, speed_knots)
and self.depth_heading_goal.ena_mask == enable_mask)
self.assertTrue(wait_for(depth_heading_goals_are_set),
msg='Mission control must publish goals')
# send data to finish the mission
auv_interface_data = StateStamped()
auv_interface_data.state.manoeuvring.pose.mean.position.z = depth
auv_interface_data.state.manoeuvring.pose.mean.orientation.z = heading
self.simulated_auv_interface_data_pub.publish(auv_interface_data)
def success_mission_status_is_reported():
return (ReportExecuteMissionState.ABORTING
not in self.mission.execute_mission_state
and ReportExecuteMissionState.COMPLETE
in self.mission.execute_mission_state)
self.assertTrue(wait_for(success_mission_status_is_reported),
msg='Mission control must report only COMPLETE')
class TestSetAltitudeHeadingAction(unittest.TestCase):
@classmethod
def setUpClass(cls):
rospy.init_node('test_set_altitude_heading')
def setUp(self):
self.altitude_heading_goal = AltitudeHeading()
self.mission = MissionInterface()
self.altitude_heading_msg = rospy.Subscriber(
'/mngr/altitude_heading', AltitudeHeading,
self.altitude_heading_goal_callback)
self.simulated_auv_interface_data_pub = rospy.Publisher('/state',
StateStamped,
queue_size=1)
def altitude_heading_goal_callback(self, msg):
self.altitude_heading_goal = msg
def test_mission_with_set_altitude_heading_action(self):
"""
The test:
- Loads a mission with a single SetAltitudeHeading action.
- Executes the mission.
- Waits until the action publishes an AltitudeHeading setpoint.
- Simulates state updates for the action to complete.
- Waits until the mission is COMPLETE.
"""
self.mission.load_mission('altitude_heading_mission_test.xml')
self.mission.execute_mission()
self.mission.read_behavior_parameters('SetAltitudeHeading')
altitude = self.mission.get_behavior_parameter('altitude')
heading = self.mission.get_behavior_parameter('heading')
speed_knots = self.mission.get_behavior_parameter('speed_knots')
# Calculate the mask
enable_mask = 0
if altitude is not None:
enable_mask |= AltitudeHeading.ALTITUDE_ENA
if heading is not None:
enable_mask |= AltitudeHeading.HEADING_ENA
if speed_knots is not None:
enable_mask |= AltitudeHeading.SPEED_KNOTS_ENA
def altitude_heading_goals_are_set():
return ((altitude is None
or self.altitude_heading_goal.altitude == float(altitude))
and
(heading is None
or self.altitude_heading_goal.heading == float(heading))
and (speed_knots is None
or self.altitude_heading_goal.speed_knots
== float(speed_knots))
and self.altitude_heading_goal.ena_mask == enable_mask)
self.assertTrue(wait_for(altitude_heading_goals_are_set),
msg='Mission control must publish goals')
# send data to finish the mission
auv_interface_data = StateStamped()
auv_interface_data.state.manoeuvring.pose.mean.orientation.x = 0.0
auv_interface_data.state.manoeuvring.pose.mean.orientation.y = 0.0
auv_interface_data.state.manoeuvring.pose.mean.orientation.z = 2.0
self.simulated_auv_interface_data_pub.publish(auv_interface_data)
def success_mission_status_is_reported():
return (ReportExecuteMissionState.ABORTING
not in self.mission.execute_mission_state
and ReportExecuteMissionState.COMPLETE
in self.mission.execute_mission_state)
self.assertTrue(wait_for(success_mission_status_is_reported),
msg='Mission control must report only COMPLETE')
class TestFixRudderAction(unittest.TestCase):
@classmethod
def setUpClass(cls):
rospy.init_node('test_fix_rudder')
def setUp(self):
self.fixed_rudder_goal = None
self.mission = MissionInterface()
self.fixed_rudder_goal = FixedRudder()
# Subscribers
self.fixed_rudder_msg = rospy.Subscriber(
'/mngr/fixed_rudder', FixedRudder,
self.callback_publish_fixed_rudder_goal)
def callback_publish_fixed_rudder_goal(self, msg):
self.fixed_rudder_goal = msg
def test_mission_with_fix_rudder_action(self):
"""
This test:
- Loads a mission with a single FixRudder action.
- Executes the mission.
- Waits until the mission is COMPLETE.
"""
self.mission.load_mission("fixed_rudder_mission_test.xml")
self.mission.execute_mission()
self.mission.read_behavior_parameters('FixRudder')
depth = self.mission.get_behavior_parameter('depth')
rudder = self.mission.get_behavior_parameter('rudder')
speed_knots = self.mission.get_behavior_parameter('speed_knots')
# Calculate the mask
enable_mask = 0
if depth is not None:
enable_mask |= FixedRudder.DEPTH_ENA
if rudder is not None:
enable_mask |= FixedRudder.RUDDER_ENA
if speed_knots is not None:
enable_mask |= FixedRudder.SPEED_KNOTS_ENA
# Check if the behavior publishes the goal
def fixed_rudder_goals_are_set():
return (
(depth is None or self.fixed_rudder_goal.depth == float(depth))
and (rudder is None
or self.fixed_rudder_goal.rudder == float(rudder)) and
(speed_knots is None
or self.fixed_rudder_goal.speed_knots == float(speed_knots))
and self.fixed_rudder_goal.ena_mask == enable_mask)
self.assertTrue(wait_for(fixed_rudder_goals_are_set),
msg='Mission control must publish goals')
# Wait for the mission to be complete
def success_mission_status_is_reported():
return (ReportExecuteMissionState.ABORTING
not in self.mission.execute_mission_state
and ReportExecuteMissionState.COMPLETE
in self.mission.execute_mission_state)
self.assertTrue(wait_for(success_mission_status_is_reported),
msg='Mission control must report only COMPLETE')