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 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 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_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 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 load_ros_topic_with_sensors_and_control_commands(self):
time.sleep(5)
while not rospy.has_param('/robot/command_topic'):
time.sleep(0.1)
# fake command publish topics
self.command_topic = rospy.get_param('/robot/command_topic')
# self.supervision_topic = rospy.get_param('/control_mapping/supervision_topic')
subscribe_topics = [
TopicConfig(topic_name=self.command_topic, msg_type="Twist"),
# TopicConfig(topic_name=self.supervision_topic, msg_type="Twist"),
# TopicConfig(topic_name=rospy.get_param('/fsm/state_topic'), msg_type="String"),
TopicConfig(topic_name='/ros_environment/state',
msg_type="RosState"),
]
for sensor in rospy.get_param('/robot/sensors', []):
sensor_topic = rospy.get_param(f'/robot/{sensor}_topic')
sensor_type = rospy.get_param(f'/robot/{sensor}_type')
subscribe_topics.append(
TopicConfig(topic_name=sensor_topic, msg_type=sensor_type))
# create publishers for all control topics < control_mapper/default.yml
self._mapping = rospy.get_param('/control_mapping/mapping', {})
publish_topics = [
# TopicConfig(topic_name=rospy.get_param('/fsm/state_topic'), msg_type='String')
]
self._control_topics = []
for state, mode in self._mapping.items():
if 'command' in mode.keys():
publish_topics.append(
TopicConfig(topic_name=mode['command'], msg_type='Twist'))
self._control_topics.append(mode['command'])
if 'supervision' in mode.keys():
publish_topics.append(
TopicConfig(topic_name=mode['supervision'],
msg_type='Twist'))
self._control_topics.append(mode['supervision'])
self._sensor_topics = []
for sensor in rospy.get_param('/robot/sensors', []):
sensor_topic = rospy.get_param(f'/robot/{sensor}_topic')
sensor_type = rospy.get_param(f'/robot/{sensor}_type')
publish_topics.append(
TopicConfig(topic_name=sensor_topic, msg_type=sensor_type))
self._sensor_topics.append((sensor_topic, sensor_type))
self.ros_topic = TestPublisherSubscriber(
subscribe_topics=subscribe_topics, publish_topics=publish_topics)
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_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_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_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_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_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_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
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])
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