Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

remove tmp yaml files generated for server and motion capture. Add motion capture yaml import feature. #538

Merged
merged 4 commits into from
Jul 23, 2024
Merged

remove tmp yaml files generated for server and motion capture. Add motion capture yaml import feature. #538

Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
3fda7eb
switch from " to ' to be more consistent
llanesc Jun 28, 2024
237bb3f
remove temp yaml files generated for server and motion capture
llanesc Jun 28, 2024
62e69b1
clean up node and ros__parameters prefix
llanesc Jun 28, 2024
ee46445
add ability for user to specify their own motion capture yaml file
llanesc Jun 28, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
125 changes: 59 additions & 66 deletions crazyflie/launch/launch.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,8 +10,8 @@

def parse_yaml(context):
# Load the crazyflies YAML file
crazyflies_yaml_file = LaunchConfiguration('crazyflies_yaml_file').perform(context)
with open(crazyflies_yaml_file, 'r') as file:
crazyflies_yaml = LaunchConfiguration('crazyflies_yaml_file').perform(context)
with open(crazyflies_yaml, 'r') as file:
crazyflies = yaml.safe_load(file)

# server params
Expand All @@ -23,10 +23,7 @@ def parse_yaml(context):
with open(server_yaml, 'r') as ymlfile:
server_yaml_content = yaml.safe_load(ymlfile)

server_yaml_content["/crazyflie_server"]["ros__parameters"]['robots'] = crazyflies['robots']
server_yaml_content["/crazyflie_server"]["ros__parameters"]['robot_types'] = crazyflies['robot_types']
server_yaml_content["/crazyflie_server"]["ros__parameters"]['all'] = crazyflies['all']

