[ROS2] Substituting a parameter from confg file while keeping others within Python launch file
Hey all,
I am trying to substitute (or prepend) specific parameters from a Python launch file in ROS2. I have filenames specified in launch files, that must be passed as full paths to ROS nodes via launch parameters. Therefore, the filenames from
The problem is, is there a way to prepend some text to specifically one parameters out of the configuration file? All I can think of is to parse the YAML config file in python and then just place it as a dict into the parameters field.
My YAML config file:
tb_lm:
efficiency_map:
ros__parameters:
filename: "PMSynRM.csv"
And this is my launch file:
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description():
ld = LaunchDescription()
config = os.path.join(
get_package_share_directory('loading_motor_dt'),
'config',
'params.yaml'
)
data_processor=Node(
package = 'loading_motor_dt',
namespace = 'tb_lm',
name = 'efficiency_map',
executable = 'efficiency_map',
parameters = [config]
)
ld.add_action(data_processor)
return ld
How can I grab the filename out of config and prepend to it the $HOME path for example (or even a string?) is there a ROS2 way to do this, or better to just transform the YAML into dict, and substitute whatever is required, and pass it to the parameters?
EDIT 1
Here is the update config file for what I am trying to achieve:
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node
import yaml
from pathlib import Path
home = str(Path.home())
"""Used to grab the contents of configuration YAML and
prepend full paths to all the filenames fields"""
def prepend_to_filenames(config_file, package_name):
with open(config_file, "r") as f:
config_yaml = yaml.safe_load(f)
for ns in config_yaml:
for node in config_yaml[ns]:
try:
filename = config_yaml[ns][node]["ros__parameters"]["filename"]
config_yaml[ns][node]["ros__parameters"]["filename"] = os.path.join(
get_package_share_directory(package_name),'data_files', filename)
except:
pass
# For DEBUG purposes only
with open(os.path.join(home,"debug.yaml"),'w') as f:
yaml.dump(config_yaml, f)
return config_yaml
def generate_launch_description():
ld = LaunchDescription()
config = os.path.join(
get_package_share_directory('loading_motor_dt'),
'config',
'params.yaml'
)
params_prepended = prepend_to_filenames(config, "loading_motor_dt")
data_processor=Node(
package = 'loading_motor_dt',
namespace = 'tb_lm_left',
name = 'efficiency_map',
executable = 'efficiency_map',
parameters = [params_prepended]
)
ld.add_action(data_processor)
return ld