ROS前驱前转小车仿真（2D）项目

一.官方网址

注：这些网址的服务器都是在国外，访问比较慢是正常情况。

1.ROS官网

https://www.ros.org/

2.urdf-模型

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

http://wiki.ros.org/xacro

3.rviz-数据可视化

http://wiki.ros.org/rviz

http://wiki.ros.org/rqt

4.gazebo-仿真环境

http://wiki.ros.org/gazebo

有些情况下，gazebo后台关闭不彻底，需要用命令行强制关闭

killall gzserver
killall gzclient

5.gmapping-建图

http://wiki.ros.org/gmapping

6.navigation-导航

http://wiki.ros.org/navigation

二.文件框架

ys001_ws         // 工作空间 （work space），用来存放项目的相关文件
├── build		 // 编译空间（Build Space），用来存储工作空间编译过程中产生的缓存信息和中间文件
├── devel		 // 开发空间（Development Space），用来放置编译生成的可执行文件。
└── src          // 代码空间（Source Space），开发过程中最常用的文件夹，用来存储所有ROS功能包的源码文件。
    ├── CMakeLists.txt -> /opt/ros/noetic/share/catkin/cmake/toplevel.cmake
    └── model   // 包名 （Package） 功能包(ROS基本单元)包含多个节点、库与配置文件，包名所有字母小写，只能由字母、数字与下划线组成
        ├── CMakeLists.txt  // 包的配置文件 配置编译规则，比如源文件、依赖项、目标文件
        ├── package.xml  //包的信息 比如:包名、版本、作者、依赖项...(以前版本是 manifest.xml)
        ├── config  //模型的配置信息 
        │   └── my_car.rviz  //rviz后缀为 rviz插件保存的配置文件，下次打开rviz可以读取这个配置好的文件。
        ├── debug.py  // 调式用的脚本，其他正常的项目，不一定与这个文件，自己定义的。
    
        ├── launch    // 启动的脚本文件 可一次性运行多个节点 
        │   ├── move_base_amcl.launch  //定位用的启动文件
        │   ├── move_base.launch	   //导航的启动文件
        │   ├── slam_gmapping.launch   //建图的启动文件
        │   ├── tele_key.launch		   //键盘控制的启动文件
        │   ├── tricycle_control.launch//模型控制的启动文件
        │   └── tricycle.launch		   //rviz和gazebo的模型启动文件
    
        ├── map							//存放扫好的地图文件
        │   ├── my_01_map.pgm			//图片格式的地图文件 与 同名的 .yaml 配合使用
        │   ├── my_01_map.yaml			//地图的扫描文件 与同名的 .pgm配合使用
        │   ├── my_02_map.pgm
        │   └── my_02_map.yaml
    
        ├── param                                //控制模型，导航算法的参数设置文件
        │   ├── ackermann_control.yaml		     // 控制关节的配置信息
        │   ├── ackermann_teleop_param.yaml      // 手动控制的速度和角度配置信息
        │   ├── costmap_common_params.yaml       // 代价地图的公共参数配置
        │   ├── global_costmap_params.yaml       // 全局地图的参数配置
        │   ├── local_costmap_params.yaml        // 局部地图的参数配置
        │   ├── move_base_params.yaml            // 底盘控制的参数配置
        │   └── teb_local_planner_params.yaml    // 路径规划的参数配置
    
        ├── scripts                             //存放python的脚本文件
        │   ├── ackermann_teleop.py             //键盘wasd控制的脚本
        │   ├── servo_commands.py              //解析键盘控制的控制信息脚本
        │   └── transform.py					//手动控制是定义cml_vel的节点
        │   ├── gazebo_odometry.py				//gazebo里的里程计计算脚本
        │   ├── send_goals_A.py					//发生目标点的脚本,下同
        │   ├── send_goals_B.py
        │   ├── send_goals_C.py
        │   ├── send_goals_D.py
        │   ├── send_goals_origin.py
        │   ├── send_goals_work.py
    
        ├── src									//存放CPP源文件
    
        ├── urdf								//存放描述模型的文件
        │   ├── ackermann.xacro                 //阿克曼模型类的轮子传动配置
        │   ├── common_properties.xacro			//rviz里模型的颜色
        │   ├── xju_tricycle_model.gazebo.xacro //gazebo仿真传感器配置文件
        │   └── xju_tricycle_model.urdf.xacro   // 模型文件
    
        └── worlds								//存放gazebo的世界模型文件
            └── simple.world

三.启动顺序

0.依赖包的安装

在正式运行之前，务必安装下面这些包，手动安装一条一条的安装，或者配置sh文件来批量安装，可以参考下面的配置来操作;

sudo apt-get install ros-noetic-ackermann-msgs
sudo apt-get install ros-noetic-navigation
sudo apt-get install ros-noetic-openslam-gmapping
sudo apt-get install ros-noetic-geographic-info
sudo apt-get install ros-noetic-controller-manager
sudo apt-get install ros-noetic-gazebo-ros-control
sudo apt-get install ros-noetic-effort-controllers
sudo apt-get install ros-noetic-joint-state-controller
sudo apt-get install ros-noetic-position-controllers
sudo apt-get install ros-noetic-teb-local-planner
sudo apt-get install ros-noetic-gmapping

配置批量安装：

用记事本新建一文件，保存时后缀名为“.sh”,这里为“install.sh”，其内容如下：

在.sh文件所在目录打开终端，输入：sh install.sh

#!/bin/sh
sudo apt-get -y install ros-noetic-ackermann-msgs
sudo apt-get -y install ros-noetic-navigation
sudo apt-get -y install ros-noetic-openslam-gmapping
sudo apt-get -y install ros-noetic-geographic-info
sudo apt-get -y install ros-noetic-controller-manager
sudo apt-get -y install ros-noetic-gazebo-ros-control
sudo apt-get -y install ros-noetic-effort-controllers
sudo apt-get -y install ros-noetic-joint-state-controller
sudo apt-get -y install ros-noetic-position-controllers
sudo apt-get -y install ros-noetic-teb-local-planner
sudo apt-get -y install ros-noetic-gmapping

1.手动控制的启动顺序

步骤1：启动模型在gazebo和rviz的显示

