【无标题】

---

title: ROS入门到仿真
date: 2021.09.10

---

写在前面的话

首先我想声明的一点是，已经快被BUG和Ubuntu的狗屎输入法搞崩溃了，又加上这网络问题，外加之前调试的一堆代码，我也是尤其深刻体会到了那句“一切不加注释的代码都是屎山！！！”是在不想浪费时间，但心理确实烦的一批。。。

好了好了，经过一些不必要的时间，我现在成功的安装上了百度输入法，虽然之前很不情愿用，但现在看来：“真香！！！”。话不多说，让我们进入今天的主题：ROS的入门到仿真

第一步 入门会很快的过一遍，并不会细讲！！！！

首先，我入门ROS看的是古月的21讲那个视频，具体链接在这里，还有就是入门的话应该不会用很长的时间，一个星期已经足矣，建立在有编程基础，学过C和C++或者是Python的情况下。那么默认大家已经掌握了这些编程规则，毕竟学了C的话剩下两个入门应该不是问题，也会很快，接下来就先看一下大概的目录结构吧。

这张图片展示的是我的工作空间，src里面的便是从入门开始要学习的有关ros的一些基础知识，具体文件的话我会想办法留下来，但是由于里面涉及到的一些编译规则的问题（具体学过CMake，Makefile的应该都清楚，即使像我一样只看了大约一个小时不到也应该明白一些），所以说演示成功的部分仅代表在我个人的电脑上可以成功运行，如果想要在你的电脑上成功运行的话，还得花一小会去改一些东西，或者直接将我的源码配套结合自己的理解直接改，不要怕，就是梭哈！！！[手动狗头]

首先是topic的相关代码

古月开篇讲的东西已经很明白了，所以这里就不做赘述。直接上源码（配合小海龟实现）

#!/usr/bin/python3
# -*- coding: utf-8 -*-

# 第一行代码以#！开头叫做“shebang”也叫sha-bang/hashbang/pound-bang/hash-pling，其作用是指定解析器来执行脚本
# 第二行代码是用来指定文件编码为utf-8的 如果没有编码类型的声明将默认以ASCII编码处理
# 该例程将发布turtle1/cmd_vel话题，消息类型geometry_msgs::Twist

import rospy
from geometry_msgs.msg import Twist


def velocity_publisher():
    # ROS节点初始化
    rospy.init_node('velocity_publisher', anonymous=True)

    # 创建一个Publisher，发布名为/turtle1/cmd_vel的topic，消息类型为geometry_msgs::Twist，队列长度10
    turtle_vel_pub = rospy.Publisher('/turtle2/cmd_vel', Twist, queue_size=10)

    # 设置循环的频率
    rate = rospy.Rate(10)

    while not rospy.is_shutdown():
        # 初始化geometry_msgs::Twist类型的消息
        vel_msg = Twist()
        vel_msg.linear.x = 0.5
        vel_msg.angular.z = 0.2

        # 发布消息
        turtle_vel_pub.publish(vel_msg)
        rospy.loginfo("Publsh turtle velocity command[%0.2f m/s, %0.2f rad/s]",
                      vel_msg.linear.x, vel_msg.angular.z)

        # 按照循环频率延时
        rate.sleep()


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

这里为什么选择Python其实很好理解，Python的话是不用编译的，不像是C++需要编译后才可以生成执行文件，这个具体Makefile应该会讲，不过还有一点事Python支持的ros功能不算多，换句话说很多功能的实现都是C++写的，所以入门的时候我把两种语言都学了。。。。

具体C++里面设置功能包，添加功能包，自己添加自己的功能包等一系列的操作可以自学，但古月都讲过，比较详细，《视觉SLAM十四讲》这本书也有所提及。

然后是位姿的相关代码，比较简单，配合古月的21讲都能看懂。这里就不展示具体实现，还是建议跟着视频自己手动敲一遍才能加深记忆有助于理解。仅放一些可能用到的源码

#位姿信息？？可能是时间有点长不太记得了。
#!/usr/bin/python3
# -*- coding:utf-8 -*-

import rospy
from turtlesim.msg import Pose


def my_pose():
    rospy.init_node('pose', anonymous=True)

    rospy.Subscriber('/turtle2/pose', Pose, callback=INFO)

    rospy.spin()


def INFO(msg):
    rospy.loginfo("Turtle pose: x:%0.6f, y:%0.6f", msg.x, msg.y)


if __name__ == "__main__":
    my_pose()

然后是publisher的实现函数：

#!/usr/bin/python3
# -*- coding:utf-8 -*-

import rospy
from topic.msg import Self


def self_publisher():
    rospy.init_node('self_publisher', anonymous=True)
    self_info_pub = rospy.Publisher('/person_info', Self, queue_size=10)
    rate = rospy.Rate(10)

    while not rospy.is_shutdown():
        # 初始化topic::Self类型消息
        self_msg = Self()
        self_msg.name = "HBT"
        self_msg.age = 20
        self_msg.sex = 1

        self_info_pub.publish(self_msg)
        rospy.loginfo("Person message[%s,%d,%d]" % (
            self_msg.name, self_msg.age, self_msg.sex))

        rate.sleep()


if __name__ == "__main__":
    try:
        self_publisher()
    except rospy.ROSInterruptException:
        pass

subscriber函数的实现

#!/usr/bin/python3
# -*- coding:utf-8 -*-

import rospy
from topic.msg import Self


