def __init__(self):
stime = time.time()
max_duration = 60
while not rospy.has_param('/modified_state_publisher/mode'
) and time.time() < stime + max_duration:
time.sleep(0.01)
self._output_path = get_output_path()
self._logger = get_logger(get_filename_without_extension(__file__),
self._output_path)
rospy.Subscriber(
rospy.get_param('/robot/modified_state_sensor/topic',
'/modified_state'),
eval(
rospy.get_param('/robot/modified_state_sensor/type',
'CombinedGlobalPoses')),
self._process_state_and_publish_frame)
self._publisher = rospy.Publisher('/modified_state_frame',
Image,
queue_size=10)
cprint(
f"subscribe to {rospy.get_param('/robot/modified_state_sensor/topic', '/modified_state')}",
self._logger)
rospy.init_node('modified_state_frame_visualizer')
def __init__(self):
start_time = time.time()
max_duration = 60
while not rospy.has_param('/robot/position_sensor') and time.time(
) < start_time + max_duration:
time.sleep(0.1)
self._output_path = get_output_path()
self._logger = get_logger(get_filename_without_extension(__file__),
self._output_path)
# fields
self._current_waypoint_index = 0
self._waypoints = rospy.get_param('/world/waypoints', [])
self._waypoint_reached_distance = rospy.get_param(
'/world/waypoint_reached_distance', 0.3)
# publishers
self._publisher = rospy.Publisher(
'/waypoint_indicator/current_waypoint',
Float32MultiArray,
queue_size=10)
# subscribe
odometry_type = rospy.get_param('/robot/position_sensor/type')
callback = f'_{camelcase_to_snake_format(odometry_type)}_callback'
assert callback in self.__dir__()
rospy.Subscriber(name=rospy.get_param('/robot/position_sensor/topic'),
data_class=eval(odometry_type),
callback=eval(f'self.{callback}'))
rospy.Subscriber(name='/fsm/reset',
data_class=Empty,
callback=self.reset)
rospy.init_node('waypoint_indicator')
self.reset()
def __init__(self):
self.count = 0
rospy.init_node('ros_expert')
stime = time.time()
max_duration = 60
while not rospy.has_param('/actor/ros_expert/specs'
) and time.time() < stime + max_duration:
time.sleep(0.01)
self._specs = rospy.get_param('/actor/ros_expert/specs')
super().__init__(
config=ActorConfig(name='ros_expert', specs=self._specs))
self._output_path = get_output_path()
self._logger = get_logger(get_filename_without_extension(__file__),
self._output_path)
cprint(f'ros specifications: {self._specs}', self._logger)
with open(os.path.join(self._output_path, 'ros_expert_specs.yml'),
'w') as f:
yaml.dump(self._specs, f)
self._reference_height = rospy.get_param('/world/starting_height', 1)
self._adjust_height = 0
self._adjust_yaw_collision_avoidance = 0
self._adjust_yaw_waypoint_following = 0
self._rate_fps = self._specs[
'rate_fps'] if 'rate_fps' in self._specs.keys() else 10
self._next_waypoint = []
noise_config = self._specs['noise'] if 'noise' in self._specs.keys(
) else {}
self._noise = eval(f"{noise_config['name']}(**noise_config['args'])"
) if noise_config else None
self._publisher = rospy.Publisher('cmd_vel', Twist, queue_size=10)
self._subscribe()
def __init__(self):
rospy.init_node('dnn_actor_ros')
start_time = time.time()
max_duration = 60
while not rospy.has_param('/output_path') and time.time() < start_time + max_duration:
time.sleep(0.1)
self._specs = rospy.get_param('/actor/dnn_actor/specs')
super().__init__(
config=ActorConfig(
name='dnn_actor',
specs=self._specs
)
)
self._output_path = get_output_path()
self._logger = get_logger(get_filename_without_extension(__file__), self._output_path)
cprint(f'&&&&&&&&&&&&&&&&&& \n {self._specs} \n &&&&&&&&&&&&&&&&&', self._logger)
with open(os.path.join(self._output_path, 'dnn_actor_specs.yml'), 'w') as f:
yaml.dump(self._specs, f)
self._rate_fps = self._specs['rate_fps'] if 'rate_fps' in self._specs.keys() else 20
self._rate = rospy.Rate(self._rate_fps)
config_dict = self._specs['model_config']
config_dict['output_path'] = self._output_path
cprint(f'loading model...', self._logger)
self._model = Model(config=ModelConfig().create(config_dict=config_dict))
self._input_sizes = self._model.get_input_sizes()
self._publisher = rospy.Publisher(self._specs['command_topic'], Twist, queue_size=10)
self._subscribe()
cprint(f'ready', self._logger)
def __init__(self):
rospy.init_node('robot_display')
stime = time.time()
max_duration = 60
while not rospy.has_param(
'/output_path') and time.time() < stime + max_duration:
time.sleep(0.01)
self._output_path = get_output_path()
self._rate_fps = 10
self._border_width = 400
self._counter = 0
self._skip_first_n = 30
self._skip_every_n = 4
# Camera matrices
self._intrinsic_matrix = np.asarray([[100, 0.0, 100], [0.0, 100, 100],
[0.0, 0.0, 1.0]])
self._cam_to_opt_rotation = R.from_quat([0.5, -0.5, 0.5,
0.5]).as_matrix()
self._base_to_cam_dict = {
0: R.from_quat([0, 0, 0, 1]).as_matrix(),
-13: R.from_quat([0, -0.113, 0, 0.994]).as_matrix(),
-90: R.from_quat([0, 0.7071068, 0, 0.7071068]).as_matrix(),
}
self._base_to_cam_translation = np.asarray([0.1, 0, 0])
self._reference_image_location = None
self._update_rate_time_tags = []
self._update_rate = None
self._logger = get_logger(get_filename_without_extension(__file__),
self._output_path)
self._subscribe()
self._add_control_specs()
def __init__(self):
start_time = time.time()
max_duration = 60
while not rospy.has_param(
'/output_path') and time.time() < start_time + max_duration:
time.sleep(0.1)
self._output_path = get_output_path()
self._logger = get_logger(os.path.basename(__file__),
self._output_path)
self._rate_fps = rospy.Rate(100)
rospy.init_node('class_name')
def __init__(self):
self._logger = get_logger('reward', get_output_path())
self._mapping = rospy.get_param('/world/reward', self.default_reward)
if 'unk' not in self._mapping.keys(): # add default unknown reward
self._mapping['unk'] = {
'weights': {
'constant': 0
},
'termination': TerminationType.Unknown.name
}
cprint(f'starting with mapping: {self._mapping}', self._logger)
self._count = 0
def __init__(self):
if not rospy.has_param("teleop"):
rospy.logfatal("no configuration was found, taking node down")
raise JoyTeleopException("no config")
self.publishers = {}
self.al_clients = {}
self.srv_clients = {}
self.service_types = {}
self.message_types = {}
self.command_list = {}
self.offline_actions = []
self.offline_services = []
self.old_buttons = []
teleop_cfg = rospy.get_param("teleop")
for i in teleop_cfg:
if i in self.command_list:
rospy.logerr("command {} was duplicated".format(i))
continue
action_type = teleop_cfg[i]['type']
self.add_command(i, teleop_cfg[i])
if action_type == 'topic':
self.register_topic(i, teleop_cfg[i])
elif action_type == 'action':
self.register_action(i, teleop_cfg[i])
elif action_type == 'service':
self.register_service(i, teleop_cfg[i])
elif action_type == 'method':
self.register_method(i, teleop_cfg[i])
else:
rospy.logerr("unknown type '%s' for command '%s'", action_type,
i)
# Don't subscribe until everything has been initialized.
rospy.Subscriber('joy', sensor_msgs.msg.Joy, self.joy_callback)
# Run a low-freq action updater
rospy.Timer(rospy.Duration(2.0), self.update_actions)
self._logger = get_logger(get_filename_without_extension(__file__),
get_output_path())
for c in self.command_list:
cprint(f'{c}: ', self._logger)
for k, e in self.command_list[c].items():
cprint(f'{k}: {e}', self._logger)
def __init__(self):
rospy.init_node('robot_display')
stime = time.time()
max_duration = 60
while not rospy.has_param(
'/output_path') and time.time() < stime + max_duration:
time.sleep(0.01)
self._output_path = get_output_path()
self._rate_fps = 10
self._border_width = 300
self._counter = 0
self._skip_first_n = 30
self._skip_every_n = 4
self._logger = get_logger(get_filename_without_extension(__file__),
self._output_path)
self._subscribe()
self._add_control_specs()
def __init__(self):
start_time = time.time()
max_duration = 60
while not rospy.has_param('/robot/position_sensor') and time.time(
) < start_time + max_duration:
time.sleep(0.1)
self._output_path = get_output_path()
self._logger = get_logger(get_filename_without_extension(__file__),
self._output_path)
# fields
self._optical_to_base_transformation_dict = {
'/forward/image_raw': {
'translation': [0, -0.086, -0.071],
'rotation': [0.553, -0.553, 0.440, 0.440]
},
'/down/image_raw': {
'translation': [-0.000, 0.050, -0.100],
'rotation': [0.707, -0.707, 0.000, 0.000]
},
'/bebop/image_raw': {
'translation': [0., 0.022, -0.097],
'rotation': [0.553, -0.553, 0.44, 0.44]
},
}
self._optical_to_base_transformation = self._optical_to_base_transformation_dict[
rospy.get_param('/robot/camera_sensor/topic')]
self._tag_transforms = {}
self._waypoint_reached_distance = rospy.get_param(
'/world/waypoint_reached_distance', 0.3)
self._calculating = False # Don't update tag detections while calculating closest
# publishers
self._publisher = rospy.Publisher('/reference_pose',
PointStamped,
queue_size=10)
# subscribe
rospy.Subscriber(name='/tag_detections',
data_class=AprilTagDetectionArray,
callback=self._set_tag_transforms)
rospy.Subscriber(name='/bebop/camera_control',
data_class=Twist,
callback=self._adjust_for_camera_twist)
# start node
rospy.init_node('april_tag_detector')
cprint(f"Initialised", logger=self._logger)
def __init__(self):
self.count = 0
rospy.init_node('altitude_control')
safe_wait_till_true(f'kwargs["rospy"].has_param("/output_path")',
True,
5,
0.1,
rospy=rospy)
self._output_path = get_output_path()
self._logger = get_logger(get_filename_without_extension(__file__),
self._output_path)
self._reference_height = rospy.get_param('/starting_height', 1)
self._rate_fps = 60
self._go_publisher = rospy.Publisher('/fsm/go', Empty, queue_size=10)
self._robot = rospy.get_param('/robot/model_name', 'default')
self._height = {}
self._publishers = {}
self._enable_motors_services = {}
self._setup()
def __init__(self):
rospy.init_node('edge_display')
stime = time.time()
max_duration = 120
while not rospy.has_param(
'/output_path') and time.time() < stime + max_duration:
time.sleep(0.01)
self._output_path = get_output_path()
self._rate_fps = 10
self._border_width = 400
self._counter = 0
self._skip_first_n = 30
self._skip_every_n = 4
self._subscribe()
self._fsm_state_name_mapper = {
"Unknown": "Loading",
"Running": "Autonomous",
"TakenOver": "Pilot",
"Terminated": "Finished",
}
def __init__(self):
stime = time.time()
max_duration = 60
while not rospy.has_param('/control_mapping/mapping'
) and time.time() < stime + max_duration:
time.sleep(0.1)
self._output_path = get_output_path()
self._logger = get_logger(get_filename_without_extension(__file__),
self._output_path)
self._mapping = rospy.get_param('/control_mapping/mapping')
cprint(f'mapping: {self._mapping}', self._logger)
# get robot command topics according to control keys
self._publishers = {} # required publishers to control robot(s)
control_types = []
for fsm_state in FsmState.members():
if fsm_state not in self._mapping.keys():
self._mapping[fsm_state] = {}
else:
control_types.extend(self._mapping[fsm_state].keys())
for control_type in set(control_types):
self._publishers[control_type] = rospy.Publisher(
rospy.get_param(f'/robot/{control_type}_topic'),
Twist,
queue_size=10)
noise_config = rospy.get_param('/control_mapping/noise', None)
self._noise = eval(f"{noise_config['name']}(**noise_config['args'])"
) if noise_config is not None else None
self._fsm_state = FsmState.Unknown
self._messages = {} # contains all controls comin from actors
self._rate_fps = rospy.get_param('/control_mapping/rate_fps', 60)
self.count = 0
self._subscribe()
rospy.init_node('control_mapper')
def __init__(self):
# wait for ROS to be ready
stime = time.time()
max_duration = 60
while not rospy.has_param(
'/fsm/mode') and time.time() < stime + max_duration:
time.sleep(0.1)
self._rate_fps = rospy.get_param('/fsm/rate_fps', 60)
self._output_path = get_output_path()
self._logger = get_logger(get_filename_without_extension(__file__),
self._output_path)
self._run_number = 0
self._reward_calculator = RewardCalculator()
self._init_fields(
) # define fields which require a resetting at each run
self._state_pub = rospy.Publisher('/fsm/state', String, queue_size=10)
self._rewards_pub = rospy.Publisher('/fsm/reward',
RosReward,
queue_size=10)
self._subscribe()
rospy.init_node('fsm', anonymous=True)
self.run()
def __init__(self):
self.count = 0
rospy.init_node('controller')
stime = time.time()
max_duration = 60
while not rospy.has_param('/actor/mathias_controller/specs'
) and time.time() < stime + max_duration:
time.sleep(0.01)
self._specs = rospy.get_param('/actor/mathias_controller/specs')
self._output_path = get_output_path()
self._logger = get_logger(get_filename_without_extension(__file__),
self._output_path)
cprint(f'controller specifications: {self._specs}', self._logger)
with open(os.path.join(self._output_path, 'controller_specs.yml'),
'w') as f:
yaml.dump(self._specs, f)
self._reference_height = rospy.get_param('/world/starting_height', 1)
self._rate_fps = self._specs[
'rate_fps'] if 'rate_fps' in self._specs.keys() else 5
self.max_input = self._specs[
'max_input'] if 'max_input' in self._specs.keys() else 1.0
# Not sure if this sample time should correspond to update rate of controller or measurement rate
self._sample_time = 1. / self._rate_fps
self.Kp_x = self._specs['Kp_x'] if 'Kp_x' in self._specs.keys(
) else 2.0
self.Ki_x = self._specs['Ki_x'] if 'Ki_x' in self._specs.keys(
) else 0.2
self.Kd_x = self._specs['Kd_x'] if 'Kd_x' in self._specs.keys(
) else 0.4
self.Kp_y = self._specs['Kp_y'] if 'Kp_y' in self._specs.keys(
) else 2.0
self.Ki_y = self._specs['Ki_y'] if 'Ki_y' in self._specs.keys(
) else 0.2
self.Kd_y = self._specs['Kd_y'] if 'Kd_y' in self._specs.keys(
) else 0.4
self.Kp_z = self._specs['Kp_z'] if 'Kp_z' in self._specs.keys(
) else 2.0
self.Ki_z = self._specs['Ki_z'] if 'Ki_z' in self._specs.keys(
) else 0.2
self.Kd_z = self._specs['Kd_z'] if 'Kd_z' in self._specs.keys() else 0.
self.K_theta = self._specs['K_theta'] if 'K_theta' in self._specs.keys(
) else 0.3
self.real_yaw = 0
self.desired_yaw = None
self.last_cmd = None
noise_config = self._specs['noise'] if 'noise' in self._specs.keys(
) else {}
self._noise = eval(f"{noise_config['name']}(**noise_config['args'])"
) if noise_config else None
self.prev_cmd = Twist()
self.pose_est = Pose()
self.vel_est = Point()
self.pose_ref = PointStamped()
self.vel_ref = Twist()
self.prev_pose_error = PointStamped()
self.prev_vel_error = PointStamped()
self.last_measurement = None
self._robot = rospy.get_param('/robot/model_name')
cprint(f'model_name: {self._robot}', self._logger)
self._publisher = rospy.Publisher('cmd_vel', Twist, queue_size=10)
self._subscribe()
def __init__(self):
rospy.init_node('teleop_twist_keyboard')
self.camera_direction = 'straight'
start_time = time.time()
max_duration = 60
while not rospy.has_param(
'/output_path') and time.time() < start_time + max_duration:
time.sleep(0.1)
self.specs = rospy.get_param('/actor/keyboard/specs')
super().__init__(config=ActorConfig(name='keyboard', specs=self.specs))
self.settings = termios.tcgetattr(sys.stdin)
self._logger = get_logger(get_filename_without_extension(__file__),
get_output_path())
self.command_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
self.rate_fps = self.specs['rate_fps']
self.speed = self.specs['speed']
self.turn = self.specs['turn']
self.message = self.specs['message']
self.moveBindings = self.specs[
'moveBindings'] if 'moveBindings' in self.specs.keys() else None
self.topicBindings = self.specs[
'topicBindings'] if 'topicBindings' in self.specs.keys() else None
if self.topicBindings is not None:
self.publishers = {
key: rospy.Publisher(name=self.topicBindings[key],
data_class=Empty,
queue_size=10)
for key in self.topicBindings.keys()
}
cprint(f'topicBindings: \n {self.topicBindings}', self._logger)
self.serviceBindings = None
if 'serviceBindings' in self.specs.keys():
self.serviceBindings = {}
for service_specs in self.specs['serviceBindings']:
self.serviceBindings[service_specs['key']] = {
'name':
service_specs['name'],
'proxy':
rospy.ServiceProxy(service_specs['name'],
eval(service_specs['type'])),
'message':
service_specs['message']
}
cprint(f'serviceBindings: \n {self.serviceBindings}', self._logger)
self.methodBindings = self.specs[
'methodBindings'] if 'methodBindings' in self.specs.keys(
) else None
if self.methodBindings is not None:
self.camera_control_publisher = rospy.Publisher(
'/bebop/camera_control', Twist)
for method in self.methodBindings.values():
assert method in self.__dir__() # make sure that method exists
self.x = 0
self.y = 0
self.z = 0
self.roll = 0
self.pitch = 0
self.yaw = 0
self.reset_control_fields()
def __init__(self):
self.count = 0
rospy.init_node('controller')
stime = time.time()
max_duration = 60
while not rospy.has_param('/actor/mathias_controller/specs') and time.time() < stime + max_duration:
time.sleep(0.01)
self._specs = rospy.get_param('/actor/mathias_controller/specs')
self._output_path = get_output_path()
self._logger = get_logger(get_filename_without_extension(__file__), self._output_path)
cprint(f'controller specifications: {self._specs}', self._logger)
# with open(os.path.join(self._output_path, 'controller_specs.yml'), 'w') as f:
# yaml.dump(self._specs, f)
self._reference_height = rospy.get_param('/world/starting_height', 1)
self._rate_fps = self._specs['rate_fps'] if 'rate_fps' in self._specs.keys() else 15
self.max_input = self._specs['max_input'] if 'max_input' in self._specs.keys() else 1.0
# Not sure if this sample time should correspond to update rate of controller or measurement rate
self._control_period = 1. / self._rate_fps
self.Kp_x = self._specs['Kp_x'] if 'Kp_x' in self._specs.keys() else .5
self.Ki_x = self._specs['Ki_x'] if 'Ki_x' in self._specs.keys() else 0.
self.Kd_x = self._specs['Kd_x'] if 'Kd_x' in self._specs.keys() else .5
self.Kp_y = self._specs['Kp_y'] if 'Kp_y' in self._specs.keys() else .5
self.Ki_y = self._specs['Ki_y'] if 'Ki_y' in self._specs.keys() else 0.
self.Kd_y = self._specs['Kd_y'] if 'Kd_y' in self._specs.keys() else .5
self.Kp_z = self._specs['Kp_z'] if 'Kp_z' in self._specs.keys() else 1.
self.Ki_z = self._specs['Ki_z'] if 'Ki_z' in self._specs.keys() else .5
self.Kd_z = self._specs['Kd_z'] if 'Kd_z' in self._specs.keys() else 0.
self.K_theta = self._specs['K_theta'] if 'K_theta' in self._specs.keys() else 1.0
self._robot = rospy.get_param('/robot/model_name')
self.model = BebopModel()
self.filter = KalmanFilter(model=self.model)
# if 'real' in self._robot:
# Velocity in yaw is not measured on real bebop drone
# Therefore, measurements should be ignored (weight of innovation = 1/mease_noise_cov)
self.filter.meas_noise_cov[7, 7] = 10e8
self.show_graph = self._specs['show_graph'] if 'show_graph' in self._specs.keys() else True
self.data = {title: {
axis: {label: [] for label in ['predicted', 'observed', 'adjusted']} for axis in ['x', 'y', 'z', 'yaw']}
for title in ['pose', 'velocity', 'command']
}
self.visualiser = rospy.Publisher('visualisation', Image, queue_size=10)
self.last_measurement = None # used to invoke slower measurement update in simulation
self.desired_yaw = None
self.real_yaw = None
noise_config = self._specs['noise'] if 'noise' in self._specs.keys() else {}
self._noise = eval(f"{noise_config['name']}(**noise_config['args'])") if noise_config else None
self.last_cmd = TwistStamped(header=Header(stamp=rospy.Time().now()))
self.pose_ref = None
# expressed in rotated global frame according to drone's yaw
self.vel_ref = TwistStamped(header=Header(frame_id="global_rotated"))
self.prev_pose_error = PointStamped(header=Header(frame_id="global"))
self.prev_vel_error = PointStamped(header=Header(frame_id="global_rotated"))
self.pose_est = PoseStamped(header=Header(frame_id="global"),
pose=Pose(orientation=Quaternion(w=1)))
self.vel_est = PointStamped(header=Header(frame_id="global_rotated"))
self._publisher = rospy.Publisher('cmd_vel', Twist, queue_size=10)
self._subscribe()
def __init__(self):
start_time = time.time()
max_duration = 60
while not rospy.has_param(
'/output_path') and time.time() < start_time + max_duration:
time.sleep(0.1)
self._output_path = get_output_path()
self._logger = get_logger(get_filename_without_extension(__file__),
self._output_path)
rospy.Subscriber(name='/fsm/state',
data_class=String,
callback=self._update_fsm_state)
# subscribe to odometry or other pose estimation type (such as GPS, gt_pose, ...)
odometry_type = rospy.get_param('/robot/position_sensor/type')
callback = f'_{camelcase_to_snake_format(odometry_type)}_callback'
assert callback in self.__dir__()
rospy.Subscriber(name=rospy.get_param('/robot/position_sensor/topic'),
data_class=eval(odometry_type),
callback=eval(f'self.{callback}'))
# fields
self._fsm_state = FsmState.Unknown
self._world_name = rospy.get_param('/world/world_name')
self._model_name = rospy.get_param('/robot/model_name', 'default')
self._gui_camera_height = rospy.get_param(
'/world/gui_camera_height',
20 if 'Turtle' in self._model_name else 50)
self._background_file = rospy.get_param(
'/world/background_file',
f'src/sim/ros/gazebo/background_images/default_2_2_10.jpg')
if not self._background_file.startswith('/'):
self._background_file = os.path.join(os.environ['CODEDIR'],
self._background_file)
self._background_image = Image.open(self._background_file)
# Camera intrinsics from assumption of horizontal and vertical field of view
width, height = self._background_image.size
horizontal_field_of_view = (40 * width / height) * 3.14 / 180
vertical_field_of_view = 40 * 3.14 / 180
self._fx = -width / 2 * np.tan(horizontal_field_of_view / 2)**(-1)
self._fy = -height / 2 * np.tan(vertical_field_of_view / 2)**(-1)
self._cx = width / 2
self._cy = height / 2
# Camera extrinsics: orientation (assumption) and translation (extracted from filename)
self._camera_global_orientation = np.asarray([[-1, 0, 0], [0, 1, 0],
[0, 0, -1]])
if len(os.path.basename(self._background_file).split('_')) > 2:
# extract camera position from background file name
x, y, z = (
float(x)
for x in self._background_file.split('.')[0].split('_')[-3:])
self._camera_global_translation = np.asarray([x, y, z])
else:
self._camera_global_translation = np.asarray([0, 0, 10])
self._minimum_distance_px = rospy.get_param(
'/world/minimum_distance_px', 30)
self._local_frame = [
np.asarray([0, 0, 0]),
np.asarray([0.17, 0, 0]),
np.asarray([0, 0.17, 0]),
np.asarray([0, 0, 0.17])
]
self._previous_position = None
self._frame_points = []
rospy.init_node('robot_mapper')
self._rate = rospy.Rate(100)
cprint(
f'specifications: \n'
f'cy: {self._cy}\n'
f'cx: {self._cx}\n'
f'fx: {self._fx}\n'
f'fy: {self._fy}\n'
f'camera_rotation: {self._camera_global_orientation}\n'
f'camera_translation: {self._camera_global_translation}\n',
self._logger)