roslaunch model tricycle.launch

步骤2：启动键盘控制的节点

roslaunch model tele_key.launch

2.建图的启动顺序

步骤1：启动模型在gazebo和rviz的显示

roslaunch model tricycle.launch

步骤2：启动建图的节点

roslaunch model slam_gmapping.launch

步骤3：启动保存地图的节点

注意后面的路径要改成自己的路径，这里的地图名为“my_0_map”，保存之后会在目标的目录下生成两个文件 .pgm .yaml 文件

rosrun map_server map_saver -f /home/z/ros_ws/ys001_ws/src/model/map/my_01_map

3.导航的启动顺序

步骤1：启动模型在gazebo和rviz的显示

roslaunch model tricycle.launch

步骤2：启动导航的节点

roslaunch model move_base.launch

四.urdf-模型文件

1.ackermann.xacro-轮子传动的配置

给仿真环境配置哪些轮子需要控制，为后面控制的仿真做好硬件关节的仿真。

参考 https://blog.csdn.net/qq_23670601/article/details/88741802

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

  
  <xacro:macro name="wheel_transmission" params="name">
    <transmission name="${name}_transmission" type="SimpleTransmission">
      <type>transmission_interface/SimpleTransmissiontype>
      <joint name="${name}_joint">
        <hardwareInterface>hardware_interface/EffortJointInterfacehardwareInterface>
      joint>
      <actuator name="${name}_motor">
        <hardwareInterface>hardware_interface/EffortJointInterfacehardwareInterface>
        <mechanicalReduction>1mechanicalReduction>
      actuator>
    transmission>
  xacro:macro>

  
  <xacro:macro name="steering_hinge_transmission" params="name">
    <transmission name="${name}_transmission" type="SimpleTransmission">
      <type>transmission_interface/SimpleTransmissiontype>
      <joint name="${name}_joint">
        <hardwareInterface>hardware_interface/EffortJointInterfacehardwareInterface>
      joint>
      <actuator name="${name}_motor">
        <hardwareInterface>hardware_interface/EffortJointInterfacehardwareInterface>
        <mechanicalReduction>1mechanicalReduction>
      actuator>
    transmission>
  xacro:macro>

robot>

具体解释：

（必须）唯一指定了一个传动的标签，名字自己定义，方便起见可以使用与joint name相同的名字。

（出现一次）定义了传动的类型，这个细节说明还没找到，用transmission_interface/SimpleTransmission就可以啦。

（可定义一个或多个）指定这个传动所依赖的关节，拥有如下标签hardwareInterface。

（在joint下，可定义一个或多个）指定支持的硬件接口空间。用于结合控制器使用硬件接口来向硬件接口发送和接受指令，请注意：
当在RobotHW中加载此transmission时，此标签的值应为hardware_interface / XXX。
在Gazebo中加载此transmission时，此标记的值应为XXX 。
（定义一个或多个）传动连接的致动器，名字自己去定义，拥有如下标签mechanicalReduction及hardwareInterface。

（可选）定义电机／关节减速比。
（可选,只有Indigo及以前版本的在这里指定，目前版本已经移到joint标签下）指定支持的硬件接口空间

2.common_properties.xacro-rviz里模型颜色

这里的颜色设定是给rviz里显示用的，gazebo里的颜色和rviz的颜色是分开的，两者不是同一个软件插件。

rgba 表示 红色 绿色 蓝色 透明度，颜色占比用0.0-1.0表示，透明度也是如此。

<robot name="xacro_properties" xmlns:xacro="http://ros.org/wiki/xacro">
    
  <material name="black">
      <color rgba="0.0 0.0 0.0 1.0"/>
  material>

  <material name="dark">
    <color rgba="0.3 0.3 0.3 1.0"/>
  material>
  <material name="light_black">
    <color rgba="0.4 0.4 0.4 1.0"/>
  material>

  <material name="blue">
    <color rgba="0.0 0.0 0.8 1.0"/>
  material>

  <material name="green">
    <color rgba="0.0 0.8 0.0 1.0"/>
  material>

  <material name="grey">
    <color rgba="0.5 0.5 0.5 1.0"/>
  material>

  <material name="orange">
    <color rgba="${255/255} ${108/255} ${10/255} 1.0"/>
  material>

  <material name="brown">
    <color rgba="${222/255} ${207/255} ${195/255} 1.0"/>
  material>

  <material name="red">
    <color rgba="0.8 0.0 0.0 1.0"/>
  material>

  <material name="white">
    <color rgba="1.0 1.0 1.0 1.0"/>
  material>
robot>

3.xju_tricycle_model.gazebo.xacro-gazebo配置

1.设置模型在gazebo里面的颜色，参考：https://blog.csdn.net/qq_42226250/article/details/110881207

2.设置轮子的摩擦力和刚度系数，参考：https://blog.csdn.net/qq_27865227/article/details/125001773

3.设置gazebo里的传感器插件（plugin），参考：http://admin.guyuehome.com/36462