def self_subscriber():
    rospy.init_node("self_subscriber", anonymous=True)

    rospy.Subscriber("/person_info", Self, back)
    # 循环等待回调函数
    rospy.spin()


def back(msg):
    rospy.loginfo("Subscriber message:name:%s,age:%d,sex:%d",
                  msg.name, msg.age, msg.sex)


if __name__ == "__main__":
    self_subscriber()

键盘控制小海龟运动的代码

#!/usr/bin/python3
# -*- coding:utf-8 -*-
from std_msgs.msg import String
from geometry_msgs.msg import Twist
import rospy
import os
import sys
import tty
import termios
import roslib
roslib.load_manifest('topic')

# 全局变量
cmd = Twist()
pub = rospy.Publisher('/cmd_vel', Twist)


def keyboardLoop():
    # 初始化
    rospy.init_node('smartcar_teleop')
    rate = rospy.Rate(rospy.get_param('~hz', 1))

    # 速度变量
    walk_vel_ = rospy.get_param('walk_vel', 0.5)
    run_vel_ = rospy.get_param('run_vel', 1.0)
    yaw_rate_ = rospy.get_param('yaw_rate', 1.0)
    yaw_rate_run_ = rospy.get_param('yaw_rate_run', 1.5)

    max_tv = walk_vel_
    max_rv = yaw_rate_

    # 显示提示信息
    print("Reading from keyboard")
    print("Use WASD keys to control the robot")
    print("Press Caps to move faster")
    print("Press q to quit")

    # 读取按键循环
    while not rospy.is_shutdown():
        fd = sys.stdin.fileno()
        old_settings = termios.tcgetattr(fd)
        # 不产生回显效果
        old_settings[3] = old_settings[3] & ~termios.ICANON & ~termios.ECHO
        try:
            tty.setraw(fd)
            ch = sys.stdin.read(1)
        finally:
            termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)

        if ch == 'w':
            max_tv = walk_vel_
            speed = 1
            turn = 0
        elif ch == 's':
            max_tv = walk_vel_
            speed = -1
            turn = 0
        elif ch == 'a':
            max_rv = yaw_rate_
            speed = 0
            turn = 1
        elif ch == 'd':
            max_rv = yaw_rate_
            speed = 0
            turn = -1
        elif ch == 'W':
            max_tv = run_vel_
            speed = 1
            turn = 0
        elif ch == 'S':
            max_tv = run_vel_
            speed = -1
            turn = 0
        elif ch == 'A':
            max_rv = yaw_rate_run_
            speed = 0
            turn = 1
        elif ch == 'D':
            max_rv = yaw_rate_run_
            speed = 0
            turn = -1
        elif ch == 'q':
            exit()
        else:
            max_tv = walk_vel_
            max_rv = yaw_rate_
            speed = 0
            turn = 0

        # 发送消息
        cmd.linear.x = speed * max_tv
        cmd.angular.z = turn * max_rv
        pub.publish(cmd)
        rate.sleep()
        # 停止机器人
        stop_robot()


def stop_robot():
    cmd.linear.x = 0.0
    cmd.angular.z = 0.0
    pub.publish(cmd)


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

关于topic的讲解还是不在赘述，有时间的话在考虑好好写一篇吧。

然后是service的相关函数，还是只过一遍代码，具体学习参考古月的21讲我感觉就挺好的

#!/usr/bin/python3
# -*- coding:utf-8 -*-

import rospy
import threading
import time
from geometry_msgs.msg import Twist
from std_srvs.srv import Trigger, TriggerResponse

pubcommand = False
turtle_pub = rospy.Publisher('/turtle2/cmd_vel', Twist, queue_size=10)


def command_thread():
    while True:
        if pubcommand:
            vel_msg = Twist()
            vel_msg.linear.x = 0.8
            vel_msg.angular.y = 0.5
            turtle_pub.publish(vel_msg)
        time.sleep(0.1)


def commandcallback(req):
    global pubcommand
    pubcommand = bool(1-pubcommand)
    rospy.loginfo("Publishe turtle velocity command [%d]", pubcommand)
    return TriggerResponse(1, "Change!!!")


def command_srv():
    rospy.init_node('command_srv')
    s = rospy.Service('/command_srv', Trigger, commandcallback)

    thread = threading.Thread(target=command_thread)
    thread.start()

    rospy.spin()


if __name__ == "__main__":
    command_srv()

这个是command的服务函数。

接下来是个人的Clint端和service端的函数

#!/usr/bin/python3
# -*- coding:utf-8 -*-

import sys
import rospy
from service.srv import Person, PersonRequest


def person_client():
    rospy.init_node('person_client')
    # 发现/spawn服务后，创建一个服务客户端，连接名为/spawn的service
    rospy.wait_for_service('/show_person')
    try:
        person_client = rospy.ServiceProxy('/show_person', Person)
        response = person_client("Tom", 20, PersonRequest.boy)
        return response.result
    except rospy.ServiceException as e:
        print("Service call failed: %s" % e)


if __name__ == "__main__":
    print("Show person result : %s" % (person_client()))

#!/usr/bin/python3
# -*- coding:utf-8 -*-

import rospy
from service.srv import Person, PersonResponse


def personCallback(req):
    rospy.loginfo("Person: name:%s  age:%d  sex:%d",
                  req.name, req.age, req.sex)
    return PersonResponse("OK")