server_params = [crazyflies] + [server_yaml_content['/crazyflie_server']['ros__parameters']]
# robot description
urdf = os.path.join(
get_package_share_directory('crazyflie'),
Expand All @@ -36,43 +33,73 @@ def parse_yaml(context):
with open(urdf, 'r') as f:
robot_desc = f.read()

server_yaml_content["/crazyflie_server"]["ros__parameters"]["robot_description"] = robot_desc
server_params[1]['robot_description'] = robot_desc

# construct motion_capture_configuration
motion_capture_yaml = os.path.join(
get_package_share_directory('crazyflie'),
'config',
'motion_capture.yaml')
motion_capture_yaml = LaunchConfiguration('motion_capture_yaml_file').perform(context)
with open(motion_capture_yaml, 'r') as ymlfile:
motion_capture_content = yaml.safe_load(ymlfile)

motion_capture_content["/motion_capture_tracking"]["ros__parameters"]["rigid_bodies"] = dict()
for key, value in crazyflies["robots"].items():
type = crazyflies["robot_types"][value["type"]]
if value["enabled"] and type["motion_capture"]["enabled"]:
motion_capture_content["/motion_capture_tracking"]["ros__parameters"]["rigid_bodies"][key] = {
"initial_position": value["initial_position"],
"marker": type["motion_capture"]["marker"],
"dynamics": type["motion_capture"]["dynamics"],
motion_capture_params = motion_capture_content['/motion_capture_tracking']['ros__parameters']
motion_capture_params['rigid_bodies'] = dict()
for key, value in crazyflies['robots'].items():
type = crazyflies['robot_types'][value['type']]
if value['enabled'] and type['motion_capture']['enabled']:
motion_capture_params['rigid_bodies'][key] = {
'initial_position': value['initial_position'],
'marker': type['motion_capture']['marker'],
'dynamics': type['motion_capture']['dynamics'],
}

# copy relevent settings to server params
server_yaml_content["/crazyflie_server"]["ros__parameters"]["poses_qos_deadline"] = motion_capture_content[
"/motion_capture_tracking"]["ros__parameters"]["topics"]["poses"]["qos"]["deadline"]

# Save server and mocap in temp file such that nodes can read it out later
with open('tmp_server.yaml', 'w') as outfile:
yaml.dump(server_yaml_content, outfile, default_flow_style=False, sort_keys=False)

with open('tmp_motion_capture.yaml', 'w') as outfile:
yaml.dump(motion_capture_content, outfile, default_flow_style=False, sort_keys=False)

server_params[1]['poses_qos_deadline'] = motion_capture_params['topics']['poses']['qos']['deadline']

return [
Node(
package='motion_capture_tracking',
executable='motion_capture_tracking_node',
condition=IfCondition(PythonExpression(["'", LaunchConfiguration('backend'), "' != 'sim' and '", LaunchConfiguration('mocap'), "' == 'True'"])),
name='motion_capture_tracking',
output='screen',
parameters= [motion_capture_params],
),
Node(
package='crazyflie',
executable='crazyflie_server.py',
condition=LaunchConfigurationEquals('backend','cflib'),
name='crazyflie_server',
output='screen',
parameters= server_params,
),
Node(
package='crazyflie',
executable='crazyflie_server',
condition=LaunchConfigurationEquals('backend','cpp'),
name='crazyflie_server',
output='screen',
parameters= server_params,
prefix=PythonExpression(['"xterm -e gdb -ex run --args" if ', LaunchConfiguration('debug'), ' else ""']),
),
Node(
package='crazyflie_sim',
executable='crazyflie_server',
condition=LaunchConfigurationEquals('backend','sim'),
name='crazyflie_server',
output='screen',
emulate_tty=True,
parameters= server_params,
)]

def generate_launch_description():
default_crazyflies_yaml_path = os.path.join(
get_package_share_directory('crazyflie'),
'config',
'crazyflies.yaml')

default_motion_capture_yaml_path = os.path.join(
get_package_share_directory('crazyflie'),
'config',
'motion_capture.yaml')

telop_yaml_path = os.path.join(
get_package_share_directory('crazyflie'),
Expand All @@ -82,23 +109,15 @@ def generate_launch_description():
return LaunchDescription([
DeclareLaunchArgument('crazyflies_yaml_file',
default_value=default_crazyflies_yaml_path),
OpaqueFunction(function=parse_yaml),
DeclareLaunchArgument('motion_capture_yaml_file',
default_value=default_motion_capture_yaml_path),
DeclareLaunchArgument('backend', default_value='cpp'),
DeclareLaunchArgument('debug', default_value='False'),
DeclareLaunchArgument('rviz', default_value='False'),
DeclareLaunchArgument('gui', default_value='True'),
DeclareLaunchArgument('mocap', default_value='True'),
DeclareLaunchArgument('server_yaml_file', default_value=''),
DeclareLaunchArgument('teleop_yaml_file', default_value=''),
DeclareLaunchArgument('mocap_yaml_file', default_value=''),
Node(
package='motion_capture_tracking',
executable='motion_capture_tracking_node',
condition=IfCondition(PythonExpression(["'", LaunchConfiguration('backend'), "' != 'sim' and '", LaunchConfiguration('mocap'), "' == 'True'"])),
name='motion_capture_tracking',
output='screen',
parameters= [PythonExpression(["'tmp_motion_capture.yaml' if '", LaunchConfiguration('mocap_yaml_file'), "' == '' else '", LaunchConfiguration('mocap_yaml_file'), "'"])],
),
OpaqueFunction(function=parse_yaml),
Node(
package='crazyflie',
executable='teleop',
Expand All @@ -120,32 +139,6 @@ def generate_launch_description():
executable='joy_node',
name='joy_node' # by default id=0
),
Node(
package='crazyflie',
executable='crazyflie_server.py',
condition=LaunchConfigurationEquals('backend','cflib'),
name='crazyflie_server',
output='screen',
parameters= [PythonExpression(["'tmp_server.yaml' if '", LaunchConfiguration('server_yaml_file'), "' == '' else '", LaunchConfiguration('server_yaml_file'), "'"])],
),
Node(
package='crazyflie',
executable='crazyflie_server',
condition=LaunchConfigurationEquals('backend','cpp'),
name='crazyflie_server',
output='screen',
parameters= [PythonExpression(["'tmp_server.yaml' if '", LaunchConfiguration('server_yaml_file'), "' == '' else '", LaunchConfiguration('server_yaml_file'), "'"])],
prefix=PythonExpression(['"xterm -e gdb -ex run --args" if ', LaunchConfiguration('debug'), ' else ""']),
),
Node(
package='crazyflie_sim',
executable='crazyflie_server',
condition=LaunchConfigurationEquals('backend','sim'),
name='crazyflie_server',
output='screen',
emulate_tty=True,
parameters= [PythonExpression(["'tmp_server.yaml' if '", LaunchConfiguration('server_yaml_file'), "' == '' else '", LaunchConfiguration('server_yaml_file'), "'"])],
),
Node(
condition=LaunchConfigurationEquals('rviz', 'True'),
package='rviz2',
Expand Down
Loading