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 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 setUp(self):
self.mission = MissionInterface()
self.attitude_servo_goal = AttitudeServo()
self.attitude_servo_sub = rospy.Subscriber(
'/mngr/attitude_servo', AttitudeServo,
self.attitude_servo_callback)
self.simulated_auv_interface_data_pub = rospy.Publisher('/state',
StateStamped,
queue_size=1)
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')
def setUp(self):
self.attitude_servo_goal = AttitudeServo()
self.mission = MissionInterface()
self.simulate_error_code = ReportFault()
self.simulate_error_code.fault_id = ReportFault.AUTOPILOT_NODE_DIED
self.attitude_servo_goal = AttitudeServo()
self.attitude_servo_msg = rospy.Subscriber(
'/mngr/attitude_servo', AttitudeServo,
self.attitude_servo_callback)
self.simulated_health_monitor_pub = rospy.Publisher(
'/health_monitor/report_fault', ReportFault, queue_size=1)
self.simulated_auv_interface_data_pub = rospy.Publisher('/state',
StateStamped,
queue_size=1)
def setUp(self):
self.report_mission = ReportMissions()
self.mission = MissionInterface()
self.mission_load_state = None
self.mission_to_load = LoadMission()
# Subscribers
self.load_state_sub = rospy.Subscriber(
'/mission_control_node/report_mission_load_state',
ReportLoadMissionState, self.callback_mission_load_state)
self.simulated_mission_control_load_mission_pub = rospy.Publisher(
'/mission_control_node/load_mission',
LoadMission,
latch=True,
queue_size=1)
self.report_missions_sub = rospy.Subscriber(
'/mission_control_node/report_missions', ReportMissions,
self.callback_report_mission)
# Publishers
self.simulated_query_mission_msg_pub = rospy.Publisher(
'/mission_control_node/query_missions',
QueryMissions,
latch=True,
queue_size=1)
self.simulated_remove_mission_msg_pub = rospy.Publisher(
'/mission_control_node/remove_missions',
RemoveMissions,
latch=True,
queue_size=1)
self.simulated_abort_mission_msg_pub = rospy.Publisher(
'/mission_control_node/abort_mission',
AbortMission,
latch=True,
queue_size=1)
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')
def setUp(self):
self.mission_interface = MissionInterface()
class TestLogToBagfileAction(unittest.TestCase):
@classmethod
def setUpClass(cls):
rospy.init_node('node_to_test_log_to_bagfile')
def setUp(self):
self.mission_interface = MissionInterface()
def _log_one_topic_to_bagfile(self, compression_type):
bag_name = 'one_topic.{}.bag'.format(compression_type)
topic_name = '/mngr/fixed_rudder'
with tempfile.NamedTemporaryFile() as f:
f.write(
textwrap.dedent('''
<root main_tree_to_execute="main">
<BehaviorTree ID="main">
<LogToBagfile prefix="{}" topics="{}" compression="{}">
<Sequence>
<FixRudder depth="1.0" rudder="2.0" speed_knots="3.0"/>
<Delay delay_msec="1000">
<AlwaysSuccess/>
</Delay>
</Sequence>
</LogToBagfile>
</BehaviorTree>
</root>'''.format(bag_name, topic_name, compression_type)))
f.flush()
result = self.mission_interface.load_mission(f.name)
self.assertEqual(result.load_state, ReportLoadMissionState.SUCCESS)
mission_id = result.mission_id
self.mission_interface.execute_mission(mission_id)
def mission_completed():
return ReportExecuteMissionState.COMPLETE in \
self.mission_interface.execute_mission_state
self.assertTrue(wait_for(mission_completed),
msg='Mission did not COMPLETE')
bag_path = os.path.join(os.path.dirname(rospy.core._log_filename),
bag_name)
self.assertTrue(os.path.isfile(bag_path), bag_path + ' is missing')
with rosbag.Bag(bag_path, 'r') as bag:
self.assertEqual(bag.get_compression_info().compression,
compression_type)
topics = bag.get_type_and_topic_info().topics
self.assertEqual(len(topics), 1)
name, info = topics.items()[0]
self.assertEqual(name, topic_name)
self.assertEqual(info.message_count, bag.get_message_count())
def test_log_to_bagfile(self):
self._log_one_topic_to_bagfile(compression_type='none')
def test_log_to_bz2_compressed_bagfile(self):
self._log_one_topic_to_bagfile(compression_type='bz2')
def test_log_to_lz4_compressed_bagfile(self):
self._log_one_topic_to_bagfile(compression_type='lz4')
def test_log_all_to_bagfile(self):
bag_name = 'all_topics.bag'
with tempfile.NamedTemporaryFile() as f:
f.write(
textwrap.dedent('''
<root main_tree_to_execute="main">
<BehaviorTree ID="main">
<LogToBagfile prefix="{}">
<Delay delay_msec="1000">
<AlwaysSuccess/>
</Delay>
</LogToBagfile>
</BehaviorTree>
</root>'''.format(bag_name)))
f.flush()
result = self.mission_interface.load_mission(f.name)
self.assertEqual(result.load_state, ReportLoadMissionState.SUCCESS)
mission_id = result.mission_id
self.mission_interface.execute_mission(mission_id)
def mission_completed():
return ReportExecuteMissionState.COMPLETE in \
self.mission_interface.execute_mission_state
self.assertTrue(wait_for(mission_completed),
msg='Mission did not COMPLETE')
bag_path = os.path.join(os.path.dirname(rospy.core._log_filename),
bag_name)
self.assertTrue(os.path.isfile(bag_path), bag_path + ' is missing')
with rosbag.Bag(bag_path, 'r') as bag:
topics = bag.get_type_and_topic_info().topics
self.assertGreaterEqual(len(topics), 1)
self.assertIn('/rosout', topics)
class TestJausRosBridgeInterface(unittest.TestCase):
"""
Simulate messages sent by the jaus ros bridge:
- Load Mission
- Execute Mission
- Query Mission
- Abort Mission
- Remove Mission
"""
@classmethod
def setUpClass(cls):
rospy.init_node('test_mission_control_interface_jaus_ros_bridge')
def setUp(self):
self.report_mission = ReportMissions()
self.mission = MissionInterface()
self.mission_load_state = None
self.mission_to_load = LoadMission()
# Subscribers
self.load_state_sub = rospy.Subscriber(
'/mission_control_node/report_mission_load_state',
ReportLoadMissionState, self.callback_mission_load_state)
self.simulated_mission_control_load_mission_pub = rospy.Publisher(
'/mission_control_node/load_mission',
LoadMission,
latch=True,
queue_size=1)
self.report_missions_sub = rospy.Subscriber(
'/mission_control_node/report_missions', ReportMissions,
self.callback_report_mission)
# Publishers
self.simulated_query_mission_msg_pub = rospy.Publisher(
'/mission_control_node/query_missions',
QueryMissions,
latch=True,
queue_size=1)
self.simulated_remove_mission_msg_pub = rospy.Publisher(
'/mission_control_node/remove_missions',
RemoveMissions,
latch=True,
queue_size=1)
self.simulated_abort_mission_msg_pub = rospy.Publisher(
'/mission_control_node/abort_mission',
AbortMission,
latch=True,
queue_size=1)
def callback_mission_execute_state(self, msg):
self.report_execute_mission = msg
def callback_report_mission(self, msg):
self.report_mission = msg
def callback_mission_load_state(self, msg):
self.mission_load_state = msg.load_state
def test_jaus_ros_bridge_interface(self):
# Test if the mission control reports FAILED if cannot load a mission
self.assertEqual(self.mission.load_mission('NO_MISSION'),
ReportLoadMissionState.FAILED)
# A valid mission is sent to the mission control
# Execute Mission and check if the mission control reports status
self.mission.load_mission('waypoint_mission_test.xml')
self.mission.execute_mission()
# Wait for the mission to report EXECUTING
def executing_mission_status_is_reported():
return ReportExecuteMissionState.EXECUTING in self.mission.execute_mission_state
self.assertTrue(wait_for(executing_mission_status_is_reported),
msg='Mission control must report EXECUTING')
# Abort mission and wait for it to wrap up
abortMission = AbortMission()
abortMission.mission_id = 1
self.simulated_abort_mission_msg_pub.publish(abortMission)
def aborting_mission_status_is_reported():
return ReportExecuteMissionState.ABORTING in self.mission.execute_mission_state
self.assertTrue(wait_for(aborting_mission_status_is_reported),
msg='Mission control must report ABORTING')
# Query mission and test response
self.simulated_query_mission_msg_pub.publish(QueryMissions())
def query_mission():
return len(self.report_mission.missions) > 0
self.assertTrue(wait_for(query_mission))
self.assertEqual(self.report_mission.missions[0].mission_description,
'Test Mission')
# Remove all missions and test if they have been removed.
self.report_mission = ReportMissions()
self.simulated_remove_mission_msg_pub.publish(RemoveMissions())
self.simulated_query_mission_msg_pub.publish(QueryMissions())
def query_after_remove_mission():
return len(self.report_mission.missions) == 0
self.assertTrue(wait_for(query_after_remove_mission))
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')
class TestMissionControlAbortsWhenHealthMonitorReportsFault(unittest.TestCase):
"""
Simulate error message sent by health monitor and checks
if the mission control publishes the abort status.
The steps int the involved are:
1) Load a mission
2) Execute the Mission
3) Simulate an error sent by health monitor
4) check if the mission control aborts the mission
a) Receive Aborting State
b) Set the fins to surface and Thruster velocity to 0 RPM
c) The abort behavior is complete
"""
@classmethod
def setUpClass(cls):
rospy.init_node('test_mission_control_aborts_procedure')
def setUp(self):
self.attitude_servo_goal = AttitudeServo()
self.mission = MissionInterface()
self.simulate_error_code = ReportFault()
self.simulate_error_code.fault_id = ReportFault.AUTOPILOT_NODE_DIED
self.attitude_servo_goal = AttitudeServo()
self.attitude_servo_msg = rospy.Subscriber(
'/mngr/attitude_servo', AttitudeServo,
self.attitude_servo_callback)
self.simulated_health_monitor_pub = rospy.Publisher(
'/health_monitor/report_fault', ReportFault, queue_size=1)
self.simulated_auv_interface_data_pub = rospy.Publisher('/state',
StateStamped,
queue_size=1)
def attitude_servo_callback(self, msg):
self.attitude_servo_goal = msg
def test_mission_control_aborts_if_health_monitor_reports_fault(self):
self.mission.load_mission('attitude_servo_mission_test.xml')
self.mission.execute_mission()
def executing_mission_status_is_reported():
return ReportExecuteMissionState.EXECUTING in self.mission.execute_mission_state
self.assertTrue(wait_for(executing_mission_status_is_reported),
msg='Mission control must report EXECUTING')
# Simulate the health monitor publishing the fault code
self.simulated_health_monitor_pub.publish(self.simulate_error_code)
def aborting_mission_status_is_reported():
return ReportExecuteMissionState.ABORTING in self.mission.execute_mission_state
self.assertTrue(wait_for(aborting_mission_status_is_reported),
msg='Mission control must report ABORTING')
# Check if the behavior publishes the attitude servo msg to
# set the fins to surface and velocity to 0 RPM
maxCtrlFinAngle = rospy.get_param('/fin_control/max_ctrl_fin_angle')
def attitude_servo_aborting_goals_are_set():
tol = 1e-3
ena_mask = AttitudeServo.PITCH_ENA | AttitudeServo.SPEED_KNOTS_ENA
return (isclose(self.attitude_servo_goal.pitch, -maxCtrlFinAngle,
tol)
and isclose(self.attitude_servo_goal.speed_knots, 0.0, tol)
and self.attitude_servo_goal.ena_mask == ena_mask)
self.assertTrue(wait_for(attitude_servo_aborting_goals_are_set),
msg='Mission control must publish goals')
# Publishes values sent to the actuators
msg = StateStamped()
msg.state.manoeuvring.pose.mean.orientation.x = 0.0
msg.state.manoeuvring.pose.mean.orientation.y = -maxCtrlFinAngle
msg.state.manoeuvring.pose.mean.orientation.z = 0.0
msg.state.manoeuvring.velocity.mean.linear.x = 0.0
msg.state.manoeuvring.velocity.mean.linear.y = 0.0
msg.state.manoeuvring.velocity.mean.linear.z = 0.0
self.simulated_auv_interface_data_pub.publish(msg)
def complete_mission_status_is_reported():
return ReportExecuteMissionState.COMPLETE == self.mission.execute_mission_state[
-1]
self.assertTrue(wait_for(complete_mission_status_is_reported),
msg='Mission control must report COMPLETE')
class TestMissionControlAbortsWhenBehaviorReturnsTimeOutFailure(
unittest.TestCase):
"""
Test if the mission control aborts after the behavior returns Failure state.
The failure is produced because the mission time is greater than the time_out
mission behavior value
Args:
sys.argv[1] = Mission filename. It must be located in the test_mission folder.
The steps involved are:
1) Load a mission
2) Execute the mission
4) Wait the mission control to publish a Failure state
5) check if the mission control aborts the mission
a) Receive Aborting State
b) Set the fins to surface and Thruster velocity to 0 RPM
c) The abort behavior is complete
"""
@classmethod
def setUpClass(cls):
rospy.init_node('test_mission_control_aborts_procedure')
def setUp(self):
self.mission = MissionInterface()
self.attitude_servo_goal = AttitudeServo()
self.attitude_servo_sub = rospy.Subscriber(
'/mngr/attitude_servo', AttitudeServo,
self.attitude_servo_callback)
self.simulated_auv_interface_data_pub = rospy.Publisher('/state',
StateStamped,
queue_size=1)
def attitude_servo_callback(self, msg):
self.attitude_servo_goal = msg
def test_mission_control_aborts_if_action_times_out(self):
self.mission.load_mission(sys.argv[1])
self.mission.execute_mission()
# Wait for the mission returns time_out Failure
def aborting_mission_status_is_reported():
return ReportExecuteMissionState.ABORTING in self.mission.execute_mission_state
self.assertTrue(wait_for(aborting_mission_status_is_reported),
msg='Mission control must report ABORTING')
# Check if the mission control publishes the attitude servo msg to
# set the fins to surface and velocity to 0 RPM
maxCtrlFinAngle = rospy.get_param('/fin_control/max_ctrl_fin_angle')
def attitude_servo_aborting_goals_are_set():
tol = 1e-3
ena_mask = AttitudeServo.PITCH_ENA | AttitudeServo.SPEED_KNOTS_ENA
return (isclose(self.attitude_servo_goal.pitch, -maxCtrlFinAngle,
tol)
and isclose(self.attitude_servo_goal.speed_knots, 0.0, tol)
and self.attitude_servo_goal.ena_mask == ena_mask)
self.assertTrue(wait_for(attitude_servo_aborting_goals_are_set),
msg='Mission control must publish goals')
# Publishes values sent to the actuators
msg = StateStamped()
msg.state.manoeuvring.pose.mean.orientation.x = 0.0
msg.state.manoeuvring.pose.mean.orientation.y = -maxCtrlFinAngle
msg.state.manoeuvring.pose.mean.orientation.z = 0.0
msg.state.manoeuvring.velocity.mean.linear.x = 0.0
msg.state.manoeuvring.velocity.mean.linear.y = 0.0
msg.state.manoeuvring.velocity.mean.linear.z = 0.0
self.simulated_auv_interface_data_pub.publish(msg)
def complete_mission_status_is_reported():
return ReportExecuteMissionState.COMPLETE == self.mission.execute_mission_state[
-1]
self.assertTrue(wait_for(complete_mission_status_is_reported),
msg='Mission control must report COMPLETE')
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()