ROS前驱前转小车仿真（2D）项目
文章目录 二.文件框架三.启动顺序 四.urdf-模型文件 五.手动控制 六.建图- 七.导航-
一.官方网址
注：这些网址的服务器都是在国外，访问比较慢是正常情况。
1.ROS官网
2.urdf-模型
3.rviz-数据可视化
4.-仿真环境
有些情况下，后台关闭不彻底，需要用命令行强制关闭
killall gzserver
killall gzclient
5.-建图
6.-导航
二.文件框架
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”,这里为“.sh”，其内容如下：
在.sh文件所在目录打开终端，输入：sh .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：启动模型在和rviz的显示
roslaunch model tricycle.launch
步骤2：启动键盘控制的节点
roslaunch model tele_key.launch
2.建图的启动顺序
步骤1：启动模型在和rviz的显示
roslaunch model tricycle.launch
步骤2：启动建图的节点
roslaunch model slam_gmapping.launch
步骤3：启动保存地图的节点
注意后面的路径要改成自己的路径，这里的地图名为“”，保存之后会在目标的目录下生成两个文件 .pgm .yaml 文件
rosrun map_server map_saver -f /home/z/ros_ws/ys001_ws/src/model/map/my_01_map
3.导航的启动顺序
步骤1：启动模型在和rviz的显示
roslaunch model tricycle.launch
步骤2：启动导航的节点
roslaunch model move_base.launch
四.urdf-模型文件 1..xacro-轮子传动的配置
给仿真环境配置哪些轮子需要控制，为后面控制的仿真做好硬件关节的仿真。
参考
<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相同的名字。
（出现一次）定义了传动的类型，这个细节说明还没找到，用ce/就可以啦。
（可定义一个或多个）指定这个传动所依赖的关节，拥有如下标签。
（在joint下，可定义一个或多个）指定支持的硬件接口空间。用于结合控制器使用硬件接口来向硬件接口发送和接受指令，请注意：
当在中加载此时，此标签的值应为 / XXX。
在中加载此时，此标记的值应为XXX 。
（定义一个或多个）传动连接的致动器，名字自己去定义，拥有如下标签及。
（可选）定义电机／关节减速比。
（可选,只有及以前版本的在这里指定，目前版本已经移到joint标签下）指定支持的硬件接口空间
.xacro-rviz里模型颜色
这里的颜色设定是给rviz里显示用的，里的颜色和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...xacro-配置
1.设置模型在里面的颜色，参考：
2.设置轮子的摩擦力和刚度系数，参考：
3.设置里的传感器插件（），参考：
注意：实际中传感器数据来源于传感器厂商提供的驱动和配到的发布节点。
<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..urdf.xacro-模型文件
1.建立三维模型
2.建立各个部件的坐标关系，即tf关系
参考：
<robotname="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><originxyz="0 0 0"rpy="0 0 0" /><geometry><box size="${base_l} ${base_w} ${base_h}" />geometry><material name="orange" />visual><collision><originxyz="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><originxyz="0 0 0"rpy="0 0 0" /><geometry><cylinder length="0.04" radius="0.03" />geometry><material name="red" />visual>link><jointname="joint_top_ladar"type="fixed"><originxyz="0.57 0.4 0.42"rpy="0 0 0" /><parentlink="base_link" /><childlink="laser_link" /><axisxyz="0 0 0" /><safety_controllerk_velocity="0" />joint><link name="imu_link"/><jointname="joint_imu"type="fixed"><originxyz="0 0 0"rpy="0 0 0" /><parentlink="base_link" /><childlink="imu_link" /><axisxyz="0 0 0" /><safety_controllerk_velocity="0" />joint>robot>五.手动控制 1..-启动模型文件节点
加载模型在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..-关节控制节点
<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_controllerfront_steering_hinge_position_controllerjoint_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..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 AckermannDriveStampedflag_move = 0def set_throttle_steer(data):global flag_movepub_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.5steer = data.drive.steering_anglepub_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 stoppedrospy.spin()if __name__ == '__main__':try:servo_commands()except rospy.ROSInterruptException:pass
4..py-速度转换节点
#!/usr/bin/env python3
'''将 cmd_vel 的速度信息转换为 小车需要的速度信息'''
import rospy
import std_msgs.msg
from ackermann_msgs.msg import AckermannDriveStamped
from geometry_msgs.msg import Twistimport 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.xturn = data.angular.zmsg = 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 = turnmsg.drive.steering_angle_velocity = 1pub.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..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_rosclass OdometryNode:# Set publisherspub_odom = rospy.Publisher('/odom', Odometry, queue_size=1)def __init__(self):# init internalsself.last_received_pose = Pose()self.last_received_twist = Twist()self.vel_direction = 1.self.last_recieved_stamp = None# Set the update raterospy.Timer(rospy.Duration(.02), self.timer_callback) # 50hzself.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 robottry:arrayIndex = msg.name.index('xju::base_footprint')except ValueError as e:# Wait for Gazebo to startuppasselse:# Extract our current position informationtime_diff = 0.02if self.last_recieved_stamp is not None:time_diff = (rospy.Time.now() - self.last_recieved_stamp).to_sec()if time_diff == 0.:returnself.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_diffself.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:returncmd = Odometry()cmd.header.stamp = self.last_recieved_stampcmd.header.frame_id = 'odom'cmd.child_frame_id = 'base_footprint'cmd.pose.pose = self.last_received_posecmd.twist.twist = self.last_received_twistcmd.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..-键盘控制节点
<launch>
<rosparam command="load" file="$(find model)/param/ackermann_teleop_param.yaml" /><node pkg="model" type="ackermann_teleop.py" name="ackermann_teleop" />
launch>7..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, Tkimport 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 = Falsedef keyeq(e, c):return e.char == c or e.keysym == cdef keyup(e):global stateglobal controlwith state_lock:if keyeq(e, UP):state[0] = Falseelif keyeq(e, LEFT):state[1] = Falseelif keyeq(e, DOWN):state[2] = Falseelif keyeq(e, RIGHT):state[3] = Falsecontrol = Truedef keydown(e):global stateglobal controlwith state_lock:if keyeq(e, QUIT):shutdown()elif keyeq(e, UP):state[0] = Truestate[2] = Falseelif keyeq(e, LEFT):state[1] = Truestate[3] = Falseelif keyeq(e, DOWN):state[2] = Truestate[0] = Falseelif keyeq(e, RIGHT):state[3] = Truestate[1] = Falsecontrol = sum(state) > 0# Up -> linear.x = 1.0
# Down -> linear.x = -1.0
# Left -> angular.z = 1.0
# Right -> angular.z = -1.0def publish_cb(_):global controlwith state_lock:if not control:returnack = AckermannDriveStamped()if state[0]:ack.drive.speed = max_velocityelif state[2]:ack.drive.speed = -max_velocityif state[1]:ack.drive.steering_angle = max_steering_angleelif state[3]:ack.drive.steering_angle = -max_steering_angleif state_pub is not None:state_pub.publish(ack)control = sum(state) > 0def exit_func():os.system("xset r on")def shutdown():root.destroy()rospy.signal_shutdown("shutdown")def main():global state_pubglobal rootglobal max_velocityglobal max_steering_anglemax_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()
六.建图-
在手动控制的基础上，添加建图节点；
参考：
1..-建图节点
<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>
七.导航-
在手动控制的基础上，扫好图的基础上，添加导航节点；
参考：
#
下面的五个 “.yaml”文件是导航参数的配置（关键），需要先准备好
可能会出现机器人在本地路径规划时与全局路径规划不符而进入膨胀区域出现假死的情况，如何尽量避免这种情形呢？
全局路径规划与本地路径规划虽然设置的参数是一样的，但是二者路径规划和避障的职能不同，可以采用不同的参数设置策略:
这样，在全局路径规划时，规划的路径会尽量远离障碍物，而本地路径规划时，机器人即便偏离全局路径也会和障碍物之间保留更大的自由空间，从而避免了陷入“假死”的情形。
1.s.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.s.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.0inflation_radius: 0.25 3..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 # 局部地图宽度 单位是 mheight: 5 # 局部地图高度 单位是 mresolution: 0.05 # 局部地图分辨率 单位是 m，一般与静态地图分辨率保持一致transform_tolerance: 0.5cost_scaling_factor: 5inflation_radius: 0.25
4..yaml-底盘控制的参数配置
shutdown_costmaps: false #当move_base在不活动状态时,是否关掉costmapcontroller_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.15.rams.yaml-基本的局部规划器参数配置
这个配置文件设定了机器人的最大和最小速度限制值，也设定了加速度的阈值。
局部路径规划有多种的规划方案：如：DWA、TEB、PMC等；阿克曼使用的是TEB（即有最小转向半径限制）。
参考：
动态调参：
TebLocalPlannerROS:# 使用teb的局部规划器odom_topic: odommap_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: Falseglobal_plan_viapoint_sep: -1#参数在TebLocalPlannerROS::pruneGlobalPlan()函数中被使用#该参数决定了从机器人当前位置的后面一定距离开始裁剪#就是把机器人走过的全局路线给裁剪掉，因为已经过去了没有比较再参与计算后面的局部规划global_plan_prune_distance: 1exact_arc_length: Falsepublish_feedback: False# Robotmax_vel_x: 3 # X 方向最大速度max_vel_x_backwards: 1max_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.2yaw_goal_tolerance: 0.1#自由目标速度。设为False时，车辆到达终点时的目标速度为0。#TEB是时间最优规划器。缺少目标速度约束将导致车辆“全速冲线”free_goal_vel: False# complete_global_plan: True # Obstaclesmin_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: Truecostmap_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 #抑制倒车的权重，正常设置1weight_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 Plannerenable_homotopy_class_planning: Falseenable_multithreading: Falsesimple_exploration: Falsemax_number_classes: 4roadmap_graph_no_samples: 15roadmap_graph_area_width: 5h_signature_prescaler: 0.5h_signature_threshold: 0.1obstacle_keypoint_offset: 0.1obstacle_heading_threshold: 0.45visualize_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..-导航节点
<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>