def person_server():
    rospy.init_node('person_server')
    # 创建一个名为/show_person的server，注册回调函数personCallback
    s = rospy.Service('/show_person', Person, personCallback)
    print("Ready to show person informtion.")
    rospy.spin()


if __name__ == "__main__":
    person_server()

这两个脚本得配合使用应该是，现在只做总结吧。

然后是产生多个小海龟的代码

#!/usr/bin/python3
# -*- coding:utf-8 -*-

import sys
import rospy
from turtlesim.srv import Spawn


def spawn():
    rospy.init_node('spawn')

    rospy.wait_for_service('/spawn')

    try:
        add_turtle = rospy.ServiceProxy('/spawn', Spawn)

        response = add_turtle(2.0, 2.0, 0.0, 'turtle2')
        return response.name
    except rospy.ServiceException as error:
        print('Service call failed:%s', error)


if __name__ == "__main__":
    print("Spawn turtle successful name:%s", spawn())

emm，好像古月的也只能找到这么多了吧？？？

资源有限，接下来是仿真的一些了。

第二步 仿真

仿真的其实也很简单，大都是类似的搬运，搬着搬着代码阅读能力也会提高的，加上自己手动写一些，理解也会更加深刻，这部分我是参考了奥特学院的视频加文档,这个UP是非常不错的，

当时ROS入门之后这个up才开始做视频，所以说前面的很大一部分我都是直接跳过去的
确实是一个很好的选择，只不过入门的知识讲的时间很长，但我当时确实时间不是很足的情况下就没有看。还是建议大家看一看的，赵老师YYDS！！！文档在这里,这个好像是用Gitbook发的，然后本质上也是Markdown的书写方式，确实可以省很多时间，与其去研究World文档怎么编辑，还不如来花几分钟入门Markdown，一劳永逸。

首先得了解关于仿真的一些工具以及一些ros当中用于调试的工具，像是RVIZ和GAZEBO等一些基础的，还有rqt一些调试的工具之外，其他的还需要读者自己去了解，这里只会讲用到的一些工具。

要想跑仿真首先得是有一个建模的小车，这个小车是我根据奥特改的一个roscar，大体上还可以，只不过amcl定位不准确，地图漂移有些严重（这里是指运动一部分时间之后的累计误差）。

下面是建模的代码，文件以.xacro方式保存

小车的底盘

<robot name="base" xmlns:xacro="http://wiki.ros.org/xacro">
    <!-- Macro for inertia matrix -->
    <xacro:macro name="sphere_inertial_matrix" params="m r">
        <inertial>
            <mass value="${m}" />
            <inertia ixx="${2*m*r*r/5}" ixy="0" ixz="0"
                iyy="${2*m*r*r/5}" iyz="0" 
                izz="${2*m*r*r/5}" />
        </inertial>
    </xacro:macro>

    <xacro:macro name="cylinder_inertial_matrix" params="m r h">
        <inertial>
            <mass value="${m}" />
            <inertia ixx="${m*(3*r*r+h*h)/12}" ixy = "0" ixz = "0"
                iyy="${m*(3*r*r+h*h)/12}" iyz = "0"
                izz="${m*r*r/2}" /> 
        </inertial>
    </xacro:macro>

    <xacro:macro name="Box_inertial_matrix" params="m l w h">
       <inertial>
               <mass value="${m}" />
               <inertia ixx="${m*(h*h + l*l)/12}" ixy = "0" ixz = "0"
                   iyy="${m*(w*w + l*l)/12}" iyz= "0"
                   izz="${m*(w*w + h*h)/12}" />
       </inertial>
   </xacro:macro>
</robot>

<!-- 摄像头相关的 xacro 文件 -->
<robot name="my_camera" xmlns:xacro="http://wiki.ros.org/xacro">
    <!-- 摄像头属性 -->
    <xacro:property name="camera_length" value="0.01" /> <!-- 摄像头长度(x) -->
    <xacro:property name="camera_width" value="0.025" /> <!-- 摄像头宽度(y) -->
    <xacro:property name="camera_height" value="0.025" /> <!-- 摄像头高度(z) -->
    <xacro:property name="camera_x" value="0.08" /> <!-- 摄像头安装的x坐标 -->
    <xacro:property name="camera_y" value="0.0" /> <!-- 摄像头安装的y坐标 -->
    <xacro:property name="camera_z" value="${base_link_length / 2 + camera_height / 2}" /> <!-- 摄像头安装的z坐标:底盘高度 / 2 + 摄像头高度 / 2  -->
    <xacro:property name="camera_mass" value="0.08" />
    <!-- 摄像头关节以及link -->
    <link name="camera">
        <visual>
            <geometry>
                <box size="${camera_length} ${camera_width} ${camera_height}" />
            </geometry>
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
            <material name="black" />
        </visual>
        <collision>
            <geometry>
                <box size="${camera_length} ${camera_width} ${camera_height}" />
            </geometry>
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
        </collision>
        <xacro:Box_inertial_matrix m="${camera_mass}" l="${camera_length}" w="${camera_width}" h="${camera_height}" />
    </link>
    <gazebo reference="camera">
    <material>
        Gazebo/Black
    </material>
    </gazebo>

    <joint name="camera2base_link" type="fixed">
        <parent link="base_link" />
        <child link="camera" />
        <origin xyz="${camera_x} ${camera_y} ${camera_z}" />
    </joint>
</robot>

然后是雷达

<!--
    小车底盘添加雷达