注意：实际中传感器数据来源于传感器厂商提供的驱动和配到的发布节点。

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

  <xacro:arg name="laser_visual" default="true"/>
  <xacro:arg name="imu_visual"   default="false"/>

  
  <gazebo reference="base_link">
    <material>Gazebo/Orangematerial>
  gazebo>

  <gazebo reference="wash_link">
    <material>Gazebo/Bluematerial>
  gazebo>

  <gazebo reference="pic_link">
    <material>Gazebo/facematerial>
  gazebo>

  <gazebo reference="front_steering_hinge">
    <material>Gazebo/Blackmaterial>
  gazebo>

  <gazebo reference="front_wheel">
    <material>Gazebo/Blackmaterial>
  gazebo>

  
  <gazebo reference="left_rear_wheel">
    <mu1 value="2.0"/>
    <mu2 value="2.0"/>
    <kp  value="10000000.0" />
    <kd  value="1.0" />
    <fdir1 value="1 0 0"/>
    <material>Gazebo/Blackmaterial>
  gazebo>

  <gazebo reference="right_rear_wheel">
    <mu1 value="2.0"/>
    <mu2 value="2.0"/>
    <kp  value="10000000.0" />
    <kd  value="1.0" />
    <fdir1 value="1 0 0"/>
    <material>Gazebo/Blackmaterial>
  gazebo>

  
  <gazebo reference="imu_link">
    <sensor type="imu" name="imu">
      <always_on>truealways_on>
      <visualize>$(arg imu_visual)visualize>
    sensor>
    <material>Gazebo/FlatBlackmaterial>
  gazebo>

  <gazebo>
    <plugin name="imu_plugin" filename="libgazebo_ros_imu.so">
      <alwaysOn>truealwaysOn>
      <bodyName>imu_linkbodyName>
      <frameName>imu_linkframeName>
      <topicName>imutopicName>
      <serviceName>imu_serviceserviceName>
      <gaussianNoise>0.0gaussianNoise>
      <updateRate>50updateRate>
      <imu>
        <noise>
          <type>gaussiantype>
          <rate>
            <mean>0.0mean>
            <stddev>2e-4stddev>
            <bias_mean>0.0000075bias_mean>
            <bias_stddev>0.0000008bias_stddev>
          rate>
          <accel>
            <mean>0.0mean>
            <stddev>1.7e-2stddev>
            <bias_mean>0.01bias_mean>
            <bias_stddev>0.001bias_stddev>
          accel>
        noise>
      imu>
    plugin>
  gazebo>

  
  <gazebo reference="laser_link">
    <material>Gazebo/Redmaterial>
    <sensor type="ray" name="lds_lfcd_sensor">
      <pose>0 0 0 0 0 0pose>
      <visualize>$(arg laser_visual)visualize>
      <update_rate>10update_rate>
      <ray>
        <scan>
          <horizontal>
            <samples>1800samples>
            <resolution>1resolution>
            <min_angle>-1.57079637min_angle>
            <max_angle>3.1415926max_angle>
          horizontal>
        scan>
        <range>
          <min>0.001min>
          <max>20max>
          <resolution>0.01resolution>
        range>
        <noise>
          <type>gaussiantype>
          <mean>0.0mean>
          <stddev>0.01stddev>
        noise>
      ray>
      <plugin name="gazebo_ros_lds_lfcd_controller" filename="libgazebo_ros_laser.so">
        <topicName>scantopicName>
        <frameName>laser_linkframeName>
      plugin>
    sensor>
  gazebo>

  
  <gazebo>
    <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
      <robotNamespace>/xjurobotNamespace>
      <robotParam>robot_descriptionrobotParam>
      <robotSimType>gazebo_ros_control/DefaultRobotHWSimrobotSimType>
      <legacyModeNS>truelegacyModeNS>
    plugin>
  gazebo>

robot>

3.xju_tricycle_model.urdf.xacro-模型文件

1.建立三维模型

2.建立各个部件的坐标关系，即tf关系

参考：https://blog.csdn.net/Kalenee/article/details/86485565

