smartcar项目(二)：xacro改造和ArbotiX控制仿真

在复现ROS探索总结（六）——使用smartcar进行仿真项目时，发现项目运行过程中出现很多问题，而且这些问题不仅出现在我这，其他人也遇到了类似的问题。基础概念不清楚是产生这些错误的根本原因。

1.项目文件结构

系统环境：Ubuntu 16.04+ROS Kinetic版本
该项目是在上一个博客的基础上开展的，所以有些文件直接采用了上一个项目的文件，下方为文件结构：

.
├── CMakeLists.txt
├── config
│   ├── smartcar_arbotix.rviz          （系统自动生成）
│   └── smartcar_arbotix.yaml          （手动创建）
├── launch
│   └── smartcar_display.rviz.launch         （手动创建）
├── meshes
├── package.xml
└── urdf
    ├── gazebo.urdf.xacro               （机器人属性，手动创建）
    ├── smartcar_body.urdf.xacro        （机器人本体结构，手动创建）
    └── smartcar.urdf.xacro             （机器人实现，手动创建）

2.xacro改造

在很多情况下，ROS对urdf文件的支持并不是很好，使用宏定义的.xacro文件兼容性更好，扩展性也更好。所以我们把之前的urdf文件重新整理编写成.xacro文件。

gazebo.urdf.xacro文件

<?xml version="1.0"?>

<robot xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" 
    xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" 
    xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor" 
    xmlns:xacro="http://www.ros.org/wiki/xacro" 
    name="smartcar_gazebo">

<!-- ASUS Xtion PRO camera for simulation -->
<!-- gazebo_ros_wge100 plugin is in kt2_gazebo_plugins package -->
<xacro:macro name="smartcar_sim">
    <gazebo reference="base_link">
        <material>Gazebo/Blue</material>
        <turnGravityOff>false</turnGravityOff>
    </gazebo>

    <gazebo reference="right_front_wheel">
        <material>Gazebo/FlatBlack</material>
    </gazebo>

    <gazebo reference="right_back_wheel">
        <material>Gazebo/FlatBlack</material>
    </gazebo>

    <gazebo reference="left_front_wheel">
        <material>Gazebo/FlatBlack</material>
    </gazebo>

    <gazebo reference="left_back_wheel">
        <material>Gazebo/FlatBlack</material>
    </gazebo>

    <gazebo reference="head">
        <material>Gazebo/White</material>
    </gazebo>

</xacro:macro>

</robot>

smartcar_body.urdf.xacro文件