-->
<robot name="my_laser" xmlns:xacro="http://wiki.ros.org/xacro">

    <!-- 雷达支架 -->
    <xacro:property name="support_length" value="0.15" /> <!-- 支架长度 -->
    <xacro:property name="support_radius" value="0.01" /> <!-- 支架半径 -->
    <xacro:property name="support_x" value="0.0" /> <!-- 支架安装的x坐标 -->
    <xacro:property name="support_y" value="0.0" /> <!-- 支架安装的y坐标 -->
    <xacro:property name="support_z" value="${base_link_length / 2 + support_length / 2}" /> <!-- 支架安装的z坐标:底盘高度 / 2 + 支架高度 / 2  -->
    <xacro:property name="support_mass" value="0.15" />
    <link name="support">
        <visual>
            <geometry>
                <cylinder radius="${support_radius}" length="${support_length}" />
            </geometry>
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
            <material name="red">
                <color rgba="0.8 0.2 0.0 0.8" />
            </material>
        </visual>
        <collision>
            <geometry>
                <cylinder radius="${support_radius}" length="${support_length}" />
            </geometry>
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
        </collision>
        <xacro:cylinder_inertial_matrix m="${support_mass}" r="${support_radius}" h="${support_length}"/>
    </link>
    <gazebo reference="support">
        <material>
            Gazebo/Red
        </material>
    </gazebo>

    <joint name="support2base_link" type="fixed">
        <parent link="base_link" />
        <child link="support" />
        <origin xyz="${support_x} ${support_y} ${support_z}" />
    </joint>


    <!-- 雷达属性 -->
    <xacro:property name="laser_length" value="0.05" /> <!-- 雷达长度 -->
    <xacro:property name="laser_radius" value="0.03" /> <!-- 雷达半径 -->
    <xacro:property name="laser_x" value="0.0" /> <!-- 雷达安装的x坐标 -->
    <xacro:property name="laser_y" value="0.0" /> <!-- 雷达安装的y坐标 -->
    <xacro:property name="laser_z" value="${support_length / 2 + laser_length / 2}" /> <!-- 雷达安装的z坐标:支架高度 / 2 + 雷达高度 / 2  -->
    <xacro:property name="laser_mass" value="0.5" />
    <!-- 雷达关节以及link -->
    <link name="laser">
        <visual>
            <geometry>
                <cylinder radius="${laser_radius}" length="${laser_length}" />
            </geometry>
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
            <material name="black" />
        </visual>
        <collision>
            <geometry>
                <cylinder radius="${laser_radius}" length="${laser_length}" />
            </geometry>
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
        </collision>
        <xacro:cylinder_inertial_matrix m="${laser_mass}" r="${laser_radius}" h="${laser_length}"/>
    </link>
    <gazebo reference="laser">
    <material>
        Gazebo/Black
    </material>
    </gazebo>

    <joint name="laser2support" type="fixed">
        <parent link="support" />
        <child link="laser" />
        <origin xyz="${laser_x} ${laser_y} ${laser_z}" />
    </joint>
</robot>

深度相机（可以理解为传感器）

<robot name="my_sensors" xmlns:xacro="http://wiki.ros.org/xacro">
    <gazebo reference="support">  
      <sensor type="depth" name="camera">
        <always_on>true</always_on>
        <update_rate>20.0</update_rate>
        <camera>
          <horizontal_fov>${60.0*PI/180.0}</horizontal_fov>
          <image>
            <format>R8G8B8</format>
            <width>640</width>
            <height>480</height>
          </image>
          <clip>
            <near>0.05</near>
            <far>8.0</far>
          </clip>
        </camera>
        <plugin name="kinect_camera_controller" filename="libgazebo_ros_openni_kinect.so">
          <cameraName>camera</cameraName>
          <alwaysOn>true</alwaysOn>
          <updateRate>10</updateRate>
          <imageTopicName>rgb/image_raw</imageTopicName>
          <depthImageTopicName>depth/image_raw</depthImageTopicName>
          <pointCloudTopicName>depth/points</pointCloudTopicName>
          <cameraInfoTopicName>rgb/camera_info</cameraInfoTopicName>
          <depthImageCameraInfoTopicName>depth/camera_info</depthImageCameraInfoTopicName>
          <frameName>support</frameName>
          <baseline>0.1</baseline>
          <distortion_k1>0.0</distortion_k1>
          <distortion_k2>0.0</distortion_k2>
          <distortion_k3>0.0</distortion_k3>
          <distortion_t1>0.0</distortion_t1>
          <distortion_t2>0.0</distortion_t2>
          <pointCloudCutoff>0.4</pointCloudCutoff>
        </plugin>
      </sensor>
    </gazebo>

</robot>

接下来就是小车静态底盘的实现

<!--
    使用 xacro 优化 URDF 版的小车底盘实现：

    实现思路:
    1.将一些常量、变量封装为 xacro:property
      比如:PI 值、小车底盘半径、离地间距、车轮半径、宽度 ....
    2.使用 宏 封装驱动轮以及支撑轮实现，调用相关宏生成驱动轮与支撑轮

