def start_test(self) -> None:
self.output_dir = f'{get_data_dir(os.environ["CODEDIR"])}/test_dir/{get_filename_without_extension(__file__)}'
os.makedirs(self.output_dir, exist_ok=True)
config = {
'output_path': self.output_dir,
'robot_name': 'turtlebot_sim',
'gazebo': False,
'fsm': False,
'control_mapping': False,
'ros_expert': False,
'waypoint_indicator': False,
'robot_mapping': True
}
# spinoff roslaunch
self._ros_process = RosWrapper(launch_file='load_ros.launch',
config=config,
visible=False)
# create publishers for all relevant sensors < sensor expert
self._pose_topic = rospy.get_param('/robot/position_sensor/topic')
self._pose_type = rospy.get_param('/robot/position_sensor/type')
self._fsm_topic = '/fsm/state'
publish_topics = [
TopicConfig(topic_name=self._pose_topic, msg_type=self._pose_type),
TopicConfig(topic_name=self._fsm_topic, msg_type='String')
]
self.ros_topic = TestPublisherSubscriber(subscribe_topics=[],
publish_topics=publish_topics)
def test_load_params(self):
config = {
'robot_name': 'turtlebot_sim',
'fsm': False,
'output_path': self.output_dir
}
ros_process = RosWrapper(launch_file='load_ros.launch',
config=config,
visible=True)
self.assertEqual(rospy.get_param('/robot/camera_sensor/topic'),
'/wa/camera/image_raw')
ros_process.terminate()
def setUp(self) -> None:
self.config = {
'robot_name': 'test_fsm_robot',
'fsm': True,
'fsm_mode': 'SingleRun',
'world_name': 'test_fsm_world',
'control_mapping': False,
'waypoint_indicator': False,
}
self.output_dir = f'{get_data_dir(os.environ["CODEDIR"])}/test_dir/{get_filename_without_extension(__file__)}'
os.makedirs(self.output_dir, exist_ok=True)
self.config['output_path'] = self.output_dir
# spinoff roslaunch
self._ros_process = RosWrapper(launch_file='load_ros.launch',
config=self.config,
visible=False)
self.delay_evaluation = rospy.get_param('/world/delay_evaluation')
self.state_topic = '/fsm/state'
self.reward_topic = '/fsm/reward'
self.reset_topic = '/fsm/reset'
self.pose_topic = rospy.get_param('/robot/position_sensor/topic')
self.depth_scan_topic = rospy.get_param('/robot/depth_sensor/topic')
self.modified_state_topic = rospy.get_param(
'/robot/modified_state_sensor/topic')
# subscribe to fsm state and reward (fsm's output)
subscribe_topics = [
TopicConfig(topic_name=self.state_topic, msg_type="String"),
TopicConfig(topic_name=self.reward_topic, msg_type="RosReward"),
]
# create publishers for all relevant sensors < FSM
publish_topics = [
TopicConfig(topic_name=self.reset_topic, msg_type='Empty'),
]
for sensor in SensorType:
if rospy.has_param(f'/robot/{sensor.name}_sensor'):
publish_topics.append(
TopicConfig(
topic_name=eval(
f'rospy.get_param("/robot/{sensor.name}_sensor/topic")'
),
msg_type=eval(
f'rospy.get_param("/robot/{sensor.name}_sensor/type")'
)))
self.ros_topic = TestPublisherSubscriber(
subscribe_topics=subscribe_topics, publish_topics=publish_topics)
class TestModifiedStatePublisher(unittest.TestCase):
def test_modified_state_frame_visualizer(self):
self.output_dir = f'{get_data_dir(os.environ["CODEDIR"])}/test_dir/{get_filename_without_extension(__file__)}'
os.makedirs(self.output_dir, exist_ok=True)
config = {
'output_path': self.output_dir,
'modified_state_publisher_mode': 'CombinedGlobalPoses',
'modified_state_frame_visualizer': True,
}
# spinoff roslaunch
self._ros_process = RosWrapper(launch_file='load_ros.launch',
config=config,
visible=True)
# subscribe to modified_state_topic
self.frame_topic = '/modified_state_frame'
subscribe_topics = [
TopicConfig(topic_name=self.frame_topic, msg_type='Image'),
]
# create publishers for modified state
self.modified_state_topic = '/modified_state'
publish_topics = [
TopicConfig(topic_name=self.modified_state_topic,
msg_type='CombinedGlobalPoses'),
]
self.ros_topic = TestPublisherSubscriber(
subscribe_topics=subscribe_topics, publish_topics=publish_topics)
# test center view
message = get_fake_combined_global_poses(0, 0, 1, 3, 1, 1, 0, 0, 0)
rospy.sleep(2)
self.ros_topic.publishers[self.modified_state_topic].publish(message)
safe_wait_till_true(
'"/modified_state_frame" in kwargs["ros_topic"].topic_values.keys()',
True,
3,
0.1,
ros_topic=self.ros_topic)
frame = process_image(
self.ros_topic.topic_values['/modified_state_frame'])
plt.imshow(frame)
plt.show()
a = 100
def tearDown(self) -> None:
self._ros_process.terminate()
shutil.rmtree(self.output_dir, ignore_errors=True)
def start_test(self) -> None:
self.output_dir = f'{get_data_dir(os.environ["CODEDIR"])}/test_dir/{get_filename_without_extension(__file__)}'
os.makedirs(self.output_dir, exist_ok=True)
config = {
'robot_name':
'turtlebot_sim',
'world_name':
'test_waypoints',
'gazebo':
False,
'fsm':
False,
'control_mapping':
False,
'waypoint_indicator':
True,
'ros_expert':
True,
'output_path':
self.output_dir,
'ros_expert_config_file_path_with_extension':
f'src/sim/ros/config/actor/ros_expert.yml'
}
# spinoff roslaunch
self._ros_process = RosWrapper(launch_file='load_ros.launch',
config=config,
visible=False)
# subscribe to command control
self.command_topic = '/actor/ros_expert/cmd_vel'
subscribe_topics = [
TopicConfig(topic_name=self.command_topic, msg_type="Twist"),
]
# create publishers for all relevant sensors < sensor expert
self._depth_topic = rospy.get_param('/robot/depth_sensor/topic')
self._depth_type = rospy.get_param('/robot/depth_sensor/type')
self._pose_topic = rospy.get_param('/robot/position_sensor/topic')
self._pose_type = rospy.get_param('/robot/position_sensor/type')
publish_topics = [
TopicConfig(topic_name=self._depth_topic,
msg_type=self._depth_type),
TopicConfig(topic_name=self._pose_topic, msg_type=self._pose_type)
]
self.ros_topic = TestPublisherSubscriber(
subscribe_topics=subscribe_topics, publish_topics=publish_topics)
def test_launch_and_terminate_gazebo(self):
random_seed = 123
config = {
'random_seed': random_seed,
'gazebo': 'true',
'world_name': 'cube_world',
'output_path': self.output_dir
}
ros_process = RosWrapper(launch_file='load_ros.launch',
config=config,
visible=False)
self.assertEqual(ros_process.get_state(), ProcessState.Running)
time.sleep(5)
self.assertGreaterEqual(count_grep_name('gzserver'), 1)
ros_process.terminate()
self.assertEqual(count_grep_name('gzserver'), 0)
class TestRos(unittest.TestCase):
"""
Basic test that validates position, depth, camera sensors are updated
"""
def setUp(self) -> None:
self.output_dir = f'{get_data_dir(os.environ["CODEDIR"])}/test_dir/{get_filename_without_extension(__file__)}'
os.makedirs(self.output_dir, exist_ok=True)
config = {
'random_seed': 123,
'gazebo': 'true',
'world_name': 'empty',
'robot_name': 'drone_sim',
'output_path': self.output_dir
}
self.ros_process = RosWrapper(launch_file='load_ros.launch',
config=config,
visible=False)
subscribe_topics = [
TopicConfig(
topic_name=rospy.get_param(f'/robot/{sensor}_sensor/topic'),
msg_type=rospy.get_param(f'/robot/{sensor}_sensor/type'))
for sensor in ['camera', 'position', 'depth']
]
self.ros_topic = TestPublisherSubscriber(
subscribe_topics=subscribe_topics, publish_topics=[])
self._unpause_client = rospy.ServiceProxy('/gazebo/unpause_physics',
Emptyservice)
def test_drone_sim(self):
self.assertEqual(self.ros_process.get_state(), ProcessState.Running)
self._unpause_client.wait_for_service()
self._unpause_client(EmptyRequest())
time.sleep(2)
for sensor in [
'camera', 'position', 'depth'
]: # collision < wrench, only published when turned upside down
self.assertTrue(
rospy.get_param(f'/robot/{sensor}_sensor/topic') in
self.ros_topic.topic_values.keys())
def tearDown(self) -> None:
self.ros_process.terminate()
shutil.rmtree(self.output_dir, ignore_errors=True)
def start(self) -> None:
self.output_dir = f'{get_data_dir(os.environ["CODEDIR"])}/test_dir/{get_filename_without_extension(__file__)}'
os.makedirs(self.output_dir, exist_ok=True)
config = {
'robot_name': 'double_drone_sim',
'gazebo': False,
'fsm': False,
'control_mapping': False,
'output_path': self.output_dir,
'modified_state_publisher': True,
'modified_state_publisher_mode': 'CombinedGlobalPoses',
}
# spinoff roslaunch
self._ros_process = RosWrapper(launch_file='load_ros.launch',
config=config,
visible=False)
# subscribe to modified_state_topic
self.modified_state_topic = rospy.get_param(
'/robot/modified_state_sensor/topic')
subscribe_topics = [
TopicConfig(
topic_name=self.modified_state_topic,
msg_type=rospy.get_param('/robot/modified_state_sensor/type')),
]
# create publishers for all topics upon which modified state publisher depends
self.tracking_pose_topic = rospy.get_param(
'/robot/tracking_position_sensor/topic')
self.fleeing_pose_topic = rospy.get_param(
'/robot/fleeing_position_sensor/topic')
publish_topics = [
TopicConfig(topic_name=self.tracking_pose_topic,
msg_type=rospy.get_param(
'/robot/tracking_position_sensor/type')),
TopicConfig(topic_name=self.fleeing_pose_topic,
msg_type=rospy.get_param(
'/robot/fleeing_position_sensor/type'))
]
self.ros_topic = TestPublisherSubscriber(
subscribe_topics=subscribe_topics, publish_topics=publish_topics)
def start(self) -> None:
self.output_dir = f'{get_data_dir(os.environ["CODEDIR"])}/test_dir/{get_filename_without_extension(__file__)}'
os.makedirs(self.output_dir, exist_ok=True)
config = {
'robot_name': 'test_control_mapper',
'fsm': False,
'control_mapping': True,
'control_mapping_config': 'test_control_mapper',
'output_path': self.output_dir,
'waypoint_indicator': False
}
# spinoff roslaunch
self._ros_process = RosWrapper(launch_file='load_ros.launch',
config=config,
visible=False)
# subscribe to robot control topics
self.command_topics = {
'command_a': rospy.get_param('/robot/command_a_topic'),
'command_b': rospy.get_param('/robot/command_b_topic'),
'command_c': rospy.get_param('/robot/command_c_topic')
}
subscribe_topics = [
TopicConfig(topic_name=topic_name, msg_type="Twist") for topic_name in self.command_topics.values()
]
# create publishers for all control topics < control_mapper/default.yml
self._mapping = rospy.get_param('/control_mapping/mapping')
publish_topics = [
TopicConfig(topic_name='/fsm/state', msg_type='String')
]
self._control_topics = [mode[key] for mode in self._mapping.values()
for key in mode.keys()]
self._control_topics = set(self._control_topics)
self.ros_topic = TestPublisherSubscriber(
subscribe_topics=subscribe_topics,
publish_topics=publish_topics + [TopicConfig(topic_name=name, msg_type='Twist')
for name in self._control_topics]
)
def test_full_config(self):
config = {
"output_path": self.output_dir,
"random_seed": 123,
"robot_name": "turtlebot_sim",
"fsm_mode":
"SingleRun", # file with fsm params loaded from config/fsm
"fsm": True,
"control_mapping": True,
"waypoint_indicator": True,
"control_mapping_config": "debug",
"world_name": "debug_turtle",
"x_pos": 0.0,
"y_pos": 0.0,
"z_pos": 0.0,
"yaw_or": 1.57,
"gazebo": True,
}
ros_process = RosWrapper(launch_file='load_ros.launch',
config=config,
visible=False)
ros_process.terminate()
def setUp(self) -> None:
self.output_dir = f'{get_data_dir(os.environ["CODEDIR"])}/test_dir/{get_filename_without_extension(__file__)}'
os.makedirs(self.output_dir, exist_ok=True)
config = {
'random_seed': 123,
'gazebo': 'true',
'world_name': 'empty',
'robot_name': 'drone_sim',
'output_path': self.output_dir
}
self.ros_process = RosWrapper(launch_file='load_ros.launch',
config=config,
visible=False)
subscribe_topics = [
TopicConfig(
topic_name=rospy.get_param(f'/robot/{sensor}_sensor/topic'),
msg_type=rospy.get_param(f'/robot/{sensor}_sensor/type'))
for sensor in ['camera', 'position', 'depth']
]
self.ros_topic = TestPublisherSubscriber(
subscribe_topics=subscribe_topics, publish_topics=[])
self._unpause_client = rospy.ServiceProxy('/gazebo/unpause_physics',
Emptyservice)
def test_april_tag_detector_real_bebop(self):
self.output_dir = f'{get_data_dir(os.environ["CODEDIR"])}/test_dir/{get_filename_without_extension(__file__)}'
os.makedirs(self.output_dir, exist_ok=True)
self._config = {
'output_path': self.output_dir,
'world_name': 'april_tag',
'robot_name': 'bebop_real',
'gazebo': False,
'april_tag_detector': True,
}
# spinoff roslaunch
self._ros_process = RosWrapper(launch_file='load_ros.launch',
config=self._config,
visible=True)
# subscribe to command control
subscribe_topics = [
TopicConfig(topic_name='/reference_ground_point',
msg_type='PointStamped'),
]
self.ros_topic = TestPublisherSubscriber(
subscribe_topics=subscribe_topics, publish_topics=[])
safe_wait_till_true(
'"/reference_ground_point" in kwargs["ros_topic"].topic_values.keys()',
True,
25,
0.1,
ros_topic=self.ros_topic)
while True:
print(
f'waypoint: {self.ros_topic.topic_values["/reference_ground_point"]}'
)
time.sleep(0.5)
def test_drone_keyboard_gazebo_with_KF_waypoints(self):
self.output_dir = f'{get_data_dir(os.environ["CODEDIR"])}/test_dir/{get_filename_without_extension(__file__)}'
os.makedirs(self.output_dir, exist_ok=True)
self._config = {
'output_path': self.output_dir,
'world_name': 'gate_test',
'robot_name': 'drone_sim',
'gazebo': True,
'fsm': True,
'fsm_mode': 'TakeOverRun',
'waypoint_indicator': True,
'control_mapping': True,
'control_mapping_config': 'mathias_controller_keyboard',
'altitude_control': False,
'keyboard': True,
'mathias_controller_with_KF': True,
'mathias_controller_config_file_path_with_extension':
f'{os.environ["CODEDIR"]}/src/sim/ros/config/actor/mathias_controller_with_KF.yml',
'yaw_or': 0,
'x_pos': 0,
'y_pos': 0
}
# spinoff roslaunch
self._ros_process = RosWrapper(launch_file='load_ros.launch',
config=self._config,
visible=True)
# subscribe to command control
self.visualisation_topic = '/actor/mathias_controller/visualisation'
subscribe_topics = [
TopicConfig(
topic_name=rospy.get_param('/robot/position_sensor/topic'),
msg_type=rospy.get_param('/robot/position_sensor/type')),
TopicConfig(topic_name='/fsm/state', msg_type='String'),
TopicConfig(topic_name=self.visualisation_topic, msg_type='Image')
]
self._reference_topic = '/reference_pose'
self._reference_type = 'PointStamped'
publish_topics = [
TopicConfig(topic_name='/fsm/reset', msg_type='Empty'),
TopicConfig(topic_name=self._reference_topic,
msg_type=self._reference_type),
]
self.ros_topic = TestPublisherSubscriber(
subscribe_topics=subscribe_topics, publish_topics=publish_topics)
self._unpause_client = rospy.ServiceProxy('/gazebo/unpause_physics',
Emptyservice)
self._pause_client = rospy.ServiceProxy('/gazebo/pause_physics',
Emptyservice)
safe_wait_till_true(
'"/fsm/state" in kwargs["ros_topic"].topic_values.keys()',
True,
30,
0.1,
ros_topic=self.ros_topic)
measured_data = {}
index = 0
while True:
measured_data[index] = {'x': [], 'y': [], 'z': []}
# publish reset
self.ros_topic.publishers['/fsm/reset'].publish(Empty())
self._unpause_client.wait_for_service()
self._unpause_client.call()
# User flies drone to starting position
while self.ros_topic.topic_values[
"/fsm/state"].data == FsmState.TakenOver.name:
time.sleep(1)
while self.ros_topic.topic_values[
"/fsm/state"].data == FsmState.Running.name:
rospy.sleep(0.1)
odom = self.ros_topic.topic_values[rospy.get_param(
'/robot/position_sensor/topic')]
waypoint = self.ros_topic.topic_values[
'/waypoint_indicator/current_waypoint']
measured_data[index]['x'].append(odom.pose.pose.position.x -
waypoint.data[0])
measured_data[index]['y'].append(odom.pose.pose.position.y -
waypoint.data[1])
measured_data[index]['z'].append(odom.pose.pose.position.z - 1)
# see it reaches the goal state:
# safe_wait_till_true('kwargs["ros_topic"].topic_values["/fsm/state"].data',
# FsmState.Terminated.name, 20, 0.1, ros_topic=self.ros_topic)
colors = ['C0', 'C1', 'C2', 'C3', 'C4']
for key in measured_data.keys():
for a in measured_data[key].keys():
if a == 'x':
style = '-'
elif a == 'y':
style = '--'
else:
style = ':'
plt.plot(measured_data[key][a],
linestyle=style,
color=colors[key % len(colors)],
label=f'{key}: {a}')
plt.legend()
plt.show()
index += 1
index %= len(colors)
def test_drone_keyboard_gazebo_with_KF(self):
self.output_dir = f'{get_data_dir(os.environ["CODEDIR"])}/test_dir/{get_filename_without_extension(__file__)}'
os.makedirs(self.output_dir, exist_ok=True)
self._config = {
'output_path': self.output_dir,
'world_name': 'empty',
'robot_name': 'drone_sim',
'gazebo': True,
'fsm': True,
'fsm_mode': 'TakeOverRun',
'control_mapping': True,
'control_mapping_config': 'mathias_controller_keyboard',
'altitude_control': False,
'keyboard': True,
'mathias_controller_with_KF': True,
'mathias_controller_config_file_path_with_extension':
f'{os.environ["CODEDIR"]}/src/sim/ros/config/actor/mathias_controller_with_KF.yml',
'yaw_or': 0,
'x_pos': 0,
'y_pos': 0
}
# spinoff roslaunch
self._ros_process = RosWrapper(launch_file='load_ros.launch',
config=self._config,
visible=True)
# subscribe to command control
self.visualisation_topic = '/actor/mathias_controller/visualisation'
subscribe_topics = [
TopicConfig(
topic_name=rospy.get_param('/robot/position_sensor/topic'),
msg_type=rospy.get_param('/robot/position_sensor/type')),
TopicConfig(topic_name='/fsm/state', msg_type='String'),
TopicConfig(topic_name=self.visualisation_topic, msg_type='Image')
]
self._reference_topic = '/reference_pose'
self._reference_type = 'PointStamped'
publish_topics = [
TopicConfig(topic_name='/fsm/reset', msg_type='Empty'),
TopicConfig(topic_name=self._reference_topic,
msg_type=self._reference_type),
]
self.ros_topic = TestPublisherSubscriber(
subscribe_topics=subscribe_topics, publish_topics=publish_topics)
self._unpause_client = rospy.ServiceProxy('/gazebo/unpause_physics',
Emptyservice)
self._pause_client = rospy.ServiceProxy('/gazebo/pause_physics',
Emptyservice)
safe_wait_till_true(
'"/fsm/state" in kwargs["ros_topic"].topic_values.keys()',
True,
30,
0.1,
ros_topic=self.ros_topic)
measured_data = {}
index = 0
while True:
# publish reset
self.ros_topic.publishers['/fsm/reset'].publish(Empty())
self._unpause_client.wait_for_service()
self._unpause_client.call()
# index = self.tweak_combined_axis_keyboard(measured_data, index, [0, 0, 0])
index = self.tweak_combined_axis_keyboard(measured_data, index,
[3, 1, 0])
def test_launch_and_terminate_ros(self):
ros_process = RosWrapper(launch_file='empty_ros.launch',
config={'output_path': self.output_dir})
self.assertEqual(ros_process.get_state(), ProcessState.Running)
self.assertTrue(count_grep_name('ros') > 0)
ros_process.terminate()
def test_takeoff_double_drone_sim(self):
config = {
"output_path": self.output_dir,
"random_seed": 123,
"robot_name": "double_drone_sim",
"world_name": "empty",
"gazebo": True,
}
ros_process = RosWrapper(launch_file='load_ros.launch',
config=config,
visible=False)
self._unpause_client = rospy.ServiceProxy('/gazebo/unpause_physics',
Emptyservice)
self._pause_client = rospy.ServiceProxy('/gazebo/pause_physics',
Emptyservice)
self.ros_topic = TestPublisherSubscriber(
subscribe_topics=[
TopicConfig(topic_name=rospy.get_param(
'/robot/tracking_position_sensor/topic'),
msg_type=rospy.get_param(
'/robot/tracking_position_sensor/type')),
TopicConfig(topic_name=rospy.get_param(
'/robot/fleeing_position_sensor/topic'),
msg_type=rospy.get_param(
'/robot/fleeing_position_sensor/type'))
],
publish_topics=[
TopicConfig(topic_name=rospy.get_param(
'/robot/tracking_command_topic'),
msg_type='Twist'),
TopicConfig(
topic_name=rospy.get_param('/robot/fleeing_command_topic'),
msg_type='Twist')
])
# unpause gazebo to receive messages
self._unpause_client.wait_for_service()
self._unpause_client.call()
safe_wait_till_true(
f'\'{rospy.get_param("/robot/tracking_position_sensor/topic")}\' '
f'in kwargs["ros_topic"].topic_values.keys()',
True,
5,
0.1,
ros_topic=self.ros_topic)
rospy.wait_for_service('/tracking/enable_motors')
enable_motors_service_tracking = rospy.ServiceProxy(
'/tracking/enable_motors', EnableMotors)
rospy.wait_for_service('/fleeing/enable_motors')
enable_motors_service_fleeing = rospy.ServiceProxy(
'/fleeing/enable_motors', EnableMotors)
# set gazebo model state
self._set_model_state = rospy.ServiceProxy('/gazebo/set_model_state',
SetModelState)
def set_model_state(name: str):
model_state = ModelState()
model_state.pose = Pose()
model_state.model_name = name
model_state.pose.position.x = 0
if 'fleeing' in name:
model_state.pose.position.x += 3. # fixed distance
model_state.pose.position.y = 0
model_state.pose.position.z = 0
yaw = 0 if 'fleeing' not in name else 3.14
model_state.pose.orientation.x, model_state.pose.orientation.y, model_state.pose.orientation.z, \
model_state.pose.orientation.w = quaternion_from_euler((0, 0, yaw))
self._set_model_state.wait_for_service()
self._set_model_state(model_state)
self._pause_client.wait_for_service()
self._pause_client.call()
for _ in range(3):
for model_name in rospy.get_param('/robot/model_name'):
set_model_state(model_name)
self._unpause_client.wait_for_service()
self._unpause_client.call()
# start motors
enable_motors_service_tracking.call(True)
time.sleep(3)
enable_motors_service_fleeing.call(True)
# fly up till reference height
reference = 1.
errors = [1]
while np.mean(errors) > 0.1:
for agent in ['tracking', 'fleeing']:
position = self.ros_topic.topic_values[rospy.get_param(
f'/robot/{agent}_position_sensor/topic')].pose.position
print(f'{agent}: {position.z}')
errors.append(np.abs(reference - position.z))
twist = Twist()
twist.linear.z = +0.5 if position.z < reference else -0.5
self.ros_topic.publishers[rospy.get_param(
f'/robot/{agent}_command_topic')].publish(twist)
while len(errors) > 10:
errors.pop(0)
time.sleep(0.1)
final_error = abs(self.ros_topic.topic_values[rospy.get_param(
'/robot/tracking_position_sensor/topic')].pose.position.z -
reference)
self.assertLess(final_error, 0.2)
final_error = abs(self.ros_topic.topic_values[rospy.get_param(
'/robot/fleeing_position_sensor/topic')].pose.position.z -
reference)
self.assertLess(final_error, 0.2)
self._pause_client.wait_for_service()
self._pause_client(EmptyRequest())
# enable_motors_service_tracking.call(False)
# time.sleep(3)
# enable_motors_service_fleeing.call(False)
ros_process.terminate()
def test_start_up_and_send_random_input(self):
self.output_dir = f'{get_data_dir(os.environ["CODEDIR"])}/test_dir/{get_filename_without_extension(__file__)}'
os.makedirs(self.output_dir, exist_ok=True)
self._config = {
'output_path': self.output_dir,
'world_name': 'default',
'robot_name': 'bebop_real',
'gazebo': False,
'fsm': False,
'control_mapping': False,
'waypoint_indicator': False,
'altitude_control': False,
'mathias_controller_with_KF': False,
'keyboard': False,
'robot_display': False,
}
# spinoff roslaunch
self._ros_process = RosWrapper(launch_file='load_ros.launch',
config=self._config,
visible=True)
# subscribe to command control
publish_topics = [
TopicConfig(topic_name='/fsm/reset', msg_type='Empty'),
TopicConfig(topic_name='/fsm/state', msg_type='String'),
TopicConfig(topic_name='/bebop/cmd_vel', msg_type='Twist'),
TopicConfig(topic_name='/bebop/image_raw', msg_type='Image'),
TopicConfig(topic_name='/mask', msg_type='Image'),
TopicConfig(topic_name='/reference_pose', msg_type='PointStamped'),
TopicConfig(topic_name=
'/bebop/states/common/CommonState/BatteryStateChanged',
msg_type='CommonCommonStateBatteryStateChanged')
]
self.ros_topic = TestPublisherSubscriber(subscribe_topics=[],
publish_topics=publish_topics)
while not rospy.is_shutdown():
# put something random on each topic every second
# reset
if np.random.binomial(1, 0.2) == 1:
self.ros_topic.publishers['/fsm/reset'].publish(Empty())
state = np.random.choice(
['Unknown', 'Running', 'TakenOver', 'Terminated', 'DriveBack'])
self.ros_topic.publishers['/fsm/state'].publish(String(state))
battery = CommonCommonStateBatteryStateChanged()
battery.percent = np.random.randint(0, 100, dtype=np.uint8)
self.ros_topic.publishers[
'/bebop/states/common/CommonState/BatteryStateChanged'].publish(
battery)
self.ros_topic.publishers['/bebop/cmd_vel'].publish(
get_random_twist())
self.ros_topic.publishers['/bebop/image_raw'].publish(
get_random_image((200, 200, 3)))
self.ros_topic.publishers['/mask'].publish(
get_random_image((200, 200, 1)))
# self.ros_topic.publishers['/reference_pose'].publish(get_random_reference_pose())
self.ros_topic.publishers['/reference_pose'].publish(
PointStamped(point=Point(x=2, y=0.5, z=0.5)))
rospy.sleep(0.1)
def test_drone_relative_positioning_real_bebop(self):
self.output_dir = f'{get_data_dir(os.environ["CODEDIR"])}/test_dir/{get_filename_without_extension(__file__)}'
os.makedirs(self.output_dir, exist_ok=True)
self._config = {
'output_path':
self.output_dir,
'world_name':
'empty',
'robot_name':
'bebop_real',
'gazebo':
False,
'fsm':
True,
'fsm_mode':
'TakeOverRun',
'control_mapping':
True,
'control_mapping_config':
'mathias_controller_keyboard',
'altitude_control':
False,
'keyboard':
True,
'mathias_controller':
True,
'mathias_controller_config_file_path_with_extension':
f'{os.environ["CODEDIR"]}/src/sim/ros/config/actor/mathias_controller_real_bebop.yml'
}
# spinoff roslaunch
self._ros_process = RosWrapper(launch_file='load_ros.launch',
config=self._config,
visible=True)
self.visualisation_topic = None
# subscribe to command control
subscribe_topics = [
TopicConfig(
topic_name=rospy.get_param('/robot/position_sensor/topic'),
msg_type=rospy.get_param('/robot/position_sensor/type')),
TopicConfig(topic_name='/fsm/state', msg_type='String')
]
self._reference_topic = '/reference_pose'
self._reference_type = 'PointStamped'
publish_topics = [
TopicConfig(topic_name='/fsm/reset', msg_type='Empty'),
TopicConfig(topic_name=self._reference_topic,
msg_type=self._reference_type),
]
self.ros_topic = TestPublisherSubscriber(
subscribe_topics=subscribe_topics, publish_topics=publish_topics)
safe_wait_till_true(
'"/fsm/state" in kwargs["ros_topic"].topic_values.keys()',
True,
10,
0.1,
ros_topic=self.ros_topic)
measured_data = {}
index = 0
while True:
# publish reset
self.ros_topic.publishers['/fsm/reset'].publish(Empty())
# index = self.tweak_steady_pose(measured_data, index)
#index = self.tweak_separate_axis_keyboard(measured_data, index, axis=0)
index = self.tweak_combined_axis_keyboard(
measured_data, index, point_ref_drone=[0.5, 0.5, 0.5])
class TestModifiedStatePublisher(unittest.TestCase):
def start(self) -> None:
self.output_dir = f'{get_data_dir(os.environ["CODEDIR"])}/test_dir/{get_filename_without_extension(__file__)}'
os.makedirs(self.output_dir, exist_ok=True)
config = {
'robot_name': 'double_drone_sim',
'gazebo': False,
'fsm': False,
'control_mapping': False,
'output_path': self.output_dir,
'modified_state_publisher': True,
'modified_state_publisher_mode': 'CombinedGlobalPoses',
}
# spinoff roslaunch
self._ros_process = RosWrapper(launch_file='load_ros.launch',
config=config,
visible=False)
# subscribe to modified_state_topic
self.modified_state_topic = rospy.get_param(
'/robot/modified_state_sensor/topic')
subscribe_topics = [
TopicConfig(
topic_name=self.modified_state_topic,
msg_type=rospy.get_param('/robot/modified_state_sensor/type')),
]
# create publishers for all topics upon which modified state publisher depends
self.tracking_pose_topic = rospy.get_param(
'/robot/tracking_position_sensor/topic')
self.fleeing_pose_topic = rospy.get_param(
'/robot/fleeing_position_sensor/topic')
publish_topics = [
TopicConfig(topic_name=self.tracking_pose_topic,
msg_type=rospy.get_param(
'/robot/tracking_position_sensor/type')),
TopicConfig(topic_name=self.fleeing_pose_topic,
msg_type=rospy.get_param(
'/robot/fleeing_position_sensor/type'))
]
self.ros_topic = TestPublisherSubscriber(
subscribe_topics=subscribe_topics, publish_topics=publish_topics)
def test_modified_state_publisher(self):
self.start()
time.sleep(5)
y_pos = 3
tracking_pose = get_fake_pose_stamped(1, 2, 3, 0, 0, -0.258819,
0.9659258)
fleeing_pose = get_fake_pose_stamped(4, 5, 6)
self.ros_topic.publishers[self.tracking_pose_topic].publish(
tracking_pose)
self.ros_topic.publishers[self.fleeing_pose_topic].publish(
fleeing_pose)
# wait safely
max_duration = 30
stime = time.time()
while self.modified_state_topic not in self.ros_topic.topic_values.keys() \
and time.time() - stime < max_duration:
time.sleep(0.1)
self.assertTrue(time.time() - stime < max_duration)
time.sleep(1)
roll, pitch, yaw = euler_from_quaternion(
(tracking_pose.pose.orientation.x,
tracking_pose.pose.orientation.y,
tracking_pose.pose.orientation.z,
tracking_pose.pose.orientation.w))
modified_state = self.ros_topic.topic_values[self.modified_state_topic]
self.assertTrue(
modified_state.tracking_x == tracking_pose.pose.position.x)
self.assertTrue(
modified_state.tracking_y == tracking_pose.pose.position.y)
self.assertTrue(
modified_state.tracking_z == tracking_pose.pose.position.z)
self.assertTrue(
modified_state.fleeing_x == fleeing_pose.pose.position.x)
self.assertTrue(
modified_state.fleeing_y == fleeing_pose.pose.position.y)
self.assertTrue(
modified_state.fleeing_z == fleeing_pose.pose.position.z)
self.assertTrue(modified_state.tracking_roll == roll)
self.assertTrue(modified_state.tracking_pitch == pitch)
self.assertAlmostEqual(modified_state.tracking_yaw, yaw)
def tearDown(self) -> None:
self._ros_process.terminate()
shutil.rmtree(self.output_dir, ignore_errors=True)
def test_april_tag_detector_gazebo_KF(self):
self.output_dir = f'{get_data_dir(os.environ["CODEDIR"])}/test_dir/{get_filename_without_extension(__file__)}'
os.makedirs(self.output_dir, exist_ok=True)
self._config = {
'output_path':
self.output_dir,
'world_name':
'april_tag',
'robot_name':
'drone_sim_down_cam',
'gazebo':
True,
'fsm':
True,
'fsm_mode':
'TakeOverRun',
'control_mapping':
True,
'control_mapping_config':
'mathias_controller_keyboard',
'april_tag_detector':
True,
'altitude_control':
False,
'mathias_controller_with_KF':
True,
'starting_height':
1.,
'keyboard':
True,
'mathias_controller_config_file_path_with_extension':
f'{os.environ["CODEDIR"]}/src/sim/ros/config/actor/mathias_controller_with_KF.yml',
}
# spinoff roslaunch
self._ros_process = RosWrapper(launch_file='load_ros.launch',
config=self._config,
visible=True)
# subscribe to command control
self.visualisation_topic = '/actor/mathias_controller/visualisation'
subscribe_topics = [
TopicConfig(
topic_name=rospy.get_param('/robot/position_sensor/topic'),
msg_type=rospy.get_param('/robot/position_sensor/type')),
TopicConfig(topic_name='/fsm/state', msg_type='String'),
TopicConfig(topic_name='/reference_pose', msg_type='PointStamped'),
TopicConfig(topic_name=self.visualisation_topic, msg_type='Image')
]
publish_topics = [
TopicConfig(topic_name='/fsm/reset', msg_type='Empty'),
]
self.ros_topic = TestPublisherSubscriber(
subscribe_topics=subscribe_topics, publish_topics=publish_topics)
safe_wait_till_true(
'"/fsm/state" in kwargs["ros_topic"].topic_values.keys()',
True,
235,
0.1,
ros_topic=self.ros_topic)
self.assertEqual(self.ros_topic.topic_values['/fsm/state'].data,
FsmState.Unknown.name)
index = 0
while True:
print(f'start loop: {index} with resetting')
# publish reset
self.ros_topic.publishers['/fsm/reset'].publish(Empty())
rospy.sleep(0.5)
print(f'waiting in overtake state')
while self.ros_topic.topic_values[
"/fsm/state"].data != FsmState.Running.name:
rospy.sleep(0.5)
# safe_wait_till_true('"/reference_ground_point" in kwargs["ros_topic"].topic_values.keys()',
# True, 10, 0.1, ros_topic=self.ros_topic)
waypoints = []
print(f'waiting in running state')
while self.ros_topic.topic_values[
"/fsm/state"].data != FsmState.TakenOver.name:
if '/reference_pose' in self.ros_topic.topic_values.keys() \
and '/bebop/odom' in self.ros_topic.topic_values.keys():
odom = self.ros_topic.topic_values[rospy.get_param(
'/robot/position_sensor/topic')]
point = transform(
[self.ros_topic.topic_values['/reference_pose'].point],
orientation=odom.pose.pose.orientation,
translation=odom.pose.pose.position)[0]
waypoints.append(point)
rospy.sleep(0.5)
if len(waypoints) != 0:
plt.clf()
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.scatter([_.x for _ in waypoints], [_.y for _ in waypoints],
[_.z for _ in waypoints],
label='waypoints')
ax.legend()
plt.savefig(os.path.join(self.output_dir,
f'image_{index}.jpg'))
# plt.show()
index += 1
class TestTakeOffAndAltitudeControl(unittest.TestCase):
@unittest.skip
def test_single_drone(self) -> None:
self.output_dir = f'{get_data_dir(os.environ["CODEDIR"])}/test_dir/{get_filename_without_extension(__file__)}'
os.makedirs(self.output_dir, exist_ok=True)
height = 5
self._config = {
'output_path': self.output_dir,
'world_name': 'empty',
'robot_name': 'drone_sim',
'gazebo': True,
'fsm': True,
'fsm_mode': 'TakeOverRun',
'control_mapping': True,
'control_mapping_config': 'ros_expert',
'altitude_control': True,
'ros_expert': True,
'starting_height': height
}
# spinoff roslaunch
self._ros_process = RosWrapper(launch_file='load_ros.launch',
config=self._config,
visible=True)
# subscribe to command control
subscribe_topics = [
TopicConfig(topic_name=rospy.get_param('/robot/position_sensor/topic'),
msg_type=rospy.get_param('/robot/position_sensor/type')),
TopicConfig(topic_name='/fsm/state',
msg_type='String')
]
publish_topics = [
TopicConfig(topic_name='/fsm/reset', msg_type='Empty')
]
self.ros_topic = TestPublisherSubscriber(
subscribe_topics=subscribe_topics,
publish_topics=publish_topics
)
self._unpause_client = rospy.ServiceProxy('/gazebo/unpause_physics', Emptyservice)
self._pause_client = rospy.ServiceProxy('/gazebo/pause_physics', Emptyservice)
self._set_model_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
# unpause gazebo to receive messages
self._unpause_client.wait_for_service()
self._unpause_client.call()
safe_wait_till_true('"/fsm/state" in kwargs["ros_topic"].topic_values.keys()',
True, 10, 0.1, ros_topic=self.ros_topic)
self.assertEqual(self.ros_topic.topic_values['/fsm/state'].data, FsmState.Unknown.name)
# pause again before start
self._pause_client.wait_for_service()
self._pause_client.call()
self._set_model_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
for _ in range(3):
print(f'round: {_}')
# set gazebo model state
model_state = ModelState()
model_state.model_name = 'quadrotor'
model_state.pose = Pose()
self._set_model_state.wait_for_service()
self._set_model_state(model_state)
# 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
z_pos = self.ros_topic.topic_values[rospy.get_param('/robot/position_sensor/topic')].pose.pose.position.z
print(f'final_height: {z_pos}')
self.assertLess(abs(z_pos - height), 0.2)
self._pause_client.wait_for_service()
self._pause_client.call()
# @unittest.skip
def test_double_drone(self) -> None:
self.output_dir = f'{get_data_dir(os.environ["CODEDIR"])}/test_dir/{get_filename_without_extension(__file__)}'
os.makedirs(self.output_dir, exist_ok=True)
height = 5
self._config = {
'output_path': self.output_dir,
'world_name': 'empty',
'robot_name': 'double_drone_sim',
'gazebo': True,
'fsm': True,
'fsm_mode': 'TakeOverRun',
'control_mapping': True,
'control_mapping_config': 'python_adversarial',
'altitude_control': True,
'ros_expert': True,
'starting_height': height
}
# spinoff roslaunch
self._ros_process = RosWrapper(launch_file='load_ros.launch',
config=self._config,
visible=False)
# subscribe to command control
subscribe_topics = [
TopicConfig(topic_name=rospy.get_param('/robot/tracking_position_sensor/topic'),
msg_type=rospy.get_param('/robot/tracking_position_sensor/type')),
TopicConfig(topic_name=rospy.get_param('/robot/fleeing_position_sensor/topic'),
msg_type=rospy.get_param('/robot/fleeing_position_sensor/type')),
TopicConfig(topic_name='/fsm/state',
msg_type='String')
]
publish_topics = [
TopicConfig(topic_name='/fsm/reset', msg_type='Empty')
]
self.ros_topic = TestPublisherSubscriber(
subscribe_topics=subscribe_topics,
publish_topics=publish_topics
)
self._unpause_client = rospy.ServiceProxy('/gazebo/unpause_physics', Emptyservice)
self._pause_client = rospy.ServiceProxy('/gazebo/pause_physics', Emptyservice)
self._set_model_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
# unpause gazebo to receive messages
self._unpause_client.wait_for_service()
self._unpause_client.call()
safe_wait_till_true('"/fsm/state" in kwargs["ros_topic"].topic_values.keys()',
True, 10, 0.1, ros_topic=self.ros_topic)
self.assertEqual(self.ros_topic.topic_values['/fsm/state'].data,
FsmState.Unknown.name)
# pause again before start
self._pause_client.wait_for_service()
self._pause_client.call()
self._set_model_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
def set_model_state(name: str):
model_state = ModelState()
model_state.pose = Pose()
model_state.model_name = name
model_state.pose.position.x = 0
if 'fleeing' in name:
model_state.pose.position.x += 3. # fixed distance
model_state.pose.position.y = 0
model_state.pose.position.z = 0
yaw = 0 if 'fleeing' not in name else 3.14
model_state.pose.orientation.x, model_state.pose.orientation.y, model_state.pose.orientation.z, \
model_state.pose.orientation.w = quaternion_from_euler((0, 0, yaw))
self._set_model_state.wait_for_service()
self._set_model_state(model_state)
for _ in range(3):
print(f'round: {_}')
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)
def __init__(self, config: EnvironmentConfig):
super().__init__(config)
self._pause_period = 1. / config.ros_config.step_rate_fps
roslaunch_arguments = config.ros_config.ros_launch_config.__dict__
# Add automatically added values according to robot_name, world_name, actor_configs
if config.ros_config.actor_configs is not None:
for actor_config in config.ros_config.actor_configs:
roslaunch_arguments[actor_config.name] = True
if actor_config.file is not None:
config_file = actor_config.file if actor_config.file.startswith('/') \
else os.path.join(os.environ['CODEDIR'], actor_config.file)
roslaunch_arguments[
f'{actor_config.name}_config_file_path_with_extension'] = config_file
# check if world config file exists
assert os.path.isfile(
os.path.join(os.environ["PWD"], 'src/sim/ros/config/world/',
roslaunch_arguments['world_name']) + '.yml')
self._ros = RosWrapper(config=roslaunch_arguments,
launch_file='load_ros.launch',
visible=config.ros_config.visible_xterm)
# Fields
self._step = 0
self._return = 0
self._current_experience = None
self._previous_observation = None
# Services
if self._config.ros_config.ros_launch_config.gazebo:
self._pause_client = rospy.ServiceProxy('/gazebo/pause_physics',
Emptyservice)
self._unpause_client = rospy.ServiceProxy(
'/gazebo/unpause_physics', Emptyservice)
self._set_model_state = rospy.ServiceProxy(
'/gazebo/set_model_state', SetModelState)
# Subscribers
# FSM
self.fsm_state = None
rospy.Subscriber(name='/fsm/state',
data_class=String,
callback=self._set_field,
callback_args=('fsm_state', {}))
# Reward topic
self.reward = None
self.terminal_state = TerminationType.Unknown
rospy.Subscriber(name='/fsm/reward',
data_class=RosReward,
callback=self._set_field,
callback_args=('reward', {}))
# Subscribe to observation
self.observation = None
if self._config.ros_config.observation != '':
sensor = SensorType[self._config.ros_config.observation]
rospy.Subscriber(
name=rospy.get_param(f'/robot/{sensor.name}_sensor/topic'),
data_class=eval(
rospy.get_param(f'/robot/{sensor.name}_sensor/type')),
callback=self._set_field,
callback_args=('observation',
rospy.get_param(
f'/robot/{sensor.name}_sensor/stats', {})))
# Subscribe to action
self.action = None
if self._config.ros_config.action_topic != 'python':
rospy.Subscriber(name=self._config.ros_config.action_topic,
data_class=Twist,
callback=self._set_field,
callback_args=('action', {}))
# Add info sensors
self.info = {}
if self._config.ros_config.info is not None:
self._subscribe_info()
# Publishers
self._reset_publisher = rospy.Publisher('/fsm/reset',
Empty,
queue_size=10)
self._action_publishers = [
rospy.Publisher(
f'/ros_python_interface/cmd_vel{f"_{i}" if i != 0 else ""}',
Twist,
queue_size=10)
for i in range(self._config.ros_config.num_action_publishers)
]
# Catch kill signals:
signal.signal(signal.SIGTERM, self._signal_handler)
# Start ROS node:
rospy.init_node('ros_python_interface', anonymous=True)
# assert fsm has started properly
if self._config.ros_config.ros_launch_config.fsm:
cprint('Wait till fsm has started properly.', self._logger)
while self.fsm_state is None:
self._run_shortly()
# assert all experience fields are updated once:
if self._config.ros_config.observation != '':
cprint('Wait till observation sensor has started properly.',
self._logger)
while self.observation is None:
self._run_shortly()
cprint('ready', self._logger)
class RosEnvironment(Environment):
def __init__(self, config: EnvironmentConfig):
super().__init__(config)
self._pause_period = 1. / config.ros_config.step_rate_fps
roslaunch_arguments = config.ros_config.ros_launch_config.__dict__
# Add automatically added values according to robot_name, world_name, actor_configs
if config.ros_config.actor_configs is not None:
for actor_config in config.ros_config.actor_configs:
roslaunch_arguments[actor_config.name] = True
if actor_config.file is not None:
config_file = actor_config.file if actor_config.file.startswith('/') \
else os.path.join(os.environ['CODEDIR'], actor_config.file)
roslaunch_arguments[
f'{actor_config.name}_config_file_path_with_extension'] = config_file
# check if world config file exists
assert os.path.isfile(
os.path.join(os.environ["PWD"], 'src/sim/ros/config/world/',
roslaunch_arguments['world_name']) + '.yml')
self._ros = RosWrapper(config=roslaunch_arguments,
launch_file='load_ros.launch',
visible=config.ros_config.visible_xterm)
# Fields
self._step = 0
self._return = 0
self._current_experience = None
self._previous_observation = None
# Services
if self._config.ros_config.ros_launch_config.gazebo:
self._pause_client = rospy.ServiceProxy('/gazebo/pause_physics',
Emptyservice)
self._unpause_client = rospy.ServiceProxy(
'/gazebo/unpause_physics', Emptyservice)
self._set_model_state = rospy.ServiceProxy(
'/gazebo/set_model_state', SetModelState)
# Subscribers
# FSM
self.fsm_state = None
rospy.Subscriber(name='/fsm/state',
data_class=String,
callback=self._set_field,
callback_args=('fsm_state', {}))
# Reward topic
self.reward = None
self.terminal_state = TerminationType.Unknown
rospy.Subscriber(name='/fsm/reward',
data_class=RosReward,
callback=self._set_field,
callback_args=('reward', {}))
# Subscribe to observation
self.observation = None
if self._config.ros_config.observation != '':
sensor = SensorType[self._config.ros_config.observation]
rospy.Subscriber(
name=rospy.get_param(f'/robot/{sensor.name}_sensor/topic'),
data_class=eval(
rospy.get_param(f'/robot/{sensor.name}_sensor/type')),
callback=self._set_field,
callback_args=('observation',
rospy.get_param(
f'/robot/{sensor.name}_sensor/stats', {})))
# Subscribe to action
self.action = None
if self._config.ros_config.action_topic != 'python':
rospy.Subscriber(name=self._config.ros_config.action_topic,
data_class=Twist,
callback=self._set_field,
callback_args=('action', {}))
# Add info sensors
self.info = {}
if self._config.ros_config.info is not None:
self._subscribe_info()
# Publishers
self._reset_publisher = rospy.Publisher('/fsm/reset',
Empty,
queue_size=10)
self._action_publishers = [
rospy.Publisher(
f'/ros_python_interface/cmd_vel{f"_{i}" if i != 0 else ""}',
Twist,
queue_size=10)
for i in range(self._config.ros_config.num_action_publishers)
]
# Catch kill signals:
signal.signal(signal.SIGTERM, self._signal_handler)
# Start ROS node:
rospy.init_node('ros_python_interface', anonymous=True)
# assert fsm has started properly
if self._config.ros_config.ros_launch_config.fsm:
cprint('Wait till fsm has started properly.', self._logger)
while self.fsm_state is None:
self._run_shortly()
# assert all experience fields are updated once:
if self._config.ros_config.observation != '':
cprint('Wait till observation sensor has started properly.',
self._logger)
while self.observation is None:
self._run_shortly()
cprint('ready', self._logger)
def _set_field(self, msg, args: Tuple) -> None:
field_name, sensor_stats = args
if field_name == 'fsm_state':
self.fsm_state = FsmState[msg.data]
elif field_name == 'observation':
msg_type = camelcase_to_snake_format(ros_message_to_type_str(msg))
self.observation = eval(f'process_{msg_type}(msg, sensor_stats)')
elif field_name == 'action':
self.action = Action(
actor_name=self._config.ros_config.action_topic,
value=process_twist(msg).value)
elif field_name == 'reward':
self.reward = msg.reward
self.terminal_state = TerminationType[msg.termination]
elif field_name.startswith('info:'):
info_key = field_name[5:]
msg_type = camelcase_to_snake_format(ros_message_to_type_str(msg))
self.info[info_key] = eval(
f'process_{msg_type}(msg, sensor_stats)')
else:
raise NotImplementedError(f'{field_name}: {msg}')
cprint(f'set field {field_name}',
self._logger,
msg_type=MessageType.debug)
def _subscribe_info(self) -> None:
for info in self._config.ros_config.info:
if info in [s.name for s in SensorType]:
sensor_topic = rospy.get_param(f'/robot/{info}_sensor/topic')
sensor_type = rospy.get_param(f'/robot/{info}_sensor/type')
sensor_stats = rospy.get_param(f'/robot/{info}_sensor/stats',
{})
rospy.Subscriber(name=sensor_topic,
data_class=eval(sensor_type),
callback=self._set_field,
callback_args=(f'info:{info}', sensor_stats))
elif info == 'current_waypoint':
# add waypoint subscriber as extra sensor
rospy.Subscriber(name='/waypoint_indicator/current_waypoint',
data_class=Float32MultiArray,
callback=self._set_field,
callback_args=('info:current_waypoint', {}))
elif info.startswith(
'/'
): # subscribe to rostopic for action, expecting twist message
rospy.Subscriber(name=info,
data_class=Twist,
callback=self._set_field,
callback_args=(f'info:{info}', {}))
self.info[info] = None
def _signal_handler(self, signal_number: int, _) -> None:
return_value = self.remove()
cprint(f'received signal {signal_number}.',
self._logger,
msg_type=MessageType.info if return_value
== ProcessState.Terminated else MessageType.error)
sys.exit(0)
def _internal_update_terminal_state(self):
if self.fsm_state == FsmState.Running and \
self._config.max_number_of_steps != -1 and \
self._config.max_number_of_steps <= self._step:
self.terminal_state = TerminationType.Done
cprint(
f'reach max number of steps {self._config.max_number_of_steps} < {self._step}',
self._logger)
def _pause_gazebo(self):
assert self._config.ros_config.ros_launch_config.gazebo
self._pause_client.wait_for_service()
self._pause_client(EmptyRequest())
#os.system("rosservice call gazebo/pause_physics")
def _unpause_gazebo(self):
assert self._config.ros_config.ros_launch_config.gazebo
self._unpause_client.wait_for_service()
self._unpause_client(EmptyRequest())
#os.system("rosservice call gazebo/unpause_physics")
def _reset_gazebo(self):
def set_model_state(name: str):
model_state = ModelState()
model_state.pose = Pose()
model_state.model_name = name
model_state.pose.position.x = self._config.ros_config.ros_launch_config.x_pos
if 'fleeing' in name:
model_state.pose.position.x += self._config.ros_config.ros_launch_config.distance_tracking_fleeing_m
model_state.pose.position.y = self._config.ros_config.ros_launch_config.y_pos
model_state.pose.position.z = self._config.ros_config.ros_launch_config.z_pos
yaw = self._config.ros_config.ros_launch_config.yaw_or if 'fleeing' not in name else \
self._config.ros_config.ros_launch_config.yaw_or + 3.14
model_state.pose.orientation.x, model_state.pose.orientation.y, model_state.pose.orientation.z, \
model_state.pose.orientation.w = quaternion_from_euler((0, 0, yaw))
self._set_model_state.wait_for_service()
self._set_model_state(model_state)
model_name = rospy.get_param('/robot/model_name')
if isinstance(model_name, list):
for model in model_name:
set_model_state(model)
else:
set_model_state(model_name)
def _clear_experience_values(self):
"""Set all experience fields to None"""
self.observation = None
if self._config.ros_config.action_topic != 'python':
self.action = None
self.reward = None
self.terminal_state = None
self.info = {
k: None
for k in self.info.keys()
if k != 'unfiltered_reward' and k != 'return'
}
if 'return' in self.info.keys():
del self.info['return']
def _update_current_experience(self) -> bool:
"""
If all experience fields are updated,
store all experience fields in _current_experience fields end return True
else False.
:return: Bool whether all fields are updated
"""
self._internal_update_terminal_state(
) # check count_steps for termination
if self._config.ros_config.observation != '' and self.observation is None:
cprint("waiting for observation",
self._logger,
msg_type=MessageType.debug)
return False
if self.reward is None:
cprint("waiting for reward",
self._logger,
msg_type=MessageType.debug)
return False
if self.terminal_state is None:
cprint("waiting for terminal state",
self._logger,
msg_type=MessageType.debug)
return False
if self.action is None and self.terminal_state == TerminationType.NotDone:
# Don't wait for next action if episode is finished
cprint("waiting for action",
self._logger,
msg_type=MessageType.debug)
return False
if None in [v for v in self.info.values() if not isinstance(v, Iterable)] and \
self.terminal_state == TerminationType.NotDone: # Don't wait for next info if episode is finished:
cprint("waiting for info",
self._logger,
msg_type=MessageType.debug)
return False
self.observation = self._filter_observation(self.observation)
self.info['unfiltered_reward'] = deepcopy(self.reward)
self._return += self.reward
self.reward = self._filter_reward(self.reward)
if self.terminal_state in [
TerminationType.Done, TerminationType.Success,
TerminationType.Failure
]:
self.info['return'] = self._return
self._current_experience = Experience(
done=deepcopy(self.terminal_state),
observation=deepcopy(self._previous_observation),
action=deepcopy(self.action),
reward=deepcopy(self.reward),
time_stamp=int(rospy.get_time() * 10**3),
info={
field_name: deepcopy(self.info[field_name])
for field_name in self.info.keys()
})
cprint(
f"update current experience: "
f"done {self._current_experience.done}, "
f"reward {self._current_experience.reward}, "
f"time_stamp {self._current_experience.time_stamp}, "
f"info: {[k for k in self._current_experience.info.keys()]}",
self._logger,
msg_type=MessageType.debug)
self._previous_observation = deepcopy(self.observation)
return True
def _run_shortly(self):
if self._config.ros_config.ros_launch_config.gazebo:
self._unpause_gazebo()
rospy.sleep(self._pause_period)
if self._config.ros_config.ros_launch_config.gazebo:
self._pause_gazebo()
def _run_and_update_experience(self):
self._clear_experience_values()
start_rospy_time = rospy.get_time()
start_time = time.time()
self._run_shortly()
while not self._update_current_experience():
self._run_shortly()
if time.time(
) - start_time > self._config.ros_config.max_update_wait_period_s:
cprint(
f"ros seems to be stuck, waiting for more than "
f"{self._config.ros_config.max_update_wait_period_s}s, so exit.",
self._logger,
msg_type=MessageType.warning)
self.remove()
sys.exit(1)
self._current_experience.info['run_time_duration_s'] = rospy.get_time(
) - start_rospy_time
def reset(self) -> Tuple[Experience, np.ndarray]:
"""
reset gazebo, reset fsm, wait till fsm in 'running' state
return experience without reward or action
"""
cprint(f'resetting', self._logger)
self._reset_filters()
self._step = 0
self._return = 0
if self._config.ros_config.ros_launch_config.gazebo:
self._reset_gazebo()
self._reset_publisher.publish(Empty())
self._clear_experience_values()
while self.fsm_state != FsmState.Running \
or self.observation is None \
or self.terminal_state is None \
or self.terminal_state is TerminationType.Unknown:
self._run_shortly()
self.observation = self._filter_observation(self.observation)
self._current_experience = Experience(
done=deepcopy(self.terminal_state),
observation=deepcopy(self.observation),
time_stamp=int(rospy.get_time() * 10**3),
info={})
self._previous_observation = deepcopy(self.observation)
return self._current_experience, deepcopy(self.observation)
def step(self, action: Action = None) -> Tuple[Experience, np.ndarray]:
self._step += 1
if action is not None:
for index, msg in enumerate(adapt_action_to_twist(action)):
try:
self._action_publishers[index].publish(msg)
except IndexError:
raise IndexError(
f'action of {action.actor_name} requires '
f'self._config.ros_config.num_action_publishers set to 2 '
f'instead of {self._config.ros_config.num_action_publishers}'
)
if self._config.ros_config.action_topic == 'python':
self.action = deepcopy(action)
self._run_and_update_experience()
return self._current_experience, deepcopy(self.observation)
def remove(self) -> bool:
return self._ros.terminate() == ProcessState.Terminated
class TestFsm(unittest.TestCase):
def setUp(self) -> None:
self.config = {
'robot_name': 'test_fsm_robot',
'fsm': True,
'fsm_mode': 'SingleRun',
'world_name': 'test_fsm_world',
'control_mapping': False,
'waypoint_indicator': False,
}
self.output_dir = f'{get_data_dir(os.environ["CODEDIR"])}/test_dir/{get_filename_without_extension(__file__)}'
os.makedirs(self.output_dir, exist_ok=True)
self.config['output_path'] = self.output_dir
# spinoff roslaunch
self._ros_process = RosWrapper(launch_file='load_ros.launch',
config=self.config,
visible=False)
self.delay_evaluation = rospy.get_param('/world/delay_evaluation')
self.state_topic = '/fsm/state'
self.reward_topic = '/fsm/reward'
self.reset_topic = '/fsm/reset'
self.pose_topic = rospy.get_param('/robot/position_sensor/topic')
self.depth_scan_topic = rospy.get_param('/robot/depth_sensor/topic')
self.modified_state_topic = rospy.get_param(
'/robot/modified_state_sensor/topic')
# subscribe to fsm state and reward (fsm's output)
subscribe_topics = [
TopicConfig(topic_name=self.state_topic, msg_type="String"),
TopicConfig(topic_name=self.reward_topic, msg_type="RosReward"),
]
# create publishers for all relevant sensors < FSM
publish_topics = [
TopicConfig(topic_name=self.reset_topic, msg_type='Empty'),
]
for sensor in SensorType:
if rospy.has_param(f'/robot/{sensor.name}_sensor'):
publish_topics.append(
TopicConfig(
topic_name=eval(
f'rospy.get_param("/robot/{sensor.name}_sensor/topic")'
),
msg_type=eval(
f'rospy.get_param("/robot/{sensor.name}_sensor/type")'
)))
self.ros_topic = TestPublisherSubscriber(
subscribe_topics=subscribe_topics, publish_topics=publish_topics)
def test_occasions(self):
print('_test_start_up')
self._test_start_up()
print('_test_delay_evaluation')
self._test_delay_evaluation()
print('_test_step')
self._test_step()
print('_test_on_collision')
self._test_on_collision()
print('_test_goal_reached')
self._test_goal_reached()
print('_test_out_of_time')
self._test_out_of_time()
def _test_start_up(self):
# FSM should start in unknown state, waiting for reset
# @ startup (before reset)
safe_wait_till_true(
'"/fsm/state" in kwargs["ros_topic"].topic_values.keys()',
True,
5,
0.1,
ros_topic=self.ros_topic)
self.assertEqual('Unknown',
self.ros_topic.topic_values[self.state_topic].data)
safe_wait_till_true(
'"/fsm/reward" in kwargs["ros_topic"].topic_values.keys()',
True,
5,
0.1,
ros_topic=self.ros_topic)
self.assertEqual(0,
self.ros_topic.topic_values[self.reward_topic].reward)
self.assertEqual(
'Unknown',
self.ros_topic.topic_values[self.reward_topic].termination)
def _test_delay_evaluation(self):
rospy.sleep(0.1)
self.ros_topic.publishers[self.reset_topic].publish(Empty())
start_time = rospy.get_time()
while self.ros_topic.topic_values[self.state_topic].data == 'Unknown':
rospy.sleep(0.01)
delay_duration = rospy.get_time() - start_time
self.assertLess(abs(self.delay_evaluation - delay_duration), 0.2)
self.assertEqual('Running',
self.ros_topic.topic_values[self.state_topic].data)
def _test_step(self):
self.ros_topic.publishers[self.pose_topic].publish(get_fake_odometry())
rospy.sleep(0.1)
self.ros_topic.publishers[self.pose_topic].publish(
get_fake_odometry(1))
rospy.sleep(0.1)
self.assertEqual(
rospy.get_param('/world/reward/step/termination'),
self.ros_topic.topic_values[self.reward_topic].termination)
reward_distance_travelled = rospy.get_param(
'/world/reward/step/weights/travelled_distance') * 1
self.assertEqual(reward_distance_travelled,
self.ros_topic.topic_values[self.reward_topic].reward)
self.ros_topic.publishers[self.pose_topic].publish(
get_fake_odometry(1, 1))
while self.ros_topic.topic_values[
self.reward_topic].reward == reward_distance_travelled:
rospy.sleep(0.1)
reward_distance_travelled = rospy.get_param(
'/world/reward/step/weights/travelled_distance') * 2
self.assertAlmostEqual(
reward_distance_travelled,
self.ros_topic.topic_values[self.reward_topic].reward,
places=1)
def _test_on_collision(self):
offset = 3
self.ros_topic.publishers[self.modified_state_topic].publish(
get_fake_modified_state([1, 0, 1, 1, offset, 1, 0, 0, 0]))
time.sleep(
0.5) # make sure modified state is received before collision
self.ros_topic.publishers[self.depth_scan_topic].publish(
get_fake_laser_scan([.2] * 360))
safe_wait_till_true(
'kwargs["ros_topic"].topic_values["/fsm/reward"].termination',
TerminationType.Failure.name,
4,
0.1,
ros_topic=self.ros_topic)
self.assertAlmostEqual(
rospy.get_param(
'/world/reward/on_collision/weights/distance_between_agents') *
offset,
self.ros_topic.topic_values[self.reward_topic].reward,
places=5)
self.assertEqual(
rospy.get_param('/world/reward/on_collision/termination'),
self.ros_topic.topic_values[self.reward_topic].termination)
def _test_goal_reached(self):
# reset
self.ros_topic.publishers[self.reset_topic].publish(Empty())
time.sleep(0.1)
safe_wait_till_true(
'kwargs["ros_topic"].topic_values["/fsm/state"].data',
FsmState.Running.name,
3,
0.1,
ros_topic=self.ros_topic)
self.ros_topic.publishers[self.pose_topic].publish(Odometry())
rospy.sleep(0.1)
goal_pos = [2, 2, 1]
self.ros_topic.publishers[self.pose_topic].publish(
get_fake_odometry(*goal_pos))
safe_wait_till_true(
'kwargs["ros_topic"].topic_values["/fsm/state"].data',
FsmState.Terminated.name,
3,
0.1,
ros_topic=self.ros_topic)
distance = np.sqrt(sum(np.asarray(goal_pos)**2))
self.assertEqual(
rospy.get_param(
'/world/reward/goal_reached/weights/distance_from_start') *
distance, self.ros_topic.topic_values[self.reward_topic].reward)
self.assertEqual(
rospy.get_param('/world/reward/goal_reached/termination'),
self.ros_topic.topic_values[self.reward_topic].termination)
def _test_out_of_time(self):
# reset
self.ros_topic.publishers[self.reset_topic].publish(Empty())
safe_wait_till_true(
'kwargs["ros_topic"].topic_values["/fsm/state"].data',
FsmState.Running.name,
4,
0.1,
ros_topic=self.ros_topic)
offset = 3
while self.ros_topic.topic_values[
self.reward_topic].termination == 'NotDone':
rospy.sleep(1)
self.ros_topic.publishers[self.modified_state_topic].publish(
get_fake_modified_state([1, 0, 1, 1, offset, 1, 0, 0, 0]))
rospy.sleep(1)
iou = 5 # TODO define correct IOU when this is implemented
self.assertEqual(
rospy.get_param('/world/reward/out_of_time/weights/iou') * iou,
self.ros_topic.topic_values[self.reward_topic].reward)
self.assertEqual(
rospy.get_param('/world/reward/out_of_time/termination'),
self.ros_topic.topic_values[self.reward_topic].termination)
def tearDown(self) -> None:
self._ros_process.terminate()
shutil.rmtree(self.output_dir, ignore_errors=True)
def test_takeoff_drone_sim(self):
config = {
"output_path": self.output_dir,
"random_seed": 123,
"robot_name": "drone_sim",
"world_name": "empty",
"gazebo": True,
}
ros_process = RosWrapper(launch_file='load_ros.launch',
config=config,
visible=False)
self._unpause_client = rospy.ServiceProxy('/gazebo/unpause_physics',
Emptyservice)
self._pause_client = rospy.ServiceProxy('/gazebo/pause_physics',
Emptyservice)
self.ros_topic = TestPublisherSubscriber(
subscribe_topics=[
TopicConfig(
topic_name=rospy.get_param('/robot/position_sensor/topic'),
msg_type=rospy.get_param('/robot/position_sensor/type'))
],
publish_topics=[
TopicConfig(topic_name=rospy.get_param('/robot/command_topic'),
msg_type='Twist')
])
# unpause gazebo to receive messages
self._unpause_client.wait_for_service()
self._unpause_client.call()
safe_wait_till_true(
f'\'{rospy.get_param("/robot/position_sensor/topic")}\' '
f'in kwargs["ros_topic"].topic_values.keys()',
True,
5,
0.1,
ros_topic=self.ros_topic)
rospy.wait_for_service('/enable_motors')
enable_motors_service = rospy.ServiceProxy('/enable_motors',
EnableMotors)
enable_motors_service.call(False)
self._pause_client.wait_for_service()
self._pause_client.call()
self._set_model_state = rospy.ServiceProxy('/gazebo/set_model_state',
SetModelState)
for _ in range(3):
# set gazebo model state
model_state = ModelState()
model_state.model_name = 'quadrotor'
model_state.pose = Pose()
self._set_model_state.wait_for_service()
self._set_model_state(model_state)
self._unpause_client.wait_for_service()
self._unpause_client.call()
enable_motors_service.call(True)
# fly up till reference height
reference = 1.
errors = [1]
while np.mean(errors) > 0.05:
position = self.ros_topic.topic_values[rospy.get_param(
'/robot/position_sensor/topic')].pose.pose.position
print(position.z)
errors.append(np.abs(reference - position.z))
if len(errors) > 5:
errors.pop(0)
twist = Twist()
twist.linear.z = +0.5 if position.z < reference else -0.5
self.ros_topic.publishers[rospy.get_param(
'/robot/command_topic')].publish(twist)
time.sleep(0.1)
final_error = abs(self.ros_topic.topic_values[rospy.get_param(
'/robot/position_sensor/topic')].pose.pose.position.z -
reference)
self.assertLess(final_error, 0.1)
self._pause_client.wait_for_service()
self._pause_client(EmptyRequest())
enable_motors_service.call(False)
ros_process.terminate()
def test_waypoints_tracking_real_bebop_with_KF_with_keyboard(self):
self.output_dir = f'{get_data_dir(os.environ["CODEDIR"])}/test_dir/{get_filename_without_extension(__file__)}'
os.makedirs(self.output_dir, exist_ok=True)
self._config = {
'output_path':
self.output_dir,
'world_name':
'hexagon',
'robot_name':
'bebop_real',
'gazebo':
False,
'fsm':
True,
'fsm_mode':
'TakeOverRun',
'control_mapping':
True,
'control_mapping_config':
'mathias_controller_keyboard',
'waypoint_indicator':
True,
'altitude_control':
False,
'mathias_controller_with_KF':
True,
'starting_height':
1.,
'keyboard':
True,
'mathias_controller_config_file_path_with_extension':
f'{os.environ["CODEDIR"]}/src/sim/ros/config/actor/mathias_controller_with_KF_real_bebop.yml',
}
# spinoff roslaunch
self._ros_process = RosWrapper(launch_file='load_ros.launch',
config=self._config,
visible=True)
# subscribe to command control
self.visualisation_topic = '/actor/mathias_controller/visualisation'
subscribe_topics = [
TopicConfig(
topic_name=rospy.get_param('/robot/position_sensor/topic'),
msg_type=rospy.get_param('/robot/position_sensor/type')),
TopicConfig(topic_name='/fsm/state', msg_type='String'),
TopicConfig(topic_name='/waypoint_indicator/current_waypoint',
msg_type='Float32MultiArray'),
TopicConfig(topic_name=self.visualisation_topic, msg_type='Image')
]
publish_topics = [
TopicConfig(topic_name='/fsm/reset', msg_type='Empty'),
]
self.ros_topic = TestPublisherSubscriber(
subscribe_topics=subscribe_topics, publish_topics=publish_topics)
safe_wait_till_true(
'"/fsm/state" in kwargs["ros_topic"].topic_values.keys()',
True,
25,
0.1,
ros_topic=self.ros_topic)
self.assertEqual(self.ros_topic.topic_values['/fsm/state'].data,
FsmState.Unknown.name)
while True:
# publish reset
self.ros_topic.publishers['/fsm/reset'].publish(Empty())
while self.ros_topic.topic_values[
"/fsm/state"].data != FsmState.Running.name:
rospy.sleep(0.1)
safe_wait_till_true(
'"/waypoint_indicator/current_waypoint" in kwargs["ros_topic"].topic_values.keys()',
True,
10,
0.1,
ros_topic=self.ros_topic)
poses = []
waypoints = []
while self.ros_topic.topic_values["/fsm/state"].data != FsmState.Terminated.name and \
self.ros_topic.topic_values["/fsm/state"].data != FsmState.TakenOver.name:
rospy.sleep(0.5)
pose = self.get_pose()
waypoint = self.ros_topic.topic_values[
'/waypoint_indicator/current_waypoint']
poses.append(pose)
waypoints.append(waypoint)
plt.figure(figsize=(15, 15))
plt.scatter([p[0] for p in poses], [p[1] for p in poses],
color='C0',
label='xy-pose')
plt.scatter([p.data[0] for p in waypoints],
[p.data[1] for p in waypoints],
color='C1',
label='xy-waypoints')
plt.legend()
plt.xlabel("x [m]")
plt.ylabel("y [m]")
plt.show()
def test_forward_cam_drone(self) -> None:
self.output_dir = f'{get_data_dir(os.environ["CODEDIR"])}/test_dir/{get_filename_without_extension(__file__)}'
os.makedirs(self.output_dir, exist_ok=True)
self._config = {
'output_path': self.output_dir,
'world_name': 'april_tag',
'robot_name': 'drone_sim',
'gazebo': True,
'fsm': True,
'fsm_mode': 'TakeOverRun',
'control_mapping': True,
'control_mapping_config': 'mathias_controller',
'altitude_control': True,
'waypoint_indicator': False,
'april_tag_detector': True,
'mathias_controller_with_KF': True,
'mathias_controller_config_file_path_with_extension':
f'{os.environ["CODEDIR"]}/src/sim/ros/config/actor/mathias_controller_with_KF.yml',
'starting_height': 0.5,
}
# spinoff roslaunch
self._ros_process = RosWrapper(launch_file='load_ros.launch',
config=self._config,
visible=True)
# subscribe to command control
self.image_topic = rospy.get_param(f'/robot/camera_sensor/topic')
subscribe_topics = [
TopicConfig(topic_name=rospy.get_param('/robot/position_sensor/topic'),
msg_type=rospy.get_param('/robot/position_sensor/type')),
TopicConfig(topic_name='/reference_ground_point',
msg_type='PointStamped'),
TopicConfig(topic_name='/fsm/state',
msg_type='String'),
TopicConfig(topic_name=self.image_topic,
msg_type='Image')
]
publish_topics = [
TopicConfig(topic_name='/fsm/reset', msg_type='Empty')
]
self.ros_topic = TestPublisherSubscriber(
subscribe_topics=subscribe_topics,
publish_topics=publish_topics
)
self._unpause_client = rospy.ServiceProxy('/gazebo/unpause_physics', Emptyservice)
self._pause_client = rospy.ServiceProxy('/gazebo/pause_physics', Emptyservice)
self._set_model_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
# unpause gazebo to receive messages
self._unpause_client.wait_for_service()
self._unpause_client.call()
safe_wait_till_true('"/fsm/state" in kwargs["ros_topic"].topic_values.keys()',
True, 10, 0.1, ros_topic=self.ros_topic)
self.assertEqual(self.ros_topic.topic_values['/fsm/state'].data, FsmState.Unknown.name)
# publish reset
self.ros_topic.publishers['/fsm/reset'].publish(Empty())
# 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 pose
pose = self.get_pose()
self._pause_client.wait_for_service()
self._pause_client.call()
a = 100
class TestWaypointIndicator(unittest.TestCase):
def start_test(self) -> None:
self.output_dir = f'{get_data_dir(os.environ["CODEDIR"])}/test_dir/{get_filename_without_extension(__file__)}'
os.makedirs(self.output_dir, exist_ok=True)
config = {
'output_path': self.output_dir,
'world_name': 'test_waypoints',
'robot_name': 'turtlebot_sim',
'gazebo': False,
'fsm': False,
'control_mapping': False,
'ros_expert': False,
'waypoint_indicator': True
}
# spinoff roslaunch
self._ros_process = RosWrapper(launch_file='load_ros.launch',
config=config,
visible=False)
# subscribe to command control
self._waypoint_topic = '/waypoint_indicator/current_waypoint'
subscribe_topics = [
TopicConfig(topic_name=self._waypoint_topic,
msg_type="Float32MultiArray"),
]
# create publishers for all relevant sensors < sensor expert
self._pose_topic = rospy.get_param('/robot/position_sensor/topic')
self._pose_type = rospy.get_param('/robot/position_sensor/type')
publish_topics = [
TopicConfig(topic_name=self._pose_topic, msg_type=self._pose_type)
]
self.ros_topic = TestPublisherSubscriber(
subscribe_topics=subscribe_topics, publish_topics=publish_topics)
def send_odom_and_read_next_waypoint(self, odom: Odometry) -> tuple:
self.ros_topic.publishers[self._pose_topic].publish(odom)
time.sleep(1)
received_waypoint: tuple = self.ros_topic.topic_values[
self._waypoint_topic].data
return received_waypoint
def compare_vectors(self, a: tuple, b: tuple) -> bool:
return sum(np.asarray(a) - np.asarray(b)) < 10**-3
def _test_first_waypoint(self):
odom = Odometry()
odom.pose.pose.position.x = -31
odom.pose.pose.position.y = -5
received_waypoint = self.send_odom_and_read_next_waypoint(odom=odom)
self.assertTrue(
self.compare_vectors(received_waypoint, self.waypoints[0]))
def _test_transition_of_waypoint(self):
odom = Odometry()
odom.pose.pose.position.x = self.waypoints[0][0]
odom.pose.pose.position.y = self.waypoints[0][1]
received_waypoint = self.send_odom_and_read_next_waypoint(odom=odom)
self.assertTrue(
self.compare_vectors(received_waypoint, self.waypoints[1]))
def test_waypoint_indicator(self):
self.start_test()
self.waypoints = rospy.get_param('/world/waypoints')
print(f'WAYPOINTS: {self.waypoints}')
stime = time.time()
max_duration = 100
while time.time() < stime + max_duration \
and '/waypoint_indicator/current_waypoint' not in self.ros_topic.topic_values.keys():
time.sleep(0.1)
self._test_first_waypoint()
self._test_transition_of_waypoint()
def tearDown(self) -> None:
self._ros_process.terminate()
shutil.rmtree(self.output_dir, ignore_errors=True)
def test_single_drone(self) -> None:
self.output_dir = f'{get_data_dir(os.environ["CODEDIR"])}/test_dir/{get_filename_without_extension(__file__)}'
os.makedirs(self.output_dir, exist_ok=True)
height = 5
self._config = {
'output_path': self.output_dir,
'world_name': 'empty',
'robot_name': 'drone_sim',
'gazebo': True,
'fsm': True,
'fsm_mode': 'TakeOverRun',
'control_mapping': True,
'control_mapping_config': 'ros_expert',
'altitude_control': True,
'ros_expert': True,
'starting_height': height
}
# spinoff roslaunch
self._ros_process = RosWrapper(launch_file='load_ros.launch',
config=self._config,
visible=True)
# subscribe to command control
subscribe_topics = [
TopicConfig(topic_name=rospy.get_param('/robot/position_sensor/topic'),
msg_type=rospy.get_param('/robot/position_sensor/type')),
TopicConfig(topic_name='/fsm/state',
msg_type='String')
]
publish_topics = [
TopicConfig(topic_name='/fsm/reset', msg_type='Empty')
]
self.ros_topic = TestPublisherSubscriber(
subscribe_topics=subscribe_topics,
publish_topics=publish_topics
)
self._unpause_client = rospy.ServiceProxy('/gazebo/unpause_physics', Emptyservice)
self._pause_client = rospy.ServiceProxy('/gazebo/pause_physics', Emptyservice)
self._set_model_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
# unpause gazebo to receive messages
self._unpause_client.wait_for_service()
self._unpause_client.call()
safe_wait_till_true('"/fsm/state" in kwargs["ros_topic"].topic_values.keys()',
True, 10, 0.1, ros_topic=self.ros_topic)
self.assertEqual(self.ros_topic.topic_values['/fsm/state'].data, FsmState.Unknown.name)
# pause again before start
self._pause_client.wait_for_service()
self._pause_client.call()
self._set_model_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
for _ in range(3):
print(f'round: {_}')
# set gazebo model state
model_state = ModelState()
model_state.model_name = 'quadrotor'
model_state.pose = Pose()
self._set_model_state.wait_for_service()
self._set_model_state(model_state)
# 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
z_pos = self.ros_topic.topic_values[rospy.get_param('/robot/position_sensor/topic')].pose.pose.position.z
print(f'final_height: {z_pos}')
self.assertLess(abs(z_pos - height), 0.2)
self._pause_client.wait_for_service()
self._pause_client.call()
class TestRobotMapper(unittest.TestCase):
def start_test(self) -> None:
self.output_dir = f'{get_data_dir(os.environ["CODEDIR"])}/test_dir/{get_filename_without_extension(__file__)}'
os.makedirs(self.output_dir, exist_ok=True)
config = {
'output_path': self.output_dir,
'robot_name': 'turtlebot_sim',
'gazebo': False,
'fsm': False,
'control_mapping': False,
'ros_expert': False,
'waypoint_indicator': False,
'robot_mapping': True
}
# spinoff roslaunch
self._ros_process = RosWrapper(launch_file='load_ros.launch',
config=config,
visible=False)
# create publishers for all relevant sensors < sensor expert
self._pose_topic = rospy.get_param('/robot/position_sensor/topic')
self._pose_type = rospy.get_param('/robot/position_sensor/type')
self._fsm_topic = '/fsm/state'
publish_topics = [
TopicConfig(topic_name=self._pose_topic, msg_type=self._pose_type),
TopicConfig(topic_name=self._fsm_topic, msg_type='String')
]
self.ros_topic = TestPublisherSubscriber(subscribe_topics=[],
publish_topics=publish_topics)
def publish_odom(self,
x: float = 0,
y: float = 0,
z: float = 0,
yaw: float = 0):
odom = adapt_vector_to_odometry((x, y, z, yaw))
self.ros_topic.publishers[self._pose_topic].publish(odom)
time.sleep(0.1)
def test_robot_mapper(self):
self.start_test()
stime = time.time()
max_duration = 100
while time.time() < stime + max_duration \
and not rospy.has_param('/output_path'):
time.sleep(0.1)
time.sleep(0.1)
self.ros_topic.publishers[self._fsm_topic].publish(
FsmState.Running.name)
self.publish_odom(x=0, y=0, z=0, yaw=0)
self.publish_odom(x=1, y=0, z=0, yaw=0)
self.publish_odom(x=0, y=2, z=0, yaw=0)
self.publish_odom(x=0, y=0, z=3, yaw=0)
self.publish_odom(x=1, y=4, z=2, yaw=0.5)
# self.publish_odom(x=1, y=0, z=0, yaw=0)
# self.publish_odom(x=1, y=0, z=0, yaw=0.7)
# self.publish_odom(x=1, y=0, z=0, yaw=-0.7)
# self.publish_odom(x=0, y=1, z=0, yaw=-0.7+1.57)
# self.publish_odom(x=0, y=1, z=0, yaw=-0.7+1.57)
time.sleep(0.1)
self.ros_topic.publishers[self._fsm_topic].publish(
FsmState.Terminated.name)
time.sleep(1)
trajectory = glob(
os.path.join(self.output_dir, 'trajectories', '*.png'))[0]
self.assertTrue(os.path.isfile(trajectory))
def tearDown(self) -> None:
self._ros_process.terminate()
shutil.rmtree(self.output_dir, ignore_errors=True)