def test_get_to_root_dir(self):
# normal usage:
get_to_root_dir()
self.assertTrue('ROOTDIR' in os.listdir())
# change to a code directory
os.chdir('src/core/test')
get_to_root_dir()
self.assertTrue('ROOTDIR' in os.listdir())
# check raise error
del os.environ['CODEDIR']
os.chdir('../..')
self.assertRaises(FileNotFoundError, get_to_root_dir)
for model_name in rospy.get_param('/robot/model_name'):
set_model_state(model_name)
# publish reset
self.ros_topic.publishers['/fsm/reset'].publish(Empty())
self._unpause_client.wait_for_service()
self._unpause_client.call()
# gets fsm in taken over state
safe_wait_till_true('kwargs["ros_topic"].topic_values["/fsm/state"].data',
FsmState.TakenOver.name, 2, 0.1, ros_topic=self.ros_topic)
# altitude control brings drone to starting_height
safe_wait_till_true('kwargs["ros_topic"].topic_values["/fsm/state"].data',
FsmState.Running.name, 45, 0.1, ros_topic=self.ros_topic)
# check current height
for agent in ['tracking', 'fleeing']:
z_pos = self.ros_topic.topic_values[rospy.get_param(f'/robot/{agent}_position_sensor/topic')].pose.position.z
self.assertLess(abs(z_pos - height), 0.2)
def tearDown(self) -> None:
self._ros_process.terminate()
shutil.rmtree(self.output_dir, ignore_errors=True)
if __name__ == '__main__':
get_to_root_dir()
unittest.main()
