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

server yaml file substitute with temporary files #454

Merged
merged 5 commits into from
Apr 9, 2024
Merged

server yaml file substitute with temporary files #454

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
ba1f506
got external yaml file to be entered in launch
knmcguire Mar 18, 2024
075de4e
used python expression to choose parameter file
knmcguire Mar 18, 2024
062c89f
clean up
knmcguire Mar 18, 2024
8974aa9
implement param load for mocap and teleop
knmcguire Apr 9, 2024
a5cf5b7
variable change
knmcguire Apr 9, 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
4 changes: 2 additions & 2 deletions crazyflie/config/crazyflies.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,7 @@ robots:
cf231:
enabled: true
uri: radio://0/80/2M/E7E7E7E7E7
initial_position: [0, 0, 0]
initial_position: [0.0, 0.0, 0.0]
type: cf21 # see robot_types
# firmware_params:
# kalman:
Expand All @@ -17,7 +17,7 @@ robots:
cf5:
enabled: false
uri: radio://0/80/2M/E7E7E7E705
initial_position: [0, -0.5, 0]
initial_position: [0.0, -0.5, 0.0]
type: cf21 # see robot_types
# firmware_params:
# kalman:
Expand Down
44 changes: 26 additions & 18 deletions crazyflie/launch/launch.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,7 @@


def generate_launch_description():

# load crazyflies
crazyflies_yaml = os.path.join(
get_package_share_directory('crazyflie'),
Expand All @@ -26,18 +27,21 @@ def generate_launch_description():
'server.yaml')

with open(server_yaml, 'r') as ymlfile:
server_yaml_contents = yaml.safe_load(ymlfile)
server_yaml_content = yaml.safe_load(ymlfile)

server_params = [crazyflies] + [server_yaml_contents["/crazyflie_server"]["ros__parameters"]]
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']

# robot description
urdf = os.path.join(
get_package_share_directory('crazyflie'),
'urdf',
'crazyflie_description.urdf')
with open(urdf, 'r') as f:

robot_desc = f.read()
server_params[1]["robot_description"] = robot_desc
server_yaml_content["/crazyflie_server"]["ros__parameters"]["robot_description"] = robot_desc

# construct motion_capture_configuration
motion_capture_yaml = os.path.join(
Expand All @@ -46,40 +50,44 @@ def generate_launch_description():
'motion_capture.yaml')

with open(motion_capture_yaml, 'r') as ymlfile:
motion_capture = yaml.safe_load(ymlfile)
motion_capture_content = yaml.safe_load(ymlfile)

motion_capture_params = motion_capture["/motion_capture_tracking"]["ros__parameters"]
motion_capture_params["rigid_bodies"] = dict()
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_params["rigid_bodies"][key] = {
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"],
}

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

# teleop params
teleop_params = os.path.join(
get_package_share_directory('crazyflie'),
'config',
'teleop.yaml')
# 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)

return LaunchDescription([
DeclareLaunchArgument('backend', default_value='cpp'),
DeclareLaunchArgument('debug', default_value='False'),
DeclareLaunchArgument('rviz', default_value='False'),
DeclareLaunchArgument('gui', 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=LaunchConfigurationNotEquals('backend','sim'),
name='motion_capture_tracking',
output='screen',
parameters=[motion_capture_params]
parameters= [PythonExpression(["'tmp_motion_capture.yaml' if '", LaunchConfiguration('mocap_yaml_file'), "' == '' else '", LaunchConfiguration('mocap_yaml_file'), "'"])],
),
Node(
package='crazyflie',
Expand All @@ -94,7 +102,7 @@ def generate_launch_description():
# ('cmd_full_state', 'cf6/cmd_full_state'),
# ('notify_setpoints_stop', 'cf6/notify_setpoints_stop'),
],
parameters=[teleop_params]
parameters= [PythonExpression(["'teleop.yaml' if '", LaunchConfiguration('teleop_yaml_file'), "' == '' else '", LaunchConfiguration('teleop_yaml_file'), "'"])],
),
Node(
package='joy',
Expand All @@ -107,15 +115,15 @@ def generate_launch_description():
condition=LaunchConfigurationEquals('backend','cflib'),
name='crazyflie_server',
output='screen',
parameters=server_params
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=server_params,
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(
Expand All @@ -125,7 +133,7 @@ def generate_launch_description():
name='crazyflie_server',
output='screen',
emulate_tty=True,
parameters=server_params
parameters= [PythonExpression(["'tmp_server.yaml' if '", LaunchConfiguration('server_yaml_file'), "' == '' else '", LaunchConfiguration('server_yaml_file'), "'"])],
),
Node(
condition=LaunchConfigurationEquals('rviz', 'True'),
Expand Down
Loading