<?xml version="1.0"?>
<robot name="smartcar"
  xmlns:xacro="http://www.ros.org/wiki/xacro">
  <xacro:property name="M_PI" value="3.14159"/>

  <!-- Macro for SmartCar body. Including Gazebo extensions, but does not include Kinect -->
  <xacro:include filename="$(find smartcar_description)/urdf/gazebo.urdf.xacro"/>

  <xacro:property name="base_x" value="0.33" />
  <xacro:property name="base_y" value="0.33" />

  <xacro:macro name="smartcar_body">

    <link name="base_link">
      <inertial>
        <origin xyz="0 0 0.055"/>
        <mass value="100.0" />
        <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
      </inertial>
      <visual>
        <geometry>
          <box size="0.25 .16 .05"/>
        </geometry>
        <origin rpy="0 0 0" xyz="0 0 0.055"/>
        <material name="blue">
          <color rgba="0 0 .8 1"/>
        </material>
      </visual>
      <collision>
        <origin rpy="0 0 0" xyz="0 0 0.055"/>
        <geometry>
          <box size="0.25 .16 .05" />
        </geometry>
      </collision>
    </link>

    <link name="left_front_wheel">
      <inertial>
        <origin xyz="0.08 0.08 0.025"/>
        <mass value="1" />
        <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
      </inertial>
      <visual>
        <geometry>
          <cylinder length=".02" radius="0.025"/>
        </geometry>
        <material name="black">
          <color rgba="0 0 0 1"/>
        </material>
      </visual>
      <collision>
        <origin rpy="0 1.57075 1.57075" xyz="0.08 0.08 0.025"/>
        <geometry>
          <cylinder length=".02" radius="0.025"/>
        </geometry>
      </collision>
    </link>

    <joint name="left_front_wheel_joint" type="continuous">
      <axis xyz="0 0 1"/>
      <parent link="base_link"/>
      <child link="left_front_wheel"/>
      <origin rpy="0 1.57075 1.57075" xyz="0.08 0.08 0.025"/>
      <limit effort="100" velocity="100"/>
      <joint_properties damping="0.0" friction="0.0"/>
    </joint>

    <link name="right_front_wheel">
      <inertial>
        <origin xyz="0.08 -0.08 0.025"/>
        <mass value="1" />
        <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
      </inertial>
      <visual>
        <geometry>
          <cylinder length=".02" radius="0.025"/>
        </geometry>
        <material name="black">
          <color rgba="0 0 0 1"/>
        </material>
      </visual>
      <collision>
        <origin rpy="0 1.57075 1.57075" xyz="0.08 -0.08 0.025"/>
        <geometry>
          <cylinder length=".02" radius="0.025"/>
        </geometry>
      </collision>
    </link>

    <joint name="right_front_wheel_joint" type="continuous">
      <axis xyz="0 0 1"/>
      <parent link="base_link"/>
      <child link="right_front_wheel"/>
      <origin rpy="0 1.57075 1.57075" xyz="0.08 -0.08 0.025"/>
      <limit effort="100" velocity="100"/>
      <joint_properties damping="0.0" friction="0.0"/>
    </joint>

    <link name="left_back_wheel">
      <inertial>
        <origin xyz="-0.08 0.08 0.025"/>
        <mass value="1" />
        <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
      </inertial>
      <visual>
        <geometry>
          <cylinder length=".02" radius="0.025"/>
        </geometry>
        <material name="black">
          <color rgba="0 0 0 1"/>
        </material>
      </visual>
      <collision>
        <origin rpy="0 1.57075 1.57075" xyz="-0.08 0.08 0.025"/>
        <geometry>
          <cylinder length=".02" radius="0.025"/>
        </geometry>
      </collision>
    </link>

    <joint name="left_back_wheel_joint" type="continuous">
      <axis xyz="0 0 1"/>
      <parent link="base_link"/>
      <child link="left_back_wheel"/>
      <origin rpy="0 1.57075 1.57075" xyz="-0.08 0.08 0.025"/>
      <limit effort="100" velocity="100"/>
      <joint_properties damping="0.0" friction="0.0"/>
    </joint>

    <link name="right_back_wheel">
      <inertial>
        <origin xyz="-0.08 -0.08 0.025"/>
        <mass value="1" />
        <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
      </inertial>
      <visual>
        <geometry>
          <cylinder length=".02" radius="0.025"/>
        </geometry>
        <material name="black">
          <color rgba="0 0 0 1"/>
        </material>
      </visual>
      <collision>
        <origin rpy="0 1.57075 1.57075" xyz="-0.08 -0.08 0.025"/>
        <geometry>
          <cylinder length=".02" radius="0.025"/>
        </geometry>
      </collision>
    </link>

    <joint name="right_back_wheel_joint" type="continuous">
      <axis xyz="0 0 1"/>
      <parent link="base_link"/>
      <child link="right_back_wheel"/>
      <origin rpy="0 1.57075 1.57075" xyz="-0.08 -0.08 0.025"/>
      <limit effort="100" velocity="100"/>
      <joint_properties damping="0.0" friction="0.0"/>
    </joint>

    <link name="head">
      <inertial>
        <origin xyz="0.08 0 0.08"/>
        <mass value="1" />
        <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
      </inertial>
      <visual>
        <geometry>
          <box size=".02 .03 .03"/>
        </geometry>
        <material name="white">
          <color rgba="1 1 1 1"/>
        </material>
      </visual>
      <collision>
        <origin xyz="0.08 0 0.08"/>
        <geometry>
          <cylinder length=".02" radius="0.025"/>
        </geometry>
      </collision>
    </link>

    <joint name="tobox" type="fixed">
      <parent link="base_link"/>
      <child link="head"/>
      <origin xyz="0.08 0 0.08"/>
    </joint>
  </xacro:macro>

</robot>

smartcar.urdf.xacro文件

<?xml version="1.0"?>

<robot name="smartcar"  
    xmlns:xi="http://www.w3.org/2001/XInclude"
    xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz"
    xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model"
    xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
    xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body"
    xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom"
    xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint"
    xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
    xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
    xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering"
    xmlns:renderable="http://playerstage.sourceforge.net/gazebo/xmlschema/#renderable"
    xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics"
    xmlns:xacro="http://www.ros.org/wiki/xacro">

  <xacro:include filename="$(find smartcar_description)/urdf/smartcar_body.urdf.xacro" />

  <!-- Body of SmartCar, with plates, standoffs and Create (including sim sensors) -->
  <smartcar_body/>

  <smartcar_sim/>

</robot>

上述三个文件之间是层层递进引用的关系，smartcar.urdf.xacro包含smartcar_body.urdf.xacro，smartcar_body.urdf.xacro再包含gazebo.urdf.xacro。

3.Arbotix控制仿真

ArbotiX安装

Arbotix是一款控制电机、舵机的控制板，并提供相应的ROS功能包，但是这个功能包的功能不仅可以驱动真实的Arbotix控制板，它还提供一个差速控制器，通过接收速度控制指令更新机器人的joint状态，从而帮助我们实现机器人在rviz中的运动。

