一、新建launch文件
这个启动文件加载启动地图,加载机器人模型运行move_base导航节点,
在gazebo和Rviz中加载机器人
nvidia@tegra-ubuntu:~/catkin_ws/src/mbot_gazebo/launch$
roslaunch mbot_gazebo nav_gmapping_view_mbot_gazebolaserandcamera_room.launch
<launch>
<!-- 设置launch文件的参数 -->
<arg name="world_name" value="$(find mbot_gazebo)/worlds/cloister.world"/>
<arg name="paused" default="false"/>
<arg name="use_sim_time" default="true"/>
<arg name="gui" default="true"/>
<arg name="headless" default="false"/>
<arg name="debug" default="false"/>
<!-- 运行gazebo仿真环境 -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="$(arg world_name)" />
<arg name="debug" value="$(arg debug)" />
<arg name="gui" value="$(arg gui)" />
<arg name="paused" value="$(arg paused)"/>
<arg name="use_sim_time" value="$(arg use_sim_time)"/>
<arg name="headless" value="$(arg headless)"/>
</include>
<!-- 加载机器人模型描述参数 -->
<param name="robot_description" command="$(find xacro)/xacro --inorder '$(find mbot_description)/urdf/xacro/gazebo/mbot_with_2laserandcamera_gazebo.xacro'" />
<!-- 运行joint_state_publisher节点,发布机器人的关节状态 -->
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" ></node>
<!-- 运行robot_state_publisher节点,发布tf -->
<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>
<!-- 在gazebo中加载机器人模型-->
<node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
args="-urdf -model mbot -param robot_description"/>
<include file="$(find mbot_navigation)/launch/gmapping2.launch"/>
<!-- 运行move_base节点 -->
<include file="$(find mbot_navigation)/launch/move_base.launch" />
<!-- 运行rviz -->
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find mbot_navigation)/rviz/nav.rviz"/>
</launch>
二、新建机器人模型启动文件
文件中包含摄像头模型和激光雷达模型
nvidia@tegra-ubuntu:~/catkin_ws/src/find mbot_description/urdf/xacro/gazebo/
mbot_with_2laserandcamera_gazebo.xacro
<?xml version="1.0"?>
<robot name="arm" xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find mbot_description)/urdf/xacro/gazebo/mbot_2base_gazebo.xacro" />
<xacro:include filename="$(find mbot_description)/urdf/xacro/sensors/lidar_gazebo.xacro" />
<xacro:include filename="$(find mbot_description)/urdf/xacro/sensors/camera_gazebo.xacro" />
<xacro:property name="lidar_offset_x" value="0" />
<xacro:property name="lidar_offset_y" value="0" />
<xacro:property name="lidar_offset_z" value="0.40" />
<xacro:property name="camera_offset_x" value="0.17" />
<xacro:property name="camera_offset_y" value="0" />
<xacro:property name="camera_offset_z" value="0.0775" />
<!-- lidar -->
<joint name="lidar_joint" type="fixed">
<origin xyz="${lidar_offset_x} ${lidar_offset_y} ${lidar_offset_z}" rpy="0 0 0" />
<parent link="head"/>
<child link="laser_link"/>
</joint>
<xacro:rplidar prefix="laser"/>
<gazebo reference="laser_link">
<material>Gazebo/Black</material>
</gazebo>
<!-- Camera -->
<joint name="camera_joint" type="fixed">
<origin xyz="${camera_offset_x} ${camera_offset_y} ${camera_offset_z}" rpy="0 0 0" />
<parent link="base_link"/>
<child link="camera_link"/>
</joint>
<xacro:usb_camera prefix="camera"/>
<gazebo reference="camera_link">
<material>Gazebo/Green</material>
</gazebo>
<mbot_base_gazebo/>
</robot>
三、新建机器人文件构建机器人主体
为了绕开激光雷达Bug,将激光雷达升高
加了两个轮子,其中有前件和后件,主动轮在中间,从动轮在前后,各两个。
nvidia@tegra-ubuntu:~/catkin_ws/src/mbot_description/urdf/xacro/gazebo/mbot_2base_gazebo.xacro
mbot_2base_gazebo.xacro
<?xml version="1.0"?>
<robot name="mbot" xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- PROPERTY LIST -->
<xacro:property name="M_PI" value="3.1415926"/>
<xacro:property name="base_mass" value="2" />
<xacro:property name="base_radius" value="0.20"/>
<xacro:property name="base_length" value="0.16"/>
<xacro:property name="base_link_length" value="0.50"/>
<xacro:property name="base_link_width" value="0.22"/>
<xacro:property name="base_link_high" value="0.115"/>
<xacro:property name="head_length" value="0.21"/>
<xacro:property name="head_width" value="0.10"/>
<xacro:property name="head_high" value="0.05"/>
<xacro:property name="head_mass" value="0.25" />
<xacro:property name="wheel_mass" value="0.5" />
<xacro:property name="wheel_radius" value="0.08"/>
<xacro:property name="wheel_length" value="0.04"/>
<xacro:property name="wheel_joint_y" value="0.1875"/>
<xacro:property name="wheel_joint_z" value="0.07"/>
<xacro:property name="caster_mass" value="0.01" />
<xacro:property name="caster_radius" value="0.015"/> <!-- wheel_radius - ( base_length/2 - wheel_joint_z) -->
<xacro:property name="caster_joint_x" value="0.18"/>
<xacro:property name="foot_length" value="0.05"/>
<xacro:property name="foot_width" value="0.335"/>
<xacro:property name="foot_high" value="0.05"/>
<xacro:property name="foot_mass" value="0.25" />
<!-- Defining the colors used in this robot -->
<material name="blue">
<color rgba="0 0 0.8 1"/>
</material>
<material name="black">
<color rgba="0 0 0 0.95"/>
</material>
<material name="green">
<color rgba="0 1 0 1"/>
</material>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
<!-- 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>
<!-- Defining the Robot white seat -->
<joint name="head_joint" type="fixed">
<origin xyz=" 0 0 ${(base_link_high + head_high)/2}" rpy="0 0 0" />
<parent link="base_link"/>
<child link="head"/>
</joint>
<link name="head">
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<box size ="${head_length} ${head_width} ${head_high}" />
</geometry>
<material name="white" />
</visual>
<collision>
<origin xyz="0 0 ${(base_link_high + head_high)/2}" rpy="0 0 0" />
<geometry>
<box size ="${head_length} ${head_width} ${head_high}" />
</geometry>
</collision>
<Box_inertial_matrix m="${head_mass}" l="${head_length}" w="${head_width}" h="${head_high}" />
</link>
<gazebo reference="head">
<material>Gazebo/White</material>
</gazebo>
<!-- Defining the Robot foot front-->
<joint name="front_joint" type="fixed">
<origin xyz=" ${(base_link_length + foot_length)/2} 0 ${(foot_high - base_link_high)/2}" rpy="0 0 0" />
<parent link="base_link"/>
<child link="front"/>
</joint>
<link name="front">
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<box size ="${foot_length} ${foot_width} ${foot_high}" />
</geometry>
<material name="black" />
</visual>
<collision>
<origin xyz=" ${(base_link_length + foot_length)/2} 0 ${(foot_high - base_link_high)/2}" rpy="0 0 0" />
<geometry>
<box size ="${foot_length} ${foot_width} ${foot_high}" />
</geometry>
</collision>
<Box_inertial_matrix m="${foot_mass}" l="${foot_length}" w="${foot_width}" h="${foot_high}" />
</link>
<gazebo reference="front">
<material>Gazebo/Black</material>
</gazebo>
<!-- Defining the Robot foot middle-->
<joint name="middle_joint" type="fixed">
<origin xyz=" 0 0 ${(foot_high - base_link_high-0.01)/2}" rpy="0 0 0" />
<parent link="base_link"/>
<child link="middle"/>
</joint>
<link name="middle">
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<box size ="${foot_length} ${foot_width} ${foot_high}" />
</geometry>
<material name="black" />
</visual>
<collision>
<origin xyz=" 0 0 ${(foot_high - base_link_high)/2}" rpy="0 0 0" />
<geometry>
<box size ="${foot_length} ${foot_width} ${foot_high}" />
</geometry>
</collision>
<Box_inertial_matrix m="${foot_mass}" l="${foot_length}" w="${foot_width}" h="${foot_high}" />
</link>
<gazebo reference="middle">
<material>Gazebo/Black</material>
</gazebo>
<!-- Defining the Robot foot back-->
<joint name="back_joint" type="fixed">
<origin xyz=" ${-(base_link_length + foot_length)/2} 0 ${(foot_high - base_link_high)/2}" rpy="0 0 0" />
<parent link="base_link"/>
<child link="back"/>
</joint>
<link name="back">
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<box size ="${foot_length} ${foot_width} ${foot_high}" />
</geometry>
<material name="black" />
</visual>
<collision>
<origin xyz=" ${-(base_link_length + foot_length)/2} 0 ${(foot_high - base_link_high)/2}" rpy="0 0 0" />
<geometry>
<box size ="${foot_length} ${foot_width} ${foot_high}" />
</geometry>
</collision>
<Box_inertial_matrix m="${foot_mass}" l="${foot_length}" w="${foot_width}" h="${foot_high}" />
</link>
<gazebo reference="back">
<material>Gazebo/Black</material>
</gazebo>
<!-- Macro for robot wheel -->
<xacro:macro name="wheel" params="prefix reflect">
<joint name="${prefix}_wheel_joint" type="continuous">
<origin xyz="0 ${reflect*(foot_width+wheel_length)/2} 0" rpy="0 0 0"/>
<parent link="middle"/>
<child link="${prefix}_wheel_link"/>
<axis xyz="0 1 0"/>
</joint>
<link name="${prefix}_wheel_link">
<visual>
<origin xyz="0 0 0" rpy="${M_PI/2} 0 0" />
<geometry>
<cylinder radius="${wheel_radius}" length = "${wheel_length}"/>
</geometry>
<material name="white" />
</visual>
<collision>
<origin xyz="0 0 0" rpy="${M_PI/2} 0 0" />
<geometry>
<cylinder radius="${wheel_radius}" length = "${wheel_length}"/>
</geometry>
</collision>
<cylinder_inertial_matrix m="${wheel_mass}" r="${wheel_radius}" h="${wheel_length}" />
</link>
<gazebo reference="${prefix}_wheel_link">
<material>Gazebo/Gray</material>
</gazebo>
<!-- Transmission is important to link the joints and the controller -->
<transmission name="${prefix}_wheel_joint_trans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${prefix}_wheel_joint" >
<hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
</joint>
<actuator name="${prefix}_wheel_joint_motor">
<hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
</xacro:macro>
<!--back wheel-->
<xacro:macro name="wheelback" params="prefix reflect">
<joint name="${prefix}_wheel_jointback" type="continuous">
<origin xyz="0 ${reflect*(foot_width+wheel_length)/2} 0" rpy="0 0 0"/>
<parent link="back"/>
<child link="${prefix}_wheel_linkback"/>
<axis xyz="0 1 0"/>
</joint>
<link name="${prefix}_wheel_linkback">
<visual>
<origin xyz="0 0 0" rpy="${M_PI/2} 0 0" />
<geometry>
<cylinder radius="${wheel_radius}" length = "${wheel_length}"/>
</geometry>
<material name="white" />
</visual>
<collision>
<origin xyz="0 0 0" rpy="${M_PI/2} 0 0" />
<geometry>
<cylinder radius="${wheel_radius}" length = "${wheel_length}"/>
</geometry>
</collision>
<cylinder_inertial_matrix m="${wheel_mass}" r="${wheel_radius}" h="${wheel_length}" />
</link>
<gazebo reference="${prefix}_wheel_linkback">
<material>Gazebo/Gray</material>
</gazebo>
<!-- Transmission is important to link the joints and the controller -->
<transmission name="${prefix}_wheel_joint_transback">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${prefix}_wheel_jointback" >
<hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
</joint>
<actuator name="${prefix}_wheel_joint_motorback">
<hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
</xacro:macro>
<!--front wheel-->
<xacro:macro name="wheelfront" params="prefix reflect">
<joint name="${prefix}_wheel_jointfront" type="continuous">
<origin xyz="0 ${reflect*(foot_width+wheel_length)/2} 0" rpy="0 0 0"/>
<parent link="front"/>
<child link="${prefix}_wheel_linkfront"/>
<axis xyz="0 1 0"/>
</joint>
<link name="${prefix}_wheel_linkfront">
<visual>
<origin xyz="0 0 0" rpy="${M_PI/2} 0 0" />
<geometry>
<cylinder radius="${wheel_radius}" length = "${wheel_length}"/>
</geometry>
<material name="white" />
</visual>
<collision>
<origin xyz="0 0 0" rpy="${M_PI/2} 0 0" />
<geometry>
<cylinder radius="${wheel_radius}" length = "${wheel_length}"/>
</geometry>
</collision>
<cylinder_inertial_matrix m="${wheel_mass}" r="${wheel_radius}" h="${wheel_length}" />
</link>
<gazebo reference="${prefix}_wheel_linkfront">
<material>Gazebo/Gray</material>
</gazebo>
<!-- Transmission is important to link the joints and the controller -->
<transmission name="${prefix}_wheel_joint_transfront">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${prefix}_wheel_jointfront" >
<hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
</joint>
<actuator name="${prefix}_wheel_joint_motorfront">
<hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
</xacro:macro>
<!-- Macro for robot caster -->
<xacro:macro name="caster" params="prefix reflect">
<joint name="${prefix}_caster_joint" type="continuous">
<origin xyz="${reflect*caster_joint_x} 0 ${-(base_link_high/2 + caster_radius)}" rpy="0 0 0"/>
<parent link="base_link"/>
<child link="${prefix}_caster_link"/>
<axis xyz="0 1 0"/>
</joint>
<link name="${prefix}_caster_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<sphere radius="${caster_radius}" />
</geometry>
<material name="black" />
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<sphere radius="${caster_radius}" />
</geometry>
</collision>
<sphere_inertial_matrix m="${caster_mass}" r="${caster_radius}" />
</link>
<gazebo reference="${prefix}_caster_link">
<material>Gazebo/Black</material>
</gazebo>
</xacro:macro>
<xacro:macro name="mbot_base_gazebo">
<link name="base_footprint">
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<box size="0.001 0.001 0.001" />
</geometry>
</visual>
</link>
<gazebo reference="base_footprint">
<turnGravityOff>false</turnGravityOff>
</gazebo>
<joint name="base_footprint_joint" type="fixed">
<origin xyz="0 0 ${base_link_high/2 + caster_radius*2}" rpy="0 0 0" />
<parent link="base_footprint"/>
<child link="base_link" />
</joint>
<link name="base_link">
<visual>
<origin xyz=" 0 0 0" rpy="0 0 0" />
<geometry>
<box size="${base_link_length} ${base_link_width} ${base_link_high}"/>
</geometry>
<material name="blue" />
</visual>
<collision>
<origin xyz=" 0 0 0" rpy="0 0 0" />
<geometry>
<box size="${base_link_length} ${base_link_width} ${base_link_high}"/>
</geometry>
</collision>
<Box_inertial_matrix m="${base_mass}" l="${base_link_length}" w="${base_link_width}" h="${base_link_high}" />
</link>
<gazebo reference="base_link">
<material>Gazebo/Blue</material>
</gazebo>
<wheel prefix="left" reflect="-1"/>
<wheel prefix="right" reflect="1"/>
<wheelback prefix="left" reflect="-1"/>
<wheelback prefix="right" reflect="1"/>
<wheelfront prefix="left" reflect="-1"/>
<wheelfront prefix="right" reflect="1"/>
<caster prefix="front" reflect="-1"/>
<caster prefix="back" reflect="1"/>
<!-- controller -->
<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_wheel_joint</leftJoint>
<rightJoint>right_wheel_joint</rightJoint>
<leftJoint>left_wheel_joint1</leftJoint>
<rightJoint>right_wheel_joint1</rightJoint>
<wheelSeparation>${wheel_joint_y*2}</wheelSeparation>
<wheelDiameter>${2*wheel_radius}</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>
</xacro:macro>
</robot>
四、启动后的Gazebo和Rviz环境
五、机器人查看模型launch文件
nvidia@tegra-ubuntu:~/catkin_ws/src/mbot_gazebo/launch$
roslaunch mbot_gazebo view_mbot_gazebo_laser_camera_room.launch
<launch>
<!-- 设置launch文件的参数 -->
<arg name="world_name" value="$(find mbot_gazebo)/worlds/room.world"/>
<arg name="paused" default="false"/>
<arg name="use_sim_time" default="true"/>
<arg name="gui" default="true"/>
<arg name="headless" default="false"/>
<arg name="debug" default="false"/>
<!-- 运行gazebo仿真环境 -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="$(arg world_name)" />
<arg name="debug" value="$(arg debug)" />
<arg name="gui" value="$(arg gui)" />
<arg name="paused" value="$(arg paused)"/>
<arg name="use_sim_time" value="$(arg use_sim_time)"/>
<arg name="headless" value="$(arg headless)"/>
</include>
<!-- 加载机器人模型描述参数 -->
<param name="robot_description" command="$(find xacro)/xacro --inorder '$(find mbot_description)/urdf/xacro/gazebo/mbot_with_2laserandcamera_gazebo.xacro'" />
<!-- 运行joint_state_publisher节点,发布机器人的关节状态 -->
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" ></node>
<!-- 运行robot_state_publisher节点,发布tf -->
<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>
<!-- 在gazebo中加载机器人模型-->
<node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
args="-urdf -model mbot -param robot_description"/>
<!-- 运行rviz可视化界面 -->
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find mbot_description)/config/mbot_urdf.rviz" required="true" />
</launch>
六、模型外观1
七、模型外观2