教程 Re:Zero ROS （五）—— 导入模型，关节控制器

 **前情提要：我们已经掌握了ros工程的各个基本要素，可以开始逐步完成小车仿真了 **

《教程 Re:Zero ROS （四）—— 略讲:Service/msg/srv/tf/urdf/launch/param》
https://blog.csdn.net/Lovely_him/article/details/107778767

教程 Re:Zero ROS （五）

—— 导入模型，关节控制器

1. 下载 & 存放模型

• 1）由上一篇知道，tf描绘的是整体坐标系。在tf的基础上，urdf描绘了整体模型，即各个关节的相对位置。
• 2）如果想要自己拼一个简易的小车模型也可以，大概思路就是：中心原点是车身；往外扩充一部分后接4个轮子；车身上再设定三个传感器的位置，摄像头、激光雷达、IMU（陀螺仪）。使用urdf自定义形状，车身为长扁平矩形，轮子是圆柱。这样就差不多完成了。需要真实一点的话可以再设定转速、摩擦力、扭距等属性。更多内容请查看wiki。

urdf
http://wiki.ros.org/urdf

• 3）我使用比赛时官方提供的小车模型文件。原文件备份已上车至github，在git库的"src"分支内的“赛道和无人车三维模型（7月9日更新）.zip”文件便是了。

lovely-him/him_ws
https://github.com/lovely-him/him_ws/tree/src

• 
• 4）解压后有4个主要文件（另外2个中文命名的是说明文件，不过命名可能乱码看不出来）。从左到右分别是：电机输出控制control_plugin.py、小车三维模型racecar_description.zip、赛道模型racetrack.world、起点终点线模型smartcar_plane.7z。其中，“电机输出控制”就是之前编写topic时做示范例子的“增强版”。“小车三维模型”就是urdf描述文件。“赛道模型”则是之后会讲，是仿真软件gazebo的障碍物描述文件。“起点终点线模型”就是gazebo的模型加载文件。
• 
• 5）smartcar_plane.7z文件，这个文件是需要解压后放到gazebo的模型库内的。模型库的地址为~/.gazebo/models，如果你没下载过模型，可能没有models文件夹，那就自己新建。另外，为了防止gazebo加载时间过长，我们还需要预先下载好gazebo官方的模型库。建议直接进入git使用浏览器下载，因为这个文件不需要安装，只需要复制放在~/.gazebo/models目录下就可以了，注：文件总共1.1G左右。

《ros-gazebo长时间加载不出来问题》
https://blog.csdn.net/weixin_45839124/article/details/106565111

• 6）在官方还没出规则前，北邮的xmy学长就开始研究ros智能车仿真了。之后官方给出模型和规则后学长就对应做了一个教程。教程是使用学长写好的ros工程，载入官方模型使用。因为没有具体教ros和Ubuntu的使用方法，只是讲述每一步的操作。在Ubuntu16.04下跟着教程一步步走勉强（花了不知道多少天 ）才实现学长的实验效果。所以我就想自己做个从零开始教程，让新手快速培养兴趣，由此，该系列教程诞生了。

《智能车仿真 —— 2020室外光电组仿真指导（一）》
https://blog.csdn.net/qq_37668436/article/details/107142166
《在ROS下搭建仿真模拟环境，编程控制小车完成定位导航仿真》
https://blog.csdn.net/qq_41133375/article/details/106494744

2.特别说明

• 本篇以下内容全是在Ubuntu-16.04版本ros-kinetic版本下的操作的。参考了北邮学长的帖子，作了我自己的理解、修改与扩展。
• 我本来是想在Ubuntu-20.04版本下实践一遍的，都是第一步导入小车模型时就失败了，不知道为何一直提示urdf.xarco模型文件语法错误，在16.04版本下是可以无错误完成的。折腾了几天后，决定还是先在16.04版本下速速完结该教程系列，之后再补充20.04版本下的扩展。
• 所以，20.04版本下的同学，以下操作内容大概会失败，请留意。有时候指令可能都不存在。并且，我使用了RoboWare Studio IDE。

3.导入模型软件包，gazebo启动，