-->
<!-- 根标签，必须声明 xmlns:xacro -->
<robot name="my_base" xmlns:xacro="http://www.ros.org/wiki/xacro">
    <!-- 封装变量、常量 -->
    <xacro:property name="PI" value="3.141592653579"/>
    <!-- 宏:黑色设置 -->
    <material name="black">
        <color rgba="0.0 0.0 0.0 1.0" />
    </material>
    <!-- 底盘属性 -->
    <xacro:property name="base_footprint_radius" value="0.001" /> <!-- base_footprint 半径  -->
    <xacro:property name="base_link_radius" value="0.1" /> <!-- base_link 半径 -->
    <xacro:property name="base_link_length" value="0.08" /> <!-- base_link 长 -->
    <xacro:property name="base_mass" value="1" />
    <xacro:property name="earth_space" value="0.015" /> <!-- 离地间距 -->

    <!-- 底盘 -->
    <link name="base_footprint">
      <visual>
        <geometry>
          <sphere radius="${base_footprint_radius}" />
        </geometry>
      </visual>
    </link>

    <link name="base_link">
      <visual>
        <geometry>
          <cylinder radius="${base_link_radius}" length="${base_link_length}" />
        </geometry>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <material name="yellow">
          <color rgba="0.5 0.3 0.0 0.5" />
        </material>
      </visual>
      <collision>
        <geometry>
          <cylinder radius="${base_link_radius}" length="${base_link_length}" />
        </geometry>
        <origin xyz="0 0 0" rpy="0 0 0" />
      </collision>
      <xacro:cylinder_inertial_matrix m="${base_mass}" r="${base_link_radius}" h="${base_link_length}"/>
    </link>
    <gazebo reference="base_link">
        <material>Gazebo/Yellow</material>
    </gazebo>

    <joint name="base_link2base_footprint" type="fixed">
      <parent link="base_footprint" />
      <child link="base_link" />
      <origin xyz="0 0 ${earth_space + base_link_length / 2 }" />
    </joint>

    <!-- 驱动轮 -->
    <!-- 驱动轮属性 -->
    <xacro:property name="wheel_radius" value="0.0325" /><!-- 半径 -->
    <xacro:property name="wheel_length" value="0.015" /><!-- 宽度 -->
    <xacro:property name="wheel_mass" value="0.05" />
    <!-- 驱动轮宏实现 -->
    <xacro:macro name="add_wheels" params="name flag">
      <link name="${name}_wheel">
        <visual>
          <geometry>
            <cylinder radius="${wheel_radius}" length="${wheel_length}" />
          </geometry>
          <origin xyz="0.0 0.0 0.0" rpy="${PI / 2} 0.0 0.0" />
          <material name="black" />
        </visual>
        <collision>
           <geometry>
            <cylinder radius="${wheel_radius}" length="${wheel_length}" />
          </geometry>
          <origin xyz="0.0 0.0 0.0" rpy="${PI / 2} 0.0 0.0" />
        </collision>
        <xacro:cylinder_inertial_matrix m="${wheel_mass}" r="${wheel_radius}" h="${wheel_length}" />
      </link>
      <gazebo reference="${name}_wheel">
        <material>Gazebo/Black</material>
      </gazebo>

      <joint name="${name}_wheel2base_link" type="continuous">
        <parent link="base_link" />
        <child link="${name}_wheel" />
        <origin xyz="0 ${flag * base_link_radius} ${-(earth_space + base_link_length / 2 - wheel_radius) }" />
        <axis xyz="0 1 0" />
      </joint>
    </xacro:macro>
    <xacro:add_wheels name="left" flag="1" />
    <xacro:add_wheels name="right" flag="-1" />
    <!-- 支撑轮 -->
    <!-- 支撑轮属性 -->
    <xacro:property name="support_wheel_radius" value="0.0075" /> <!-- 支撑轮半径 -->
    <xacro:property name="support_wheel_mass" value="0.01" />
    <!-- 支撑轮宏 -->
    <xacro:macro name="add_support_wheel" params="name flag" >
      <link name="${name}_wheel">
        <visual>
            <geometry>
                <sphere radius="${support_wheel_radius}" />
            </geometry>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <material name="black" />
        </visual>
        <collision>
            <geometry>
                <sphere radius="${support_wheel_radius}" />
            </geometry>
            <origin xyz="0 0 0" rpy="0 0 0" />
        </collision>
        <xacro:sphere_inertial_matrix m="${support_wheel_mass}" r="${support_wheel_radius}" />
      </link>
      <gazebo reference="${name}_wheel">
          <material>Gazebo/Black</material>
      </gazebo>

      <joint name="${name}_wheel2base_link" type="continuous">
          <parent link="base_link" />
          <child link="${name}_wheel" />
          <origin xyz="${flag * (base_link_radius - support_wheel_radius)} 0 ${-(base_link_length / 2 + earth_space / 2)}" />
          <axis xyz="1 1 1" />
      </joint>
    </xacro:macro>

    <xacro:add_support_wheel name="front" flag="1" />
    <xacro:add_support_wheel name="back" flag="-1" />

</robot>

要想要底盘实现机械运动到目前为止还是远远不够的，必须再加上控制的代码才能实现小车的运动。

摄像头控制代码