<robot
  name="xju" xmlns:xacro="http://ros.org/wiki/xacro">
  
  <xacro:include filename="$(find model)/urdf/ackermann.xacro"/>
  <xacro:include filename="$(find model)/urdf/xju_tricycle_model.gazebo.xacro"/>
  <xacro:include filename="$(find model)/urdf/common_properties.xacro"/>
  

  
  <xacro:property name="base_m" value="10" /> 
  <xacro:property name="base_l" value="1.2" />
  <xacro:property name="base_w" value="1" />
  <xacro:property name="base_h" value="0.8" />

  
  <xacro:property name="wheel_r" value="0.1" />
  <xacro:property name="wheel_l" value="0.08" />

  
  <xacro:property name="real_x" value="-0.4" />
  <xacro:property name="real_r_y" value="-0.5" />
  <xacro:property name="real_l_y" value="0.5" />

  
  <xacro:property name="wheel_z" value="${-(base_h/2)}" />

  
  <xacro:property name="effort" value="30" />
  <xacro:property name="velocity" value="50" />

  
  <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>

  
  <link name="base_footprint"/>

  <joint name="base_footprint_to_base_link" type="fixed">
    <parent link="base_footprint"/>
    <child link="base_link"/>
    <origin xyz="0 0 ${(base_h/2)+wheel_r}" />
  joint>

  <link name="base_link">
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <box size="${base_l} ${base_w} ${base_h}" />
      geometry>
      <material name="orange" />
    visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <box size="${base_l} ${base_w} ${base_h}" />
      geometry>
    collision>
    <xacro:Box_inertial_matrix m="${base_m}" l="${base_l}" w="${base_w}" h="${base_h}" />
  link>

  
  <link name="wash_link">
    <inertial>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <mass value="0.5" />
      <inertia ixx="1.35E-05" ixy="0" ixz="0" iyy="2.5E-05" iyz="0" izz="1.35E-05" />
    inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <cylinder length="3" radius="0.5" />
      geometry>
      <material name="green" />
    visual>
    <collision>
      <origin xyz="0 0 0" rpy="1.57 0 0" />
      <geometry>
        <cylinder length="0.01" radius="0.005" />
      geometry>
    collision>
  link>

  <joint name="wash_joint" type="fixed">
    <origin xyz="0 -0.5 ${wheel_z+1.5}" rpy="0 0 0" />
    <parent link="base_link" />
    <child link="wash_link" />
    <axis xyz="0 0 1" />
  joint>



   
  <link name="front_steering_hinge">
    <inertial>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <mass value="0.5" />
      <inertia ixx="1.35E-05" ixy="0" ixz="0" iyy="2.5E-05" iyz="0" izz="1.35E-05" />
    inertial>
    <visual>
      <origin xyz="0 0 0" rpy="1.57 0 0" />
      <geometry>
        <cylinder length="0.0001" radius="0.00001" />
      geometry>
      <material name="black" />
    visual>
    <collision>
      <origin xyz="0 0 0" rpy="1.57 0 0" />
      <geometry>
        <cylinder length="0.01" radius="0.005" />
      geometry>
    collision>
  link>

  <joint name="front_steering_hinge_joint" type="revolute">
    <origin xyz="0.6 0 ${wheel_z}" rpy="0 0 0" />
    <parent link="base_link" />
    <child link="front_steering_hinge" />
    <axis xyz="0 0 1" />
    
    
    <limit lower="-0.87266" upper="0.87266" effort="${effort}" velocity="${velocity}"/>
  joint>
  <xacro:steering_hinge_transmission name="front_steering_hinge" />

  
  <link name="front_wheel">
    <inertial>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <mass value="2" />
      <inertia ixx="0.002867" ixy="0" ixz="0" iyy="0.0049" iyz="0" izz="0.002867" />
    inertial>
    <visual>
      <origin xyz="0 0 0" rpy="1.57 0 0" />
      <geometry>
        <cylinder length="${wheel_l}" radius="${wheel_r}" />
      geometry>
      <material name="black" />
    visual>
    <collision>
      <origin xyz="0 0 0" rpy="1.57 0 0" />
      <geometry>
        <cylinder length="${wheel_l}" radius="${wheel_r}" />
      geometry>
    collision>
  link>

  <joint name="front_wheel_joint" type="continuous">
    <origin xyz="0 0 0" rpy="0 0 0" />
    <parent link="front_steering_hinge" />
    <child link="front_wheel" />
    <axis xyz="0 1 0" />
    <limit effort="${effort}" velocity="${velocity}" />
  joint>
  <xacro:wheel_transmission name="front_wheel" />


  
  <link name="left_rear_wheel">
    <inertial>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <mass value="2" />
      <inertia ixx="0.002867" ixy="0" ixz="0" iyy="0.0049" iyz="0" izz="0.002867" />
    inertial>
    <visual>
      <origin xyz="0 0 0" rpy="1.57 0 0" />
      <geometry>
        <cylinder length="${wheel_l}" radius="${wheel_r}" />
      geometry>
      <material name="black" />
    visual>
    <collision>
      <origin xyz="0 0 0" rpy="1.57 0 0" />
      <geometry>
        <cylinder length="${wheel_l}" radius="${wheel_r}" />
      geometry>
    collision>
  link>
  <joint name="left_rear_wheel_joint" type="continuous">
    <origin xyz="${real_x} ${real_l_y} ${wheel_z}" rpy="0 0 0" />
    <parent link="base_link" />
    <child link="left_rear_wheel" />
    <axis xyz="0 1 0" />
    <limit effort="0" velocity="1000" />
  joint>
  <xacro:wheel_transmission name="left_rear_wheel" />

  
  <link name="right_rear_wheel">
    <inertial>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <mass value="2" />
      <inertia ixx="0.002867" ixy="0" ixz="0" iyy="0.0049" iyz="0" izz="0.002867" />
    inertial>
    <visual>
      <origin xyz="0 0 0" rpy="1.57 0 0" />
      <geometry>
        <cylinder length="${wheel_l}" radius="${wheel_r}" />
      geometry>
      <material name="black" />
    visual>
    <collision>
      <origin xyz="0 0 0" rpy="1.57 0 0" />
      <geometry>
        <cylinder length="${wheel_l}" radius="${wheel_r}" />
      geometry>
    collision>
  link>
  <joint name="right_rear_wheel_joint" type="continuous">
    <origin xyz="${real_x} ${real_r_y} ${wheel_z}" rpy="0 0 0" />
    <parent link="base_link" />
    <child link="right_rear_wheel" />
    <axis xyz="0 1 0" />
    <limit effort="0" velocity="1000" />
  joint>
  <xacro:wheel_transmission name="right_rear_wheel" />

  
  <link name="laser_link">
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <cylinder length="0.04" radius="0.03" />
      geometry>
      <material name="red" />
    visual>

  link>
  <joint
    name="joint_top_ladar"
    type="fixed">
    <origin
      xyz="0.57 0.4 0.42"
      rpy="0 0 0" />
    <parent
      link="base_link" />
    <child
      link="laser_link" />
    <axis
      xyz="0 0 0" />
    <safety_controller
      k_velocity="0" />
  joint>

  
  <link name="imu_link"/>
  <joint
    name="joint_imu"
    type="fixed">
    <origin
      xyz="0 0 0"
      rpy="0 0 0" />
    <parent
      link="base_link" />
    <child
      link="imu_link" />
    <axis
      xyz="0 0 0" />
    <safety_controller
      k_velocity="0" />
  joint>

robot>

五.手动控制

1.tricycle.launch-启动模型文件节点

加载模型在gazeb和rviz显示

<launch>

  <arg name="x_pos" default="-7.0"/>
  <arg name="y_pos" default="-7.0"/>
  <arg name="z_pos" default="1"/>
  <arg name="gui" default="true"/>


  
  <param name="robot_description" command="$(find xacro)/xacro '$(find model)/urdf/xju_tricycle_model.urdf.xacro'"/>
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    
    <arg name="world_name" value="$(find model)/worlds/simple.world"/>
    <arg name="paused" value="false"/>
    <arg name="use_sim_time" value="true"/>
    <arg name="gui" value="$(arg gui)"/>
    <arg name="headless" value="false"/>
    <arg name="debug" value="false"/>
  include>

  
  <node pkg="gazebo_ros" type="spawn_model" name="spawn_urdf"
        args="-urdf -model xju -x $(arg x_pos) -y $(arg y_pos) -z $(arg z_pos) -param robot_description" />

  
  <include file="$(find model)/launch/tricycle_control.launch" />


  
  

  
  <node pkg="rviz" type="rviz" name="rviz" args="-d $(find model)/config/my_car.rviz"/>


launch>

2.tricycle_control.launch-关节控制节点

<launch>
   
  <rosparam file="$(find model)/param/ackermann_control.yaml" command="load"/>

   
  <node name="controller_manager" pkg="controller_manager" type="spawner" respawn="false"
     output="screen" ns="/xju" args="front_wheel_velocity_controller
                                     front_steering_hinge_position_controller
                                     joint_state_controller"/>
   
  <node name= "robot_state_publisher" pkg= "robot_state_publisher" type= "robot_state_publisher">
     <remap from="/joint_states" to="/xju/joint_states"/>
  node>

   
  <node pkg="model" type="servo_commands.py" name="servo_commands" output="screen"/>

   
  <node pkg="model" type="transform.py" name="transform" output="screen"/>

   
  <node pkg="model" name="gazebo_odometry_node" type="gazebo_odometry.py"/>