• 0）我们重新建一个工程，把之前的试验工程放一边。新建的方法前面已介绍过，跳过。然后使用RoboWare Studio IDE打开。可以看到，我新建的工程目录为“~/car_ws"，之后注意对应我的代码中的工程目录。

• 

• 1）将解压的racecar_description放到你的ros工程的src文件下。现在你的工程src目录应该如下图。

• 
  

• 2）racecar_description文件夹内的launch为启动文件，model为起点线和终点线的jpg图片，meshes为摄像头的配置文件（全局搜索一下这个文件夹的名字，可以发现在camera.xaro摄像头属性文件内调用了），rviz是仿真软件rviz的配置文件，urdf是小车的模型本体。现在还差一个worlds地图（赛道）模型文件。我们把压缩包内的racetrack.world文件放入racecar_description/worlds文件夹内。

• 
  

• 3）为了防止软件包名重复和强迫症患者，我们把软件包内的名字修改一下。不过软件的名字是在新建软件包时就已确定，关联了CMakeLists.txt与package.xml，还有其他大大小小的文件。可以使用IDE内的搜索替换功能，把所有使用到软件包名的地方进行替换。然后使用catkin_make编译，无误即可。

• 

• 4）打开racecar_description/launch/racecar.launch文件，由注释可得知，还需要修改地图文件的路径，把$(find racecar_gazebo)修改为$(find racecar_description)。

• 

• 5）编译，无误后在IDE内直接右键启动launch文件，这便是RotoWare的便捷之处，对比普通版本的VS IDE，会发现有越来越多不一样的地方。第一次启动gazebo可能会较慢，卡死在加载画面，即使下载了mods文件。建议等待过长时直接终止launch运行，然后再次打开，就会秒进 。最终效果如下下图。

• 无误后，恭喜，你已成功导入模型并顺利加载了urdf模型文件，在Ubuntu20.04下死活不成功……应该是需要修改某些地方，以后发现问题后再发帖补上。

• 

4.创建launch启动gazebo

• 0）虽然上一节中已经使用launch启动了gazebo，不过那是软件包内自带的。为了加深了解，我从零开始一步步创建一个。

• 1）在工程内创建一个新的软件包，编译，然后在目录下创建launch文件夹与launch文件。步骤之前进过，跳过。最后目录如下。

• 

• 2）启动gazebo的代码很简单，就是导入一个ros官方写好的launch，该launch已经写好了启动软件的内容，并预留了部分配置参数。如果想查看该launch可以使用roscd直接跳转目录，然后使用vim打开查看。有关launch具体的语法、标签含义，请自行另外学习。

《浅谈Ros中使用launch启动文件的方法（一）》
https://blog.csdn.net/CH_monsy/article/details/107664893

• 开头是参数定义与赋值；然后是引用 gazebo的启动文件；配置参数；其中配置地图文件的路径。可以复制以下代码。

<?xml version="1.0"?>
<launch>
	    <!-- 设置launch文件的参数 -->
    <arg name="paused" default="false"/>
    <arg name="use_sim_time" default="true"/>
    <arg name="gui" default="true"/>
    <arg name="headless" default="false"/>
    <arg name="debug" default="false"/>

    	<!--运行gazebo仿真环境-->
    <include file="$(find gazebo_ros)/launch/empty_world.launch">
        <arg name="debug" value="$(arg debug)" />
        <arg name="gui" value="$(arg gui)" />
        <arg name="paused" value="$(arg paused)"/>
        <arg name="use_sim_time" value="$(arg use_sim_time)"/>
        <arg name="headless" value="$(arg headless)"/>
        <!-- 此处改成参赛者放置.world文件的地址-->
    	<arg name="world_name" value="$(find carpack_mods)/worlds/racetrack.world"/>
   	</include>
</launch>

• 3）保存后，勿忘给予launch文件执行权限。编译无误后IDE内右键启动launch。gazebo内只地图模型，正确。
• 
• 4）额外补充一下gazebo的知识，使用rqt_graph查看节点，通过wiki的介绍，大概了解一下各个topic的作用。后续的编程中多少会用到。（部分调试话题被我隐藏了）

gazebo
http://wiki.ros.org/gazebo

• 

