多线激光雷达+imu的gazebo仿真
多线激光雷达+imu的gazebo仿真
前言：在做2D/3D slam的课题研究时，往往我们手里头没有昂贵的多线激光雷达和惯性传感器，ros提供了相应的传感器插件，通过插件可以在物理仿真环境下，模拟传感器运行时的效果。
文章目录
- 多线激光雷达+imu的gazebo仿真
-
- 1.搭建小车底盘模型
- 2.添加多线激光雷达
- 3.添加IMU
- 4.测试仿真
1.搭建小车底盘模型
gazebo下xacro描述文件多加了碰撞标签和惯性矩阵,定义常见形状的惯性矩阵描述文件my_inertia.xacro，
<robot name="base" xmlns:xacro="http://wiki.ros.org/xacro">
<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文件格式对urdf文件格式进行了升级，提高了urdf格式代码的复用性。小车底盘的描述文件 my_car_Cylider.xacro如下：
<robot name="my_base" xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:property name="PI" value="3.141"/>
<material name="black">
<color rgba="0.0 0.0 0.0 1.0" />
material>
<xacro:property name="base_footprint_radius" value="0.001" />
<xacro:property name="base_link_radius" value="0.1" />
<xacro:property name="base_link_length" value="0.08" />
<xacro:property name="earth_space" value="0.02" />
<xacro:property name="base_link_m" value="1.0" />
<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_link_m}" r="${base_link_radius}" h="${base_link_length}" />
link>
<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>
<gazebo reference="base_link">
<material>Gazebo/Yellowmaterial>
gazebo>
<xacro:property name="wheel_radius" value="0.0325" />
<xacro:property name="wheel_length" value="0.015" />
<xacro:property name="wheel_m" 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_m}" r="${wheel_radius}" h="${wheel_length}" />
link>
<joint name="${name}" 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>
<gazebo reference="${name}_wheel">
<material>Gazebo/Whitematerial>
gazebo>
xacro:macro>
<xacro:add_wheels name="base_l_wheel_joint" flag="1" />
<xacro:add_wheels name="base_r_wheel_joint" flag="-1" />
<xacro:property name="support_wheel_radius" value="0.01" />
<xacro:property name="support_wheel_m" value="0.03" />
<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_m}" r="${support_wheel_radius}" />
link>
<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>
<gazebo reference="${name}_wheel">
<material>Gazebo/Blackmaterial>
gazebo>
xacro:macro>
<xacro:add_support_wheel name="front" flag="1" />
<xacro:add_support_wheel name="back" flag="-1" />
robot>
2.添加多线激光雷达
多线激光雷达的配置文件my_sensor_vodyne.xacro
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="velodyne">
<gazebo reference="vodyne">
<sensor type="ray" name="vodyne16">
<pose>0 0 0 0 0 0pose>
<visualize>falsevisualize>
<update_rate>10update_rate>
<ray>
<scan>
<horizontal>
<samples>1800samples>
<resolution>0.2resolution>
<min_angle>-${M_PI}min_angle>
<max_angle> ${M_PI}max_angle>
horizontal>
<vertical>
<samples>16samples>
<resolution>2resolution>
<min_angle>-${15*M_PI/180.0}min_angle>
<max_angle> ${15*M_PI/180.0}max_angle>
vertical>
scan>
<range>
<min>0.055min>
<max>100.0max>
<resolution>0.1resolution>
range>
<noise>
<type>gaussiantype>
<mean>0.0mean>
<stddev>0.0stddev>
noise>
ray>
<plugin name="gazebo_ros_laser_controller" filename="libgazebo_ros_velodyne_laser.so">
<topicName>/velodyne_pointstopicName>
<frameName>vodyneframeName>
<min_range>0.9min_range>
<max_range>130.0max_range>
<gaussianNoise>0.0gaussianNoise>
plugin>
sensor>
gazebo>
robot>
在gazebo环境下添加多线激光雷达物理模型my_vodyne_gazebo.xacro
<robot name="my_vodyne" xmlns:xacro="http://wiki.ros.org/xacro">
<xacro:property name="M_PI" value="3.1415926"/>
<xacro:property name="laser_length" value="0.03" />
<xacro:property name="laser_radius" value="0.03" />
<xacro:property name="laser_x" value="0.0" />
<xacro:property name="laser_y" value="0.0" />
<xacro:property name="laser_z" value="0.055" />
<xacro:property name="laser_m" value="0.1" />
<link name="vodyne">
<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_m}" r="${laser_radius}" h="${laser_length}" />
link>
<joint name="vodynetoBaselink" type="fixed">
<parent link="base_link" />
<child link="vodyne" />
<origin xyz="${laser_x} ${laser_y} ${laser_z}" />
joint>
<gazebo reference="vodyne">
<material>Gazebo/Bluematerial>
gazebo>
robot>
3.添加IMU
imu的配置文件my_sensor_imu.xacro
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<gazebo reference="imu">
<gravity>truegravity>
<sensor name="imu_sensor" type="imu">
<always_on>truealways_on>
<update_rate>100update_rate>
<visualize>truevisualize>
<topic>__default_topic__topic>
<plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
<topicName>imutopicName>
<bodyName>imubodyName>
<updateRateHZ>100.0updateRateHZ>
<gaussianNoise>0.0gaussianNoise>
<xyzOffset>0 0 0xyzOffset>
<rpyOffset>0 0 0rpyOffset>
<frameName>imuframeName>
plugin>
<pose>0 0 0 0 0 0pose>
sensor>
gazebo>
robot>
在gazebo环境下添加imu物理模型my_imu_gazebo.xacro
<robot name="imu" xmlns:xacro="http://wiki.ros.org/xacro">
<xacro:property name="imu_offset_x" value="0" />
<xacro:property name="imu_offset_y" value="0" />
<xacro:property name="imu_offset_z" value="0.02" />
<xacro:property name="imu_size" value="0.01" />
<xacro:property name="imu_m" value="0.01" />
<joint name="imutoVodyne" type="fixed">
<origin xyz="${imu_offset_x} ${imu_offset_y} ${imu_offset_z}" rpy="0 0 0" />
<parent link="vodyne"/>
<child link="imu"/>
joint>
<link name="imu">
<visual>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
<box size="${imu_size} ${imu_size} ${imu_size}"/>
geometry>
<material name= "red" >
<color rgba="1.0 0.0 0.0 1.0" />
material>
visual>
<collision>
<geometry>
<box size="${imu_size} ${imu_size} ${imu_size}" />
geometry>
<origin xyz="0.0 0.0 0" rpy="0.0 0.0 0.0" />
collision>
<xacro:Box_inertial_matrix m = "${imu_m}" l = "${imu_size}" w = "${imu_size}" h = "${imu_size}"/>
link>
<gazebo reference="imu">
<material>Gazebo/Redmaterial>
gazebo>
robot>
4.测试仿真
将小车底盘、多线激光雷达、Imu组合到一起的xacro文件 my_car_vodyne_imu.xacro
<robot name="my_car_camera_laser" xmlns:xacro="http://wiki.ros.org/xacro" reference= "base_footprint_radius">
<xacro:include filename="my_inertial.xacro" />
<xacro:include filename="my_car_Cylider.xacro" />
<xacro:include filename="my_vodyne_gazebo.xacro" />
<xacro:include filename="my_imu_gazebo.xacro" />
<xacro:include filename="$(find myrobot_gazebo)/urdf/sensor/my_sensor_rplidar.xacro" />
<xacro:include filename="$(find myrobot_gazebo)/urdf/sensor/my_sensor_imu.xacro" />
<xacro:include filename="$(find myrobot_gazebo)/urdf/sensor/my_sensor_vodyne.xacro" />
robot>
编写launch文件 my_car_vodyne_imu.launch
<launch>
<param name="robot_description" command="my_car_vodyne_imu.xacro" />
<include file="$(find gazebo_ros)/launch/empty_world.launch">
include>
<node pkg="gazebo_ros" type="spawn_model" name="model" args="-urdf -model mycar -param robot_description" />
<node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher" output="screen" />
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen" />
launch>
打开终端，启动launch文件，可以看到组合到一起的效果
另开一个终端 输入
rostopic pub -r 10 /cmd_vel geometry_msgs/Twist '{linear: {x: 0.2, y: 0, z: 0}, angular: {x: 0, y: 0, z: 0.5}}'
gazebo下测试的效果