launch>

3.servo_commands.py-速度分配节点

#!/usr/bin/env python3
'''
    将速度分配给每一个驱动电机
'''
import rospy
from std_msgs.msg import Bool
from std_msgs.msg import Float32
from std_msgs.msg import Float64
from ackermann_msgs.msg import AckermannDriveStamped

flag_move = 0

def set_throttle_steer(data):

    global flag_move

    pub_vel_front_wheel = rospy.Publisher('/xju/front_wheel_velocity_controller/command', Float64, queue_size=1)

    pub_pos_front_steering_hinge = rospy.Publisher('/xju/front_steering_hinge_position_controller/command', Float64, queue_size=1)

    throttle = data.drive.speed*14.5
    steer = data.drive.steering_angle
    pub_vel_front_wheel.publish(throttle)
    pub_pos_front_steering_hinge.publish(steer)

def servo_commands():

    rospy.init_node('servo_commands', anonymous=True)

    rospy.Subscriber("/ackermann_cmd_mux/output", AckermannDriveStamped, set_throttle_steer, queue_size=1, buff_size=52428800, tcp_nodelay=True)

    # spin() simply keeps python from exiting until this node is stopped
    rospy.spin()

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

4.transform.py-速度转换节点

#!/usr/bin/env python3
'''
   将 cmd_vel 的速度信息转换为 小车需要的速度信息

'''
import rospy
import std_msgs.msg
from ackermann_msgs.msg import AckermannDriveStamped
from geometry_msgs.msg import Twist

import time
import threading
pub = rospy.Publisher("/ackermann_cmd_mux/output", AckermannDriveStamped,queue_size=1)

def thread_job():
    rospy.spin()

def callback(data):
    speed = data.linear.x
    turn = data.angular.z

    msg = AckermannDriveStamped();
    msg.header.stamp = rospy.Time.now();
    msg.header.frame_id = "base_link";

    msg.drive.speed = speed;
    msg.drive.acceleration = 1;
    msg.drive.jerk = 1;
    msg.drive.steering_angle = turn
    msg.drive.steering_angle_velocity = 1

    pub.publish(msg)

def SubscribeAndPublish():
    rospy.init_node('nav_sim', anonymous=True)
    rospy.Subscriber('cmd_vel', Twist, callback,queue_size=1,buff_size=52428800)
    #rospy.Subscriber('cmd_vel', Twist, callback,queue_size=1,buff_size=52428800)
    rospy.spin()


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

5.gazebo_odometry.py-里程计计算节点

实际中这个里程计改为实物的编码器传回来的数据

#!/usr/bin/env python3

'''
This script makes Gazebo less fail by translating gazebo status messages to odometry data.
Since Gazebo also publishes data faster than normal odom data, this script caps the update to 20hz.
Winter Guerra
从gazebo 仿真环境里获得里程计信息.
'''

import rospy
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Pose, Twist, Transform, TransformStamped
from gazebo_msgs.msg import LinkStates
from std_msgs.msg import Header
from ackermann_msgs.msg import AckermannDriveStamped
import numpy as np
import math
import tf
import tf2_ros

class OdometryNode:
    # Set publishers
    pub_odom = rospy.Publisher('/odom', Odometry, queue_size=1)

    def __init__(self):
        # init internals
        self.last_received_pose = Pose()
        self.last_received_twist = Twist()
        self.vel_direction = 1.
        self.last_recieved_stamp = None

        # Set the update rate
        rospy.Timer(rospy.Duration(.02), self.timer_callback) # 50hz

        self.tf_pub = tf2_ros.TransformBroadcaster()

        # Set subscribers 从link状态里获取
        rospy.Subscriber('/gazebo/link_states', LinkStates, self.sub_robot_pose_update, queue_size=1, buff_size=52428800, tcp_nodelay=True)
        rospy.Subscriber("/ackermann_cmd_mux/output", AckermannDriveStamped, self.get_vel_direction, queue_size=1, buff_size=52428800, tcp_nodelay=True)

    def sub_robot_pose_update(self, msg):
        # Find the index of the robot
        try:
            arrayIndex = msg.name.index('xju::base_footprint')
        except ValueError as e:
            # Wait for Gazebo to startup
            pass
        else:
            # Extract our current position information
            time_diff = 0.02
            if self.last_recieved_stamp is not None:
                time_diff = (rospy.Time.now() - self.last_recieved_stamp).to_sec()
            if time_diff == 0.:
                return
            self.last_received_twist.linear.x = self.vel_direction * math.sqrt((msg.pose[arrayIndex].position.x - self.last_received_pose.position.x)**2 + (msg.pose[arrayIndex].position.y - self.last_received_pose.position.y)**2) / time_diff
            (r1, p1, y1) = tf.transformations.euler_from_quaternion([msg.pose[arrayIndex].orientation.x, msg.pose[arrayIndex].orientation.y, msg.pose[arrayIndex].orientation.z, msg.pose[arrayIndex].orientation.w])
            (r2, p2, y2) = tf.transformations.euler_from_quaternion([self.last_received_pose.orientation.x, self.last_received_pose.orientation.y, self.last_received_pose.orientation.z, self.last_received_pose.orientation.w])
            self.last_received_twist.angular.z = (y1 - y2) / time_diff
            self.last_received_pose = msg.pose[arrayIndex]
            self.last_recieved_stamp = rospy.Time.now()

    def get_vel_direction(self, msg):
        if msg.drive.speed < 0:
            self.vel_direction = -1.
        else:
            self.vel_direction = 1.

    def timer_callback(self, event):
        if self.last_recieved_stamp is None:
            return

        cmd = Odometry()
        cmd.header.stamp = self.last_recieved_stamp
        cmd.header.frame_id = 'odom'
        cmd.child_frame_id = 'base_footprint'
        cmd.pose.pose = self.last_received_pose
        cmd.twist.twist = self.last_received_twist
        cmd.pose.covariance =[1e-3, 0, 0, 0, 0, 0,
                              0, 1e-3, 0, 0, 0, 0,
                              0, 0, 1e6, 0, 0, 0,
                              0, 0, 0, 1e6, 0, 0,
                              0, 0, 0, 0, 1e6, 0,
                              0, 0, 0, 0, 0, 1e3]

        cmd.twist.covariance = [1e-9, 0, 0, 0, 0, 0,
                                0, 1e-3, 1e-9, 0, 0, 0,
                                0, 0, 1e6, 0, 0, 0,
                                0, 0, 0, 1e6, 0, 0,
                                0, 0, 0, 0, 1e6, 0,
                                0, 0, 0, 0, 0, 1e-9]


        self.pub_odom.publish(cmd)

        tf = TransformStamped(
            header=Header(
                frame_id=cmd.header.frame_id,
                stamp=cmd.header.stamp
            ),
            child_frame_id=cmd.child_frame_id,
            transform=Transform(
                translation=cmd.pose.pose.position,
                rotation=cmd.pose.pose.orientation
            )
        )
        self.tf_pub.sendTransform(tf)

