def test_safe_wait_till_true(self):
class FakeObject:
def __init__(self):
self.field_a = 1
f = FakeObject()
safe_wait_till_true('kwargs["f"].field_a', 1, 2, 0.1, f=f)
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_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 __init__(self):
self.count = 0
rospy.init_node('altitude_control')
safe_wait_till_true(f'kwargs["rospy"].has_param("/output_path")',
True,
5,
0.1,
rospy=rospy)
self._output_path = get_output_path()
self._logger = get_logger(get_filename_without_extension(__file__),
self._output_path)
self._reference_height = rospy.get_param('/starting_height', 1)
self._rate_fps = 60
self._go_publisher = rospy.Publisher('/fsm/go', Empty, queue_size=10)
self._robot = rospy.get_param('/robot/model_name', 'default')
self._height = {}
self._publishers = {}
self._enable_motors_services = {}
self._setup()
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_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 _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_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_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
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_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_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 tweak_combined_axis_keyboard(self, measured_data, index,
point_ref_drone):
initial_point_ref_drone = copy.deepcopy(point_ref_drone)
# gets fsm in taken over state
safe_wait_till_true(
'kwargs["ros_topic"].topic_values["/fsm/state"].data',
FsmState.TakenOver.name,
20,
0.1,
ros_topic=self.ros_topic)
# once drone is in good starting position invoke 'go' with key m
while self.ros_topic.topic_values[
"/fsm/state"].data != FsmState.Running.name:
# while taking off, update reference point for PID controller to remain at same height
self.ros_topic.publishers[self._reference_topic].publish(
PointStamped(header=Header(frame_id="agent"),
point=Point(x=point_ref_drone[0],
y=point_ref_drone[1],
z=point_ref_drone[2])))
last_pose = self.get_pose()
rospy.sleep(0.5)
measured_data[index] = {'x': [], 'y': [], 'z': [], 'yaw': []}
point_ref_global = transform(points=[np.asarray(point_ref_drone)],
orientation=R.from_euler(
'XYZ', (0, 0, last_pose[-1]),
degrees=False).as_matrix(),
translation=np.asarray(last_pose[:3]))[0]
# Mathias controller should keep drone in steady pose
while self.ros_topic.topic_values[
"/fsm/state"].data != FsmState.TakenOver.name:
point_drone_global = self.get_pose()
point_ref_drone = Point(
x=point_ref_global[0] - point_drone_global[0],
y=point_ref_global[1] - point_drone_global[1],
z=point_ref_global[2] - point_drone_global[2])
# rotate pose error to rotated frame
pose_error_local = transform(points=[point_ref_drone],
orientation=R.from_euler(
'XYZ', (0, 0, -last_pose[-1]),
degrees=False).as_matrix())[0]
measured_data[index]['x'].append(pose_error_local.x)
measured_data[index]['y'].append(pose_error_local.y)
measured_data[index]['z'].append(pose_error_local.z)
# measured_data[index]['yaw'].append(last_pose[-1])
rospy.sleep(0.5)
if False and '/bebop/land' in self.ros_topic.publishers.keys():
self.ros_topic.publishers['/bebop/land'].publish(Empty())
colors = ['C0', 'C1', 'C2', 'C3', 'C4']
styles = {'x': '-', 'y': '--', 'z': ':', 'yaw': '-.'}
fig = plt.figure(figsize=(15, 15))
for key in measured_data.keys():
for a in measured_data[key].keys():
if len(measured_data[key][a]) != 0:
plt.plot(measured_data[key][a],
linestyle=styles[a],
linewidth=3 if key == index else 1,
color=colors[key % len(colors)],
label=f'{key}: {a}')
plt.legend()
#plt.savefig(os.path.join(self.output_dir, 'measured_data.jpg'))
plt.show()
# print visualisation if it's in ros topic:
if self.visualisation_topic in self.ros_topic.topic_values.keys():
frame = process_image(
self.ros_topic.topic_values[self.visualisation_topic])
plt.figure(figsize=(15, 20))
plt.imshow(frame)
plt.axis('off')
plt.tight_layout()
plt.savefig(
f'{os.environ["HOME"]}/kf_study/pid_tweak_{initial_point_ref_drone[0]}_{initial_point_ref_drone[1]}_{initial_point_ref_drone[2]}.png'
)
plt.clf()
plt.close()
index += 1
index %= len(colors)
return index
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': False,
'control_mapping_config': 'python_adversarial',
'altitude_control': False,
'ros_expert': False,
'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 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_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_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
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()
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])