<robot name="my_sensors" xmlns:xacro="http://wiki.ros.org/xacro">
  <!-- 被引用的link -->
  <gazebo reference="camera">
    <!-- 类型设置为 camara -->
    <sensor type="camera" name="camera_node">
      <update_rate>30.0</update_rate> <!-- 更新频率 -->
      <!-- 摄像头基本信息设置 -->
      <camera name="head">
        <horizontal_fov>1.3962634</horizontal_fov>
        <image>
          <width>1280</width>
          <height>720</height>
          <format>R8G8B8</format>
        </image>
        <clip>
          <near>0.02</near>
          <far>300</far>
        </clip>
        <noise>
          <type>gaussian</type>
          <mean>0.0</mean>
          <stddev>0.007</stddev>
        </noise>
      </camera>
      <!-- 核心插件 -->
      <plugin name="gazebo_camera" filename="libgazebo_ros_camera.so">
        <alwaysOn>true</alwaysOn>
        <updateRate>0.0</updateRate>
        <cameraName>/camera</cameraName>
        <imageTopicName>image_raw</imageTopicName>
        <cameraInfoTopicName>camera_info</cameraInfoTopicName>
        <frameName>camera</frameName>
        <hackBaseline>0.07</hackBaseline>
        <distortionK1>0.0</distortionK1>
        <distortionK2>0.0</distortionK2>
        <distortionK3>0.0</distortionK3>
        <distortionT1>0.0</distortionT1>
        <distortionT2>0.0</distortionT2>
      </plugin>
    </sensor>
  </gazebo>
</robot>

雷达控制代码

<robot name="my_sensors" xmlns:xacro="http://wiki.ros.org/xacro">

  <!-- 雷达 -->
  <gazebo reference="laser">
    <sensor type="ray" name="rplidar">
      <pose>0 0 0 0 0 0</pose>
      <visualize>true</visualize>
      <update_rate>5.5</update_rate>
      <ray>
        <scan>
          <horizontal>
            <samples>360</samples>
            <resolution>1</resolution>
            <min_angle>-3.1415926</min_angle>
            <max_angle>3.1415926</max_angle>
          </horizontal>
        </scan>
        <range>
          <min>0.10</min>
          <max>30.0</max>
          <resolution>0.01</resolution>
        </range>
        <noise>
          <type>gaussian</type>
          <mean>0.0</mean>
          <stddev>0.01</stddev>
        </noise>
      </ray>
      <plugin name="gazebo_rplidar" filename="libgazebo_ros_laser.so">
        <topicName>/scan</topicName>
        <frameName>laser</frameName>
      </plugin>
    </sensor>
  </gazebo>

</robot>

最后是连接起整个小车的综合代码，可以理解为一个将各个组件拼接在一起的文件

<!-- 组合小车底盘与摄像头 -->
<robot name="mycar" xmlns:xacro="http://wiki.ros.org/xacro">
    <xacro:include filename="base.xacro" />
    <xacro:include filename="car.xacro" />
    <xacro:include filename="camera.xacro" />
    <xacro:include filename="laser.xacro" />
    <xacro:include filename="car_move.xacro" />
    <xacro:include filename="ctrl_camera.xacro" />
    <xacro:include filename="ctrl_laser.xacro" />
    <xacro:include filename="kinect.xacro" />
</robot>

小车的物理属性都创建好了之后就可以让他按照一定的物理属性运动啦

<robot name="my_car_move" xmlns:xacro="http://wiki.ros.org/xacro">

    <!-- 传动实现:用于连接控制器与关节 -->
    <xacro:macro name="joint_trans" params="joint_name">
        <!-- Transmission is important to link the joints and the controller -->
        <transmission name="${joint_name}_trans">
            <type>transmission_interface/SimpleTransmission</type>
            <joint name="${joint_name}">
                <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
            </joint>
            <actuator name="${joint_name}_motor">
                <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
                <mechanicalReduction>1</mechanicalReduction>
            </actuator>
        </transmission>
    </xacro:macro>

    <!-- 每一个驱动轮都需要配置传动装置 -->
    <xacro:joint_trans joint_name="left_wheel2base_link" />
    <xacro:joint_trans joint_name="right_wheel2base_link" />

    <!-- 控制器 -->
    <gazebo>
        <plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
            <rosDebugLevel>Debug</rosDebugLevel>
            <publishWheelTF>true</publishWheelTF>
            <robotNamespace>/</robotNamespace>
            <publishTf>1</publishTf>
            <publishWheelJointState>true</publishWheelJointState>
            <alwaysOn>true</alwaysOn>
            <updateRate>100.0</updateRate>
            <legacyMode>true</legacyMode>
            <leftJoint>left_wheel2base_link</leftJoint> <!-- 左轮 -->
            <rightJoint>right_wheel2base_link</rightJoint> <!-- 右轮 -->
            <wheelSeparation>${base_link_radius * 2}</wheelSeparation> <!-- 车轮间距 -->
            <wheelDiameter>${wheel_radius * 2}</wheelDiameter> <!-- 车轮直径 -->
            <broadcastTF>1</broadcastTF>
            <wheelTorque>30</wheelTorque>
            <wheelAcceleration>1.8</wheelAcceleration>
            <commandTopic>/cmd_vel</commandTopic> <!-- 运动控制话题 -->
            <odometryFrame>odom</odometryFrame> 
            <odometryTopic>/odom</odometryTopic> <!-- 里程计话题 -->
            <robotBaseFrame>base_footprint</robotBaseFrame> <!-- 根坐标系 -->
        </plugin>
    </gazebo>

</robot>

具体的API也可以仿照官网Wiki的文件自己配置，配置过程无非就是把调用的借口放在一个实现方式的里面，再加上参数就可以啦。

emm，URDF的文件找不到在哪了，可能是之前重装系统的时候删掉了，毕竟现在展示的代码都是在那之后又自己写的。。所以说有好多都没有加注释。。。

