致谢源代码网址：https://github.com/Tutorgaming/kamtoa-simulation
kamtoa simulation学习与示例分析（一）
源码学习与分析是学习ROS，包括RVIZ和Gazebo等必须的过程，大量代码的阅读能够提高加快理解熟练使用ROS Kinetic。
首先，先看文件组织：
一般README中有详细的使用说明，包括安装，使用和示例教程。
CMakeLists.txt：
# toplevel CMakeLists.txt for a catkin workspace
# catkin/cmake/toplevel.cmake
cmake_minimum_required(VERSION 2.8.3)
set(CATKIN_TOPLEVEL TRUE)
# search for catkin within the workspace
set(_cmd "catkin_find_pkg" "catkin" "${CMAKE_SOURCE_DIR}")
execute_process(COMMAND ${_cmd}
RESULT_VARIABLE _res
OUTPUT_VARIABLE _out
ERROR_VARIABLE _err
OUTPUT_STRIP_TRAILING_WHITESPACE
ERROR_STRIP_TRAILING_WHITESPACE
)
if(NOT _res EQUAL 0 AND NOT _res EQUAL 2)
# searching fot catkin resulted in an error
string(REPLACE ";" " " _cmd_str "${_cmd}")
message(FATAL_ERROR "Search for 'catkin' in workspace Failed (${_cmd_str}): ${_err}")
endif()
# include catkin from workspace or via find_package()
if(_res EQUAL 0)
set(catkin_EXTRAS_DIR "${CMAKE_SOURCE_DIR}/${_out}/cmake")
# include all.cmake without add_subdirectory to let it operate in same scope
include(${catkin_EXTRAS_DIR}/all.cmake NO_POLICY_ScopE)
add_subdirectory("${_out}")
else()
# use either CMAKE_PREFIX_PATH explicitly passed to CMake as a command line argument
# or CMAKE_PREFIX_PATH from the environment
if(NOT DEFINED CMAKE_PREFIX_PATH)
if(NOT "$ENV{CMAKE_PREFIX_PATH}" STREQUAL "")
string(REPLACE ":" ";" CMAKE_PREFIX_PATH $ENV{CMAKE_PREFIX_PATH})
endif()
endif()
# list of catkin workspaces
set(catkin_search_path "")
foreach(path ${CMAKE_PREFIX_PATH})
if(EXISTS "${path}/.catkin")
list(FIND catkin_search_path ${path} _index)
if(_index EQUAL -1)
list(APPEND catkin_search_path ${path})
endif()
endif()
endforeach()
# search for catkin in all workspaces
set(CATKIN_TOPLEVEL_FIND_PACKAGE TRUE)
find_package(catkin QUIET
NO_POLICY_ScopE
PATHS ${catkin_search_path}
NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH)
unset(CATKIN_TOPLEVEL_FIND_PACKAGE)
if(NOT catkin_FOUND)
message(FATAL_ERROR "find_package(catkin) Failed. catkin was neither found in the workspace nor in the CMAKE_PREFIX_PATH. One reason may be that no ROS setup.sh was sourced before.")
endif()
endif()
catkin_workspace()
这里，先看kamtoa_gazebo文件夹下：
CMakeLists.txt中包含需要的功能包，如下截取部分代码片段：
cmake_minimum_required(VERSION 2.8.3) project(kamtoa_gazebo) ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin required COMPONENTS xyz) ## is used,also find other catkin packages find_package(catkin required COMPONENTS gazebo_msgs gazebo_plugins gazebo_ros gazebo_ros_control kamtoa_description ) ## System dependencies are found with CMake's conventions # find_package(Boost required COMPONENTS system)需要的功能包如果不全，可以使用sudo apt-get install ros-kinetic-gazebo-msgs，以类似方式安装（catkin）。当然编译需要对应的功能包，运行也需要，这里缺一不可，请看package.xml：
<?xml version="1.0"?>
<package>
<name>kamtoa_gazebo</name>
<version>0.0.0</version>
<description>The kamtoa_gazebo package</description>
<!-- One maintainer tag required,multiple allowed,one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="c3mx@todo.todo">c3mx</maintainer>
<!-- One license tag required,one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD,MIT,Boost Software License,GPLv2,GPLv3,LGPLv2.1,LGPLv3 -->
<license>Todo</license>
<!-- Url tags are optional,but mutiple are allowed,one per tag -->
<!-- Optional attribute type can be: website,bugtracker,or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/kamtoa_gazebo</url> -->
<!-- Author tags are optional,mutiple are allowed,one per tag -->
<!-- Authors do not have to be maintianers,but Could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *_depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use run_depend for packages you need at runtime: -->
<!-- <run_depend>message_runtime</run_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>gazebo_msgs</build_depend>
<build_depend>gazebo_plugins</build_depend>
<build_depend>gazebo_ros</build_depend>
<build_depend>gazebo_ros_control</build_depend>
<build_depend>kamtoa_description</build_depend>
<run_depend>gazebo_msgs</run_depend>
<run_depend>gazebo_plugins</run_depend>
<run_depend>gazebo_ros</run_depend>
<run_depend>gazebo_ros_control</run_depend>
<run_depend>kamtoa_description</run_depend>
<!-- The export tag contains other,unspecified,tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>
注意这里的<build_depend>和<run_depend>，分别对应编译和运行， 有时编译没有问题，运行却报错，请务必注意。
world文件夹下存放gazebo模型，打开可以看到：
building1.world和whiz_ex.world，具体解析可以参考博客中GazeboSim中内容，具体的模型文件为sdf 1.6对应Gazebo7，以whiz_ex_window为例，分为model.config和model.sdf如下：
model.sdf：
<?xml version='1.0'?>
<sdf version='1.6'>
<model name='whiz_ex_window'>
<pose frame=''>0.232213 1.38881 0 0 -0 0</pose>
<link name='Door_6'>
<collision name='Door_6_Collision'>
<geometry>
<Box>
<size>0.9 0.17 2</size>
</Box>
</geometry>
<pose frame=''>0 0 1 0 -0 0</pose>
</collision>
<visual name='Door_6_Visual'>
<pose frame=''>0 0 1 0 -0 0</pose>
<geometry>
<Box>
<size>0.9 0.17 2</size>
</Box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
</material>
</visual>
<pose frame=''>0.29567 1.0429 0 0 0 -3.12055</pose>
</link>
<link name='Door_7'>
<collision name='Door_7_Collision'>
<geometry>
<Box>
<size>0.949838 0.2 2</size>
</Box>
</geometry>
<pose frame=''>0 0 1 0 -0 0</pose>
</collision>
<visual name='Door_7_Visual'>
<pose frame=''>0 0 1 0 -0 0</pose>
<geometry>
<Box>
<size>0.949838 0.2 2</size>
</Box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
</material>
</visual>
<pose frame=''>-0.776238 1.0429 0 0 -0 3.14159</pose>
</link>
<link name='Door_8'>
<collision name='Door_8_Collision'>
<geometry>
<Box>
<size>0.9 0.17 2</size>
</Box>
</geometry>
<pose frame=''>0 0 1 0 -0 0</pose>
</collision>
<visual name='Door_8_Visual'>
<pose frame=''>0 0 1 0 -0 0</pose>
<geometry>
<Box>
<size>0.9 0.17 2</size>
</Box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
</material>
</visual>
<pose frame=''>-4.14363 1.09228 0 0 -0 0</pose>
</link>
<link name='Door_9'>
<collision name='Door_9_Collision'>
<geometry>
<Box>
<size>0.9 0.17 2</size>
</Box>
</geometry>
<pose frame=''>0 0 1 0 -0 0</pose>
</collision>
<visual name='Door_9_Visual'>
<pose frame=''>0 0 1 0 -0 0</pose>
<geometry>
<Box>
<size>0.9 0.17 2</size>
</Box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
</material>
</visual>
<pose frame=''>-1.75347 -0.819856 0 0 0 -3.12358</pose>
</link>
<link name='Wall_0'>
<pose frame=''>-1.6817 -3.67821 0 0 -0 0</pose>
<visual name='Wall_0_Visual_0'>
<pose frame=''>-1.17971 0 1.25 0 -0 0</pose>
<geometry>
<Box>
<size>5.89059 0.15 2.5</size>
</Box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
</visual>
<collision name='Wall_0_Collision_0'>
<geometry>
<Box>
<size>5.89059 0.15 2.5</size>
</Box>
</geometry>
<pose frame=''>-1.17971 0 1.25 0 -0 0</pose>
</collision>
<visual name='Wall_0_Visual_1'>
<pose frame=''>3.9453 0 1.25 0 -0 0</pose>
<geometry>
<Box>
<size>0.35941 0.15 2.5</size>
</Box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
</visual>
<collision name='Wall_0_Collision_1'>
<geometry>
<Box>
<size>0.35941 0.15 2.5</size>
</Box>
</geometry>
<pose frame=''>3.9453 0 1.25 0 -0 0</pose>
</collision>
<visual name='Wall_0_Visual_2'>
<pose frame=''>2.76559 0 0 0 -0 0</pose>
<geometry>
<Box>
<size>2 0.15 0</size>
</Box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
</visual>
<collision name='Wall_0_Collision_2'>
<geometry>
<Box>
<size>2 0.15 0</size>
</Box>
</geometry>
<pose frame=''>2.76559 0 0 0 -0 0</pose>
</collision>
<visual name='Wall_0_Visual_3'>
<pose frame=''>2.76559 0 2.45 0 -0 0</pose>
<geometry>
<Box>
<size>2 0.15 0.1</size>
</Box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
</visual>
<collision name='Wall_0_Collision_3'>
<geometry>
<Box>
<size>2 0.15 0.1</size>
</Box>
</geometry>
<pose frame=''>2.76559 0 2.45 0 -0 0</pose>
</collision>
</link>
<link name='Wall_1'>
<collision name='Wall_1_Collision'>
<geometry>
<Box>
<size>3.75 0.15 2.5</size>
</Box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
</collision>
<visual name='Wall_1_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<Box>
<size>3.75 0.15 2.5</size>
</Box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
</visual>
<pose frame=''>2.3683 -1.87821 0 0 -0 1.5708</pose>
</link>
<link name='Wall_13'>
<collision name='Wall_13_Collision'>
<geometry>
<Box>
<size>9.25 0.15 2.5</size>
</Box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
</collision>
<visual name='Wall_13_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<Box>
<size>9.25 0.15 2.5</size>
</Box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
</visual>
<pose frame=''>-1.19255 3.74668 0 0 -0 3.14159</pose>
</link>
<link name='Wall_15'>
<pose frame=''>-4.65794 2.07148 0 0 0 -1.5708</pose>
<visual name='Wall_15_Visual_0'>
<pose frame=''>-1.67569 0 1.25 0 -0 0</pose>
<geometry>
<Box>
<size>0.148611 0.15 2.5</size>
</Box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
</visual>
<collision name='Wall_15_Collision_0'>
<geometry>
<Box>
<size>0.148611 0.15 2.5</size>
</Box>
</geometry>
<pose frame=''>-1.67569 0 1.25 0 -0 0</pose>
</collision>
<visual name='Wall_15_Visual_1'>
<pose frame=''>1.07431 0 1.25 0 -0 0</pose>
<geometry>
<Box>
<size>1.35139 0.15 2.5</size>
</Box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
</visual>
<collision name='Wall_15_Collision_1'>
<geometry>
<Box>
<size>1.35139 0.15 2.5</size>
</Box>
</geometry>
<pose frame=''>1.07431 0 1.25 0 -0 0</pose>
</collision>
<visual name='Wall_15_Visual_2'>
<pose frame=''>-0.601389 0 2.4 0 -0 0</pose>
<geometry>
<Box>
<size>2 0.15 0.2</size>
</Box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
</visual>
<collision name='Wall_15_Collision_2'>
<geometry>
<Box>
<size>2 0.15 0.2</size>
</Box>
</geometry>
<pose frame=''>-0.601389 0 2.4 0 -0 0</pose>
</collision>
</link>
<link name='Wall_16'>
<collision name='Wall_16_Collision'>
<geometry>
<Box>
<size>1.25 0.15 2.5</size>
</Box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
</collision>
<visual name='Wall_16_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<Box>
<size>1.25 0.15 2.5</size>
</Box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
</visual>
<pose frame=''>-5.20794 0.396477 0 0 -0 3.14159</pose>
</link>
<link name='Wall_17'>
<pose frame=''>-5.75794 -1.65352 0 0 0 -1.5708</pose>
<visual name='Wall_17_Visual_0'>
<pose frame=''>-1.07794 0 1.25 0 -0 0</pose>
<geometry>
<Box>
<size>2.09412 0.15 2.5</size>
</Box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
</visual>
<collision name='Wall_17_Collision_0'>
<geometry>
<Box>
<size>2.09412 0.15 2.5</size>
</Box>
</geometry>
<pose frame=''>-1.07794 0 1.25 0 -0 0</pose>
</collision>
<visual name='Wall_17_Visual_1'>
<pose frame=''>1.04706 0 0.25 0 -0 0</pose>
<geometry>
<Box>
<size>2.15588 0.15 0.5</size>
</Box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
</visual>
<collision name='Wall_17_Collision_1'>
<geometry>
<Box>
<size>2.15588 0.15 0.5</size>
</Box>
</geometry>
<pose frame=''>1.04706 0 0.25 0 -0 0</pose>
</collision>
<visual name='Wall_17_Visual_2'>
<pose frame=''>2.01762 0 1.5 0 -0 0</pose>
<geometry>
<Box>
<size>0.214766 0.15 2</size>
</Box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
</visual>
<collision name='Wall_17_Collision_2'>
<geometry>
<Box>
<size>0.214766 0.15 2</size>
</Box>
</geometry>
<pose frame=''>2.01762 0 1.5 0 -0 0</pose>
</collision>
<visual name='Wall_17_Visual_3'>
<pose frame=''>0.939679 0 1.9 0 -0 0</pose>
<geometry>
<Box>
<size>1.94111 0.15 1.2</size>
</Box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
</visual>
<collision name='Wall_17_Collision_3'>
<geometry>
<Box>
<size>1.94111 0.15 1.2</size>
</Box>
</geometry>
<pose frame=''>0.939679 0 1.9 0 -0 0</pose>
</collision>
</link>
<link name='Wall_19'>
<collision name='Wall_19_Collision'>
<geometry>
<Box>
<size>2.5 0.15 2.5</size>
</Box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
</collision>
<visual name='Wall_19_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<Box>
<size>2.5 0.15 2.5</size>
</Box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
</visual>
<pose frame=''>-2.53404 1.06604 0 0 -0 0</pose>
</link>
<link name='Wall_20'>
<collision name='Wall_20_Collision'>
<geometry>
<Box>
<size>2 0.15 2.5</size>
</Box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
</collision>
<visual name='Wall_20_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<Box>
<size>2 0.15 2.5</size>
</Box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
</visual>
<pose frame=''>-1.35904 0.141035 0 0 0 -1.5708</pose>
</link>
<link name='Wall_21'>
<collision name='Wall_21_Collision'>
<geometry>
<Box>
<size>0.25 0.15 2.5</size>
</Box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
</collision>
<visual name='Wall_21_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<Box>
<size>0.25 0.15 2.5</size>
</Box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
</visual>
<pose frame=''>-1.40904 -0.783965 0 0 -0 3.14159</pose>
</link>
<link name='Wall_23'>
<collision name='Wall_23_Collision'>
<geometry>
<Box>
<size>3.5 0.15 2.5</size>
</Box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
</collision>
<visual name='Wall_23_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<Box>
<size>3.5 0.15 2.5</size>
</Box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
</visual>
<pose frame=''>-3.91607 -0.818528 0 0 -0 3.14159</pose>
</link>
<link name='Wall_24'>
<collision name='Wall_24_Collision'>
<geometry>
<Box>
<size>0.25 0.15 2.5</size>
</Box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
</collision>
<visual name='Wall_24_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<Box>
<size>0.25 0.15 2.5</size>
</Box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
</visual>
<pose frame=''>-5.64107 -0.818528 0 0 -0 3.14159</pose>
</link>
<link name='Wall_26'>
<collision name='Wall_26_Collision'>
<geometry>
<Box>
<size>2 0.15 2.5</size>
</Box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
</collision>
<visual name='Wall_26_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<Box>
<size>2 0.15 2.5</size>
</Box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
</visual>
<pose frame=''>-3.31229 0.121197 0 0 0 -1.5708</pose>
</link>
<link name='Wall_28'>
<collision name='Wall_28_Collision'>
<geometry>
<Box>
<size>1.75 0.15 2.5</size>
</Box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
</collision>
<visual name='Wall_28_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<Box>
<size>1.75 0.15 2.5</size>
</Box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
</visual>
<pose frame=''>-1.86043 2.95447 0 0 0 -1.5708</pose>
</link>
<link name='Wall_30'>
<collision name='Wall_30_Collision'>
<geometry>
<Box>
<size>0.75 0.15 2.5</size>
</Box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
</collision>
<visual name='Wall_30_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<Box>
<size>0.75 0.15 2.5</size>
</Box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
</visual>
<pose frame=''>-1.56043 2.15447 0 0 -0 0</pose>
</link>
<link name='Wall_32'>
<collision name='Wall_32_Collision'>
<geometry>
<Box>
<size>2.75 0.15 2.5</size>
</Box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
</collision>
<visual name='Wall_32_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<Box>
<size>2.75 0.15 2.5</size>
</Box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
</visual>
<pose frame=''>-0.228595 2.39065 0 0 0 -1.5708</pose>
</link>
<link name='Wall_35'>
<collision name='Wall_35_Collision'>
<geometry>
<Box>
<size>2.75 0.15 2.5</size>
</Box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
</collision>
<visual name='Wall_35_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<Box>
<size>2.75 0.15 2.5</size>
</Box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
</visual>
<pose frame=''>1.29304 2.34692 0 0 0 -1.5708</pose>
</link>
<link name='Wall_37'>
<collision name='Wall_37_Collision'>
<geometry>
<Box>
<size>1.5 0.15 2.5</size>
</Box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
</collision>
<visual name='Wall_37_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<Box>
<size>1.5 0.15 2.5</size>
</Box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
</visual>
<pose frame=''>1.36463 1.03216 0 0 -0 0</pose>
</link>
<link name='Wall_4'>
<pose frame=''>4.05869 -2.66379 0 0 -0 0</pose>
<visual name='Wall_4_Visual_0'>
<pose frame=''>-1.38368 0 1.25 0 -0 0</pose>
<geometry>
<Box>
<size>0.732645 0.15 2.5</size>
</Box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
</visual>
<collision name='Wall_4_Collision_0'>
<geometry>
<Box>
<size>0.732645 0.15 2.5</size>
</Box>
</geometry>
<pose frame=''>-1.38368 0 1.25 0 -0 0</pose>
</collision>
<visual name='Wall_4_Visual_1'>
<pose frame=''>1.61632 0 1.25 0 -0 0</pose>
<geometry>
<Box>
<size>0.267355 0.15 2.5</size>
</Box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
</visual>
<collision name='Wall_4_Collision_1'>
<geometry>
<Box>
<size>0.267355 0.15 2.5</size>
</Box>
</geometry>
<pose frame=''>1.61632 0 1.25 0 -0 0</pose>
</collision>
<visual name='Wall_4_Visual_2'>
<pose frame=''>0.232645 0 2.45 0 -0 0</pose>
<geometry>
<Box>
<size>2.5 0.15 0.1</size>
</Box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
</visual>
<collision name='Wall_4_Collision_2'>
<geometry>
<Box>
<size>2.5 0.15 0.1</size>
</Box>
</geometry>
<pose frame=''>0.232645 0 2.45 0 -0 0</pose>
</collision>
</link>
<link name='Wall_41'>
<collision name='Wall_41_Collision'>
<geometry>
<Box>
<size>0.25 0.15 2.5</size>
</Box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
</collision>
<visual name='Wall_41_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<Box>
<size>0.25 0.15 2.5</size>
</Box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
</visual>
<pose frame=''>-0.228595 1.04065 0 0 0 -1.5708</pose>
</link>
<link name='Wall_44'>
<collision name='Wall_44_Collision'>
<geometry>
<Box>
<size>3.75 0.15 2.5</size>
</Box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
</collision>
<visual name='Wall_44_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<Box>
<size>3.75 0.15 2.5</size>
</Box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
</visual>
<pose frame=''>-0.233629 -1.95772 0 0 0 -1.5708</pose>
</link>
<link name='Wall_50'>
<pose frame=''>5.74567 0.386841 0 0 0 -1.57472</pose>
<visual name='Wall_50_Visual_0'>
<pose frame=''>-2.83254 0 1.25 0 -0 0</pose>
<geometry>
<Box>
<size>0.586231 0.15 2.5</size>
</Box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
</visual>
<collision name='Wall_50_Collision_0'>
<geometry>
<Box>
<size>0.586231 0.15 2.5</size>
</Box>
</geometry>
<pose frame=''>-2.83254 0 1.25 0 -0 0</pose>
</collision>
<visual name='Wall_50_Visual_1'>
<pose frame=''>1.30211 0 1.25 0 -0 0</pose>
<geometry>
<Box>
<size>3.64708 0.15 2.5</size>
</Box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
</visual>
<collision name='Wall_50_Collision_1'>
<geometry>
<Box>
<size>3.64708 0.15 2.5</size>
</Box>
</geometry>
<pose frame=''>1.30211 0 1.25 0 -0 0</pose>
</collision>
<visual name='Wall_50_Visual_2'>
<pose frame=''>-1.53042 0 2.4 0 -0 0</pose>
<geometry>
<Box>
<size>2.018 0.15 0.2</size>
</Box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
</visual>
<collision name='Wall_50_Collision_2'>
<geometry>
<Box>
<size>2.018 0.15 0.2</size>
</Box>
</geometry>
<pose frame=''>-1.53042 0 2.4 0 -0 0</pose>
</collision>
</link>
<link name='Wall_52'>
<pose frame=''>4.58265 3.43747 0 0 -0 3.14159</pose>
<visual name='Wall_52_Visual_0'>
<pose frame=''>-0.530284 0 1.25 0 -0 0</pose>
<geometry>
<Box>
<size>1.43943 0.15 2.5</size>
</Box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
</visual>
<collision name='Wall_52_Collision_0'>
<geometry>
<Box>
<size>1.43943 0.15 2.5</size>
</Box>
</geometry>
<pose frame=''>-0.530284 0 1.25 0 -0 0</pose>
</collision>
<visual name='Wall_52_Visual_1'>
<pose frame=''>1.16972 0 1.25 0 -0 0</pose>
<geometry>
<Box>
<size>0.160568 0.15 2.5</size>
</Box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
</visual>
<collision name='Wall_52_Collision_1'>
<geometry>
<Box>
<size>0.160568 0.15 2.5</size>
</Box>
</geometry>
<pose frame=''>1.16972 0 1.25 0 -0 0</pose>
</collision>
<visual name='Wall_52_Visual_2'>
<pose frame=''>0.639432 0 2.25 0 -0 0</pose>
<geometry>
<Box>
<size>0.9 0.15 0.5</size>
</Box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
</visual>
<collision name='Wall_52_Collision_2'>
<geometry>
<Box>
<size>0.9 0.15 0.5</size>
</Box>
</geometry>
<pose frame=''>0.639432 0 2.25 0 -0 0</pose>
</collision>
</link>
<link name='Wall_53'>
<collision name='Wall_53_Collision'>
<geometry>
<Box>
<size>0.463251 0.15 2.5</size>
</Box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
</collision>
<visual name='Wall_53_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<Box>
<size>0.463251 0.15 2.5</size>
</Box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
</visual>
<pose frame=''>3.38255 3.59207 0 0 -0 1.73174</pose>
</link>
<link name='Wall_55'>
<pose frame=''>1.06733 -0.11797 0 0 -0 0.030549</pose>
<visual name='Wall_55_Visual_0'>
<pose frame=''>-1.28558 0 1.25 0 -0 0</pose>
<geometry>
<Box>
<size>0.181978 0.15 2.5</size>
</Box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
</visual>
<collision name='Wall_55_Collision_0'>
<geometry>
<Box>
<size>0.181978 0.15 2.5</size>
</Box>
</geometry>
<pose frame=''>-1.28558 0 1.25 0 -0 0</pose>
</collision>
<visual name='Wall_55_Visual_1'>
<pose frame=''>0.540989 0 1.25 0 -0 0</pose>
<geometry>
<Box>
<size>1.67116 0.15 2.5</size>
</Box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
</visual>
<collision name='Wall_55_Collision_1'>
<geometry>
<Box>
<size>1.67116 0.15 2.5</size>
</Box>
</geometry>
<pose frame=''>0.540989 0 1.25 0 -0 0</pose>
</collision>
<visual name='Wall_55_Visual_2'>
<pose frame=''>-0.744593 0 2.25 0 -0 0</pose>
<geometry>
<Box>
<size>0.9 0.15 0.5</size>
</Box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
</visual>
<collision name='Wall_55_Collision_2'>
<geometry>
<Box>
<size>0.9 0.15 0.5</size>
</Box>
</geometry>
<pose frame=''>-0.744593 0 2.25 0 -0 0</pose>
</collision>
</link>
<link name='Wall_57'>
<pose frame=''>-0.766361 -0.161298 0 0 -0 0.006708</pose>
<visual name='Wall_57_Visual_0'>
<pose frame=''>-0.534301 0 1.25 0 -0 0</pose>
<geometry>
<Box>
<size>0.146885 0.15 2.5</size>
</Box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
</visual>
<collision name='Wall_57_Collision_0'>
<geometry>
<Box>
<size>0.146885 0.15 2.5</size>
</Box>
</geometry>
<pose frame=''>-0.534301 0 1.25 0 -0 0</pose>
</collision>
<visual name='Wall_57_Visual_1'>
<pose frame=''>0.511599 0 1.25 0 -0 0</pose>
<geometry>
<Box>
<size>0.19229 0.15 2.5</size>
</Box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
</visual>
<collision name='Wall_57_Collision_1'>
<geometry>
<Box>
<size>0.19229 0.15 2.5</size>
</Box>
</geometry>
<pose frame=''>0.511599 0 1.25 0 -0 0</pose>
</collision>
<visual name='Wall_57_Visual_2'>
<pose frame=''>-0.022702 0 2.25 0 -0 0</pose>
<geometry>
<Box>
<size>0.876312 0.15 0.5</size>
</Box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
</visual>
<collision name='Wall_57_Collision_2'>
<geometry>
<Box>
<size>0.876312 0.15 0.5</size>
</Box>
</geometry>
<pose frame=''>-0.022702 0 2.25 0 -0 0</pose>
</collision>
</link>
<static>1</static>
</model>
</sdf>
主要是Gazebo仿真场景中各类物体的具体属性参数。
model.config：
<?xml version="1.0" ?>
<model>
<name>whiz_ex_window</name>
<version>1.0</version>
<sdf version="1.6">model.sdf</sdf>
<author>
<name></name>
<email></email>
</author>
<description></description>
</model>
可以做一些署名，邮箱，说明等。
launch：
以gazebo_kamtoa.launch为例，具体说明：
<?xml version="1.0"?>
<!--
ENTRY FILE FOR LAUNCHING FULL SIMULATION
Company : Obodroid Corporation
Author : Theppasith N. <theppasith@gmail.com>
-->
<launch>
<!-- Parameters by Console Arguments -->
<arg name="model_name" default="kamtoa_robot"/>
<arg name="urdf_path" default="$(find kamtoa_description)/urdf/kamtoa_prototype/$(arg model_name).xacro"/>
<arg name="world_name" default="building1.world"/>
<!-- ROBOT Description file for RVIZ and Gazebo [Globally Set]-->
<param name ="robot_description" command="$(find xacro)/xacro --inorder $(arg urdf_path)" />
<!-- Initialize RVIZ for visualize sensing system -->
<!-- use model from robot_description param-->
<include file="$(find som_o_description)/launch/rviz_gui.launch" />
<!-- Initialize Gazebo World -->
<include file="$(find kamtoa_gazebo)/launch/world.launch">
<arg name="world_name" value="$(arg world_name)"/>
</include>
<!-- Spawn URDF in the gazebo world -->
<!-- use model from robot_description param-->
<include file="$(find kamtoa_gazebo)/launch/spawner.launch">
<arg name="model_name" value="$(arg model_name)"/>
<!-- Param name robot_description (global) -->
</include>
</launch>
分为基本参数，机器人描述（RVIZ和Gazebo），RVIZ初始化，Gazebo初始化等。这时，如果在终端运行：
~$ roslaunch kamtoa_gazebo gazebo_kamtoa.launch
可以启动roscore，rviz和gazebo如下：
relaybotBox@relaybotBox-desktop:~$ roslaunch kamtoa_gazebo gazebo_kamtoa.launch ... logging to /home/relaybotBox/.ros/log/e9deba26-9786-11e6-be4d-00e0b4159b08/roslaunch-relaybotBox-desktop-5005.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.
started roslaunch server http://relaybotBox-desktop:41694/
SUMMARY
========
ParaMETERS
* /robot_description: <?xml version="1....
* /rosdistro: kinetic
* /rosversion: 1.12.5
* /use_gui: True
* /use_sim_time: True
NODES
/
gazebo (gazebo_ros/gazebo)
joint_state_publisher (joint_state_publisher/joint_state_publisher)
robot_state_publisher (robot_state_publisher/state_publisher)
rviz (rviz/rviz)
urdf_spawner (gazebo_ros/spawn_model)
auto-starting new master
process[master]: started with pid [5019]
ROS_MASTER_URI=http://localhost:11311
setting /run_id to e9deba26-9786-11e6-be4d-00e0b4159b08
process[rosout-1]: started with pid [5033]
started core service [/rosout]
process[joint_state_publisher-2]: started with pid [5046]
process[robot_state_publisher-3]: started with pid [5054]
process[rviz-4]: started with pid [5059]
process[gazebo-5]: started with pid [5066]
process[urdf_spawner-6]: started with pid [5090]
spawn_model script started
[INFO] [WallTime: 1477051642.645687] [0.000000] Loading model xml from ros parameter
[INFO] [WallTime: 1477051642.655133] [0.000000] Waiting for service /gazebo/spawn_urdf_model
[ INFO] [1477051642.933367561]: Finished loading Gazebo ROS API Plugin.
[ INFO] [1477051642.935174498]: waitForService: Service [/gazebo/set_physics_properties] has not been advertised,waiting...
[INFO] [WallTime: 1477051643.878953] [0.000000] Calling service /gazebo/spawn_urdf_model
[INFO] [WallTime: 1477051643.975877] [1288.802000] Spawn status: SpawnModel: Model pushed to spawn queue,but spawn service timed out waiting for model to appear in simulation under the name kamtoa_robot
Warning [parser.cc:778] XML Element[visualize],child of element[link] not defined in SDF. Ignoring[visualize]. You may have an incorrect SDF file,or an sdformat version that doesn't support this element.
[urdf_spawner-6] process has finished cleanly
log file: /home/relaybotBox/.ros/log/e9deba26-9786-11e6-be4d-00e0b4159b08/urdf_spawner-6*.log
[ INFO] [1477051645.070944164,1288.802000000]: Camera Plugin (robotNamespace = /),Info: Using the 'robotNamespace' param: '/'
[ INFO] [1477051645.096626172,1288.802000000]: Camera Plugin (ns = /) <tf_prefix_>,set to ""
[ INFO] [1477051645.459096731,1288.802000000]: Laser Plugin (robotNamespace = /),Info: Using the 'robotNamespace' param: '/'
[ INFO] [1477051645.459748522,1288.802000000]: Starting Laser Plugin (ns = /)!
[ INFO] [1477051645.476788533,1288.802000000]: Laser Plugin (ns = /) <tf_prefix_>,set to ""
[ INFO] [1477051645.565133751,1288.802000000]: Starting plugin Diffdrive(ns = //)!
[ INFO] [1477051645.565451828,1288.802000000]: Diffdrive(ns = //): <rosDebugLevel> = Debug
[ INFO] [1477051645.568106826,1288.802000000]: Diffdrive(ns = //): <tf_prefix> =
[DEBUG] [1477051645.568619326,1288.802000000]: Diffdrive(ns = //): <commandTopic> = cmd_vel
[DEBUG] [1477051645.568724979,1288.802000000]: Diffdrive(ns = //): <odometryTopic> = odom
[DEBUG] [1477051645.568867307,1288.802000000]: Diffdrive(ns = //): <odometryFrame> = odom
[DEBUG] [1477051645.568927646,1288.802000000]: Diffdrive(ns = //): <robotBaseFrame> = base_footprint
[DEBUG] [1477051645.569256884,1288.802000000]: Diffdrive(ns = //): <publishWheelTF> = false
[DEBUG] [1477051645.569338164,1288.802000000]: Diffdrive(ns = //): <publishWheelJointState> = false
[DEBUG] [1477051645.569393655,1288.802000000]: Diffdrive(ns = //): <legacyMode> = ture
[DEBUG] [1477051645.569843607,1288.802000000]: Diffdrive(ns = //): <wheelSeparation> = 0.29999999999999999
[DEBUG] [1477051645.569951337,1288.802000000]: Diffdrive(ns = //): <wheelDiameter> = 0.082000000000000003
[DEBUG] [1477051645.570012696,1288.802000000]: Diffdrive(ns = //): <wheelacceleration> = 1.8
[DEBUG] [1477051645.570077175,1288.802000000]: Diffdrive(ns = //): <wheelTorque> = 30
[DEBUG] [1477051645.570139351,1288.802000000]: Diffdrive(ns = //): <updaterate> = 100
[ WARN] [1477051645.570811579,1288.802000000]: Diffdrive(ns = //): <odometrySource> no matching key to 1
[DEBUG] [1477051645.571414503,1288.802000000]: Diffdrive(ns = //): <odometrySource> = default := 1
[DEBUG] [1477051645.572053861,1288.802000000]: Diffdrive(ns = //): <leftJoint> = front_left_wheel_joint
[DEBUG] [1477051645.572185748,1288.802000000]: Diffdrive(ns = //): <rightJoint> = front_right_wheel_joint
[ INFO] [1477051645.576771098,1288.802000000]: Diffdrive(ns = //): Try to subscribe to cmd_vel!
[ INFO] [1477051645.593900394,1288.802000000]: Diffdrive(ns = //): Subscribe to cmd_vel!
[ INFO] [1477051645.597711053,1288.802000000]: Diffdrive(ns = //): Advertise odom on odom !
[ INFO] [1477051645.648506913,1288.825000000]: waitForService: Service [/gazebo/set_physics_properties] is Now available.
[ INFO] [1477051645.836105586,1288.862000000]: Physics dynamic reconfigure ready.
可以用如下工具进行分析：
rqt_action rqt_logger_level rqt_robot_monitor rqt_bag rqt_moveit rqt_robot_steering rqt_bag_plugins rqt_msg rqt_runtime_monitor rqt_console rqt_nav_view rqt_rviz rqt_dep rqt_plot rqt_service_caller rqt_graph rqt_pose_view rqt_shell rqt_gui rqt_publisher rqt_srv rqt_gui_cpp rqt_py_common rqt_tf_tree rqt_gui_py rqt_py_console rqt_top rqt_image_view rqt_reconfigure rqt_topic rqt_launch rqt_robot_dashboard rqt_web
以rqt_topic为例，可以参看全部主题：
~$ rosrun rqt_topic rqt_topic
如果需要查看camera_rgb，可以运行如下命令：
~$ rosrun rqt_image_view rqt_image_view
运行rosrun rqt_graph rqt_graph：
此时，gazebo与其他节点并无通信，然后启动如下命令：
~$ roslaunch kamtoa_teleop teleop_keyboard.launch
<launch>
<node pkg="kamtoa_teleop" type="kamtoa_keyboard.py" name="kamtoa_keyboard" output="screen" />
</launch>
这时，就可以使用键盘通过/cmd_vel控制gazebo中的机器人在环境中运动，当然键盘控制还有其他一些功能，请看如下源码。
#!/usr/bin/env python
# copyright (c) 2011,Willow Garage,Inc.
# All rights reserved.
#
# Redistribution and use in source and binary forms,with or without
# modification,are permitted provided that the following conditions are met:
#
# * Redistributions of source code must retain the above copyright
# notice,this list of conditions and the following disclaimer.
# * Redistributions in binary form must reproduce the above copyright
# notice,this list of conditions and the following disclaimer in the
# documentation and/or other materials provided with the distribution.
# * Neither the name of the Willow Garage,Inc. nor the names of its
# contributors may be used to endorse or promote products derived from
# this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE copYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
# AND ANY EXPRESS OR IMPLIED WARRANTIES,INCLUDING,BUT NOT LIMITED TO,THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND fitness FOR A PARTIculaR PURPOSE
# ARE disCLaimED. IN NO EVENT SHALL THE copYRIGHT OWNER OR CONTRIBUTORS BE
# LIABLE FOR ANY DIRECT,INDIRECT,INCIDENTAL,SPECIAL,EXEMPLARY,OR
# CONSEQUENTIAL damAGES (INCLUDING,PROCUREMENT OF
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,DATA,OR PROFITS; OR BUSInesS
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,WHETHER IN
# CONTRACT,STRICT LIABILITY,OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH damAGE.
import rospy
from geometry_msgs.msg import Twist
import sys,select,termios,tty
msg = """
Control Your Turtlebot!
---------------------------
Moving around:
u i o
j k l
m,.
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%
space key,k : force stop
anything else : stop smoothly
CTRL-C to quit
"""
lightSwitches = {
'1':'light0','2':'light1','3':'light2','4':'light3','5':'light4'
}
moveBindings = {
'i':(1,0),'o':(1,-1),'j':(0,1),'l':(0,'u':(1,',':(-1,'.':(-1,'m':(-1,}
speedBindings={
'q':(1.1,1.1),'z':(.9,.9),'w':(1.1,'x':(.9,'e':(1,'c':(1,}
def getKey():
tty.setraw(sys.stdin.fileno())
rlist,_,_ = select.select([sys.stdin],[],0.1)
if rlist:
key = sys.stdin.read(1)
else:
key = ''
termios.tcsetattr(sys.stdin,termios.TCSADRAIN,settings)
return key
speed = .2
turn = 1
def vels(speed,turn):
return "currently:\tspeed %s\tturn %s " % (speed,turn)
if __name__=="__main__":
settings = termios.tcgetattr(sys.stdin)
rospy.init_node('Kamtoa_teleop_keyboard')
pub = rospy.Publisher('cmd_vel',Twist,queue_size=5)
light_select = ''
x = 0
th = 0
status = 0
count = 0
acc = 0.1
target_speed = 0
target_turn = 0
control_speed = 0
control_turn = 0
try:
print msg
print vels(speed,turn)
while(1):
key = getKey()
if key in moveBindings.keys():
x = moveBindings[key][0]
th = moveBindings[key][1]
count = 0
elif key in speedBindings.keys():
speed = speed * speedBindings[key][0]
turn = turn * speedBindings[key][1]
count = 0
print vels(speed,turn)
if (status == 14):
print msg
status = (status + 1) % 15
elif key == ' ' or key == 'k' :
x = 0
th = 0
control_speed = 0
control_turn = 0
elif key in lightSwitches.keys():
print "lightSwitch pressed!"
else:
count = count + 1
if count > 4:
x = 0
th = 0
if (key == '\x03'):
break
target_speed = speed * x
target_turn = turn * th
if target_speed > control_speed:
control_speed = min( target_speed,control_speed + 0.02 )
elif target_speed < control_speed:
control_speed = max( target_speed,control_speed - 0.02 )
else:
control_speed = target_speed
if target_turn > control_turn:
control_turn = min( target_turn,control_turn + 0.1 )
elif target_turn < control_turn:
control_turn = max( target_turn,control_turn - 0.1 )
else:
control_turn = target_turn
twist = Twist()
twist.linear.x = control_speed; twist.linear.y = 0; twist.linear.z = 0
twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = control_turn
pub.publish(twist)
except:
print e
finally:
twist = Twist()
twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0
twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0
pub.publish(twist)
termios.tcsetattr(sys.stdin,settings)
使用手机控制机器人运动也很方便，先查询一下地址：
relaybotBox@relaybotBox-desktop:~$ ifconfig -a
enp3s0 Link encap:以太网 硬件地址 00:e0:b4:15:9b:08
inet 地址:192.168.3.23 广播:192.168.3.255 掩码:255.255.255.0
inet6 地址: fe80::3805:e5fe:19f0:19e2/64 Scope:Link
UP broADCAST RUNNING MULTICAST MTU:1500 跃点数:1
接收数据包:32257 错误:0 丢弃:0 过载:0 帧数:0
发送数据包:41654 错误:0 丢弃:0 过载:0 载波:0
碰撞:0 发送队列长度:1000
接收字节:23057526 (23.0 MB) 发送字节:17828352 (17.8 MB)
enp4s0 Link encap:以太网 硬件地址 00:e0:b4:15:9b:09
UP broADCAST MULTICAST MTU:1500 跃点数:1
接收数据包:0 错误:0 丢弃:0 过载:0 帧数:0
发送数据包:0 错误:0 丢弃:0 过载:0 载波:0
碰撞:0 发送队列长度:1000
接收字节:0 (0.0 B) 发送字节:0 (0.0 B)
lo Link encap:本地环回
inet 地址:127.0.0.1 掩码:255.0.0.0
inet6 地址: ::1/128 Scope:Host
UP LOOPBACK RUNNING MTU:65536 跃点数:1
接收数据包:8314866 错误:0 丢弃:0 过载:0 帧数:0
发送数据包:8314866 错误:0 丢弃:0 过载:0 载波:0
碰撞:0 发送队列长度:1
接收字节:671600054 (671.6 MB) 发送字节:671600054 (671.6 MB)
在手机设置IP：192.168.3.23。连接成功后可以看到：
如何让机器人在环境中进行导航呢？运行下面命令：
~$ roslaunch kamtoa_navigation kamtoa_navigation.launch
其中代码如下：
<?xml version="1.0"?>
<!-- Fully Autonomous Navigation Launch file-->
<launch>
<!-- Map server -->
<arg name="map" default="sim_world"/>
<node name="map_server" pkg="map_server" type="map_server" args="$(find kamtoa_navigation)/maps/$(arg map).yaml"/>
<!-- Initial pose for the Localizer -->
<arg name="initial_pose_x" default="0.0"/>
<arg name="initial_pose_y" default="0.0"/>
<arg name="initial_pose_a" default="0.0"/>
<!-- Localizaer : AMCL -->
<include file="$(find kamtoa_navigation)/launch/modules/amcl.launch.xml">
<arg name="initial_pose_x" value="$(arg initial_pose_x)"/>
<arg name="initial_pose_y" value="$(arg initial_pose_y)"/>
<arg name="initial_pose_a" value="$(arg initial_pose_a)"/>
</include>
<!-- VeLocity Smoother -->
<include file="$(find kamtoa_navigation)/launch/modules/veLocity_smoother.launch.xml"/>
<!-- Navigation Stack (move_base's path planner,costmaps & obstacles manager) -->
<include file="$(find kamtoa_navigation)/launch/modules/move_base.launch.xml"/>
</launch>
是不是有很多眼熟的内容，导入Map，机器人位置初值，AMCL，速度平滑，导航等。
这是再看下rqt_graph：
各个节点之间联系清晰可见。
路径规划效果如下：
其他内容后续补充。