迷你无人车 Gazebo 仿真

迷你无人车 Gazebo 仿真

从零开始搭建一台ROS开源迷你无人车

利用NEOR mini无人车的urdf模型文件，结合Gazebo物理仿真引擎和ROS开源社区的算法功能包，在Ubuntu + ROS开发环境下模拟出一台和NEOR mini实体无人车功能一致的仿真无人车。实现的过程如下图所示：

从三维模型到URDF文件

URDF全称为：统一机器人描述格式(Unified Robot Description Format)。URDF文件使用XML格式描述机器人模型中各个刚体（link）之间的位置与刚体之间使用关节（joint）链接，并定义相对运动的姿态

可以通过xml格式自定义简单形状的URDF机器人模型；也可以使用sw_urdf_expoter插件将机器人的设计图纸自动转换为urdf文件，在这里使用的是第二种方式。NEOR mini是一款后轮独立驱动，前轮转向的类阿克曼运动学的移动底盘。使用sw_urdf_expoter之前需要将整个NEOR mini三维模型分成如下几大块：

robot name is: neor_mini                  解释（explanation）
root Link: base_linkhas 6 child(ren)          主体实体
    child(1): front_steer_link               前轮转向实体
    child(2): front_steer_left_link          左前轮转向机构实体
        child(1):  front_left_wheel_link     左前轮实体
    child(3): front_steer_right_link         右前轮转向机构实体
        child(1):  front_right_wheel_link    右前轮实体
    child(4): left_rear_link                 左后轮驱动实体
    child(5): rear_wheel_link                驱动实体
    child(6):  right_rear_link               右后轮驱动实体

然后，设置好每个实体的旋转轴，利用sw_urdf_expoter即可导出对应的urdf模型文件。在导出之前需要检查上述各个实体的名称，若相同，导出的urdf模型很有可能会出现凌乱的现象，需要重命名名字重复的实体。导出的urdf模型文件以ROS功能包的形式，其文件树和urdf模型可视化如下：

阿克曼运动学插件 steer-drive-ros

URDF模型文件离动起来还需要程序去驱动对应的关节和实体完成指定的转动。前面提到了NEOR mini是一款类阿克曼运动学方式的移动底盘。因此该驱动程序需要将接收到的速度指令，解算为mini的每个轮子的转速和转向关节的旋转角度；该转换过程称之为“运动学解算”

同理根据实际的执行情况，可以从后左右轮的转速和前轮的转向角度逆推出后轮中点出的线角速度（v,w）。从而可以推算出无人车的航迹信息

💡 其实就是阿克曼运动学正逆解的过程

steer_drive_ros功能包集内置了阿克曼类型urdf在gazebo下的驱动程序steer_bot_hardware_gazebo以及完成正逆向运动学的解算程序steer_drive_controller;整个功能包集的运行过程如下图示（摘自 wiki）：

修改urdf文件适配steer-drive-ros

紧接下来的目标是：参照steer_drive_controller解算程序对urdf模型的构造要求，修改neor_mini.urdf文件以符合要求，然后更新对应的配置文件信息（关节和实体名称）。根据官网的解释，该程序所支持的urdf模型构造和已有的neor_mini.urdf的构造分别如下左右图所示：

💡 这里暂时省略具体的修改过程

为了能够在gazebo中顺利加载修改后的urdf文件，需要在标签内加入gazebo_ros_control标签。内容如下：

<gazebo>
  <pluginname="gazebo_ros_control"filename="libgazebo_ros_control.so">
    <robotSimType>steer_bot_hardware_gazebo/SteerBotHardwareGazebo</robotSimType>
    <legacyModeNS>false</legacyModeNS>
  </plugin>
</gazebo>

至此，修改后的urdf文件已经具备被steer-drive-ros程序驱动，并在gazebo中运动的必备条件。随后需要的就是更新改程序的配置文件中的相关驱动关节和实体的名字，link 和 joint 如下

redwall@redwall-G3-3500:~/neor_mini/mini_sim18_ws/src/neor_mini/urdf$ check_urdf neor_mini_gazebo_sensors.urdf 
robot name is: neor_mini
---------- Successfully Parsed XML ---------------
root Link: base_link has 10 child(ren)
    child(1):  laser_link
    child(2):  laser_link2
    child(3):  camera_link
    child(4):  front_steer_link
    child(5):  front_steer_left_link
        child(1):  front_left_wheel_link
    child(6):  front_steer_right_link
        child(1):  front_right_wheel_link
    child(7):  imu_link
    child(8):  left_rear_link
    child(9):  rear_wheel_link
    child(10):  right_rear_link

roslaunch urdf_tutorial display.launch model:=./neor_mini_gazebo_sensors.urdf

