从零开始搭建自动驾驶仿真环境（一）——创建仿真小车

1、本文章是默认已经有ROS使用基础和C++编程基础的，如果不具备，请学习ROS和C++。
2、本教程是基于LINUX 18.04；ROS melodic 环境；

---

一、创建sim_car 功能包

进入创建的工作空间中，运行如下代码

catkin_create_pkg sim_car roscpp rospy std_msgs

二、创建urdf、launch、meshes、config/rviz文件夹

cd sim_car
mkdir urdf
mkdir launch
mkdir meshes
mkdir config
cd config
mkdir rviz

三、创建sim_car_node

在创建的包中的src文件夹创建一个名字为sim_car_node.cpp的文件。（目前该文件只是一个空文件，后续会添加功能性代码。）

四、创建仿真小车URDF文件

URDF是 Unified Robot Description Format 的首字母缩写，直译为统一(标准化)机器人描述格式，可以以一种 XML 的方式描述机器人的部分结构，比如底盘、摄像头、激光雷达、机械臂以及不同关节的自由度等,该文件可以被 C++ 内置的解释器转换成可视化的机器人模型，是 ROS 中实现机器人仿真的重要组件。
小车urdf代码如下：

<robot name="mycar">
    <!-- 设置 base_footprint  -->
    <link name="base_footprint">
        <visual>
            <geometry>
                <sphere radius="0.001" />
            </geometry>
        </visual>
    </link>

    <!-- 添加底盘 -->
    <!-- 
        参数
            形状:圆柱 
            半径:10     cm 
            高度:8      cm 
            离地:1.5    cm

    -->
    <link name="base_link">
        <visual>
            <geometry>
                <cylinder radius="0.1" length="0.08" />
            </geometry>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <material name="yellow">
                <color rgba="0.8 0.3 0.1 0.5" />
            </material>
        </visual>
    </link>

    <joint name="base_link2base_footprint" type="fixed">
        <parent link="base_footprint" />
        <child link="base_link"/>
        <origin xyz="0 0 0.055" />
    </joint>

    <!-- 添加驱动轮 -->
    <!--
        驱动轮是侧翻的圆柱
        参数
            半径: 3.25 cm
            宽度: 1.5  cm
            颜色: 黑色
        关节设置:
            x = 0
            y = 底盘的半径 + 轮胎宽度 / 2
            z = 离地间距 + 底盘长度 / 2 - 轮胎半径 = 1.5 + 4 - 3.25 = 2.25(cm)
            axis = 0 1 0
    -->
    <link name="left_wheel">
        <visual>
            <geometry>
                <cylinder radius="0.0325" length="0.015" />
            </geometry>
            <origin xyz="0 0 0" rpy="1.5705 0 0" />
            <material name="black">
                <color rgba="0.0 0.0 0.0 1.0" />
            </material>
        </visual>

    </link>

    <joint name="left_wheel2base_link" type="continuous">
        <parent link="base_link" />
        <child link="left_wheel" />
        <origin xyz="0 0.1 -0.0225" />
        <axis xyz="0 1 0" />
    </joint>


    <link name="right_wheel">
        <visual>
            <geometry>
                <cylinder radius="0.0325" length="0.015" />
            </geometry>
            <origin xyz="0 0 0" rpy="1.5705 0 0" />
            <material name="black">
                <color rgba="0.0 0.0 0.0 1.0" />
            </material>
        </visual>

    </link>

    <joint name="right_wheel2base_link" type="continuous">
        <parent link="base_link" />
        <child link="right_wheel" />
        <origin xyz="0 -0.1 -0.0225" />
        <axis xyz="0 1 0" />
    </joint>


    <!-- 添加万向轮(支撑轮) -->
    <!--
        参数
            形状: 球体
            半径: 0.75 cm
            颜色: 黑色

        关节设置:
            x = 自定义(底盘半径 - 万向轮半径) = 0.1 - 0.0075 = 0.0925(cm)
            y = 0
            z = 底盘长度 / 2 + 离地间距 / 2 = 0.08 / 2 + 0.015 / 2 = 0.0475 
            axis= 1 1 1

    -->
    <link name="front_wheel">
        <visual>
            <geometry>
                <sphere radius="0.0075" />
            </geometry>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <material name="black">
                <color rgba="0.0 0.0 0.0 1.0" />
            </material>
        </visual>
    </link>

    <joint name="front_wheel2base_link" type="continuous">
        <parent link="base_link" />
        <child link="front_wheel" />
        <origin xyz="0.0925 0 -0.0475" />
        <axis xyz="1 1 1" />
    </joint>

    <link name="back_wheel">
        <visual>
            <geometry>
                <sphere radius="0.0075" />
            </geometry>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <material name="black">
                <color rgba="0.0 0.0 0.0 1.0" />
            </material>
        </visual>
    </link>

    <joint name="back_wheel2base_link" type="continuous">
        <parent link="base_link" />
        <child link="back_wheel" />
        <origin xyz="-0.0925 0 -0.0475" />
        <axis xyz="1 1 1" />
    </joint>