5.启动rviz，并载入urdf模型

• 1）启动rviz需要一个配置文件，启动指令rviz时是使用了默认的空配置文件，我们希望使用launch启动rviz，并且针对工程作了相应的配置。首先启动指令roscore，然后启动指令rviz，打开空的rviz。

• 

• 2）先添加一个机器人模型（中科院的教程中由详细教程），然后左上角保存配置文件File -> Save Config As。另保存到carpack_control/rviz内，rviz文件夹需要新建。然后就可以在IDE内看到相应的rivz配置文件了。

• 

• 2）要往rviz内加载模型，先需要有urdf模型数据。在launch内直接调用xacro文件解析器urdf模型，运行robot_state_publisher节点，向tf发布机器人关节状态。最后启动rviz。

urdf
http://wiki.ros.org/urdf
ros初探xacro
https://blog.csdn.net/qq_43786066/article/details/104420700

        <!-- 加载机器人模型描述参数 -->
    <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find carpack_mods)/urdf/racecar.urdf.xacro'"/>

        <!--运行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>

    <!--运行rviz节点，启动rviz-->
    <node name="rviz" pkg="rviz" type="rviz"
          args="-d $(find carpack_control)/rviz/car_rviz.rviz" required="true"/>

• 其实不事先使用指令rviz生成rviz配置文件也没关系，因为launch启动rviz节点时如果路径下没有rviz文件就会自动创建一个空的配置文件，只是还是需要先准备好文件夹。
  
• 3）如果你一开始的rviz内没有模型，只有一堆白色物块，那是因为左边的Displays -> Global Options -> Fixed Frame没有选择模型的坐标系。如果模型没有正确的建立坐标关系就会变为白色。
• 
• 4）往rviz内加载小车模型成功后，再往gazebo内加载小车模型。

    	<!--模型车的位置（比赛时）不能修改-->
    <arg name="x_pos" default="-0.5"/>
    <arg name="y_pos" default="0"/>
    <arg name="z_pos" default="0.0"/>
    
	<!-- 在gazebo中加载机器人模型-->
	<node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
	        args="-urdf -model shcrobot -param robot_description -x $(arg x_pos) -y $(arg y_pos) -z $(arg z_pos)"/> 

• 5）至此，car_gazebo.launch文件能成功启动rviz与gazebo并分别加载小车模型了。都是，我们一般使用时，只有最后调试才会启动rviz，前期编写，更多只会启动gazebo。（个人目前阶段） 所以，我们把启动rviz与gazebo的launch文件分开。新建一个car_rviz.launch文件，将car_gazebo.launch内的代码剪切，并加上头尾。这样，这部分工作就算完成了。

• 6）最后展示两个launch文件：

car_rviz.launch

<?xml version="1.0"?>
<launch>
    <!--运行rviz节点，启动rviz-->
    <node name="rviz" pkg="rviz" type="rviz"
          args="-d $(find carpack_control)/rviz/car_rviz.rviz" required="true"/>
</launch>

car_gazebo.launch

<?xml version="1.0"?>
<launch>
        <!-- 设置launch文件的参数 -->
    <arg name="paused" default="false"/>
    <arg name="use_sim_time" default="true"/>
    <arg name="gui" default="true"/>
    <arg name="headless" default="false"/>
    <arg name="debug" default="false"/>
        <!--模型车的位置（比赛时）不能修改-->
    <arg name="x_pos" default="-0.5"/>
    <arg name="y_pos" default="0"/>
    <arg name="z_pos" default="0.0"/>

        <!--运行gazebo仿真环境-->
    <include file="$(find gazebo_ros)/launch/empty_world.launch">
        <arg name="debug" value="$(arg debug)" />
        <arg name="gui" value="$(arg gui)" />
        <arg name="paused" value="$(arg paused)"/>
        <arg name="use_sim_time" value="$(arg use_sim_time)"/>
        <arg name="headless" value="$(arg headless)"/>
        <!-- 此处改成参赛者放置.world文件的地址-->
        <arg name="world_name" value="$(find carpack_mods)/worlds/racetrack.world"/>
   	</include>

        <!-- 加载机器人模型描述参数 -->
    <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find carpack_mods)/urdf/racecar.urdf.xacro'"/>

        <!--运行joint_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>

	<!-- 加不加都没关系，不想弹出调参工具时，将“false”改为“true”即可 -->
    <!-- 读取联合位置，然后将它们发布到joint_states -->
    <!--node name="joint_state_publisher" pkg="joint_state_publisher"
          type="joint_state_publisher">
        <param name="rate" value="30"/>
        <param name="use_gui" value="false"/>
        <remap from="joint_states" to="/racecar/joint_states"/>
    </node-->

    <!-- 在gazebo中加载机器人模型-->
    <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
            args="-urdf -model shcrobot -param robot_description -x $(arg x_pos) -y $(arg y_pos) -z $(arg z_pos)"/> 