rosrun tf2_tools view_frames

URDF在Gazebo中动起来

在前面所做准备的基础上，现在需要更新steer-drive-ros运行时加载的配置文件。根据该程序的参数在wiki中的解释1、2，将所有的参数分为4个配置文件（.yaml），如下：

ctrl_ackermann_steering_controller.yaml  for:   steer_drive_controller
ctrl_joint_state_publisher.yaml          for:   ros_controller
ctrl_gains.yaml                          for:  steer_bot_hardware_gazebo
ctrl_steer_bot_hardware_gazebo.yaml      for:   steer_bot_hardware_gazebo

为了方便管理，这里创建了一个ros 功能包，存储配置文件和程序启动的launch文件，以及使用package.xml管理程序运行所需的依赖包。创建的命令如下：

cd your_workspace/src
catkin_create steer_mini_gazebo roscpp

回车之后，其文件树如下：

.
├── CMakeLists.txt                #存储该功能包编译时的信息
├── include
│   └──  steer_mini_gazebo
├── package.xml                  # 存储依赖包名字
└── src                          # 存储源程序.cpp

此时，该功能包是不会被编译的，此时需要在steer_mini_gazebo/src目录下创建steer_mini_gazebo_node.cpp文件，并在该文件里面添加如下所示的源程序：

#include<ros/ros.h>
 int  main(int argc, char** argv)
{
      ros::init(argc, argv, "steer_mini_gazebo_node"); // 节点名称
      ros::NodeHandle nh;
      //.... 节点功能，此处没有功能，只是为了能够参与编译。
      ros::spin();//用于触发topic、service的响应队列
      return 0;
 }

修改steer_mini_gazebo/CMakeLists.txt 文件，去掉如下两条语句前的“#”注释

# add_executable(${PROJECT_NAME}_node  src/steer_mini_gazebo_node.cpp)
......
#target_link_libraries(${PROJECT_NAME}_node
#   ${catkin_LIBRARIES}
#)

steer_mini_gazebo可以被编译生成可执行程序节点。紧接着，需要在其中添加前面所述的四个配置文件和程序启动的launch文件。添加后的功能包文件树如下：

.
├── CMakeLists.txt
├── mini_control
│   └── config
│       ├──  ctrl_ackermann_steering_controller.yaml
│       ├── ctrl_gains.yaml
│       ├── ctrl_joint_state_publisher.yaml
│       └── ctrl_steer_bot_hardware_gazebo.yaml
├── mini_gazebo
│   └── launch
│       ├── steer_mini_sim.launch
│       └── steer_mini_sim_sensors.launch
├── package.xml
└── src
     └── steer_mini_gazebo_node.cpp

配置程序启动的steer_mini_sim.launch文件，内容如下：

<?xml version="1.0"?>
<launch>

    <arg name="x" default="0.0"/>
    <arg name="y" default="0.0"/>
    <arg name="z" default="0.0" />
    <arg name="roll" default="0.0"/>
    <arg name="pitch" default="0.0"/>
    <arg name="yaw" default="0.0"/>

    <!-- Gazebo  -->
    <!--Load the surrounding environment into Gazebo-->
    <include file="$(find gazebo_ros)/launch/empty_world.launch" >
        <arg name="world_name" value="$(find neor_mini)/worlds/cooneo_office.world"/>
    </include>

    <!-- Load the robot description -->
    <param name="robot_description" command="cat $(find neor_mini)/urdf/neor_mini_gazebo_sensors.urdf"/>

    <!-- Load ros_controllers configuration parameters -->
    <rosparam file="$(find steer_mini_gazebo)/mini_control/config/ctrl_ackermann_steering_controller.yaml" command="load"   />
    <rosparam file="$(find steer_mini_gazebo)/mini_control/config/ctrl_gains.yaml" command="load"   />
    <rosparam file="$(find steer_mini_gazebo)/mini_control/config/ctrl_joint_state_publisher.yaml" command="load"   />
    <rosparam file="$(find steer_mini_gazebo)/mini_control/config/ctrl_steer_bot_hardware_gazebo.yaml" command="load"   />

    <!-- Spawn the controllers -->
    <node pkg="controller_manager" type="spawner" name="controller_spawner"  args="joint_state_publisher ackermann_steering_controller" output="screen" respawn="false" />

    <!-- Launch  the robot state publisher -->
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher">
        <param name="publish_frequency" value="50.0"/>
    </node>

    <!-- Launch a rqt steering GUI for publishing to /steer_bot/steer_drive_controller/cmd_vel -->
    <node pkg="rqt_robot_steering" type="rqt_robot_steering" name="rqt_robot_steering" >
        <param name="default_topic" value="ackermann_steering_controller/cmd_vel"/>                         <!-- default velocity control topic name -->
        <param name="default_vx_max" value="1.0"/>                        <!-- linear velocity max value    m/s -->
        <param name="default_vx_min" value="-1.0"/>                       <!-- linear velocity min value    m/s-->
        <param name="default_vw_max" value="0.69"/>                    <!-- angular velocity max value  rad/s (adaptor for urdf joint limit) -->
        <param name="default_vw_min" value="-0.69"/>                   <!-- angular velocity min value  rad/s (adaptor for urdf joint limit) -->
    </node> 

    <!--Start the Gazebo node and configure the location and posture of the accident when the model is loaded -->
    <node name="spawn_vehicle" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model neor_mini -gazebo_namespace /gazebo 
              -x $(arg x) -y $(arg y) -z $(arg z)
              -R $(arg roll) -P $(arg pitch) -Y $(arg yaw)"
          respawn="false" output="screen" />
    <!-- ******************************************************************************************************************************************* -->

   <node name="laser_to_base_link" pkg="tf" type="static_transform_publisher" args="0.0 0.0 0.135 0 0 0 base_link laser_link 40 " />
   <node name="imu_to_base_link" pkg="tf" type="static_transform_publisher" args="-0.10 0.0 0.02 0 0 0 base_link imu_link 40 " />
   <node name="camera_to_base_link" pkg="tf" type="static_transform_publisher" args="0.12 0.0 0.12 0 0 0 base_link camera_link 40 " />