$ sudo apt-get install ros-kinetic-arbotix-*

smartcar_arbotix.yaml配置文件

port: /dev/ttyUSB0
baud: 115200
rate: 20
sync_write: True
sync_read: True
read_rate: 20
write_rate: 20

controllers: {
      #  Pololu motors: 1856 cpr = 0.3888105m travel = 4773 ticks per meter (empirical: 4100)
      base_controller:
         {
            type: diff_controller,
            base_frame_id: base_link,
            base_width: 0.26,
            ticks_meter: 4100,
            Kp: 12,
            Kd: 12,
            Ki: 0,
            Ko: 50,
            accel_limit: 1.0,
         },
   }

4.ROS launch启动文件：`smartcar_display.rviz.launch`

<launch>
    <param name="/use_sim_time" value="false" />

    <!-- 调用xacro文件解析器 -->
    <arg name="model" default="$(find xacro)/xacro --inorder '$(find smartcar_description)/urdf/smartcar.urdf.xacro'" />

    <!-- 设置GUI参数，显示关节控制插件 -->
    <arg name="use_gui" default="false" />

    <!-- 加载机器人的Xacro模型 -->
    <param name="robot_description" command="$(arg model)" />

    <!-- 启动arbotix_driver节点，发布控制信息 -->
    <node name="arbotix" pkg="arbotix_python" type="arbotix_driver" output="screen">
        <rosparam file="$(find smartcar_description)/config/smartcar_arbotix.yaml" command="load" />
        <param name="sim" value="true"/>
    </node>

    <!-- 运行joint_state_publisher_gui节点，发布机器人关节状态 -->
    <node name="joint_state_publisher_gui" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui">
    </node>
    
    <!-- 运行robot_state_publisher节点，发布TF -->
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher">
        <param name="publish_frequency" type="double" value="20.0" />
    </node>

    <!-- 静态坐标变换，车和轮子不会发生相对位移 -->
    <node pkg="tf" type="static_transform_publisher" name="odom_left_wheel_broadcaster" args="0 0 0 0 0 0 /base_link /left_front_link 100" />
    <node pkg="tf" type="static_transform_publisher" name="odom_right_wheel_broadcaster" args="0 0 0 0 0 0 /base_link /right_front_link 100" />

    <!-- 运行rviz可视化界面 -->
    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find smartcar_description)/config/smartcar_arbotix.rviz" required="true" />
</launch>

运动指令

rostopic pub -r 10 /cmd_vel geometry_msgs/Twist ‘{linear: {x: 0.5, y: 0, z: 0}, angular: {x: 0, y: 0, z: 0.5}}’

5.注意事项

5.1.`xacro`前缀缺失提示

xacro: in-order processing became default in ROS Melodic. You can drop the option.
Deprecated: xacro tag ‘mrobot_body’ w/o ‘xacro:’ xml namespace prefix (will be forbidden in Noetic)
when processing file: /home/wxy/catkin_ws/src/mrobot_gazebo/urdf/mrobot.urdf.xacro
Use the following command to fix incorrect tag usage:
find . -iname “*.xacro” | xargs sed -i ‘s#<\([/]\?\)if\|unless\|include\|arg\|property\|macro\|insert_block#<\1xacro:\2#g'

由于ROS系统版本的差异，在Kinetic版本中的xacro文件，凡是带有如下名称的标签块，都需要加前缀xacro:

include、arg、property、macro

5.2.xacro声明错误

具体的错误提示，忘记记录，古月大佬在博客中给出的声明是

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

凡是涉及http://ros.org/wiki/xacro，均需要改成http://www.ros.org/wiki/xacro

5.3.xacro解析器调用错误

原调用语句：

<!-- Load the URDF/Xacro model of our robot -->  
<arg name="urdf_file" default="$(find xacro)/xacro.py '$(find smartcar_description)/urdf/smartcar.urdf.xacro'" />

将原有的解析器调用更改如下

<!-- 调用xacro文件解析器 -->
<arg name="model" default="$(find xacro)/xacro --inorder '$(find smartcar_description)/urdf/smartcar.urdf.xacro'" />

不再使用xacro.py进行xacro文件解析

5.4.rviz可视化界面调用

原调用语句：

<node name="rviz" pkg="rviz" type="rviz" args="-d $(find smartcar_description)/urdf.vcg" />

将原有的解析器调用更改如下

<!-- 运行rviz可视化界面 -->
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find smartcar_description)/config/smartcar_arbotix.rviz" required="true" />

使用smartcar_arbotix.rviz进行Rviz的配置文件保存，而不是urdf.vcg，同时该配置文件可以通过加载机器人之后，在rviz的UI界面进行文件保存，所以是自动生成的。

