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()
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 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 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 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()
class TestRobotDisplay(unittest.TestCase):
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 tearDown(self) -> None:
print(f'shutting down...')
self._ros_process.terminate()
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)
class TestChessboardWorld(unittest.TestCase):
#@unittest.skip
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 get_pose(self):
if rospy.get_param('/robot/position_sensor/topic') in self.ros_topic.topic_values.keys():
odom = self.ros_topic.topic_values[rospy.get_param('/robot/position_sensor/topic')]
quaternion = (odom.pose.pose.orientation.x,
odom.pose.pose.orientation.y,
odom.pose.pose.orientation.z,
odom.pose.pose.orientation.w)
_, _, yaw = euler_from_quaternion(quaternion)
return odom.pose.pose.position.x, odom.pose.pose.position.y, odom.pose.pose.position.z, yaw
else:
return None
def tearDown(self) -> None:
self._ros_process.terminate()
shutil.rmtree(self.output_dir, ignore_errors=True)
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 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)
class TestMathiasController(unittest.TestCase):
@unittest.skip
def test_drone_keyboard_gazebo(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': True,
'yaw_or': 1.5,
'x_pos': 1,
'y_pos': 2
}
# 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)
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, point_ref_drone=[1, 0, 0])
index = self.tweak_combined_axis_keyboard(
measured_data, index, point_ref_drone=[0, 1, 0])
index = self.tweak_combined_axis_keyboard(
measured_data, index, point_ref_drone=[2, 2, 0])
@unittest.skip
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 get_pose(self):
"""
returns x, y, z, yaw of drone in global frame
"""
if rospy.get_param('/robot/position_sensor/topic'
) in self.ros_topic.topic_values.keys():
odom = self.ros_topic.topic_values[rospy.get_param(
'/robot/position_sensor/topic')]
quaternion = (odom.pose.pose.orientation.x,
odom.pose.pose.orientation.y,
odom.pose.pose.orientation.z,
odom.pose.pose.orientation.w)
_, _, yaw = euler_from_quaternion(quaternion)
return odom.pose.pose.position.x, odom.pose.pose.position.y, odom.pose.pose.position.z, yaw
else:
return None
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
@unittest.skip
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)
@unittest.skip
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])
@unittest.skip
def test_drone_relative_positioning_real_bebop_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':
'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_with_KF':
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)
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),
TopicConfig(topic_name='/bebop/land', 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,
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=1)
# index = self.tweak_combined_axis_keyboard(measured_data, index, point=[0., 0., 0.])
#index = self.tweak_combined_axis_keyboard(measured_data, index, point=[0., 0., 0.5])
index = self.tweak_combined_axis_keyboard(measured_data,
index,
point=[3, 1, 1])
index = self.tweak_combined_axis_keyboard(measured_data,
index,
point=[3, 1, -1])
@unittest.skip
def test_waypoints_tracking_in_gazebo_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': 'gate_test', # 'hexagon',
'robot_name': 'drone_sim',
'gazebo': True,
'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.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='/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)
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)
self._unpause_client.wait_for_service()
self._unpause_client.call()
safe_wait_till_true(
'"/fsm/state" in kwargs["ros_topic"].topic_values.keys()',
True,
15,
0.1,
ros_topic=self.ros_topic)
self.assertEqual(self.ros_topic.topic_values['/fsm/state'].data,
FsmState.Unknown.name)
self._pause_client.wait_for_service()
self._pause_client.call()
while True:
# publish reset
self.ros_topic.publishers['/fsm/reset'].publish(Empty())
self._unpause_client.wait_for_service()
self._unpause_client.call()
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)
print(self.ros_topic.topic_values["/fsm/state"].data)
# 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)
self._pause_client.wait_for_service()
self._pause_client.call()
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()
@unittest.skip
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()
@unittest.skip
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)
#@unittest.skip
def test_april_tag_detector_real_bebop_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':
'bebop_real',
'gazebo':
False,
'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_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='/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,
25,
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
@unittest.skip
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 tearDown(self) -> None:
print(f'shutting down...')
self._ros_process.terminate()
class RosEnvironment(Environment):
def __init__(self, config: EnvironmentConfig):
super().__init__(config)
self._pause_period = 1.0 / 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.getcwd(),
"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)
]
self._waypoint_publisher = rospy.Publisher(
"/reference_pose", PointStamped, queue_size=10
)
# 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, waypoint: np.ndarray = 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)
if waypoint is not None:
assert len(waypoint) == 3
msg = PointStamped()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = "agent"
msg.point = Point(waypoint[0], waypoint[1], waypoint[2])
self._waypoint_publisher.publish(msg)
self._run_and_update_experience()
return self._current_experience, deepcopy(self.observation)
def remove(self) -> bool:
return self._ros.terminate() == ProcessState.Terminated
class TestRosExpert(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 = {
'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 send_scan_and_read_twist(self, scan: LaserScan) -> Twist:
self.ros_topic.publishers[self._depth_topic].publish(scan)
time.sleep(1)
received_twist: Twist = self.ros_topic.topic_values[self.command_topic]
return received_twist
def _test_collision_avoidance_with_laser_scan(self):
time.sleep(5)
# publish depth scan making robot go left
scan = LaserScan()
scan.ranges = [1] * 20 + [15] * 30 + [1] * (360 - 50)
twist = self.send_scan_and_read_twist(scan)
self.assertEqual(twist.angular.z, 1)
# publish depth scan making robot go right
scan = LaserScan()
scan.ranges = [1] * 80 + [2] * (360 - 80)
twist = self.send_scan_and_read_twist(scan)
self.assertEqual(twist.angular.z, -1)
# publish depth scan making robot go straight
scan = LaserScan()
scan.ranges = [2] * 360
twist = self.send_scan_and_read_twist(scan)
self.assertEqual(twist.angular.z, 0)
def _test_waypoint_following(self):
waypoints = rospy.get_param('/world/waypoints')
odom = Odometry()
odom.pose.pose.position.x = waypoints[0][0]
odom.pose.pose.position.y = waypoints[0][1]
odom.pose.pose.orientation.x = 0
odom.pose.pose.orientation.y = 0
odom.pose.pose.orientation.z = 0
odom.pose.pose.orientation.w = 1
self.ros_topic.publishers[self._pose_topic].publish(odom)
time.sleep(3)
received_twist: Twist = self.ros_topic.topic_values[self.command_topic]
self.assertEqual(received_twist.angular.z, 0)
# -30 degrees rotated in yaw => compensate in + yaw
odom = Odometry()
odom.pose.pose.position.x = waypoints[0][0]
odom.pose.pose.position.y = waypoints[0][1]
odom.pose.pose.orientation.x = 0
odom.pose.pose.orientation.y = 0
odom.pose.pose.orientation.z = -0.258819
odom.pose.pose.orientation.w = 0.9659258
self.ros_topic.publishers[self._pose_topic].publish(odom)
time.sleep(3)
received_twist: Twist = self.ros_topic.topic_values[self.command_topic]
self.assertEqual(received_twist.angular.z, 1)
# +30 degrees rotated in yaw => compensate in + yaw
odom = Odometry()
odom.pose.pose.position.x = waypoints[0][0]
odom.pose.pose.position.y = waypoints[0][1]
odom.pose.pose.orientation.x = 0
odom.pose.pose.orientation.y = 0
odom.pose.pose.orientation.z = 0.258819
odom.pose.pose.orientation.w = 0.9659258
self.ros_topic.publishers[self._pose_topic].publish(odom)
time.sleep(3)
received_twist: Twist = self.ros_topic.topic_values[self.command_topic]
self.assertEqual(received_twist.angular.z, -1)
def test_ros_expert(self):
self.start_test()
self._test_collision_avoidance_with_laser_scan()
self._test_waypoint_following()
self.end_test()
def end_test(self) -> None:
self._ros_process.terminate()
shutil.rmtree(self.output_dir, ignore_errors=True)
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)
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()
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)
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)
class TestControlMapper(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': '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]
)
# @unittest.skip
def test_control_mapper(self):
self.start()
# for each fsm state
for fsm_state in [FsmState.Unknown,
FsmState.Running,
FsmState.DriveBack,
FsmState.TakenOver,
FsmState.Terminated]:
print(f'FSM STATE: {fsm_state}')
# publish fsm state
self.ros_topic.publishers['/fsm/state'].publish(fsm_state.name)
# wait
time.sleep(0.5)
# publish on all controls
solution = {}
for index, control_topic in enumerate(list(set(self._control_topics))):
control_command = Twist()
control_command.linear.x = index * 100
solution[control_topic] = control_command.linear.x
self.ros_topic.publishers[control_topic].publish(control_command)
# wait
time.sleep(0.5)
print(f'published actor topics: {solution}')
# assert control is equal to intended control
for control_type, actor_topic in self._mapping[fsm_state.name].items():
received_control = self.ros_topic.topic_values[self.command_topics[control_type]]
self.assertEqual(received_control.linear.x, solution[actor_topic])
def tearDown(self) -> None:
self._ros_process.terminate()
shutil.rmtree(self.output_dir, ignore_errors=True)