# Start the node
if __name__ == '__main__':
    rospy.init_node("gazebo_odometry_node")
    node = OdometryNode()
    rospy.spin()

6.tele_key.launch-键盘控制节点

<launch>

  
  <rosparam command="load" file="$(find model)/param/ackermann_teleop_param.yaml" />
  
  <node pkg="model" type="ackermann_teleop.py" name="ackermann_teleop" />
launch>

7.ackermann_teleop.py-手动控制界面节点

#!/usr/bin/env python3

# 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 ackermann_msgs.msg import AckermannDriveStamped

# 定义需要控制的键
UP = "w"
LEFT = "a"
DOWN = "s"
RIGHT = "d"
QUIT = "q"


state = [False, False, False, False]
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

    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 = True


def keydown(e):
    global state
    global control

    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(_):
    global control
    with state_lock:
        if not control:
            return
        ack = AckermannDriveStamped()
        if state[0]:
            ack.drive.speed = max_velocity
        elif state[2]:
            ack.drive.speed = -max_velocity

        if state[1]:
            ack.drive.steering_angle = max_steering_angle
        elif state[3]:
            ack.drive.steering_angle = -max_steering_angle

        if state_pub is not None:
            state_pub.publish(ack)
        control = sum(state) > 0


def exit_func():
    os.system("xset r on")


def shutdown():
    root.destroy()
    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.6)

    state_pub = rospy.Publisher(
        "/ackermann_cmd_mux/output", AckermannDriveStamped, queue_size=1
    )
    rospy.Timer(rospy.Duration(0.1), publish_cb)
    atexit.register(exit_func)
    os.system("xset r off")

    root = Tk()
    root.attributes("-topmost",1) # 窗口置顶
    frame = Frame(root, width=100, height=100)
    frame.bind("", keydown) # 监听事件
    frame.bind("", keyup)
    frame.pack()
    frame.focus_set()
    lab = Label(
        frame,
        height=10,
        width=30,
        text="聚焦窗口 wads控制  Q 退出",
    )
    lab.pack()
    print("Press %c to quit" % QUIT)
    root.mainloop()


if __name__ == "__main__":
    rospy.init_node("keyboard_teleop", disable_signals=True)

    signal.signal(signal.SIGINT, lambda s, f: shutdown())
    main()

六.建图-gmapping

在手动控制的基础上，添加建图节点；

参考：

https://www.bilibili.com/video/BV1mJ411R7Ni?p=37&vd_source=45f9a599303782222954b67591ac1ed9

http://wiki.ros.org/gmapping

https://blog.csdn.net/m0_63647490/article/details/123130882

https://blog.csdn.net/qq_42037180/article/details/100819788

1.slam_gmapping.launch-建图节点

<launch>
    
    <arg name="scan_topic"  default="scan" /> 
    <arg name="base_frame"  default="base_footprint"/>
    <arg name="odom_frame"  default="odom"/>
    <param name="use_sim_time" value="false"/>

    
    
    <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">

      <param name="base_frame" value="$(arg base_frame)"/>   
     <param name="odom_frame" value="$(arg odom_frame)"/>         
      
      <param name="map_update_interval" value="1.0"/>  
      <param name="maxUrange" value="20"/>  
      <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.01"/>
      <param name="srt" value="0.02"/>
      <param name="str" value="0.01"/>
      <param name="stt" value="0.02"/>
      <param name="linearUpdate" value="0.5"/> 
      <param name="angularUpdate" value="0.218"/> 
      <param name="temporalUpdate" value="5.0"/>
      <param name="resampleThreshold" value="0.5"/>
      <param name="particles" value="80"/> 
      <param name="xmin" value="-1.0"/>
      <param name="ymin" value="-1.0"/>
      <param name="xmax" value="1.0"/>
      <param name="ymax" value="1.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>
launch>

七.导航-navigation

在手动控制的基础上，扫好图的基础上，添加导航节点；

参考：

http://wiki.ros.org/navigation#Basic_ROS_Navigation_Tutorials

http://www.autolabor.com.cn/book/ROSTutorials/di-7-zhang-ji-qi-ren-dao-822a28-fang-771f29/72-dao-hang-shi-xian/724-dao-hang-shi-xian-04-lu-jing-gui-hua.html

https://blog.csdn.net/majingming123/article/details/128236979

下面的五个 “.yaml”文件是导航参数的配置（关键），需要先准备好

可能会出现机器人在本地路径规划时与全局路径规划不符而进入膨胀区域出现假死的情况，如何尽量避免这种情形呢？

全局路径规划与本地路径规划虽然设置的参数是一样的，但是二者路径规划和避障的职能不同，可以采用不同的参数设置策略:

• 全局代价地图可以将膨胀半径和障碍物系数设置的偏大一些；
• 本地代价地图可以将膨胀半径和障碍物系数设置的偏小一些。

这样，在全局路径规划时，规划的路径会尽量远离障碍物，而本地路径规划时，机器人即便偏离全局路径也会和障碍物之间保留更大的自由空间，从而避免了陷入“假死”的情形。

1.costmap_common_params.yaml-局路径规划与本地路径规划通用参数

包括:机器人的尺寸、距离障碍物的安全距离、传感器信息等。配置参考如下:

