how to automate xacro property extraction from .xacro file and use it in a python script in ROS

i have created a xacro file for a wheeled robot. a snippet of it is like this:

<robot name="em_3905" xmlns:xacro="http://www.ros.org/wiki/xacro">
  <!-- Degree-to-radian conversions -->
  <xacro:property name="degrees_45" value="0.785398163"/>
  <xacro:property name="degrees_90" value="1.57079633"/>

  <!-- chassis_length is measured along the x axis, chassis_width
       along the y axis, and chassis_height along the z axis. -->
  <xacro:property name="chassis_length" value="4"/>
  <xacro:property name="chassis_width" value="1"/>
  <xacro:property name="chassis_height" value="0.01"/>
  <xacro:property name="chassis_mass" value="2.788"/>
  <xacro:property name="wheel_radius" value="0.2"/>

i have a python script which is dependent on the chassis_length and chassis_width and wheel_radius parameters from this xacro file.

and my python_script:

#!/usr/bin/env python

import rospy
import numpy as np
from numpy import pi

from std_msgs.msg import Float64
from geometry_msgs.msg import Twist

class steering:

    def __init__(self):
        rospy.init_node("steering_controller", anonymous=True)
        rospy.Subscriber("/cmd_vel", Twist, self.cmd_vel_callback)

        self.steering_L_pub = rospy.Publisher("/xacro/left_steering_wheel_position_controller/command", Float64, queue_size=10)
        self.steering_R_pub = rospy.Publisher("/xacro/right_steering_wheel_position_controller/command", Float64, queue_size=10)
        


        self.str_L = Float64()
        self.str_R = Float64()
        self.drive_L = Float64()
        self.drive_R = Float64()

        self.Vx = 0.0
        self.Wz = 0.0

        self.max_alp = pi/2.0

        ## Car paramters
        self.wheel_radius = 0.2 # meter
        self.chassis_length = 4 
        self.chassis_width = 1 

        self.run()

        rospy.spin()

instead of changing these parameters on both the files manually, i want to automate this by somehow linking these files in ROS. I know i can use a simple file open and extract operation in python but is there a better way to do it in ROS like with the use of set_params? if yes, how do you get params from xacro file.

Solution

What I did on previous projects is actually load the Xacro as a robot model onto the parameter server and then fetch the URDF into the node from the parameter server and extract the corresponding parameters from it. This way you make sure that your workspace is consistent and every node has the same set of parameters. Generally this is though only helpful if the parameters you define are then also used inside the URDF and not only used as intermediate steps. (If you really have to, you could make them visible with additionally un-used dummy elements.

Let's go through it with a simple robot model given by a Xacro file urdf/some_robot.xacro as:

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

  <xacro:property name="angle_x" value="${pi/4}"/>
  <xacro:property name="angle_y" value="${pi/2}"/>
  <xacro:property name="length" value="1"/>
  <xacro:property name="width" value="2"/>
  <xacro:property name="height" value="3"/>
  <xacro:property name="mass" value="4"/>

  <link name="some_link">
    <visual name="some_link_visual">
      <geometry>
        <box size="${length} ${width} ${height}"/>
      </geometry>
      <origin xyz="0 0 0" rpy="${angle_x} ${angle_y} 0"/>
    </visual>
    <inertial>
      <mass value="${mass}"/>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <inertia ixx="1" ixy="0" ixz="0"
               iyy="1" iyz="0"
               izz="1"/>
    </inertial>
  </link>
</robot>

As you can see it is also possible to calculate with pi without having to input its numeric value manually: see 4. Math expressions.

Let's then create a launch file named launch/parsing_example.launch which loads this Xacro as the robot description onto the parameter server:

<launch>
  <param name="robot_description" command="$(find xacro)/xacro '$(find parsing_example)/urdf/some_robot.xacro'"/>
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
</launch>

And finally a node scripts/parsing_example that actually reads the robot description

#!/usr/bin/env python
import roslib; roslib.load_manifest("urdfdom_py");
import rospy
from urdf_parser_py.urdf import URDF

if __name__ == '__main__':
  rospy.init_node("parsing_example", anonymous=True)

  # Fetch model from parameter server
  robot = URDF.from_parameter_server()
  # Check if robot model is valid
  robot.check_valid();
  # Print the robot model
  print(robot)

  # Extract information from node
  link = robot.link_map["some_link"]
  print(link.inertial.mass)
  print(link.visual.geometry.size[0])

The robot model is in the end a structure and you might extract the information contained in different ways

You can use the maps such as link_map and joint_map with the corresponding name of the link or joint robot.link_map["some_link"]
You could access them by their list index link.visuals[0].geometry.size[0]
Or you could simply loop through them for joint in robot.joints: print(joint)

You should then be able to launch the code by opening two new consoles, sourcing the workspace and typing

$ roslaunch parsing_example some_robot.launch
$ rosrun parsing_example parsing_example.py