</launch>

6.编写关节控制器 - 前

• 0）在第三篇教程中，编写的topic话题练习output_control.py文件，便是关节控制器的用户控制部分的雏形，在此之前还需要设置好底层的关节控制器。在编写前，我们需要知道，控制器节点要订阅/发布哪些话题。我们运行上一节编写好的car_gazebo.launch与car_rviz.launch文件。

• 1）然后启动rqt_graph工具，这次我们从尝试rqt_gui工具集中打开。执行指令，然后从选项里打开rqt_graph 工具。选择Plugins -> Introspection -> Bode Graph。

rosrun rqt_gui rqt_gui

• - 2）然后确保rqt_graph工具的筛选栏与下图一致，这样便能得到和我一样的结构图。这些节点基本都可以在launch文件内找到对应的关系

• 1.：/joint_state：这是原本应该由gazebo发布的话题，但是因为还没配置，所以没连接上。这个话题记录的是各个关节的详细消息。另外，后期会被我用来当编码器的消息。因为它记载了轮子（关节）的转数与摆角。其实是因为这个车模没有搭载编码器传感器。

• 2.：/robot_state_publisher：这个节点订阅/joint_state话题，然后发送至tf树，至关重要，没有如果关节的坐标信息没有发往tf树就没有模型可言。

• 3.：/gazebo_gui：这个节点其实就是gazebo界面（大概）。

• 4.：/gazebo：节点主要节点之一，发布了众多话题信息。仿真软件gazebo内的一些基本消息都由/gazebo命名空间内的话题发布；小车模型上的摄像头传感器消息都由camera命名空间内的话题发布；而/scan与/imu_data话题分别是小车模型上的激光雷达与陀螺仪传感器发布的话题。全都由该节点发布。

• 

• 3）现在我们打开tf树看看，依旧是rqt_gui工具箱。打开后不够地方显示就把rqt_graph工具窗口关闭了。

• 

• 4）可以看到tf树的全貌。其中一个椭圆圈就表示一个坐标系。这个模型由不同的方块组成，每个模块又有自己的坐标系/相对位置，然后构成的一个树状图。其中标注的消息Broadcaster表示这个坐标关系是由哪个节点提供的；Average rate表示提供的频率，即这个关系刷新的频率。部分频率显示10000，那是因为这个相对坐标是固定不变 ，一直跟随它的相对坐标，所以刷新频率是最大值，即不用刷新的意思 。你可以在rviz中查看整个小车的各个组成方块，取消勾后不显示，一个个勾选上查看效果。

• 

• 5）其中需要变化的关节就是四个轮子的旋转位置，连续改变旋转的位置就达到转速的效果；还有两个前轮的摆动角度。

• 

• 6）一开始打开gazebo前，可以先将car_gazebo.launch文件内添加这一段代码。可以开启rqt_gui的话题消息发布工具。这是个可变动的参数就对应着tf树上10个可变动的相对坐标消息。如果拉动进度条改变参数，可以在rviz软件界面看到相应的变化。

<!-- 读取联合位置，然后将它们发布到joint_states -->
	<node name="joint_state_publisher" pkg="joint_state_publisher"
		type="joint_state_publisher">
		<param name="rate" value="30"/>
		<param name="use_gui" value="true"/>
	</node>

• 
• 7）下图我改了轮子的摆向和高度（现实中可能控制改变高度，这里这是演示）。实际在仿真时我们程序只控制6个关节消息，虽然其他关节可动可调，但是放到现实情况下是不可控的。所以
• 