</robot>

五、验证写好的urdf文件

在终端中运行如下指令：

check_urdf mycar.urdf

如果安装了urdf的库，终端中会输出如下信息：

alex@alex:~/test_ws/src/sim_car/urdf$ check_urdf mycar.urdf 
robot name is: mycar
---------- Successfully Parsed XML ---------------
root Link: base_footprint has 1 child(ren)
    child(1):  base_link
        child(1):  back_wheel
        child(2):  front_wheel
        child(3):  left_wheel
        child(4):  right_wheel

如果没有安装该库，需要按照终端中提示进行安装，然后在检测urdf文件是否正确。

六、创建launch启动脚本

在launch文件夹中创建start_sim_car_launch.launch文件，然后在文件中输入如下代码：

<launch>
    <param name ="robot_description" textfile="$(find sim_car)/urdf/mycar.urdf">
    </param>
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find sim_car)/config/rviz/display_mycar.rviz" />
    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" />
    <node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher" />
    <node pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" name="joint_state_publisher_gui" />
</launch>

七、创建rviz显示配置脚本

我这里使用一个懒人方法，在终端中打开rviz，然后选择另存为（save as）将该文件保存在sim_car/config/rviz中，命名为display_mycar.rviz。
然后打开该文件，删除文件内容，复制下列代码.

Panels:
  - Class: rviz/Displays
    Help Height: 78
    Name: Displays
    Property Tree Widget:
      Expanded:
        - /Global Options1
        - /Status1
        - /Grid1
        - /RobotModel1
      Splitter Ratio: 0.5
    Tree Height: 549
  - Class: rviz/Selection
    Name: Selection
  - Class: rviz/Tool Properties
    Expanded:
      - /2D Pose Estimate1
      - /2D Nav Goal1
      - /Publish Point1
    Name: Tool Properties
    Splitter Ratio: 0.5886790156364441
  - Class: rviz/Views
    Expanded:
      - /Current View1
    Name: Views
    Splitter Ratio: 0.5
  - Class: rviz/Time
    Experimental: false
    Name: Time
    SyncMode: 0
    SyncSource: ""
Preferences:
  PromptSaveOnExit: true
Toolbars:
  toolButtonStyle: 2
Visualization Manager:
  Class: ""
  Displays:
    - Alpha: 0.5
      Cell Size: 1
      Class: rviz/Grid
      Color: 160; 160; 164
      Enabled: true
      Line Style:
        Line Width: 0.029999999329447746
        Value: Lines
      Name: Grid
      Normal Cell Count: 0
      Offset:
        X: 0
        Y: 0
        Z: 0
      Plane: XY
      Plane Cell Count: 10
      Reference Frame: <Fixed Frame>
      Value: true
    - Alpha: 1
      Class: rviz/RobotModel
      Collision Enabled: false
      Enabled: true
      Links:
        All Links Enabled: true
        Expand Joint Details: false
        Expand Link Details: false
        Expand Tree: false
        Link Tree Style: Links in Alphabetic Order
        back_wheel:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        base_footprint:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        base_link:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        front_wheel:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        left_wheel:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        right_wheel:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
      Name: RobotModel
      Robot Description: robot_description
      TF Prefix: ""
      Update Interval: 0
      Value: true
      Visual Enabled: true
  Enabled: true
  Global Options:
    Background Color: 48; 48; 48
    Default Light: true
    Fixed Frame: base_link
    Frame Rate: 30
  Name: root
  Tools:
    - Class: rviz/Interact
      Hide Inactive Objects: true
    - Class: rviz/MoveCamera
    - Class: rviz/Select
    - Class: rviz/FocusCamera
    - Class: rviz/Measure
    - Class: rviz/SetInitialPose
      Theta std deviation: 0.2617993950843811
      Topic: /initialpose
      X std deviation: 0.5
      Y std deviation: 0.5
    - Class: rviz/SetGoal
      Topic: /move_base_simple/goal
    - Class: rviz/PublishPoint
      Single click: true
      Topic: /clicked_point
  Value: true
  Views:
    Current:
      Angle: 0
      Class: rviz/TopDownOrtho
      Enable Stereo Rendering:
        Stereo Eye Separation: 0.05999999865889549
        Stereo Focal Distance: 1
        Swap Stereo Eyes: false
        Value: false
      Invert Z Axis: false
      Name: Current View
      Near Clip Distance: 0.009999999776482582
      Scale: 996.9389038085938
      Target Frame: <Fixed Frame>
      Value: TopDownOrtho (rviz)
      X: 0.021359950304031372
      Y: -0.0027024373412132263
    Saved: ~