5.5.Rviz设置

将Global中的fixed frame改成odom，小车就能够动起来。

6.参考资料

1.ROS探索总结（六）——使用smartcar进行仿真
2.ROS探索总结（六）——使用smartcar进行仿真复现
3.ROS探索（4）——SmartCar模擬

最后编辑于：2021.01.21 16:25:09

人面猴
序言：七十年代末，一起剥皮案震惊了整个滨河市，随后出现的几起案子，更是在滨河造成了极大的恐慌，老刑警刘岩，带你破解...
沈念sama阅读 219,490评论 6赞 508
死咒
序言：滨河连续发生了三起死亡事件，死亡现场离奇诡异，居然都是意外死亡，警方通过查阅死者的电脑和手机，发现死者居然都...
沈念sama阅读 93,581评论 3赞 395
救了他两次的神仙让他今天三更去死
文/潘晓璐我一进店门，熙熙楼的掌柜王于贵愁眉苦脸地迎上来，“玉大人，你说我怎么就摊上这事。” “怎么了？”我有些...
开封第一讲书人阅读 165,830评论 0赞 356
道士缉凶录：失踪的卖姜人
文/不坏的土叔我叫张陵，是天一观的道长。经常有香客问我，道长，这世上最难降的妖魔是什么？我笑而不...
开封第一讲书人阅读 58,957评论 1赞 295
港岛之恋（遗憾婚礼）
正文为了忘掉前任，我火速办了婚礼，结果婚礼上，老公的妹妹穿的比我还像新娘。我一直安慰自己，他们只是感情好，可当我...
茶点故事阅读 67,974评论 6赞 393
恶毒庶女顶嫁案：这布局不是一般人想出来的
文/花漫我一把揭开白布。她就那样静静地躺着，像睡着了一般。火红的嫁衣衬着肌肤如雪。梳的纹丝不乱的头发上，一...
开封第一讲书人阅读 51,754评论 1赞 307
城市分裂传说
那天，我揣着相机与录音，去河边找鬼。笑死，一个胖子当着我的面吹牛，可吹牛的内容都是我干的。我是一名探鬼主播，决...
沈念sama阅读 40,464评论 3赞 420
双鸳鸯连环套：你想象不到人心有多黑
文/苍兰香墨我猛地睁开眼，长吁一口气：“原来是场噩梦啊……” “哼！你这毒妇竟也来了？” 一声冷哼从身侧响起，我...
开封第一讲书人阅读 39,357评论 0赞 276
万荣杀人案实录
序言：老挝万荣一对情侣失踪，失踪者是张志新（化名）和其女友刘颖，没想到半个月后，有当地人在树林里发现了一具尸体，经...
沈念sama阅读 45,847评论 1赞 317
护林员之死
正文独居荒郊野岭守林人离奇死亡，尸身上长有42处带血的脓包…… 初始之章·张勋以下内容为张勋视角年9月15日...
茶点故事阅读 37,995评论 3赞 338
白月光启示录
正文我和宋清朗相恋三年，在试婚纱的时候发现自己被绿了。大学时的朋友给我发了我未婚夫和他白月光在一起吃饭的照片。...
茶点故事阅读 40,137评论 1赞 351
活死人
序言：一个原本活蹦乱跳的男人离奇死亡，死状恐怖，灵堂内的尸体忽然破棺而出，到底是诈尸还是另有隐情，我是刑警宁泽，带...
沈念sama阅读 35,819评论 5赞 346
日本核电站爆炸内幕
正文年R本政府宣布，位于F岛的核电站，受9级特大地震影响，放射性物质发生泄漏。R本人自食恶果不足惜，却给世界环境...
茶点故事阅读 41,482评论 3赞 331
男人毒药：我在死后第九天来索命
文/蒙蒙一、第九天我趴在偏房一处隐蔽的房顶上张望。院中可真热闹，春花似锦、人声如沸。这庄子的主人今日做“春日...
开封第一讲书人阅读 32,023评论 0赞 22
一桩弑父案，背后竟有这般阴谋
文/苍兰香墨我抬头看了看天上的太阳。三九已至，却和暖如春，着一层夹袄步出监牢的瞬间，已是汗流浃背。一阵脚步声响...
开封第一讲书人阅读 33,149评论 1赞 272
情欲美人皮
我被黑心中介骗来泰国打工，没想到刚下飞机就差点儿被人妖公主榨干…… 1. 我叫王不留，地道东北人。一个月前我还...
沈念sama阅读 48,409评论 3赞 373
代替公主和亲
正文我出身青楼，却偏偏与公主长得像，于是被迫代替她去往敌国和亲。传闻我的和亲对象是个残疾皇子，可洞房花烛夜当晚...
茶点故事阅读 45,086评论 2赞 355