ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
1

[ROS2] Any idea on how to pass parameters (YAML) to a Xacro file via launch python script?

asked 2022-06-05 13:13:24 -0500

Andromeda gravatar image

updated 2022-06-05 13:14:03 -0500

In my launch python script I can load and pass parameters from a .yaml file into my code without any problem:

from ament_index_python.packages       import get_package_share_path
from launch_ros.parameter_descriptions import ParameterValue
from launch_ros.actions                import Node
from launch.substitutions              import Command, LaunchConfiguration
from launch.actions                    import DeclareLaunchArgument, ExecuteProcess
from launch                            import LaunchDescription

import os

def generate_launch_description():

    package_p     = get_package_share_path('my_pkg')
    urdf_path      = os.path.join(package_p, 'my_robot.urdf')
    config_path   = os.path.join(package_p, 'my_params.yaml')

    prog = Node(package    = "my_pkg",
                       executable  = "my_pkg",
                       output         = "screen",
                       parameters  = [config_path]) 

    return LaunchDescription([prog])

The parameters defined in the my_params.yaml file are passed to my node.

The problem is that I need to pass the same parameters to my xacro file, where I defined my robot.

<?xml version="1.0"?>
<robot name="my_robot" xmlns:xacro="http://www.ros.org/wiki/xacro">

    <xacro:arg name="first_parameter"/>

</robot>

As you can see I don't know how to "catch" the parameters into the xacro/urdf file.

Do you have any idea?

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2022-06-13 09:26:52 -0500

djchopp gravatar image

updated 2022-06-13 13:11:19 -0500

Is your end goal to use xacro for your robot_state_publisher at runtime? Xacro actually has a function (load_yaml()) that allows you to read in a yaml from a file path. In this case you can hand the path of the file through your xacro call using the xacro_arg_name:=param_to_pass notation. If you are running Galactic or later here is how I would do it (with launch configurations for full runtime flexibility):

EDIT:

For foxy you may need to use an opaque functions as described in the second answer of this question

Launch File:

#!/usr/bin/env python3

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, LogInfo
from launch.substitutions import LaunchConfiguration, Command, PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.descriptions import ParameterValue
from launch_ros.substitutions import FindPackageShare

def generate_launch_description():
  this_pkg = FindPackageShare('test_package')

  model_path = LaunchConfiguration('model_path')
  params_path = LaunchConfiguration('params_path')

  return LaunchDescription([
    DeclareLaunchArgument(
      'model_path',
      default_value=PathJoinSubstitution([this_pkg,'xacro','model.xacro']),
      description='Full path to model xacro file including filename and extension'
    ),

    DeclareLaunchArgument(
      'params_path',
      default_value=PathJoinSubstitution([this_pkg,'params','model.yaml']),
      description='Full path to parameter yaml file including filename and extension'
    ),        

    Node(
      package='robot_state_publisher',
      executable='robot_state_publisher',
      name='robot_state_publisher',
      output='screen',
      parameters=[{
        'robot_description': ParameterValue(Command(['xacro ',model_path,' params_path:=',params_path]), value_type=str)
      }],
    )

  ])

Xacro File: model.xacro

<?xml version="1.0"?>
<robot name="model" xmlns:xacro="http://www.ros.org/wiki/xacro">
  <!-- 
    The xacro:arg "params_path" exposes the arg to launch file.
    The xacro:property "params_path" is required to get the evaluated symbol into xacro before "load_yaml"
    The xacro:proterty "mp" (which stands for "model parameters") gets the dictionary from "params_path"
    and is accessed using ${mp['key_name']} which will evaluate to the key value

    The xacro:arg and xacro:property "params_path" shares a name but does not seem to clobber eachother.
    They are not required to have the same name, it was just convenient
  -->
  <xacro:arg name="params_path" default=""/> <!-- Need argument to get from launch file -->
  <xacro:property name="params_path" value="$(arg params_path)"/> <!-- Need seperate property for xacro inorder processing -->
  <xacro:property name="mp" value="${load_yaml(params_path)}"/> <!-- Read in the yaml dict as mp (short for model parameters) -->


  <xacro:property name="PI" value="3.1415926535897931" />

  <xacro:property name="wheel_mass" value="${mp['wheel_mass']}"/>
  <xacro:property name="wheel_width" value="${mp['wheel_width']}"/>
  <xacro:property name="wheel_radius" value="${mp['wheel_radius']}"/>


  <link name="base_link"/>

  <link name="body_link">
    <!-- Visual box geometry -->
    <visual name="visual">
      <geometry>
        <box size="1 1 0.5"/>
      </geometry>
    </visual>

    <!-- Collision box geometry-->
    <collision name="collision">
      <geometry>
        <box size="1 1 0.5"/>
      </geometry>
    </collision>

    <!-- Inertia for a box -->
    <inertial>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <mass value="1645.0"/>
      <inertia
          ixx="${(1.0/12.0) * 1645.0 * ( 1 * 1 + 0.5 * 0.5 )}"  
          ixy="0" 
          ixz="0" 
          iyy="${(1.0/12.0) * 1645.0 * ( 1 * 1 + 0.5 * 0.5 )}"  
          iyz="0" 
          izz="${(1.0/12.0) * 1645 ...
(more)
edit flag offensive delete link more

Question Tools

3 followers

Stats

Asked: 2022-06-05 13:13:24 -0500

Seen: 1,256 times

Last updated: Jun 13 '22