7.编写关节控制器 - 中

• 0）上一节已经扯完了小车模型的关节消息，主要是向让初学者结合理解urdf模型、tf树、关节控制等概念。（我个人说的概念都是经过个人理解加工的，并不专业规范，若由感觉前后矛盾的描述，欢迎在评论区指正交流。我无聊常逛论坛。）
• 1）为了区分launch的启动功能，我们再新建一个launch启动文件。并在开头载入car_gazebo.launch文件，以启动gazebo和加载模型。
• 

car_control.launch

<?xml version="1.0"?>
<launch>
    <!--先启动 gazebo 并加载 模型关节消息 -->
    <include file="$(find carpack_control)/launch/car_gazebo.launch" />
</launch>

• 2）然后我们需要加载底层联合控制器controller_manager和参数。首先需要加载参数，因为参数比较多，我们写在另一个文件内，然后再把这个写满参数的文件include进launch文件内。联合控制器的参数可以直接复制以下内容。ros-wiki内有controller_manager节点配置参数的编写方法。我这个是复制北邮学长例程的，在我的him_ws工程下就有这个文件，可以复制。放到下图对应目录下即可。

智能车仿真 —— 2020室外光电创意组线上仿真赛
https://www.guyuehome.com/9123
controller_manager
http://wiki.ros.org/controller_manager

• 

racecar:
  # Publish all joint states --公布所有--------------------
  joint_state_controller:
    type: joint_state_controller/JointStateController
    publish_rate: 50
  
  # Velocity Controllers ----速度控制器---------------------
  left_rear_wheel_velocity_controller:
    type: effort_controllers/JointVelocityController
    joint: left_rear_axle
    pid: {
    
    p: 1.5, i: 0.0, d: 0.0, i_clamp: 0.0}
  right_rear_wheel_velocity_controller:
    type: effort_controllers/JointVelocityController
    joint: right_rear_axle
    pid: {
    
    p: 1.5, i: 0.0, d: 0.0, i_clamp: 0.0}
  left_front_wheel_velocity_controller:
    type: effort_controllers/JointVelocityController
    joint: left_front_axle
    pid: {
    
    p: 0.7, i: 0.0, d: 0.0, i_clamp: 0.0}
  right_front_wheel_velocity_controller:
    type: effort_controllers/JointVelocityController
    joint: right_front_axle
    pid: {
    
    p: 0.7, i: 0.0, d: 0.0, i_clamp: 0.0}

  # Position Controllers ---位置控制器-----------------------
  left_steering_hinge_position_controller:
    joint: left_steering_joint
    type: effort_controllers/JointPositionController
    pid: {
    
    p: 10.0, i: 0.0, d: 0.5}
  right_steering_hinge_position_controller:
    joint: right_steering_joint
    type: effort_controllers/JointPositionController
    pid: {
    
    p: 10.0, i: 0.0, d: 0.5}

• 3）注意参数的命名空间（前缀）为racecar。接着在launch文件内编写，导入参数并启动controller_manager节点。注意节点的命名空间与参数的命名空间一致，都为racecar。且args标签内的6个topic话题名便是我们用户控制的关节了。

	    <!-- 从yaml文件加载联合控制器的参数 -->
	<rosparam file="$(find carpack_control)/config/controller_racecar.yaml" command="load"/>
	
	<!-- 加载控制器 spawner -->
	<node name="controller_manager" pkg="controller_manager" type="spawner" 
	      respawn="false" output="screen" ns="/racecar" 
	      args="left_rear_wheel_velocity_controller       right_rear_wheel_velocity_controller
	            left_front_wheel_velocity_controller      right_front_wheel_velocity_controller
	            left_steering_hinge_position_controller   right_steering_hinge_position_controller
	            joint_state_controller"/>

• 4）启动launch文件，当你看到输出窗口打印这些信息时表示controller_manager已经加载成功。同时，rqt_graph查看节点有生成关节的话题与/racecar/joint_states话题。

• 