其实配置仿真大部分都用的是官方的接口，所以说难度不大，要说有难度的还得是自己写适配于自己的设备的代码，到时候不仅要运动学分析，正逆解算还得学矩阵论相关，坐标变换等，这都是我入门阶段所了解到的一些内容，具体应用到实际的话肯定不止我纸面上写的这么一点而已，所以说学无止境。

配置查分小车的launch文件

<launch>
    <node pkg="amcl" type="amcl" name="amcl" output="screen">
        <!-- Publish scans from best pose at a max of 10 Hz -->
        <param name="odom_model_type" value="diff"/><!-- 里程计模式为差分 -->
        <param name="odom_alpha5" value="0.1"/>
        <param name="transform_tolerance" value="0.2" />
        <param name="gui_publish_rate" value="10.0"/>
        <param name="laser_max_beams" value="30"/>
        <param name="min_particles" value="500"/>
        <param name="max_particles" value="5000"/>
        <param name="kld_err" value="0.05"/>
        <param name="kld_z" value="0.99"/>
        <param name="odom_alpha1" value="0.2"/>
        <param name="odom_alpha2" value="0.2"/>
        <!-- translation std dev, m -->
        <param name="odom_alpha3" value="0.8"/>
        <param name="odom_alpha4" value="0.2"/>
        <param name="laser_z_hit" value="0.5"/>
        <param name="laser_z_short" value="0.05"/>
        <param name="laser_z_max" value="0.05"/>
        <param name="laser_z_rand" value="0.5"/>
        <param name="laser_sigma_hit" value="0.2"/>
        <param name="laser_lambda_short" value="0.1"/>
        <param name="laser_lambda_short" value="0.1"/>
        <param name="laser_model_type" value="likelihood_field"/>
        <!-- <param name="laser_model_type" value="beam"/> -->
        <param name="laser_likelihood_max_dist" value="2.0"/>
        <param name="update_min_d" value="0.2"/>
        <param name="update_min_a" value="0.5"/>
        
        <!-- 移植只需修改参数即可 -->
        <param name="odom_frame_id" value="odom"/><!-- 里程计坐标系 -->
        <param name="base_frame_id" value="base_footprint"/><!-- 添加机器人基坐标系 -->
        <param name="global_frame_id" value="map"/><!-- 添加地图坐标系 -->

        <param name="resample_interval" value="1"/>
        <param name="transform_tolerance" value="0.1"/>
        <param name="recovery_alpha_slow" value="0.0"/>
        <param name="recovery_alpha_fast" value="0.0"/>
    </node>
</launch>

然后我就按照我文件的默认位置写啦，具体的注释都有讲解，该怎么配置也都写得很详细

<launch>
<!--是否设置为仿真，在仿真环境里面使用  true  -->
<param name="use_sim_time" value="true"/>
    <!-- gampping 节点 -->
    <include file="$(find my_navigation)/launch/gampping.launch" />
    <!-- <node pkg="rviz" type="rviz" name="rviz" /> -->
    <!-- 可以保存 rviz 配置并后期直接使用-->
    
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find my_navigation)/rviz/demo_amcl.rviz"/>
   
    <!-- 将 Urdf 文件的内容加载到参数服务器 -->
    
    <param name="robot_description" command="$(find xacro)/xacro $(find my_gazebo)/xacro/connect.xacro" />
    <!-- 启动 gazebo -->
    <include file="$(find gazebo_ros)/launch/empty_world.launch" />
    
    <!-- <include file="$(find gazebo_ros)/launch/empty_world.launch">
       <arg name="world_name" value="$(find my_gazebo)/worlds/1.world" />
    </include> -->
    <!-- 在 gazebo 中显示机器人模型 -->
    <node pkg="gazebo_ros" type="spawn_model" name="model" args="-urdf -model mycar -param robot_description"  />
</launch>

上面的是进行AMCL的代码，接下来是跑gmapping的代码

<launch>
  <!--是否设置为仿真，在仿真环境里面使用  true  -->
  <param name="use_sim_time" value="true"/>
    <include file="$(find my_navigation)/launch/gampping.launch" />
    <!-- <node pkg="rviz" type="rviz" name="rviz" /> -->
    <!-- 可以保存 rviz 配置并后期直接使用-->
    
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find my_navigation)/rviz/demo1.rviz"/>
   
    <!-- 将 Urdf 文件的内容加载到参数服务器 -->
    
    <param name="robot_description" command="$(find xacro)/xacro $(find my_gazebo)/xacro/connect.xacro" />
    <!-- 启动 gazebo -->
    <include file="$(find gazebo_ros)/launch/empty_world.launch" />
    
    <!-- <include file="$(find gazebo_ros)/launch/empty_world.launch">
       <arg name="world_name" value="$(find my_gazebo)/worlds/1.world" />
    </include> -->
    <!-- 在 gazebo 中显示机器人模型 -->
    <node pkg="gazebo_ros" type="spawn_model" name="model" args="-urdf -model mycar -param robot_description"  />
</launch>

需要配合gmapping的配置