</launch>

想要编译成功，还需要在steer_mini_gazebo/package.xml中将所需的依赖包添加进去：

  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>ackermann_steering_controller</build_depend>
  <build_depend>controller_manager</build_depend>
  <build_depend>gazebo_ros</build_depend>
  <build_depend>gazebo_ros_control</build_depend>
  <build_depend>joint_state_controller</build_depend>
  <build_depend>robot_state_publisher</build_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>roslaunch</build_depend>
  <build_depend>rqt_robot_steering</build_depend>
  <build_depend>steer_bot_hardware_gazebo</build_depend>
  <build_depend>steer_drive_controller</build_depend>
  <build_export_depend>ackermann_steering_controller</build_export_depend>
  <build_export_depend>controller_manager</build_export_depend>
  <build_export_depend>gazebo_ros</build_export_depend>
  <build_export_depend>gazebo_ros_control</build_export_depend>
  <build_export_depend>joint_state_controller</build_export_depend>
  <build_export_depend>robot_state_publisher</build_export_depend>
  <build_export_depend>roscpp</build_export_depend>
  <build_export_depend>roslaunch</build_export_depend>
  <build_export_depend>rqt_robot_steering</build_export_depend>
  <build_export_depend>steer_bot_hardware_gazebo</build_export_depend>
  <build_export_depend>steer_drive_controller</build_export_depend>
  <build_export_depend>ros_control</build_export_depend>
  <exec_depend>ackermann_steering_controller</exec_depend>
  <exec_depend>controller_manager</exec_depend>
  <exec_depend>gazebo_ros</exec_depend>
  <exec_depend>gazebo_ros_control</exec_depend>
  <exec_depend>joint_state_controller</exec_depend>
  <exec_depend>robot_state_publisher</exec_depend>
  <exec_depend>roscpp</exec_depend>
  <exec_depend>roslaunch</exec_depend>
  <exec_depend>rqt_robot_steering</exec_depend>
  <exec_depend>steer_bot_hardware_gazebo</exec_depend>
  <exec_depend>steer_drive_controller</exec_depend>
  <exec_depend>ros_control</exec_depend>

效果如下

可以看到目前对于迷你无人车的控制是通过 ROS 自带的 rqt steering GUI 实现，很不方便，尝试通过手柄进行控制

	<!-- Launch a rqt steering GUI for publishing to /steer_bot/steer_drive_controller/cmd_vel -->
    <node pkg="rqt_robot_steering" type="rqt_robot_steering" name="rqt_robot_steering" >
        <param name="default_topic" value="ackermann_steering_controller/cmd_vel"/>                         <!-- default velocity control topic name -->
        <param name="default_vx_max" value="1.0"/>                        <!-- linear velocity max value    m/s -->
        <param name="default_vx_min" value="-1.0"/>                       <!-- linear velocity min value    m/s-->
        <param name="default_vw_max" value="0.69"/>                    <!-- angular velocity max value  rad/s (adaptor for urdf joint limit) -->
        <param name="default_vw_min" value="-0.69"/>                   <!-- angular velocity min value  rad/s (adaptor for urdf joint limit) -->
    </node>