致谢源代码网址: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:
各个节点之间联系清晰可见。
路径规划效果如下:
其他内容后续补充。