• 5）这里由出现另一个问题，gazebo节点发布的/racecar/joint_states话题与/robot_state_publisher节点订阅的/joint_states话题没有关联起来。这里有两个解决方法，要么重映射/robot_state_publisher节点发布的话题，要么重映射gazebo节点发布的话题。

• 我一开始学的时候是参考北邮学长的方法，重映射了/robot_state_publisher节点发布的话题。采取第二种方法的话，重映射gazebo节点发布的话题会很麻烦，并不是直接在gazebo节点或是controller_manager节点下重映射操作。使用还是推荐直接重映射/robot_state_publisher节点。

car_gazebo.launch

	    <!--运行joint_state_publisher节点，向tf发布机器人关节状态-->
	<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher">
	    <param name="publish_frequency" type="double" value="20.0" />
	    <remap from="/joint_states" to="/racecar/joint_states"/>
	</node>

8.编写关节控制器 - 后

• 0）扯完前面的准备操作后，终于可以开始编写用户层面的.py文件，早在第三篇教程就想了，没想到中间拖了那么多需要讲的基础概念。
• 1）在目录carpack_control/scripts下新建.py文件，原本自动创建的Int与src文件夹因为用不到就删了 。然后把之前编写的topic代码复制进去。
• 

#!/usr/bin/env python
#-*-coding:utf-8-*-

# 加载ros的Python基础包
import rospy
# 加载topic话题 的 msg消息
from std_msgs.msg import Float64
from geometry_msgs.msg import Twist

class main_class:
    # 初始化函数
    def __init__(self):
        # 创建node节点 —— 电机控制
        rospy.init_node('motor_control', anonymous=True)
        # 订阅topic话题 —— 电机pwm输出
        rospy.Subscriber("motor_output", Twist, self.callback)
        # 发布topic话题 —— 线速度输出
        self.pub_linear = rospy.Publisher('linear_output', Float64, queue_size=10)
        # 发布topic话题 —— 角速度输出
        self.pub_angular = rospy.Publisher('angular_output', Float64, queue_size=10)
        # 阻塞等待
        rospy.spin()
    # 回调函数
    def callback(self,data):
        # 创建 msg 消息, 注意:ros的float64是一个结构体
        angle = Float64()
        speed = Float64()
        # 提取 线速度 与 角速度
        speed.data = ((data.linear.x) * 8)
        angle.data = ((data.angular.z) * 1)
        # 向topic话题 发送 msg消息
        self.pub_linear.publish(speed.data)
        self.pub_angular.publish(angle.data)

if __name__ == '__main__':
    try:
        main_class()
    except rospy.ROSInterruptException:
        pass

• 2）将发布的话题改为我们需要的那6个关节话题，因为创建时加了命名空间，所以这里也要加上，同时还有一个后缀/command。复制粘贴修改完后，大概是这样：

class main_class:
    # 初始化函数
    def __init__(self):
        # 创建node节点 —— 电机控制
        rospy.init_node('motor_control', anonymous=True)
        # 订阅topic话题 —— 电机pwm输出
        rospy.Subscriber("motor_output", Twist, self.callback)
        # 发布topic话题 —— 线速度输出
        self.pub_lrw = rospy.Publisher('/racecar/left_rear_wheel_velocity_controller/command', Float64, queue_size=10)
        self.pub_rrw = rospy.Publisher('/racecar/right_rear_wheel_velocity_controller/command', Float64, queue_size=10)
        self.pub_lfw = rospy.Publisher('/racecar/left_front_wheel_velocity_controller/command', Float64, queue_size=10)
        self.pub_rfw = rospy.Publisher('/racecar/right_front_wheel_velocity_controller/command', Float64, queue_size=10)
        # 发布topic话题 —— 角速度输出
        self.pub_lsh = rospy.Publisher('/racecar/left_steering_hinge_position_controller/command', Float64, queue_size=10)
        self.pub_rsh = rospy.Publisher('/racecar/right_steering_hinge_position_controller/command', Float64, queue_size=10)
        # 阻塞等待
        rospy.spin()
    # 回调函数
    def callback(self,data):
        # 创建 msg 消息, 注意:ros的float64是一个结构体
        angle = Float64()
        speed = Float64()
        # 提取 线速度 与 角速度
        speed.data = ((data.linear.x) * 8)
        angle.data = ((data.angular.z) * 1)
        # 向topic话题 发送 msg消息
        self.pub_lrw.publish(speed.data)
        self.pub_rrw.publish(speed.data)
        self.pub_lfw.publish(speed.data)
        self.pub_rfw.publish(speed.data)
        self.pub_lsh.publish(angle.data)
        self.pub_rsh.publish(angle.data)

