如何自动从 .xacro 文件中提取 xacro 属性并在 ROS 的 python 脚本中使用它

编程问答 2022-04-24

问题描述

我为轮式机器人创建了一个 xacro 文件。 它的一个片段是这样的：

textRole: "display"

我有一个 python 脚本，它依赖于这个 xacro 文件中的 <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"/> 参数。

和我的 python_script：

chassis_length and chassis_width and wheel_radius

我不想手动更改这两个文件上的这些参数，而是希望通过以某种方式将这些文件链接到 ROS 中来自动执行此操作。 我知道我可以在 python 中使用一个简单的文件打开和提取操作，但是有没有更好的方法在 ROS 中使用 set_params 来做到这一点？ 如果是，您如何从 xacro 文件中获取参数。

解决方法

我在之前的项目中实际上是将Xacro作为机器人模型加载到参数服务器上，然后从参数服务器获取URDF到节点中并提取相应的参数 从它。这样您就可以确保您的工作区是一致的，并且每个节点都具有相同的参数集。通常，这仅在您定义的参数也用于 URDF 内部而不只是用作中间步骤时才有用。 （如果你真的需要，你可以使用额外的未使用的虚拟元素使它们可见。

让我们通过 Xacro 文件 urdf/some_robot.xacro 给出的简单机器人模型来完成它：

<?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>

如您所见，也可以使用 pi 进行计算，而无需手动输入其数值：see 4. Math expressions。

然后让我们创建一个名为 launch/parsing_example.launch 的启动文件，该文件将这个 Xacro 作为 robot description 加载到参数服务器上：

<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>

最后是一个实际读取机器人描述的节点 scripts/parsing_example

#!/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])

机器人模型最终是一个结构，您可以提取包含在 different ways

中的信息
  • 您可以使用地图，例如link_map and joint_map 和相应的链接或关节名称robot.link_map["some_link"]
  • 您可以通过它们的列表索引 link.visuals[0].geometry.size[0]
    • 访问它们
  • 或者你可以简单地遍历它们 对于robot.joints中的关节： 打印（联合）

然后，您应该能够通过打开两个新控制台、找到工作区并键入来启动代码

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