Window Geometry:
  Displays:
    collapsed: false
  Height: 846
  Hide Left Dock: false
  Hide Right Dock: false
  QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004f30000003efc0100000002fb0000000800540069006d00650100000000000004f3000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000282000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
  Selection:
    collapsed: false
  Time:
    collapsed: false
  Tool Properties:
    collapsed: false
  Views:
    collapsed: false
  Width: 1267
  X: 416
  Y: 0

八、修改CMakeLists文件

在sim_car文件夹下找到CMakeLists.txt文件，然后修改为下面内容：

cmake_minimum_required(VERSION 3.0.2)
project(sim_car)

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
)

catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES sim_car
 CATKIN_DEPENDS roscpp rospy std_msgs
#  DEPENDS system_lib
)

include_directories(
# include
  ${catkin_INCLUDE_DIRS}
)

add_executable(sim_car src/sim_car_node.cpp)

add_dependencies(sim_car ${sim_car_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

九、编译工作空间

如果当前使用的空间是已经编译好的，为了不破坏编译的文件，可以使用如下编译方式进行新功能包的单独编译：

catkin_make -DCATKIN_WHITELIST_PACKAGES="sim_car"

如果使用的是新创建的功能包，则直接运行catkin_make即可。

十、运行功能包

新的功能包编译成功之后，进入工作空间中（注意不是新建的功能包中）在终端中输入如下代码运行新的功能包：

 source devel/setup.bash 
roslaunch sim_car start_sim_car_launch.launch  

如果运行正常，会出现如下界面

问题总结机解决

如果报错，请按照下列步骤进行解决：
1、重新安装对应ros版本的功能包

sudo apt-get install ros-melodic-joint-state-publisher
sudo apt-get install ros-melodic-joint-state-publisher-gui

然后进入/opt/ros/melodic/lib/joint_state_publisher 文件夹;
运行下列命令给赋最高权限

 sudo chmod 777 joint_state_publisher

然后编辑joint_state_publisher代码，在代码中添加如下内容，然后保存退出

reload(sys)  
sys.setdefaultencoding('utf8')  

joint_state_publisher的完整代码展示如下

import sys

import rospy
reload(sys)  
sys.setdefaultencoding('utf8')  
import joint_state_publisher

if __name__ == '__main__':
    try:
        rospy.init_node('joint_state_publisher')
        jsp = joint_state_publisher.JointStatePublisher()

        use_gui = joint_state_publisher.get_param('use_gui')
        if use_gui:
            rospy.logwarn("The 'use_gui' parameter was specified, which is deprecated.  We'll attempt to find and run the GUI, but if this fails you should install the 'joint_state_publisher_gui' package instead and run that.  This backwards compatibility option will be removed in Noetic.")
            try:
                import signal
                import threading

                import joint_state_publisher_gui

                from python_qt_binding.QtWidgets import QApplication

                app = QApplication(sys.argv)

                num_rows = joint_state_publisher.get_param('num_rows', 0)
                jsp_gui = joint_state_publisher_gui.JointStatePublisherGui('Joint State Publisher',
                                                                           jsp, num_rows)
                jsp_gui.show()
                jsp_gui.sliderUpdateTrigger.emit()

                threading.Thread(target=jsp_gui.jsp.loop).start()
                signal.signal(signal.SIGINT, signal.SIG_DFL)
                sys.exit(app.exec_())
            except ImportError:
                rospy.logerr("Could not find the GUI, install the 'joint_state_publisher_gui' package")
                sys.exit(1)
        else:
            jsp.loop()

    except rospy.ROSInterruptException:
        pass

然后在进入/opt/ros/melodic/lib/joint_state_publisher_gui文件夹下，运行下列命令给赋最高权限

 sudo chmod 777 joint_state_publisher_gui

然后编辑joint_state_publisher_gui代码，在代码中添加如下内容后，保存退出

import signal
import sys
import threading
reload(sys)  
sys.setdefaultencoding('utf8')  
import rospy

from python_qt_binding.QtWidgets import QApplication

import joint_state_publisher
import joint_state_publisher_gui

if __name__ == '__main__':
    try:
        rospy.init_node('joint_state_publisher_gui')
        app = QApplication(sys.argv)
        num_rows = joint_state_publisher.get_param('num_rows', 0)
        jsp_gui = joint_state_publisher_gui.JointStatePublisherGui('Joint State Publisher',
                                                                   joint_state_publisher.JointStatePublisher(),
                                                                   num_rows)
        jsp_gui.show()
        jsp_gui.sliderUpdateTrigger.emit()

        threading.Thread(target=jsp_gui.jsp.loop).start()
        signal.signal(signal.SIGINT, signal.SIG_DFL)
        sys.exit(app.exec_())

    except rospy.ROSInterruptException:
        pass

本文参照《autolabor-ROSTutorials》教程进行练习、备忘使用，如有侵权，请联系删除。