• 3）添加.py文件后，别忘记修改CMakeLists.txt、package.xml、和修改可执行权限。然后编译，添加进launch文件中。每次修改.py文件后都需要重新编译，而launch文件修改完后不想要重新编译也可以。

car_gazebo.launch

<!-- 启动关节控制器 用户编写的Python文件 -->
<node name="motor_control" pkg="carpack_control" type="motor_control.py"/>

• 早期学习时，我后把学到要添加修改的内容都怼上了，.py每导入一个包的内容就在package.xml和CMakeLists.txt添加“依赖”。后来我发现北邮学长的例程中没有只是修改了catkin_install_python的内容而已，后来发现确实只添加这一块地方其他都不改也可以正常运行。因为我还每完全理解CMakeLists各个标签的作用，所以不过多介绍。
  CMakeLists.txt

## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
 catkin_install_python(PROGRAMS
#   scripts/my_python_script
    scripts/motor_control.py
    DESTINATION ${
    
    CATKIN_PACKAGE_BIN_DESTINATION}
 )

• 

9.创建按键功能

• 0）这一部分可选，如果由按键功能后续测试domo坐标时会方便一点，都是没有也没关系。因为你跟着教程作不测试也可以，虽然我个人推荐要自己实践能加深掌握。
• 1）按键功能大部分都是Python知识，读取键盘上的按键输入，然后根据按下的键向/motor_output话题发布对应的键值消息。然后/motor_control节点就订阅，将控制消息再分别发送到车模关节的话题上。
• 2）ros本身就有自带的按键输入控制，中科院的教程例程中也有，不过二者的按键输入节点都运行在终端，输入也是在终端，不美观。北邮学长的按键输入是作了一个可视化窗口，美观不少而且方便使用。但是它的按键输入.py文件发布的消息不是geometry_msgs/Twist，而是ackermann_msgs/AckermannDriveStamped。所以我们利用之前学习的知识稍作修改。我们先利用rosmsg show指令查看两种msg消息的区别。

rosmsg show geometry_msgs/Twist
rosmsg show ackermann_msgs/AckermannDriveStamped

• 

• 3）对比可知，我们只需要将msg.drive.speed与msg.drive.steering_angle修改为msg.linear.x与msg.angular.z，其他的赋值都删除，并且修改初始化类型等即可。内容较多，不作对比，直接放出已经修改后的代码。

#!/usr/bin/env python
#-*-coding:utf-8-*-

# Copyright (c) 2019, The Personal Robotics Lab, The MuSHR Team, The Contributors of MuSHR
# License: BSD 3-Clause. See LICENSE.md file in root directory.

import atexit
import os
import signal
from threading import Lock
from Tkinter import Frame, Label, Tk

import rospy
from geometry_msgs.msg import Twist

# 定义全局变量
UP = "w"
LEFT = "a"
DOWN = "s"
RIGHT = "d"
QUIT = "q"
# 状态
state = [False, False, False, False]
# python多线程语法，创建一个锁，防止多个程序同时调用打印功能
state_lock = Lock()
# 初始化赋值
state_pub = None
root = None
control = False

# 检测哪个按键被按下
def keyeq(e, c):
    return e.char == c or e.keysym == c

# 按键是否被松开
def keyup(e):
    global state
    global control
    # python语法
    with state_lock:
        if keyeq(e, UP):
            state[0] = False
        elif keyeq(e, LEFT):
            state[1] = False
        elif keyeq(e, DOWN):
            state[2] = False
        elif keyeq(e, RIGHT):
            state[3] = False
        control = sum(state) > 0