<launch>
    <!-- gampping 节点 -->
    <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
      <!-- 设置雷达话题 -->
      <remap from="scan" to="scan"/>
      <!-- 关键参数  三个坐标系 -->
      <param name="base_frame" value="base_footprint"/><!--底盘坐标系 设置为基坐标系名字 在这里是 base_footprint -->
      <param name="odom_frame" value="odom"/> <!--里程计坐标系-->
      <param name="map_update_interval" value="0.5"/>
      <!-- 其他参数一般设置为默认即可（下面为默认参数值） -->
      <param name="maxUrange" value="16.0"/>
      <param name="sigma" value="0.05"/>
      <param name="kernelSize" value="1"/>
      <param name="lstep" value="0.05"/>
      <param name="astep" value="0.05"/>
      <param name="iterations" value="5"/>
      <param name="lsigma" value="0.075"/>
      <param name="ogain" value="3.0"/>
      <param name="lskip" value="0"/>
      <param name="srr" value="0.1"/>
      <param name="srt" value="0.2"/>
      <param name="str" value="0.1"/>
      <param name="stt" value="0.2"/>
      <param name="linearUpdate" value="1.0"/>
      <param name="angularUpdate" value="0.5"/>
      <param name="temporalUpdate" value="3.0"/>
      <param name="resampleThreshold" value="0.5"/>
      <param name="particles" value="30"/>
      <param name="xmin" value="-50.0"/>
      <param name="ymin" value="-50.0"/>
      <param name="xmax" value="50.0"/>
      <param name="ymax" value="50.0"/>
      <param name="delta" value="0.05"/>
      <param name="llsamplerange" value="0.01"/>
      <param name="llsamplestep" value="0.01"/>
      <param name="lasamplerange" value="0.005"/>
      <param name="lasamplestep" value="0.005"/>
    </node>

    <node pkg="joint_state_publisher" name="joint_state_publisher" type="joint_state_publisher" />
    <node pkg="robot_state_publisher" name="robot_state_publisher" type="robot_state_publisher" />
</launch>

建图操作和保存地图操作

<launch>
    <!-- 设置地图的配置文件 -->
    <arg name="map" default="nav.yaml" />
    <!-- 运行地图服务器，并且加载设置的地图-->
    <node name="map_server" pkg="map_server" type="map_server" args="$(find my_navigation)/map/$(arg map)"/>
</launch>

然后是保存操作

<launch>
    <arg name="filename" value="$(find my_navigation)/map/nav" />
    <node name="map_save" pkg="map_server" type="map_saver" args="-f $(arg filename)" />
</launch>

move_base相关代码

<launch>

    <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true">
        <rosparam file="$(find my_navigation)/param/costmap_common_params.yaml" command="load" ns="global_costmap" />
        <rosparam file="$(find my_navigation)/param/costmap_common_params.yaml" command="load" ns="local_costmap" />
        <rosparam file="$(find my_navigation)/param/local_costmap_params.yaml" command="load" />
        <rosparam file="$(find my_navigation)/param/global_costmap_params.yaml" command="load" />
        <rosparam file="$(find my_navigation)/param/base_local_planner_params.yaml" command="load" />
    </node>

</launch>

最后一步是跑整个建图之后的导航的代码

<launch>
    <!-- 设置地图的配置文件 -->
    <arg name="map" default="nav.yaml" />
    <!-- 运行地图服务器，并且加载设置的地图-->
    <node name="map_server" pkg="map_server" type="map_server" args="$(find roscar_navigation)/map/$(arg map)"/>
    
    <!-- 启动SLAM -->
    <!-- <include file="$(find roscar_navigation)/launch/gmapping.launch" /> -->
    
    <!-- 启动AMCL节点 二维地图定位 -->
    <include file="$(find roscar_navigation)/launch/amcl_diff.launch" />
    <!-- 使用官方虚拟定位兼容AMCL输出 -->
    <node pkg="fake_localization" type="fake_localization" name="fake_localization" output="screen" />

    <!-- 运行move_base节点 -->
    <include file="$(find roscar_navigation)/launch/move_base.launch" />

    <node pkg="joint_state_publisher" name="joint_state_publisher" type="joint_state_publisher" />
    <!-- <node pkg="robot_state_publisher" name="robot_state_publisher" type="robot_state_publisher" /> -->
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"  output="screen" >
        <param name="publish_frequency" type="double" value="50.0" />
    </node>

    <!-- <node pkg="rviz" type="rviz" name="rviz" /> -->
    <!-- 可以保存 rviz 配置并后期直接使用--> 
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find roscar_navigation)/rviz/demo_self_move.rviz"/>  
    <!-- 将 Urdf 文件的内容加载到参数服务器 -->
    
    <param name="robot_description" command="$(find xacro)/xacro $(find roscar_gazebo)/xacro/connect.xacro" />
    <!-- 启动 gazebo -->
    <include file="$(find gazebo_ros)/launch/empty_world.launch" />
    
    <!-- <include file="$(find gazebo_ros)/launch/empty_world.launch">
       <arg name="world_name" value="$(find roscar_gazebo)/worlds/roscar_demo.world" />
    </include> -->
    <!-- 在 gazebo 中显示机器人模型 -->
    <node pkg="gazebo_ros" type="spawn_model" name="model" args="-urdf -model mycar -param robot_description"  />

    <!-- 对于虚拟定位，需要设置一个/odom与/map之间的静态坐标变换  前六个参数分别是xyz和rpy,10代表10ms刷新一次-->
    <node pkg="tf" type="static_transform_publisher" name="map_odom_broadcaster" args="0 0 0 0 0 0 /map /odom 10" />

</launch>

具体效果如图

写在最后：以上代码的代码文件可以问我要，具体可以实现导航的在roscar_navigation里面launch文件的self_move.launch 中，其他补充的话还需要完善

未完待续。。。。。。