obstacle_range: 2.5 # 用于障碍物探测，比如: 值为 3.0，意味着检测到距离小于 3 米的障碍物时，就会引入代价地图，才会被识别到
raytrace_range: 3.0 #用于清除障碍物，比如：值为 3.5，意味着清除代价地图中 3.5 米以外的障碍物，只有在这个范围内不存在的才会被消除

#机器人几何参，如果机器人是圆形，设置 robot_radius,如果是其他形状设置 footprint
# footprint: [[-0.08, -0.15], [-0.08, 0.15],[0.42, 0.15], [0.42, -0.15]]
robot_radius: 0.2

#膨胀半径，扩展在碰撞区域以外的代价区域，使得机器人规划路径避开障碍物
inflation_radius: 0.2 #膨胀半径

transform_tolerance: 0.5

#导航包所需要的传感器
observation_sources: scan

#对传感器的坐标系和数据进行配置
scan: 
  data_type: LaserScan 
  topic: scan 
  marking: true 
  clearing: true

# 地图类型
map_type: costmap

2.global_costmap_params.yaml-全局规划地图的参数

global_costmap:
  global_frame: map #地图坐标系
  robot_base_frame: base_footprint #机器人坐标系
  # 以此实现坐标变换

  update_frequency: 1.0 #代价地图更新频率
  publish_frequency: 0.5 #代价地图的发布频率
  static_map: true       # 是否使用一个地图或者地图服务器来初始化全局代价地图
  transform_tolerance: 0.5 #等待坐标变换发布信息的超时时间
  cost_scaling_factor: 10.0
  inflation_radius: 0.25  

3.local_costmap_params.yaml-局部规划地图的参数

local_costmap:
  global_frame: map  #里程计坐标系
  robot_base_frame: base_footprint  #机器人坐标系
  update_frequency: 5.0 #代价地图更新频率
  publish_frequency: 2.0 #代价地图的发布频率
  static_map: false #不需要静态地图，可以提升导航效果
  rolling_window: true #是否使用动态窗口，默认为false，在静态的全局地图中，地图不会变化
  width: 5 # 局部地图宽度 单位是 m
  height: 5 # 局部地图高度 单位是 m
  resolution: 0.05 # 局部地图分辨率 单位是 m，一般与静态地图分辨率保持一致
  transform_tolerance: 0.5
  cost_scaling_factor: 5
  inflation_radius: 0.25

4.move_base_params.yaml-底盘控制的参数配置

shutdown_costmaps: false #当move_base在不活动状态时,是否关掉costmap

controller_frequency: 5.0 #向底盘控制移动话题cmd_vel发送命令的频率.
controller_patience: 3.0 #在空间清理操作执行前,控制器花多长时间等有效控制下发

planner_frequency: 0.5 #全局规划操作的执行频率.如果设置为0.0,则全局规划器仅
                        #在接收到新的目标点或者局部规划器报告路径堵塞时才会重新执行规划操作
planner_patience: 5.0  #在空间清理操作执行前,留给规划器多长时间来找出一条有效规划.


oscillation_timeout: 10.0 #执行修复机制前,允许振荡的时长.
oscillation_distance: 0.1 #来回运动在多大距离以上不会被认为是振荡

conservative_reset_dist: 0.1

5.teb_local_planner_params.yaml-基本的局部规划器参数配置

这个配置文件设定了机器人的最大和最小速度限制值，也设定了加速度的阈值。

局部路径规划有多种的规划方案：如：DWA、TEB、PMC等；阿克曼使用的是TEB（即有最小转向半径限制）。

参考：http://www.xbhp.cn/news/158710.html

动态调参：rosrun rqt_reconfigure rqt_reconfigure

TebLocalPlannerROS:
  # 使用teb的局部规划器
  odom_topic: odom
  map_frame: /odom
      
  # Trajectoty 这部分主要是用于调整轨迹

  teb_autosize: True #优化期间允许改变轨迹的时域长度
  dt_ref: 0.3 #期望的轨迹时间分辨率
  dt_hysteresis: 0.03 #根据当前时间分辨率自动调整大小的滞后现象，通常约为。建议使用dt ref的10%
  
  #覆盖全局规划器提供的局部子目标的方向;规划局部路径时会覆盖掉全局路径点的方位角,
  #对于车辆的2D规划,可以设置为False,可实现对全局路径的更好跟踪。
  global_plan_overwrite_orientation: True

#指定考虑优化的全局计划子集的最大长度,如果为0或负数：禁用；长度也受本地Costmap大小的限制
  max_global_plan_lookahead_dist: 3.0 

  feasibility_check_no_poses: 1 #检测位姿可到达的时间间隔，default：4
    
  #如果为true，则在目标落后于起点的情况下，可以使用向后运动来初始化基础轨迹
  #(仅在机器人配备了后部传感器的情况下才建议这样做）
  allow_init_with_backwards_motion: False

  global_plan_viapoint_sep: -1

  #参数在TebLocalPlannerROS::pruneGlobalPlan()函数中被使用
  #该参数决定了从机器人当前位置的后面一定距离开始裁剪
  #就是把机器人走过的全局路线给裁剪掉，因为已经过去了没有比较再参与计算后面的局部规划
  global_plan_prune_distance: 1

  exact_arc_length: False
  publish_feedback: False

  # Robot
  max_vel_x: 3 # X 方向最大速度
  max_vel_x_backwards: 1
  max_vel_theta: 2 # 最大角度
  acc_lim_x: 3  # X 加速限制
  acc_lim_theta: 2 # 角速度加速限制

  #仅适用于全向轮
  # max_vel_y (double, default: 0.0)  
  # acc_lim_y (double, default: 0.5)

  # ********************** Carlike robot parameters ********************
  min_turning_radius: 0.26       # 最小转弯半径 注意车辆运动学中心是后轮中点
  wheelbase: 0.258                 # 即前后轮距离

