一、创建编译功能包
切换到catkin_ws/src目录下，如下：
cd catkin_ws/src
catkin_create_pkg mbot_sim_gazebo_mapping urdf xacro
返回到catkin_ws目录下，编译该功能包，如下：
cd ..
catkin_make mbot_sim_gazebo_mapping
在该功能包下创建include、src、launch、urdf、world、map、script文件夹，在urdf文件下创建urdf文件夹和xacro文件夹。
cd src/mbot_sim_gazebo_mapping
mkdir include
mkdir src
mkdir launch
mkdir urdf
mkdir world
mkdir map
mkdir script
cd urdf
mkdir urdf
mkdir xacro二、xacro文件
1、创建robot_base.xacro文件
切换到xacro文件下，创建robot_base.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="20" />
<xacro:property name="base_radius" value="0.20"/>
<xacro:property name="base_length" value="0.16"/>
<xacro:property name="wheel_mass" value="2" />
<xacro:property name="wheel_radius" value="0.06"/>
<xacro:property name="wheel_length" value="0.025"/>
<xacro:property name="wheel_joint_y" value="0.19"/>
<xacro:property name="wheel_joint_z" value="0.05"/>
<xacro:property name="caster_mass" value="0.5" />
<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"/>
<!-- Defining the colors used in this robot -->
<material name="yellow">
<color rgba="1 0.4 0 1"/>
</material>
<material name="black">
<color rgba="0 0 0 0.95"/>
</material>
<material name="gray">
<color rgba="0.75 0.75 0.75 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>
<!-- Macro for robot wheel -->
<xacro:macro name="wheel" params="prefix reflect">
<joint name="${prefix}_wheel_joint" type="continuous">
<origin xyz="0 ${reflect*wheel_joint_y} ${-wheel_joint_z}" rpy="0 0 0"/>
<parent link="base_link"/>
<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="gray" />
</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>
<!-- 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_length/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_length/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>
<cylinder length="${base_length}" radius="${base_radius}"/>
</geometry>
<material name="yellow" />
</visual>
<collision>
<origin xyz=" 0 0 0" rpy="0 0 0" />
<geometry>
<cylinder length="${base_length}" radius="${base_radius}"/>
</geometry>
</collision>
<cylinder_inertial_matrix m="${base_mass}" r="${base_radius}" h="${base_length}" />
</link>
<gazebo reference="base_link">
<material>Gazebo/Blue</material>
</gazebo>
<wheel prefix="left" reflect="-1"/>
<wheel 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>
<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>
<odometrySource>world</odometrySource>
<publishOdomTF>1</publishOdomTF>
</plugin>
</gazebo>
</xacro:macro>
</robot>2、创建robot_camera.xacro文件
在xacro文件下，创建robot_camera.xacro文件，文件内容如下：
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="camera">
<xacro:macro name="usb_camera" params="prefix:=camera">
<!-- Create laser reference frame -->
<link name="${prefix}_link">
<inertial>
<mass value="0.1" />
<origin xyz="0 0 0" />
<inertia ixx="0.01" ixy="0.0" ixz="0.0"
iyy="0.01" iyz="0.0"
izz="0.01" />
</inertial>
<visual>
<origin xyz=" 0 0 0 " rpy="0 0 0" />
<geometry>
<box size="0.01 0.04 0.04" />
</geometry>
<material name="black"/>
</visual>
<collision>
<origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
<geometry>
<box size="0.01 0.04 0.04" />
</geometry>
</collision>
</link>
<gazebo reference="${prefix}_link">
<material>Gazebo/Black</material>
</gazebo>
<gazebo reference="${prefix}_link">
<sensor type="camera" name="camera_node">
<update_rate>30.0</update_rate>
<camera name="head">
<horizontal_fov>1.3962634</horizontal_fov>
<image>
<width>1280</width>
<height>720</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.02</near>
<far>300</far>
</clip>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.007</stddev>
</noise>
</camera>
<plugin name="gazebo_camera" filename="libgazebo_ros_camera.so">
<alwaysOn>true</alwaysOn>
<updateRate>0.0</updateRate>
<cameraName>/camera</cameraName>
<imageTopicName>image_raw</imageTopicName>
<cameraInfoTopicName>camera_info</cameraInfoTopicName>
<frameName>camera_link</frameName>
<hackBaseline>0.07</hackBaseline>
<distortionK1>0.0</distortionK1>
<distortionK2>0.0</distortionK2>
<distortionK3>0.0</distortionK3>
<distortionT1>0.0</distortionT1>
<distortionT2>0.0</distortionT2>
</plugin>
</sensor>
</gazebo>
</xacro:macro>
</robot>3、创建robot_lidar.xacro文件
在xacro文件下，创建robot_lidar.xacro文件，文件内容如下：
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="lidar">
<xacro:macro name="hokuyo_lidar" params="prefix:=lidar">
<link name="hokuyo_link">
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.1 0.1 0.1"/>
</geometry>
</collision>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="package://mbot_sim_gazebo_mapping/meshes/hokuyo.dae"/>
</geometry>
</visual>
<inertial>
<mass value="1e-5" />
<origin xyz="0 0 0" rpy="0 0 0"/>
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
</inertial>
</link>
<gazebo reference="hokuyo_link">
<sensor type="ray" name="head_hokuyo_sensor">
<pose>0 0 0 0 0 0</pose>
<visualize>false</visualize>
<update_rate>40</update_rate>
<ray>
<scan>
<horizontal>
<samples>720</samples>
<resolution>1</resolution>
<min_angle>-1.570796</min_angle>
<max_angle>1.570796</max_angle>
</horizontal>
</scan>
<range>
<min>0.10</min>
<max>30.0</max>
<resolution>0.01</resolution>
</range>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.01</stddev>
</noise>
</ray>
<plugin name="gazebo_ros_head_hokuyo_controller" filename="libgazebo_ros_laser.so">
<topicName>/scan</topicName>
<frameName>hokuyo_link</frameName>
</plugin>
</sensor>
</gazebo>
</xacro:macro>
</robot>4、创建robot.xacro文件
在xacro文件下，创建robot.xacro文件，文件内容如下：
<?xml version="1.0"?>
<robot name="arm" xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find mbot_sim_gazebo_mapping)/urdf/xacro/robot_base.xacro" />
<xacro:include filename="$(find mbot_sim_gazebo_mapping)/urdf/xacro/robot_camera.xacro" />
<xacro:include filename="$(find mbot_sim_gazebo_mapping)/urdf/xacro/robot_lidar.xacro" />
<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.10" />
<xacro:property name="lidar_offset_x" value="-0.17" />
<xacro:property name="lidar_offset_y" value="0" />
<xacro:property name="lidar_offset_z" value="0.10" />
<xacro:mbot_base_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"/>
<joint name="lidar_joint" type="fixed">
<origin xyz="${lidar_offset_x} ${lidar_offset_y} ${lidar_offset_z}" rpy="0 0 0" />
<parent link="base_link"/>
<child link="hokuyo_link"/>
</joint>
<xacro:hokuyo_lidar prefix="lidar"/>
</robot>接下来需要将.gazebo/models/hokuyo/目录中的meshes文件夹整体复制到我们的功能包mbot_sim_gazebo下面。将桌面上准备的room.world文件，放在功能包下的world文件夹里面。将opt/ros/melodic/lib/teleop_twist_keyboard文件夹下的teleop_twist_keyboard.py文件放入目前功能包里面的script文件夹
三、launch启动文件
1、创建robot_gazebo_mapping.launch文件
在启动文件中配置rviz和gmaping包，robot_gazebo_mapping.launch文件内容如下：
<launch>
<!-- 运行gazebo仿真环境 -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="$(find mbot_sim_gazebo_mapping)/world/room.world"/>
</include>
<!--机器人参数设置-->
<arg name="model" default="$(find xacro)/xacro --inorder '$(find mbot_sim_gazebo_mapping)/urdf/xacro/robot.xacro'" />
<!--robot_description 参数名称是参数服务器预先设定好的，不能改变-->
<param name="robot_description" command="$(arg model)" />
<!-- 运行joint_state_publisher节点，发布机器人的关节状态 -->
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
<!-- 运行robot_state_publisher节点，发布tf -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
<!-- 在gazebo中加载机器人模型-->
<node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
args="-urdf -model mrobot -param robot_description"/>
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find mbot_sim_gazebo_mapping)/config/default.rviz"/>
<!--gmapping 包实现同步定位和地图建立-->
<node name="slam_gmapping" pkg="gmapping" type="slam_gmapping">
<remap from="scan" to="/scan"/>
<param name="base_link" value="base_footprint"/>
</node>
</launch>四、下载安装建图定位导航相关的功能包
1、下载与安装
sudo apt-get install ros-melodic-slam-gmapping
sudo apt-get install ros-melodic-gmapping
sudo apt-get install ros-melodic-navigation2、运行robot_gazebo_mapping.launch
roslaunch mbot_sim_gazebo_mapping robot_gazebo_mapping.launch
效果如下图：
在rviz里面，添加RobotModel，添加LaserScan，添加Map
然后将LaserScan的Topic设置为/scan
将Map的Topic设置为/map
将Global Options——Fixed Frame设置为base_footprint
效果如下：
键盘控制
在功能包里的script文件夹里创建一个名为teleop_twist_keyboard.py的空白文档，内容如下：
#!/usr/bin/env python
from __future__ import print_function
import threading
import roslib; roslib.load_manifest('teleop_twist_keyboard')
import rospy
from geometry_msgs.msg import Twist
import sys, select, termios, tty
msg = """
Reading from the keyboard and Publishing to Twist!
---------------------------
Moving around:
q w e
a s d
z x c
For Holonomic mode (strafing), hold down the shift key:
---------------------------
U I O
J K L
M < >
t : up (+z)
b : down (-z)
anything else : stop
q/z : increase/decrease max speeds by 10%
w/x : increase/decrease only linear speed by 10%
e/c : increase/decrease only angular speed by 10%
CTRL-C to quit
"""
moveBindings = {
'w':(1,0,0,0),
'e':(1,0,0,-1),
'a':(0,0,0,1),
'd':(0,0,0,-1),
'q':(1,0,0,1),
'x':(-1,0,0,0),
'c':(-1,0,0,1),
'z':(-1,0,0,-1),
'O':(1,-1,0,0),
'I':(1,0,0,0),
'J':(0,1,0,0),
'L':(0,-1,0,0),
'U':(1,1,0,0),
'<':(-1,0,0,0),
'>':(-1,-1,0,0),
'M':(-1,1,0,0),
't':(0,0,1,0),
'b':(0,0,-1,0),
}
speedBindings={
'q':(1.1,1.1),
'z':(.9,.9),
'w':(1.1,1),
'x':(.9,1),
'e':(1,1.1),
'c':(1,.9),
}
class PublishThread(threading.Thread):
def __init__(self, rate):
super(PublishThread, self).__init__()
self.publisher = rospy.Publisher('cmd_vel', Twist, queue_size = 1)
self.x = 0.0
self.y = 0.0
self.z = 0.0
self.th = 0.0
self.speed = 0.0
self.turn = 0.0
self.condition = threading.Condition()
self.done = False
# Set timeout to None if rate is 0 (causes new_message to wait forever
# for new data to publish)
if rate != 0.0:
self.timeout = 1.0 / rate
else:
self.timeout = None
self.start()
def wait_for_subscribers(self):
i = 0
while not rospy.is_shutdown() and self.publisher.get_num_connections() == 0:
if i == 4:
print("Waiting for subscriber to connect to {}".format(self.publisher.name))
rospy.sleep(0.5)
i += 1
i = i % 5
if rospy.is_shutdown():
raise Exception("Got shutdown request before subscribers connected")
def update(self, x, y, z, th, speed, turn):
self.condition.acquire()
self.x = x
self.y = y
self.z = z
self.th = th
self.speed = speed
self.turn = turn
# Notify publish thread that we have a new message.
self.condition.notify()
self.condition.release()
def stop(self):
self.done = True
self.update(0, 0, 0, 0, 0, 0)
self.join()
def run(self):
twist = Twist()
while not self.done:
self.condition.acquire()
# Wait for a new message or timeout.
self.condition.wait(self.timeout)
# Copy state into twist message.
twist.linear.x = self.x * self.speed
twist.linear.y = self.y * self.speed
twist.linear.z = self.z * self.speed
twist.angular.x = 0
twist.angular.y = 0
twist.angular.z = self.th * self.turn
self.condition.release()
# Publish.
self.publisher.publish(twist)
# Publish stop message when thread exits.
twist.linear.x = 0
twist.linear.y = 0
twist.linear.z = 0
twist.angular.x = 0
twist.angular.y = 0
twist.angular.z = 0
self.publisher.publish(twist)
def getKey(key_timeout):
tty.setraw(sys.stdin.fileno())
rlist, _, _ = select.select([sys.stdin], [], [], key_timeout)
if rlist:
key = sys.stdin.read(1)
else:
key = ''
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
return key
def vels(speed, turn):
return "currently:\tspeed %s\tturn %s " % (speed,turn)
if __name__=="__main__":
settings = termios.tcgetattr(sys.stdin)
rospy.init_node('teleop_twist_keyboard')
speed = rospy.get_param("~speed", 0.5)
turn = rospy.get_param("~turn", 1.0)
repeat = rospy.get_param("~repeat_rate", 0.0)
key_timeout = rospy.get_param("~key_timeout", 0.0)
if key_timeout == 0.0:
key_timeout = None
pub_thread = PublishThread(repeat)
x = 0
y = 0
z = 0
th = 0
status = 0
try:
pub_thread.wait_for_subscribers()
pub_thread.update(x, y, z, th, speed, turn)
print(msg)
print(vels(speed,turn))
while(1):
key = getKey(key_timeout)
if key in moveBindings.keys():
x = moveBindings[key][0]
y = moveBindings[key][1]
z = moveBindings[key][2]
th = moveBindings[key][3]
elif key in speedBindings.keys():
speed = speed * speedBindings[key][0]
turn = turn * speedBindings[key][1]
print(vels(speed,turn))
if (status == 14):
print(msg)
status = (status + 1) % 15
else:
# Skip updating cmd_vel if key timeout and robot already
# stopped.
if key == '' and x == 0 and y == 0 and z == 0 and th == 0:
continue
x = 0
y = 0
z = 0
th = 0
if (key == '\x03'):
break
pub_thread.update(x, y, z, th, speed, turn)
except Exception as e:
print(e)
finally:
pub_thread.stop()
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)执行如下代码，实现模型的键盘控制：
rosrun mbot_sim_gazebo_mapping teleop_twist_keyboard.py
通过qweasdzxc让小车运动
效果如下：
通过键盘控制机器人的移动，完成建图过程，移动过程中可以观察到rviz中地图的变化，在这里我已经把整个图扫完了。
五、保存地图
在mbot_sim_gazebo_mapping功能包下，切换到map文件夹下，右键打开终端，执行如下命令，完成地图保存：
rosrun map_server map_saver -f myMapFile
然后打开该功能包里面的map文件，如果里面有1个刚扫完的图片和一个文件，即可。后期需要用到这两个文件
1583
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