# 按键是否被按下？
def keydown(e):
    global state
    global control
    # python语法
    with state_lock:
        if keyeq(e, QUIT):
            shutdown()
        elif keyeq(e, UP):
            state[0] = True
            state[2] = False
        elif keyeq(e, LEFT):
            state[1] = True
            state[3] = False
        elif keyeq(e, DOWN):
            state[2] = True
            state[0] = False
        elif keyeq(e, RIGHT):
            state[3] = True
            state[1] = False
        control = sum(state) > 0

# Up -> linear.x = 1.0
# Down -> linear.x = -1.0
# Left ->  angular.z = 1.0
# Right -> angular.z = -1.0

# 订阅话题的回调函数
def publish_cb(_):
    with state_lock:
        if not control:
            return
        # 创建msg消息
        ack = Twist()
        # 根据按下的键赋值
        if state[0]:
            ack.linear.x = max_velocity
        elif state[2]:
            ack.linear.x = -max_velocity

        if state[1]:
            ack.angular.z = max_steering_angle
        elif state[3]:
            ack.angular.z = -max_steering_angle
        # 话题发布消息
        if state_pub is not None:
            state_pub.publish(ack)

def exit_func():
    # system函数可以将字符串转化成命令在服务器上运行；
    os.system("xset r on")

def shutdown():
    root.destroy()
    # destroy()只是终止mainloop并删除所有小部件
    rospy.signal_shutdown("shutdown")

# 主函数
def main():
    global state_pub
    global root
    # 声明全局变量
    global max_velocity
    global max_steering_angle
    # 获取变量，从参数服务器中中
    max_velocity = rospy.get_param("~speed", 2.0)
    max_steering_angle = rospy.get_param("~max_steering_angle", 0.34)
    # 不设立默认值了，确保有写
    key_publisher = "/motor_output"
    # 创建topic话题，发送按键信息
    state_pub = rospy.Publisher(
        key_publisher, Twist, queue_size=1
    )
    # 创建周期性调用函数“publish_cb”，频率是1/0.1=10Hz，
    rospy.Timer(rospy.Duration(0.1), publish_cb)
    # 注册函数。在程序结束时，先注册的后运行 
    atexit.register(exit_func)
    os.system("xset r off")
    # 创建窗口对象的背景色
    root = Tk()
    # 框架控件；在屏幕上显示一个矩形区域，多用来作为容器
    frame = Frame(root, width=100, height=100)
    frame.bind("<KeyPress>", keydown)
    frame.bind("<KeyRelease>", keyup)
    frame.pack()
    frame.focus_set()
    # 窗口显示
    lab = Label(
        frame,
        height=10,
        width=30,
        text="Focus on this window\nand use the WASD keys\nto drive the car.\n\nPress Q to quit",
    )
    lab.pack()
    print("Press %c to quit" % QUIT)
    root.mainloop()

if __name__ == "__main__":
    rospy.init_node("key_control", disable_signals=True)
    # 安全操作
    signal.signal(signal.SIGINT, lambda s, f: shutdown())
    main()

• 4）将.py文件保存在工程中，并修改CMakeLists.txt与launch文件。然后修改权限与编译。无误后就可以启动launch查看效果。
• 

<!-- 启动按键控制器 用户编写的Python文件 -->
<node name="key_control" pkg="carpack_control" type="key_control.py"/>

 catkin_install_python(PROGRAMS
#   scripts/my_python_script
    scripts/motor_control.py
    scripts/key_control.py
    DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )

• 5）最后，启动launch后会弹出一个tk小框框，在这个tk下按下“wasd”按键，可以看到gazebo内的小车运动了起来，你可以尝试一下使用按键控制，跑完整个赛道。ohhhhhhh
• 

10.结语

• 0）本来想在Ubuntu20.04版本下操作的，没想到卡了几天没成功，之后又懒惰了几天，拖了一个星期才开始动笔写。
• 1）可喜可贺，结果几万字的敲打，终于可以让模型小车动起来了，下一篇讲完最后一点内容就结束啦。目前已经实现了控制、运动了，接下来只需要导入一些前人已经写好的ros功能包，就能实现导航、定位、避障功能了。

《教程 Re:Zero ROS （六）—— 获取&编写&检验 -＞ odom坐标系》
https://blog.csdn.net/Lovely_him/article/details/107948765