#设置为true时，ROS话题（rostopic） cmd_vel/angular/z 内的数据是舵机角度，
  cmd_angle_instead_rotvel: True 
  # ********************************************************************

  footprint_model: # types: "point", "circular", "two_circles", "line", "polygon" 多边形勿重复第一个顶点，会自动闭合
    type: "line"
    # radius: 0.2 # for type "circular"
    line_start: [-0.13, 0.0] # for type "line"
    line_end: [0.13, 0.0] # for type "line"
    # front_offset: 0.2 # for type "two_circles"
    # front_radius: 0.2 # for type "two_circles"
    # rear_offset: 0.2 # for type "two_circles"
    # rear_radius: 0.2 # for type "two_circles"
    # vertices: [ [0.25, -0.05], [0.18, -0.05], [0.18, -0.18], [-0.19, -0.18], [-0.25, 0], [-0.19, 0.18], [0.18, 0.18], [0.18, 0.05], [0.25, 0.05] ] # for type "polygon"

  # GoalTolerance 目标公差
  xy_goal_tolerance: 0.2
  yaw_goal_tolerance: 0.1
  #自由目标速度。设为False时，车辆到达终点时的目标速度为0。
  #TEB是时间最优规划器。缺少目标速度约束将导致车辆“全速冲线”
  free_goal_vel: False
  
  # complete_global_plan: True   
  # Obstacles
      
  min_obstacle_dist: 0.3 # 与障碍的最小期望距离,
  include_costmap_obstacles: True #应否考虑到局部costmap的障碍设置为True后才能规避实时探测到的、建图时不存在的障碍物。
  costmap_obstacles_behind_robot_dist: 2.0 #考虑后面n米内的障碍物
  obstacle_poses_affected: 30 #为了保持距离，每个障碍物位置都与轨道上最近的位置相连。
  costmap_converter_spin_thread: True
  costmap_converter_rate: 5


  # Optimization 优化参数
  no_inner_iterations: 5 # 被外循环调用后内循环执行次数
  no_outer_iterations: 4 # 执行外循环优化次数
  optimization_activate: True # 激活优化过程
  optimization_verbose: False # 打印优化过程
  penalty_epsilon: 0.1 # 对硬约束近似
  weight_max_vel_x: 2 # 最大速度权重
  weight_max_vel_theta: 1 # 最大角速度权重
  weight_acc_lim_x: 1 # 最大加速度权重
  weight_acc_lim_theta: 1 # 最大角速度权重
  weight_kinematics_nh: 1000 
  weight_kinematics_forward_drive: 1 #抑制倒车的权重，正常设置1
  weight_kinematics_turning_radius: 1 #最小转弯半径，我们没必要最小转弯
  weight_optimaltime: 1 #优化时间参数，让小车多走直线和内道
  weight_obstacle: 50 # 优化过程中和障碍物最小距离权重
  weight_dynamic_obstacle: 10 # not in use yet # 动态障碍物最小距离权重
  alternative_time_cost: False # not in use yet 
  selection_alternative_time_cost: False


  # Homotopy Class Planner

  enable_homotopy_class_planning: False
  enable_multithreading: False
  simple_exploration: False
  max_number_classes: 4
  roadmap_graph_no_samples: 15
  roadmap_graph_area_width: 5
  h_signature_prescaler: 0.5
  h_signature_threshold: 0.1
  obstacle_keypoint_offset: 0.1
  obstacle_heading_threshold: 0.45
  visualize_hc_graph: False

  # # Recovery
  # shrink_horizon_backup: True
  # shrink_horizon_min_duration: 10
  # oscillation_recovery: True
  # oscillation_v_eps: 0.1
  # oscillation_omega_eps: 0.1
  # oscillation_recovery_min_duration: 10
  # oscillation_filter_duration: 10

6.move_base.launch-导航节点

<launch>
  
   <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
   
    <rosparam file="$(find model)/param/costmap_common_params.yaml" command="load" ns="global_costmap" /> 
    <rosparam file="$(find model)/param/costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find model)/param/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find model)/param/global_costmap_params.yaml" command="load" /> 
    <rosparam file="$(find model)/param/teb_local_planner_params.yaml" command="load" />
    <rosparam file="$(find model)/param/move_base_params.yaml" command="load" />
    
    <param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS" />
    
   node>

  
   	<node name="map_server" pkg="map_server" type="map_server" args="$(find model)/map/my_02_map.yaml" output="screen">
                <param name="frame_id" value="map"/>
    node>

  
   <node name="nav_sim" pkg="model" type="transform.py" >node>

  
    <arg name="use_map_topic"   default="True"/>
    <arg name="scan_topic"      default="/scan"/> 
    <arg name="initial_pose_x"  default="-0.5"/>
    <arg name="initial_pose_y"  default="0.0"/>
    <arg name="initial_pose_a"  default="0.0"/>
    <arg name="odom_frame_id"   default="odom"/>
    <arg name="base_frame_id"   default="base_footprint"/>
    <arg name="global_frame_id" default="map"/>

  <node pkg="amcl" type="amcl" name="amcl">
      <param name="use_map_topic"             value="$(arg use_map_topic)"/>
      
      <param name="odom_model_type"           value="diff"/> 
      <param name="odom_alpha5"               value="0.1"/>
      <param name="gui_publish_rate"          value="10.0"/>
      <param name="laser_max_beams"           value="810"/>
      <param name="laser_max_range"           value="-1"/>
      <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"/>
      
      <param name="odom_alpha3"               value="0.2"/>
      <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_model_type"          value="likelihood_field"/>
      
      <param name="laser_likelihood_max_dist" value="2.0"/>
      <param name="update_min_d"              value="0.1"/>
      <param name="update_min_a"              value="0.2"/>
      <param name="odom_frame_id"             value="$(arg odom_frame_id)"/>  
      <param name="base_frame_id"             value="$(arg base_frame_id)"/> 
      <param name="global_frame_id"           value="$(arg global_frame_id)"/>
      <param name="resample_interval"         value="1"/>
      
      <param name="transform_tolerance"       value="1.0"/>
      <param name="recovery_alpha_slow"       value="0.0"/>
      <param name="recovery_alpha_fast"       value="0.0"/>
      <param name="initial_pose_x"            value="$(arg initial_pose_x)"/>
      <param name="initial_pose_y"            value="$(arg initial_pose_y)"/>
      <param name="initial_pose_a"            value="$(arg initial_pose_a)"/>
      <remap from="/scan"                     to="$(arg scan_topic)"/>
      <remap from="/tf_static"                to="/tf_static"/>
    node>
   launch>